publish disjoint set image rather than imshow it. Just cosmetic changes

Author: mpkuse

src/PoseGraphSLAM.cpp

@@ -1880,7 +1880,7 @@ void PoseGraphSLAM::reinit_ceres_problem_onnewloopedge_optimize6DOF()
             // reint_problem.SetParameterBlockConstant(  get_raw_ptr_to_opt_variable_q(ww_start) );
             // reint_problem.SetParameterBlockConstant(  get_raw_ptr_to_opt_variable_t(ww_start)  );
 
-            ceres::CostFunction * regularixa_cost = NodePoseRegularization::Create( getNodePose(ww_start), 1.2 );
+            ceres::CostFunction * regularixa_cost = NodePoseRegularization::Create( getNodePose(ww_start), 2.3 );
             ceres::ResidualBlockId resi_id = reint_problem.AddResidualBlock( regularixa_cost, NULL,  get_raw_ptr_to_opt_variable_q(ww_start), get_raw_ptr_to_opt_variable_t(ww_start) );
             regularization_terms_list.push_back( resi_id );
 

src/VizPoseGraph.cpp

@@ -7,6 +7,14 @@ void VizPoseGraph::setVisualizationPublisher( const ros::Publisher& pub )
   pub_pgraph = pub;
 }
 
+void VizPoseGraph::setImagePublisher( const ros::Publisher& pub )
+{
+
+  pub_image = pub;
+}
+
+
+
 // TODO removal
 void VizPoseGraph::setPathPublisher( const ros::Publisher& pub )
 {
@@ -453,3 +461,10 @@ void VizPoseGraph::publishThisVisualMarker( const visualization_msgs::Marker& th
 {
     pub_pgraph.publish( the_marker );
 }
+
+
+void VizPoseGraph::publishImage( const cv::Mat& im )
+{
+    sensor_msgs::ImagePtr msg = cv_bridge::CvImage( std_msgs::Header(), "bgr8", im ).toImageMsg();
+    pub_image.publish( msg );
+}

src/VizPoseGraph.h

@@ -40,6 +40,8 @@ class VizPoseGraph
 public:
     VizPoseGraph( const NodeDataManager* _manager, const PoseGraphSLAM* _slam ) : manager(_manager), slam(_slam){};
     void setVisualizationPublisher( const ros::Publisher& pub );
+    void setImagePublisher( const ros::Publisher& pub );
+
     void setPathPublisher( const ros::Publisher& pub ); //< TODO: removal
     void setOdometryPublisher( const ros::Publisher& pub );
 
@@ -60,14 +62,16 @@ class VizPoseGraph
     void publishSlamResidueVisual( int n ) const;
     void publishCameraVisualMarker( const Matrix4d& wTc, const string& ns, float r, float g, float b );
 
-    void publishThisVisualMarker( const visualization_msgs::Marker& the_marker ); 
+    void publishThisVisualMarker( const visualization_msgs::Marker& the_marker );
+    void publishImage( const cv::Mat& im );
 
 private:
     const NodeDataManager * manager=NULL;
     const PoseGraphSLAM * slam = NULL;
 
     // Publish Marker
     ros::Publisher pub_pgraph; //< Marker
+    ros::Publisher pub_image; //< publishes image.
     ros::Publisher pub_path_opt; //< Path, TODO: Removal
     ros::Publisher pub_odometry_opt; // nav_msgs::Odometry //TODO Removal
 };

src/keyframe_pose_graph_slam_node.cpp

@@ -245,17 +245,23 @@ void periodic_publish_optimized_poses_smart( const NodeDataManager * manager, co
 }
 
 
-void monitor_disjoint_set_datastructure( const NodeDataManager * manager )
+void monitor_disjoint_set_datastructure( const NodeDataManager * manager, const VizPoseGraph * viz )
 {
     ros::Rate loop_rate(1);
 
     #if 1
     while( ros::ok() )
     {
         cv::Mat im_disp;
-        manager->getWorldsConstPtr()->disjoint_set_status_image(im_disp);
+        // manager->getWorldsConstPtr()->disjoint_set_status_image(im_disp); // will get bubles as well as the text
+        manager->getWorldsConstPtr()->disjoint_set_status_image(im_disp, true, false); // only bubles
+
+        #if 1 // set this to zero to imshow the image. helpful for debugging.
+        viz->publishImage( im_disp );
+        #else
         cv::imshow( "disjoint_set_status_image" , im_disp );
         cv::waitKey(30);
+        #endif
 
         loop_rate.sleep();
     }
@@ -556,6 +562,11 @@ int main( int argc, char ** argv)
     ROS_INFO( "Publish to %s", marker_topic.c_str() );
     ros::Publisher pub = nh.advertise<visualization_msgs::Marker>( marker_topic , 1000 );
 
+    //--- Image ---//
+    string im_topic = string( "viz/disjoint_set_status_image");
+    ROS_INFO( "Publish Image to %s", im_topic.c_str() );
+    ros::Publisher pub_im = nh.advertise<sensor_msgs::Image>( im_topic , 1000 );
+
 
     //--- Optimzied Path Publisher ---//
     string opt_path_topic = string( "opt_path");
@@ -583,13 +594,14 @@ int main( int argc, char ** argv)
     // another class for viz.
     VizPoseGraph * viz = new VizPoseGraph( manager, slam );
     viz->setVisualizationPublisher( pub );
+    viz->setImagePublisher( pub_im );
     viz->setPathPublisher( pub_path );
     viz->setOdometryPublisher( pub_odometry_opt );
 
     // setup manager publishers threads - adhoc
     // std::thread th3( periodic_publish_optimized_poses_smart, manager, slam, viz );
     std::thread th4( periodic_publish_odoms, manager, viz );
-    std::thread th5( monitor_disjoint_set_datastructure, manager );
+    std::thread th5( monitor_disjoint_set_datastructure, manager, viz );
 
     opt_traj_publisher_options options;
     options.line_color_style = 10;