fixes on the pose computation between the worlds. Helps the code base be stable. also fixed some asserts in debug mode

Author: mpkuse

src/NodeDataManager.cpp

@@ -469,12 +469,14 @@ bool NodeDataManager::saveAsJSON( const string& base_path )
         json node;
         node["timestamp"] = getNodeTimestamp(i).toSec();
         node["idx"] = i;
+        node["world_id"] = which_world_is_this( getNodeTimestamp(i) );
 
         Matrix4d wTc;
         getNodePose(i, wTc );
         std::stringstream ss;
         ss << wTc.format(CSVFormat);
         node["wTc"] = ss.str();
+        node["wTc_pretty"] = PoseManipUtils::prettyprintMatrix4d(wTc);
 
         Matrix<double,6,6> cov;
         std::stringstream ss2;
@@ -503,11 +505,22 @@ bool NodeDataManager::saveAsJSON( const string& base_path )
         edge["timestamp0"] = getNodeTimestamp(p.first).toSec();
         edge["timestamp1"] = getNodeTimestamp(p.second).toSec();
 
+        edge["world0_id"] = which_world_is_this( getNodeTimestamp(p.first) );
+        edge["world1_id"] = which_world_is_this( getNodeTimestamp(p.second) );
+        if( edge["world0_id"] < 0 || edge["world1_id"] < 0 )
+            edge["code"] = -1;
+        else if( edge["world0_id"] == edge["world1_id"] ) {
+            edge["code"] = 1;
+        } else {
+            edge["code"] = 2;
+        }
+
 
         const Matrix4d b_T_a = getEdgePose(i);
         std::stringstream ss;
         ss << b_T_a.format(CSVFormat);
         edge["b_T_a"] = ss.str();
+        edge["b_T_a_pretty"] = PoseManipUtils::prettyprintMatrix4d(b_T_a);
 
         edge["weight"] = getEdgeWeight( i );
         edge["description"] = getEdgeDescriptionString(i);
@@ -518,6 +531,35 @@ bool NodeDataManager::saveAsJSON( const string& base_path )
 
     }
 
+    // World Info
+    json all_world;
+    all_info["meta_data"]["n_worlds"] = n_worlds();
+    for( int i=0 ; i<n_worlds() ; i++ ) {
+        json xworld;
+        xworld["id"] = i;
+        xworld["nodeidx_of_world_i_started"] = nodeidx_of_world_i_started( i );
+        xworld["nodeidx_of_world_i_ended"] = nodeidx_of_world_i_ended( i );
+
+        all_world.push_back( xworld );
+    }
+    all_info[ "world_info"] = all_world;
+
+    // Kidnap Info
+    json all_kidnaps;
+    all_info["meta_data"]["n_worlds"] = n_kidnaps();
+    for( int i=0 ; i<n_kidnaps() ; i++ ) {
+        json kidnap;
+        kidnap["idx"] = i;
+        kidnap["stamp_of_kidnap_i_started"] = stamp_of_kidnap_i_started(i).toSec();
+        kidnap["stamp_of_kidnap_i_ended"] = stamp_of_kidnap_i_ended(i).toSec();
+
+        all_kidnaps.push_back( kidnap );
+    }
+    all_info[ "kidnap_info"] = all_kidnaps;
+    all_info["disjoint_set_status"] = getWorldsConstPtr()->disjoint_set_status();
+
+
+
 
     // Save file
     cout << TermColor::GREEN() << "[NodeDataManager::saveAsJSON]\n" <<  all_info["meta_data"] << endl;
@@ -695,6 +737,8 @@ void NodeDataManager::rcvd_kidnap_indicator_callback( const std_msgs::HeaderCons
     }
 
     ROS_ERROR( "[posegraph solver NodeDataManager::rcvd_kidnap_indicator_callback] rcvd_header is something other than `kidnapped` or `unkidnapped`. This should not be happening and is a fatal error.");
+    exit(2);
+
 }
 
 

src/PoseGraphSLAM.cpp

@@ -1369,8 +1369,8 @@ bool PoseGraphSLAM::saveAsJSON(const string base_path)
 // the start of each worlds as constant. You can tune this as need be.
 // The reason I do not use ceres::SetParameterBlockConstant() is that it cannot be
 // set to not constant which hinders when the initial guess moves.
-// #define __reint_node_regularization_info( msg ) msg;
-#define __reint_node_regularization_info( msg ) ;
+#define __reint_node_regularization_info( msg ) msg;
+// #define __reint_node_regularization_info( msg ) ;
 
 
 // Print Ceres Brief Report and other info on solving
@@ -1570,37 +1570,52 @@ void PoseGraphSLAM::reinit_ceres_problem_onnewloopedge_optimize6DOF()
 
                     )
                     // compute the relative transforms between the 2 worlds
