added vl runtime to msg

rsoussan · Jun 30, 2024 · a20db25 · a20db25
1 parent 8ab2585
commit a20db25
Show file tree

Hide file tree

Showing 3 changed files with 7 additions and 0 deletions.
diff --git a/communications/ff_msgs/msg/VisualLandmarks.msg b/communications/ff_msgs/msg/VisualLandmarks.msg
@@ -22,3 +22,4 @@ Header header # header with timestamp
 uint32 camera_id # image ID, associated with registration pulse
 geometry_msgs/Pose pose # estimated camera pose from features
 ff_msgs/VisualLandmark[] landmarks # list of all landmarks
+float32 runtime # Time it took to calculate the pose and matching landmarks
diff --git a/localization/localization_node/include/localization_node/localization.h b/localization/localization_node/include/localization_node/localization.h
@@ -24,6 +24,7 @@
 #include <config_reader/config_reader.h>
 #include <cv_bridge/cv_bridge.h>
 #include <ff_msgs/VisualLandmarks.h>
+#include <localization_common/timer.h>
 #include <sensor_msgs/PointCloud2.h>
 
 #include <deque>
@@ -54,6 +55,7 @@ class Localizer {
   // Success params for adjusting keypoint thresholds
   std::deque<int> successes_;
   ThresholdParams params_;
+  localization_common::Timer timer_ = localization_common::Timer("VL Runtime");
 };
 
 };  // namespace localization_node

diff --git a/localization/localization_node/src/localization.cc b/localization/localization_node/src/localization.cc
@@ -111,6 +111,7 @@ bool Localizer::Localize(cv_bridge::CvImageConstPtr image_ptr, ff_msgs::VisualLa
   vl->header.stamp = image_ptr->header.stamp;
   vl->header.frame_id = "world";
 
+  timer_.Start();
   map_->DetectFeatures(image_ptr->image, multithreaded, &image_descriptors, image_keypoints);
   camera::CameraModel camera(Eigen::Vector3d(),
                              Eigen::Matrix3d::Identity(),
@@ -122,14 +123,17 @@ bool Localizer::Localize(cv_bridge::CvImageConstPtr image_ptr, ff_msgs::VisualLa
     successes_.emplace_back(0);
     AdjustThresholds();
     // LOG(INFO) << "Failed to localize image.";
+    timer_.Stop();
     return false;
   }
   successes_.emplace_back(1);
   AdjustThresholds();
+  timer_.Stop();
 
   Eigen::Affine3d global_pose = camera.GetTransform().inverse();
   Eigen::Quaterniond quat(global_pose.rotation());
 
+  vl->runtime = timer_.last_value();
   vl->pose.position = msg_conversions::eigen_to_ros_point(global_pose.translation());
   vl->pose.orientation = msg_conversions::eigen_to_ros_quat(quat);
   assert(landmarks.size() == observations.size());