Merge branch 'master' of https://github.com/mpkuse/solve_keyframe_pose_graph

Author: mpkuse

src/NodeDataManager.cpp

@@ -1077,9 +1077,9 @@ void NodeDataManager::print_worlds_info( int verbosity ) const
 
     if( rel_pose_between_worlds ) {
     //// Relative transforms between worlds
-    this->getWorldsPtr()->print_summary(2);
+    this->getWorldsConstPtr()->print_summary(2);
     } else {
-    this->getWorldsPtr()->print_summary(0);
+    this->getWorldsConstPtr()->print_summary(0);
     }
 
 

src/VizPoseGraph.cpp

@@ -448,7 +448,7 @@ void VizPoseGraph::publishSlamResidueVisual( int n ) const
 
 void VizPoseGraph::publishCameraVisualMarker( const Matrix4d& wTc, const string& ns, float r, float g, float b,
     float linewidth_multiplier, float camera_size_multiplier,
-    float offset_x, float offset_y, float offset_z )
+    float offset_x, float offset_y, float offset_z ) const
 {
     visualization_msgs::Marker marker2 ;
     RosMarkerUtils::init_camera_marker( marker2, camera_size_multiplier );
@@ -463,7 +463,7 @@ void VizPoseGraph::publishCameraVisualMarker( const Matrix4d& wTc, const string&
     pub_pgraph.publish( marker2 );
 }
 
-void VizPoseGraph::publishXYZAxis( const Matrix4d& wT_axis, const string ns, int id, float scale )
+void VizPoseGraph::publishXYZAxis( const Matrix4d& wT_axis, const string ns, int id, float scale ) const
 {
     visualization_msgs::Marker axis;
     RosMarkerUtils::init_line_marker( axis );
@@ -495,13 +495,13 @@ void VizPoseGraph::publishXYZAxis( const Matrix4d& wT_axis, const string ns, int
 
 }
 
-void VizPoseGraph::publishThisVisualMarker( const visualization_msgs::Marker& the_marker )
+void VizPoseGraph::publishThisVisualMarker( const visualization_msgs::Marker& the_marker ) const
 {
     pub_pgraph.publish( the_marker );
 }
 
 
-void VizPoseGraph::publishImage( const cv::Mat& im )
+void VizPoseGraph::publishImage( const cv::Mat& im ) const
 {
     sensor_msgs::ImagePtr msg = cv_bridge::CvImage( std_msgs::Header(), "bgr8", im ).toImageMsg();
     pub_image.publish( msg );

src/VizPoseGraph.h

@@ -65,11 +65,11 @@ class VizPoseGraph
     void publishSlamResidueVisual( int n ) const;
     void publishCameraVisualMarker( const Matrix4d& wTc, const string& ns, float r, float g, float b,
         float linewidth_multiplier=1.0, float camera_size_multiplier=10.,
-        float offset_x=0.0, float offset_y=0.0, float offset_z=0.0  );
+        float offset_x=0.0, float offset_y=0.0, float offset_z=0.0  ) const;
 
-    void publishXYZAxis( const Matrix4d& wT_axis, const string ns, int id, float scale=1.0 );
-    void publishThisVisualMarker( const visualization_msgs::Marker& the_marker );
-    void publishImage( const cv::Mat& im );
+    void publishXYZAxis( const Matrix4d& wT_axis, const string ns, int id, float scale=1.0 ) const;
+    void publishThisVisualMarker( const visualization_msgs::Marker& the_marker ) const;
+    void publishImage( const cv::Mat& im ) const;
 
 private:
     const NodeDataManager * manager=NULL;

src/keyframe_pose_graph_slam_node.cpp

@@ -336,6 +336,7 @@ int main( int argc, char ** argv)
     // cmpr->bf_traj_publish_disable();
     std::thread bf_pub_th( &Composer::bf_traj_publish_thread, cmpr, 15 );
 
+
     // ++ start camera visual publisher thread
     cmpr->cam_visual_publish_enable();
     // cmpr->cam_visual_publish_disable();