updated localization node

rsoussan · Jul 22, 2024 · c627c79 · c627c79
1 parent 7e16023
commit c627c79
Show file tree

Hide file tree

Showing 3 changed files with 5 additions and 4 deletions.
diff --git a/localization/localization_node/include/localization_node/localization.h b/localization/localization_node/include/localization_node/localization.h
@@ -48,11 +48,12 @@ class Localizer {
   bool ReadParams(config_reader::ConfigReader& config, bool fatal_failure = true);
   bool Localize(cv_bridge::CvImageConstPtr image_ptr, ff_msgs::VisualLandmarks* vl,
      Eigen::Matrix2Xd* image_keypoints = NULL);
+  const camera::CameraParameters& camera_params() const { return *cam_params_; }
  private:
   void AdjustThresholds();
 
   sparse_mapping::SparseMap* map_;
-  std::shared_ptr camera::CameraParameters cam_params_;
+  std::shared_ptr<camera::CameraParameters> cam_params_;
   // Success params for adjusting keypoint thresholds
   std::deque<int> successes_;
   ThresholdParams params_;

diff --git a/localization/localization_node/src/localization.cc b/localization/localization_node/src/localization.cc
@@ -30,7 +30,7 @@ namespace localization_node {
 Localizer::Localizer(sparse_mapping::SparseMap* map): map_(map) {}
 
 bool Localizer::ReadParams(config_reader::ConfigReader& config, bool fatal_failure) {
-  cam_params_.reset(camera::CameraParameters(&config, "nav_cam"));
+  cam_params_ = std::make_shared<camera::CameraParameters>(&config, "nav_cam");
   std::string prefix;
   const auto detector_name = map_->GetDetectorName();
   if (detector_name == "ORGBRISK") {

diff --git a/localization/localization_node/src/localization_nodelet.cc b/localization/localization_node/src/localization_nodelet.cc
@@ -249,7 +249,7 @@ void LocalizationNodelet::Localize(void) {
       Eigen::Vector2d undistorted, distorted;
       undistorted[0] = vl.landmarks[i].u;
       undistorted[1] = vl.landmarks[i].v;
-      (map_->GetCameraParameters()).Convert<camera::UNDISTORTED_C, camera::DISTORTED>(undistorted, &distorted);
+      (inst_->camera_params()).Convert<camera::UNDISTORTED_C, camera::DISTORTED>(undistorted, &distorted);
       cv::circle(used_image->image, cv::Point(distorted[0], distorted[1]), 10, CV_RGB(255, 255, 255), 3, 8);
       cv::circle(used_image->image, cv::Point(distorted[0], distorted[1]), 6, CV_RGB(0, 0, 0), 3, 8);
     }
@@ -261,7 +261,7 @@ void LocalizationNodelet::Localize(void) {
       Eigen::Vector2d undistorted, distorted;
       undistorted[0] = image_keypoints.col(i)[0];
       undistorted[1] = image_keypoints.col(i)[1];
-      (map_->GetCameraParameters()).Convert<camera::UNDISTORTED_C, camera::DISTORTED>(undistorted, &distorted);
+      (inst_->camera_params()).Convert<camera::UNDISTORTED_C, camera::DISTORTED>(undistorted, &distorted);
       cv::circle(detected_image->image, cv::Point(distorted[0], distorted[1]), 10, CV_RGB(255, 255, 255), 3, 8);
       cv::circle(detected_image->image, cv::Point(distorted[0], distorted[1]), 6, CV_RGB(0, 0, 0), 2, 8);
     }