publishing the number of features

ctu-mrs · Nov 19, 2024 · 4c84f85 · 4c84f85
1 parent cf216b4
commit 4c84f85
Show file tree

Hide file tree

Showing 2 changed files with 14 additions and 0 deletions.
diff --git a/ov_msckf/src/ros/ROS1Visualizer.cpp b/ov_msckf/src/ros/ROS1Visualizer.cpp
@@ -62,6 +62,11 @@ ROS1Visualizer::ROS1Visualizer(std::shared_ptr<ros::NodeHandle> nh, std::shared_
   pub_points_sim = nh->advertise<sensor_msgs::PointCloud2>("points_sim", 2);
   PRINT_DEBUG("Publishing: %s\n", pub_points_sim.getTopic().c_str());
 
+  pub_num_points_slam = nh->advertise<std_msgs::Float64>("num_points_slam", 2);
+  PRINT_DEBUG("Publishing: %s\n", pub_num_points_slam.getTopic().c_str());
+  pub_num_points_msckf = nh->advertise<std_msgs::Float64>("num_points_msckf", 2);
+  PRINT_DEBUG("Publishing: %s\n", pub_num_points_msckf.getTopic().c_str());
+
   // Our tracking image
   it_pub_tracks = it.advertise("trackhist", 2);
   PRINT_DEBUG("Publishing: %s\n", it_pub_tracks.getTopic().c_str());
@@ -714,12 +719,20 @@ void ROS1Visualizer::publish_features() {
   cloud.header.frame_id = _uav_name_ + "/ov_global";
   pub_points_msckf.publish(cloud);
 
+  std_msgs::Float64 num_msckf_points;
+  num_msckf_points.data = feats_msckf.size();
+  pub_num_points_msckf.publish(num_msckf_points);
+
   // Get our good SLAM features
   std::vector<Eigen::Vector3d> feats_slam = _app->get_features_SLAM();
   sensor_msgs::PointCloud2 cloud_SLAM = ROSVisualizerHelper::get_ros_pointcloud(feats_slam);
   cloud_SLAM.header.frame_id = _uav_name_ + "/ov_global";
   pub_points_slam.publish(cloud_SLAM);
 
+  std_msgs::Float64 num_slam_points;
+  num_slam_points.data = feats_slam.size();
+  pub_num_points_slam.publish(num_slam_points);
+
   // Get our good ARUCO features
   std::vector<Eigen::Vector3d> feats_aruco = _app->get_features_ARUCO();
   sensor_msgs::PointCloud2 cloud_ARUCO = ROSVisualizerHelper::get_ros_pointcloud(feats_aruco);

diff --git a/ov_msckf/src/ros/ROS1Visualizer.h b/ov_msckf/src/ros/ROS1Visualizer.h
@@ -143,6 +143,7 @@ class ROS1Visualizer {
   image_transport::Publisher it_pub_tracks, it_pub_loop_img_depth, it_pub_loop_img_depth_color;
   ros::Publisher pub_poseimu, pub_odomimu, pub_pathimu;
   ros::Publisher pub_points_msckf, pub_points_slam, pub_points_aruco, pub_points_sim;
+  ros::Publisher pub_num_points_msckf, pub_num_points_slam;
   ros::Publisher pub_loop_pose, pub_loop_point, pub_loop_extrinsic, pub_loop_intrinsics;
   ros::Publisher pub_process_hz;
   std::shared_ptr<tf::TransformBroadcaster> mTfBr;