fixed a nasty bug relating to node regularization. had stored the references and it was a mess. not storing the values

mpkuse · mpkuse · commit 85689c204050 · 2019-03-07T19:57:44.000+08:00
diff --git a/src/PoseGraphSLAM.cpp b/src/PoseGraphSLAM.cpp
@@ -1354,8 +1354,8 @@ bool PoseGraphSLAM::saveAsJSON(const string base_path)
 // #define __reint_allocation_cout(msg)  msg;
 #define __reint_allocation_cout(msg)  ;
 
-// #define __reint_odom_cout(msg) msg;
-#define __reint_odom_cout(msg) ;
+#define __reint_odom_cout(msg) msg;
+// #define __reint_odom_cout(msg) ;
 
 
 
@@ -1382,7 +1382,7 @@ void PoseGraphSLAM::reinit_ceres_problem_onnewloopedge_optimize6DOF()
     ceres::Solver::Summary reint_summary;
     reint_options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;
     reint_options.minimizer_progress_to_stdout = false;
-    reint_options.max_num_iterations = 50;
+    reint_options.max_num_iterations = 10;
     // reint_options.enable_fast_removal = true;
     eigenquaternion_parameterization = new ceres::EigenQuaternionParameterization;
     // robust_norm = new ceres::CauchyLoss(1.0);
@@ -1859,6 +1859,7 @@ void PoseGraphSLAM::reinit_ceres_problem_onnewloopedge_optimize6DOF()
         //-----------------------------
         //  Mark nodes as constant. Possibly also need to mark starts of each worlds as constants. Can also try setting each sets 1st node as constant.
         //------------------------------