-                    Matrix4d wa_T_a = this->getNodePose( _a );
-                    Matrix4d wb_T_b = this->getNodePose( _b );
+                    Matrix4d wa_T_a = manager->getNodePose( _a );
+                    Matrix4d wb_T_b = manager->getNodePose( _b );
                     Matrix4d b_T_a_observed = manager->getEdgePose(e);
 
                     Matrix4d wb_T_a = wb_T_b * b_T_a_observed;
                     Matrix4d wb_T_wa = wb_T_a * wa_T_a.inverse();
                     __reinit_loopedge_cout(
                     cout << "I just computed the rel pose between 2 worlds, wb_T_wa=" << TermColor::iBLUE() << PoseManipUtils::prettyprintMatrix4d(wb_T_wa) << TermColor::RESET() << endl;
                     )
+                    // Done...!
 
+
+                    // The following bit of code, records the setID states before `setPoseBetweenWorlds` and after `setPoseBetweenWorlds`
+                    // This is done so that we know which world's base changed. This is used later to initialize/reinitialize the guesses for optimization
+
+                    //---- get setID (before)
                     std::map<int,int> mipmap_before_union, mipmap_after_union;
                     __reinit_loopedge_cout(
                     cout << "CURRENT STATUS OF WORLDS (disjoint set) before:\n";
-
                     manager->getWorldsPtr()->print_summary();
                     )
                     manager->getWorldsPtr()->getWorld2SetIDMap( mipmap_before_union );
 
+
+
+                    //---- setPoseBetweenWorlds()
                     __reinit_loopedge_cout(
                     cout << "setting var rel_pose_between_worlds__wb_T_wa[ " << __b_world_is << "," << __a_world_is << " ] = " << PoseManipUtils::prettyprintMatrix4d(wb_T_wa)  << endl;
                     )
                     string info_string = "this pose computed from edge "+ std::to_string(_a) + " <--> " + std::to_string(_b);
                     // THIS IS THE ONLY PLACE WHERE THE 2 SETS CAN MERGE AND THE SETID OF THE WORLDS CAN CHANGE.
                     manager->getWorldsPtr()->setPoseBetweenWorlds( __b_world_is, __a_world_is, wb_T_wa,  info_string );
 
+
+
+
+                    //---- get setID (after)
                     __reinit_loopedge_cout(
                     cout << "CURRENT STATUS OF WORLDS (disjoint set) after:\n";
                     manager->getWorldsPtr()->print_summary();
                     )
                     manager->getWorldsPtr()->getWorld2SetIDMap( mipmap_after_union );
 
+
+
+                    //---- findout the changes from `mipmap_before_union` to `mipmap_after_union`
                     __reinit_loopedge_cout(
                     cout << TermColor::iWHITE();
                     cout << "Changes in the setID of the worlds\n";
@@ -1889,13 +1904,13 @@ void PoseGraphSLAM::reinit_ceres_problem_onnewloopedge_optimize6DOF()
 
             // Matrix4d ww_start_pose = manager->getNodePose(ww_start);
             // Functionally similar to marking it as constant
-            double regularization_weight = max( 1.1, log( 1+ww_end - ww_start )/2. );
+            double regularization_weight = max( 2.1, log( 1+ww_end - ww_start )/2. );
 
-            for( int s=0; s<1 ; s++ )
+            for( int s=0; s<3 ; s++ )
             {
             __reint_node_regularization_info( cout << "s="<< s << " "; )
-            // Matrix4d ww_start_pose = this->getNodePose(ww_start+s);
-            Matrix4d ww_start_pose = manager->getNodePose(ww_start+s);
+            Matrix4d ww_start_pose = this->getNodePose(ww_start+s);
+            // Matrix4d ww_start_pose = manager->getNodePose(ww_start+s);
             __reint_node_regularization_info( cout << "regularization_weight=" << regularization_weight << "   ww_start_pose : " << PoseManipUtils::prettyprintMatrix4d( ww_start_pose ) << endl; )
             ceres::CostFunction * regularixa_cost = NodePoseRegularization::Create( ww_start_pose, regularization_weight );
             ceres::ResidualBlockId resi_id = reint_problem.AddResidualBlock( regularixa_cost, NULL,  get_raw_ptr_to_opt_variable_q(ww_start+s), get_raw_ptr_to_opt_variable_t(ww_start+s) );

src/keyframe_pose_graph_slam_node.cpp

@@ -713,6 +713,9 @@ int main( int argc, char ** argv)
     th_slam.join();
 
 
+
+    #define __LOGGING__ 1 // make this 1 to enable logging. 0 to disable logging. rememeber to catkin_make after this change
+    #if __LOGGING__
     ///// Save Pose Graph for Debugging
     const string DATA_PATH = "/Bulk_Data/_tmp_posegraph/";
 
@@ -728,8 +731,6 @@ int main( int argc, char ** argv)
     std::system( cmd_mkdir.c_str() );
 
 
-    #define __LOGGING__ 1 // make this 1 to enable logging. 0 to disable logging. rememeber to catkin_make after this change
-    #if __LOGGING__
     // save
     // manager->saveForDebug( DATA_PATH );
     manager->saveAsJSON( DATA_PATH );