From c627c797f93c92e6e4c53f9cda627760f08c94c1 Mon Sep 17 00:00:00 2001 From: Ryan Soussan Date: Mon, 22 Jul 2024 11:10:34 -0700 Subject: [PATCH] updated localization node --- .../include/localization_node/localization.h | 3 ++- localization/localization_node/src/localization.cc | 2 +- localization/localization_node/src/localization_nodelet.cc | 4 ++-- 3 files changed, 5 insertions(+), 4 deletions(-) diff --git a/localization/localization_node/include/localization_node/localization.h b/localization/localization_node/include/localization_node/localization.h index f4e6b729e5..37acce00d5 100644 --- 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 cam_params_; // Success params for adjusting keypoint thresholds std::deque successes_; ThresholdParams params_; diff --git a/localization/localization_node/src/localization.cc b/localization/localization_node/src/localization.cc index 7ed5da6ed3..5c7b218da4 100644 --- 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(&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 index 5363bafadb..0f2260083c 100644 --- 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(undistorted, &distorted); + (inst_->camera_params()).Convert(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(undistorted, &distorted); + (inst_->camera_params()).Convert(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); }