+        #if 1
         for( int v=0 ; v<regularization_terms_list.size() ; v++ ) {
             reint_problem.RemoveResidualBlock( regularization_terms_list[v] );
             cout << "Remove " << v << " th regularization term\n";
@@ -1868,30 +1869,36 @@ void PoseGraphSLAM::reinit_ceres_problem_onnewloopedge_optimize6DOF()
         std::map< int, bool > mark_as_constant;
         // mark_as_constant[0] = true;
         mark_as_constant[1] = true;
+        cout << "There are " << manager->n_worlds() << " worlds. Loop through each world to get its start node\n";
+        cout << "Number of opt_nodes = " << this->n_opt_variables() << endl;
         for( int ww=0 ; ww<manager->n_worlds(); ww++ ) {
             // if( mark_as_constant.count(ww) ==0  )
                 // continue;
             int ww_setid = manager->getWorldsConstPtr()->find_setID_of_world_i(ww);
-            cout << "&&&&&&&&&&&&world#" << ww << " is in setID=" << ww_setid << endl;
+            int ww_start = manager->nodeidx_of_world_i_started( ww );
+            cout << TermColor::CYAN() << "&&&&&&&&&&&&world#" << ww << " is in setID=" << ww_setid << " with ww_start=" << ww_start << TermColor::RESET() << endl;
             if( ww_setid >= 0 && ww_setid==ww ) {
 
+            cout << "node#" << ww_start << " is in world#" << ww << " which has setID as " << ww_setid << endl;
+            cout << "Mark node#" << ww_start << " as constant. \n";
 
-            int ww_start = manager->nodeidx_of_world_i_started( ww );
             // 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), 2.3 );
+            // Matrix4d ww_start_pose = manager->getNodePose(ww_start);
+            Matrix4d ww_start_pose = this->getNodePose(ww_start);
+            cout << "ww_start_pose : " << PoseManipUtils::prettyprintMatrix4d( ww_start_pose ) << endl;
+            ceres::CostFunction * regularixa_cost = NodePoseRegularization::Create( ww_start_pose, 3.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 );
 
-            cout << "node#" << ww_start << " is in world#" << ww << " which has setID as " << ww_setid << endl;
-            cout << "Mark node#" << ww_start << " as constant. \n";
+
             }
             else
                 cout << "skip\n";
         }
         cout << "Total elements in `regularization_terms_list` = " << regularization_terms_list.size() << endl;
-
+        #endif
 
 
 
diff --git a/src/PoseGraphSLAM.h b/src/PoseGraphSLAM.h
@@ -277,7 +277,7 @@ class SixDOFError
 class NodePoseRegularization
 {
 public:
-    NodePoseRegularization( const Matrix4d& _nodepose, const double _weight = 1.0 ) : nodepose( _nodepose ), weight( _weight )
+    NodePoseRegularization( const Matrix4d _nodepose, const double _weight = 1.0 ) : nodepose( _nodepose ), weight( _weight )
     {
 
     }
@@ -303,11 +303,11 @@ class NodePoseRegularization
         Eigen::Map<Matrix<T,6,1> > residuals( residue_ptr );
         residuals.block(0,0, 3,1) = T(weight) * delta.col(3).topRows(3);
         residuals.block(3,0, 3,1) = T(weight) * T(2.0) * delta_q.vec();
-        
+
         return true;
     }
 
-    static ceres::CostFunction* Create( const Matrix4d& _observed_node_pose, const double xweight )
+    static ceres::CostFunction* Create( const Matrix4d _observed_node_pose, const double xweight )
     {
       return ( new ceres::AutoDiffCostFunction<NodePoseRegularization,6,4,3>
         (
@@ -317,7 +317,7 @@ class NodePoseRegularization
     }
 
 private:
-    const Matrix4d& nodepose;
+    const Matrix4d nodepose;
     const double weight;
 };
 
diff --git a/src/VizPoseGraph.cpp b/src/VizPoseGraph.cpp
@@ -457,6 +457,38 @@ void VizPoseGraph::publishCameraVisualMarker( const Matrix4d& wTc, const string&
     pub_pgraph.publish( marker2 );
 }
 
+void VizPoseGraph::publishXYZAxis( const Matrix4d& wT_axis, const string ns, int id )
+{
+    visualization_msgs::Marker axis;
+    RosMarkerUtils::init_line_marker( axis );
+    axis.id = 0;
+    axis.ns = ns.c_str();
+    axis.scale.x = 0.2;
+
+
+    // Add pts
+    RosMarkerUtils::add_point_to_marker( 0,0,0, axis, true );
+    RosMarkerUtils::add_point_to_marker( 1,0,0, axis, false );
+    RosMarkerUtils::add_point_to_marker( 0,0,0, axis, false );
+    RosMarkerUtils::add_point_to_marker( 0,1,0, axis, false );
+    RosMarkerUtils::add_point_to_marker( 0,0,0, axis, false );
+    RosMarkerUtils::add_point_to_marker( 0,0,1, axis, false );
+
+    // Add colors to each pt
+    RosMarkerUtils::add_colors_to_marker( 1.0,0,0, axis, true );
+    RosMarkerUtils::add_colors_to_marker( 1.0,0,0, axis, false );
+    RosMarkerUtils::add_colors_to_marker( 0,1.0,0, axis, false );
+    RosMarkerUtils::add_colors_to_marker( 0,1.0,0, axis, false );
+    RosMarkerUtils::add_colors_to_marker( 0,0,1.0, axis, false );
+    RosMarkerUtils::add_colors_to_marker( 0,0,1.0, axis, false );
+
+
+
+    RosMarkerUtils::setpose_to_marker( wT_axis, axis );
+    pub_pgraph.publish( axis );
+
+}
+
 void VizPoseGraph::publishThisVisualMarker( const visualization_msgs::Marker& the_marker )
 {
     pub_pgraph.publish( the_marker );
diff --git a/src/VizPoseGraph.h b/src/VizPoseGraph.h
@@ -62,6 +62,7 @@ class VizPoseGraph
     void publishSlamResidueVisual( int n ) const;
     void publishCameraVisualMarker( const Matrix4d& wTc, const string& ns, float r, float g, float b );
 
+    void publishXYZAxis( const Matrix4d& wT_axis, const string ns, int id );
     void publishThisVisualMarker( const visualization_msgs::Marker& the_marker );
     void publishImage( const cv::Mat& im );
 
diff --git a/src/Worlds.cpp b/src/Worlds.cpp
@@ -310,11 +310,15 @@ void Worlds::disjoint_set_status_image(cv::Mat& im_disp, bool enable_bubbles, bo
     if( enable_bubbles ) {
         int uu = n_worlds(); //disjoint_set.element_count();
         // circles
+        cv::putText( im_disp, "Worlds:", cv::Point(15, 15), cv::FONT_HERSHEY_SIMPLEX,
+                0.4, cv::Scalar(255,255,255), 1.5 );
+        cv::putText( im_disp, "SetIDs of Worlds:", cv::Point(15, 45), cv::FONT_HERSHEY_SIMPLEX,
+                0.4, cv::Scalar(255,255,255), 1.5 );
         for( int i=0 ; i<uu; i++ ) {
             // World Colors
             cv::Scalar color = FalseColors::randomColor( i );
             // cout << color << endl;
-            cv::Point pt = cv::Point(20*i+15, 15);
+            cv::Point pt = cv::Point(20*i+15, 25);
             cv::circle( im_disp, pt, 10, color, -1 );
             cv::putText( im_disp, to_string(i), pt, cv::FONT_HERSHEY_SIMPLEX,
                     0.4, cv::Scalar(255,255,255), 1.5 );
@@ -323,7 +327,7 @@ void Worlds::disjoint_set_status_image(cv::Mat& im_disp, bool enable_bubbles, bo
             // Set Colors
             int setId =  find_setID_of_world_i( i );
             color = FalseColors::randomColor( setId );
-            pt = cv::Point(20*i+15, 45);
+            pt = cv::Point(20*i+15, 55);
             cv::circle( im_disp, pt, 10, color, -1 );
             cv::putText( im_disp, to_string(setId), pt, cv::FONT_HERSHEY_SIMPLEX,
                     0.4, cv::Scalar(255,255,255), 1.5 );
diff --git a/src/keyframe_pose_graph_slam_node.cpp b/src/keyframe_pose_graph_slam_node.cpp
@@ -73,9 +73,11 @@ void periodic_publish_odoms( const NodeDataManager * manager, const VizPoseGraph
 {
     cout << "Start `periodic_publish`\n";
     ros::Rate loop_rate(15);
-    double offset_x = 30., offset_y=0., offset_z=0.;
+    // double offset_x = 30., offset_y=0., offset_z=0.;
+    double offset_x = 0., offset_y=0., offset_z=0.;
 
     map<int, vector<Matrix4d> > jmb;
+    bool published_axis = false;
     while( ros::ok() )
     {
         if( manager->getNodeLen() == 0 ) {
@@ -132,9 +134,9 @@ void periodic_publish_odoms( const NodeDataManager * manager, const VizPoseGraph
         #endif
 
 
-        // only publish the latest odometry. Set this to zero if you dont need this.
-        #if 1
-        int to_publish_key = -1;
+
+        #if 1// only publish the latest odometry. Set this to zero if you dont need this.
+        int to_publish_key = -1; //lets the largest key value
         for( auto it=jmb.begin() ; it!=jmb.end() ; it++ ) {
 
             if( it->first > to_publish_key &&  it->first >=0 )
@@ -151,6 +153,16 @@ void periodic_publish_odoms( const NodeDataManager * manager, const VizPoseGraph
         #endif
 
 
+
+        if( published_axis==false || rand() % 100 == 0 ) {
+        // Publish Axis - publish infrequently
+        Matrix4d odm_axis_pose = Matrix4d::Identity();
+        odm_axis_pose(0,3) += offset_x; odm_axis_pose(1,3) += offset_y; odm_axis_pose(2,3) += offset_z;
+        viz->publishXYZAxis( odm_axis_pose, "odom-axis", 0  );
+        published_axis = true;
+        }
+
+
         loop_rate.sleep();
     }
     cout << "END `periodic_publish`\n";
@@ -334,7 +346,7 @@ void opt_traj_publisher_colored_by_world( const NodeDataManager * manager, const
 
     ros::Rate loop_rate(20);
     map<int, vector<Matrix4d> > jmb;
-    vector< Vector3d > lbm; // a corrected poses. Same index as the node.
+    vector< Vector3d > lbm; // a corrected poses. Same index as the node. These are used for loopedges.
     while( ros::ok() )
     {
         // cout << "[opt_traj_publisher_colored_by_world]---\n";
@@ -361,7 +373,7 @@ void opt_traj_publisher_colored_by_world( const NodeDataManager * manager, const
             int world_id = manager->which_world_is_this( manager->getNodeTimestamp(i) );
 
             // i>=0 and i<solvedUntil()
-            if( i>=0 && i< ____solvedUntil ) {
+            if( i>=0 && i<= ____solvedUntil ) {
                 Matrix4d w_T_c_optimized, w_T_c;
 
                 // cerr << "world_id=" << world_id << "   ";
@@ -393,20 +405,37 @@ void opt_traj_publisher_colored_by_world( const NodeDataManager * manager, const
 
             // i>solvedUntil() < manager->getNodeLen() only odometry available here.
 
-            if( i>=(____solvedUntil) && manager->getNodeLen() ) {
+            if( i>(____solvedUntil)  ) {
                 int last_idx=-1;
                 Matrix4d w_TM_i;
 
+                /*
                 if( ____solvedUntil == 0 ) {
                     w_TM_i = manager->getNodePose( i );
                 } else if( world_id < 0 ) {
                     last_idx = manager->nodeidx_of_world_i_ended( -world_id-1 );
                 } else if(world_id == ____solvedUntil_worldid && ____solvedUntil_worldid_is_neg==false) {
                     last_idx = ____solvedUntil;
+                }
+                else if(world_id == ____solvedUntil_worldid && ____solvedUntil_worldid_is_neg==true) {
+                    last_idx = ____solvedUntil;
                 } else {
                     // last_idx = manager->nodeidx_of_world_i_started( world_id );
                     w_TM_i = manager->getNodePose( i );
                 }
+                */
+
+                if( ____solvedUntil == 0 ) {
+                    w_TM_i = manager->getNodePose( i );
+                } else {
+                    if( world_id >= 0 && ____solvedUntil_worldid == world_id )
+                        last_idx = ____solvedUntil;
+                    else if( world_id >=0 && ____solvedUntil_worldid != world_id  )
+                        w_TM_i = manager->getNodePose( i );
+                    else
+                        last_idx = ____solvedUntil;
+
+                }
 
                 if( last_idx >= 0 ) {
                 Matrix4d w_T_last = slam->getNodePose(last_idx );
diff --git a/src/utils/RosMarkerUtils.cpp b/src/utils/RosMarkerUtils.cpp
@@ -314,3 +314,54 @@ void RosMarkerUtils::add_points_to_marker( const MatrixXd& X, visualization_msgs
         marker.points.push_back( pt );
     }
 }
+
+
+void RosMarkerUtils::add_colors_to_marker( const Vector3d& color_rgb, visualization_msgs::Marker& marker, bool clear_prev_colors )
+{
+    assert( (X.rows() == 3) && "[RosMarkerUtils::add_colors_to_marker] X need to of size 3xN representing rgb colors of the points\n" );
+    // geometry_msgs::Point pt;
+    std_msgs::ColorRGBA pt_color;
+
+    if( clear_prev_colors )
+        marker.colors.clear();
+
+
+
+        pt_color.r = color_rgb(0); pt_color.g = color_rgb(1); pt_color.b = color_rgb(2); pt_color.a = 1.0;
+        marker.colors.push_back( pt_color );
+
+}
+
+void RosMarkerUtils::add_colors_to_marker( float c_r, float c_g, float c_b, visualization_msgs::Marker& marker, bool clear_prev_colors )
+{
+    assert( (X.rows() == 3) && "[RosMarkerUtils::add_colors_to_marker] X need to of size 3xN representing rgb colors of the points\n" );
+    // geometry_msgs::Point pt;
+    std_msgs::ColorRGBA pt_color;
+
+    if( clear_prev_colors )
+        marker.colors.clear();
+
+
+
+        pt_color.r = c_r; pt_color.g = c_g; pt_color.b = c_b; pt_color.a = 1.0;
+        marker.colors.push_back( pt_color );
+
+}
+
+
+
+void RosMarkerUtils::add_colors_to_marker( const MatrixXd& X, visualization_msgs::Marker& marker, bool clear_prev_colors )
+{
+    assert( (X.rows() == 3) && "[RosMarkerUtils::add_colors_to_marker] X need to of size 3xN representing rgb colors of the points\n" );
+    // geometry_msgs::Point pt;
+    std_msgs::ColorRGBA pt_color;
+
+    if( clear_prev_colors )
+        marker.colors.clear();
+
+
+    for( int i=0 ; i<X.cols() ; i++ ) {
+        pt_color.r = X(0,i); pt_color.g = X(1,i); pt_color.b = X(2,i); pt_color.a = 1.0;
+        marker.colors.push_back( pt_color );
+    }
+}
diff --git a/src/utils/RosMarkerUtils.h b/src/utils/RosMarkerUtils.h
@@ -57,4 +57,10 @@ class RosMarkerUtils
     static void add_point_to_marker( const Vector4d& X, visualization_msgs::Marker& marker, bool clear_prev_points=true );
     static void add_points_to_marker( const MatrixXd& X, visualization_msgs::Marker& marker, bool clear_prev_points=true ); //X : 3xN or 4xN.
 
+    ////////////// Add colors to individual points ///////////////
+    static void add_colors_to_marker( const Vector3d& color_rgb, visualization_msgs::Marker& marker, bool clear_prev_colors );
+    static void add_colors_to_marker( float c_r, float c_g, float c_b, visualization_msgs::Marker& marker, bool clear_prev_colors );
+    static void add_colors_to_marker( const MatrixXd& X, visualization_msgs::Marker& marker, bool clear_prev_colors );
+
+
 };

Original file line number	Diff line number	Diff line change
`@@ -277,7 +277,7 @@ class SixDOFError`
`277`	`277`	`class NodePoseRegularization`
`278`	`278`	`{`
`279`	`279`	`public:`
`280`		`- NodePoseRegularization( const Matrix4d& _nodepose, const double _weight = 1.0 ) : nodepose( _nodepose ), weight( _weight )`
	`280`	`+ NodePoseRegularization( const Matrix4d _nodepose, const double _weight = 1.0 ) : nodepose( _nodepose ), weight( _weight )`
`281`	`281`	`{`
`282`	`282`
`283`	`283`	`}`
`@@ -303,11 +303,11 @@ class NodePoseRegularization`
`303`	`303`	`Eigen::Map<Matrix<T,6,1> > residuals( residue_ptr );`
`304`	`304`	`residuals.block(0,0, 3,1) = T(weight) * delta.col(3).topRows(3);`
`305`	`305`	`residuals.block(3,0, 3,1) = T(weight) * T(2.0) * delta_q.vec();`
`306`		`-`
	`306`	`+`
`307`	`307`	`return true;`
`308`	`308`	`}`
`309`	`309`
`310`		`- static ceres::CostFunction* Create( const Matrix4d& _observed_node_pose, const double xweight )`
	`310`	`+ static ceres::CostFunction* Create( const Matrix4d _observed_node_pose, const double xweight )`
`311`	`311`	`{`
`312`	`312`	`return ( new ceres::AutoDiffCostFunction<NodePoseRegularization,6,4,3>`
`313`	`313`	`(`
`@@ -317,7 +317,7 @@ class NodePoseRegularization`
`317`	`317`	`}`
`318`	`318`
`319`	`319`	`private:`
`320`		`- const Matrix4d& nodepose;`
	`320`	`+ const Matrix4d nodepose;`
`321`	`321`	`const double weight;`
`322`	`322`	`};`
`323`	`323`