diff --git a/ov_msckf/src/ros/ROS1Visualizer.cpp b/ov_msckf/src/ros/ROS1Visualizer.cpp index fdcc054b..e4ac929c 100644 --- a/ov_msckf/src/ros/ROS1Visualizer.cpp +++ b/ov_msckf/src/ros/ROS1Visualizer.cpp @@ -62,6 +62,11 @@ ROS1Visualizer::ROS1Visualizer(std::shared_ptr nh, std::shared_ pub_points_sim = nh->advertise("points_sim", 2); PRINT_DEBUG("Publishing: %s\n", pub_points_sim.getTopic().c_str()); + pub_num_points_slam = nh->advertise("num_points_slam", 2); + PRINT_DEBUG("Publishing: %s\n", pub_num_points_slam.getTopic().c_str()); + pub_num_points_msckf = nh->advertise("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 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 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 index b429d5da..448d1fc3 100644 --- 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 mTfBr;