manager need to have camera pose in its world and not in its setID_of_world onstateload. Adjusted this part, looks like everything is correct now, try a few more test cases before merge with master

Author: mpkuse

src/Composer.cpp

@@ -54,12 +54,14 @@ void Composer::pose_assember_thread( int looprate )
         for( int i=0 ; i<manager->getNodeLen() ; i++ )
         {
             int world_id = manager->which_world_is_this( manager->getNodeTimestamp(i) );
+            int setID_of_worldID = manager->getWorldsConstPtr()->find_setID_of_world_i( world_id  );
             __Composer__pose_assember_thread_posedebug(
-            cout << "i=" << i << " world#" << world_id << endl;
+            cout << "\t%% i=" << i << " world#" << world_id << " setID_of_worldID=" << setID_of_worldID << endl;
             )
 
             if( i>=0 && i<= ____solvedUntil )
             {
+                __Composer__pose_assember_thread_posedebug( cout << "in i>=0 && i<= ____solvedUntil\n" );
                 Matrix4d w_T_c;
                 // If the optimized pose exists use that else use the odometry pose
                 int from_slam_or_from_odom = -1; // 1: from slam ; 2: from odom ; 3: kidnapped node
@@ -74,6 +76,9 @@ void Composer::pose_assember_thread( int looprate )
                             w_T_c = manager->getNodePose( i );
                             // cerr << "w_T_c=" << PoseManipUtils::prettyprintMatrix4d( w_T_c ) << endl;
                             from_slam_or_from_odom = 2;
+
+
+
                         }
                     }
                 } else {
@@ -107,11 +112,19 @@ void Composer::pose_assember_thread( int looprate )
 
             if( i>(____solvedUntil)  )
             {
+                __Composer__pose_assember_thread_posedebug( cout << "in i>(____solvedUntil)\n" );
+
                 int last_idx=-1;
                 Matrix4d w_TM_i;
+                #if 1 // added during loadStateFromDisk implementation
+                int w_TM_i_from_mgr_or_from_slam = -1; // 0: mgr, 1: slam
+                #endif
 
                 if( ____solvedUntil == 0 ) {
                     w_TM_i = manager->getNodePose( i );
+                    #if 1 // added during loadStateFromDisk implementation
+                    w_TM_i_from_mgr_or_from_slam = 0;
+                    #endif 
                 } else {
                     if( world_id >= 0 && ____solvedUntil_worldid == world_id ) {
                         last_idx = ____solvedUntil;}
@@ -152,6 +165,29 @@ void Composer::pose_assember_thread( int looprate )
                 }
 
 
+                #if 1
+                // added during loadStateFromDisk implementation
+                // idea is always set ws_T_c and not the usual w_T_c
+                //  ws_T_c := ws_T_w * w_T_c
+                if( world_id != setID_of_worldID && w_TM_i_from_mgr_or_from_slam==0 ) {
+                    __Composer__pose_assember_thread_posedebug(
+                    cout << "[world_id != setID_of_worldID]i=" << i << " world#" << world_id << " setID_of_worldID=" << setID_of_worldID << endl;
+                    );
+                    if( manager->getWorldsConstPtr()->is_exist(setID_of_worldID,world_id) ) {
+                        __Composer__pose_assember_thread_posedebug(
+                        cout << "[world_id != setID_of_worldID]relative pose exist beween these worlds.\n";
+                        );
+                        Matrix4d ws_T_w = manager->getWorldsConstPtr()->getPoseBetweenWorlds(setID_of_worldID,world_id);
+                        Matrix4d ws_T_c = ws_T_w * w_TM_i;
+                        w_TM_i = ws_T_c;
+                    } else {
+                        __Composer__pose_assember_thread_posedebug( cout << "[world_id != setID_of_worldID]relative pose doesnt exist beween these worlds.\n"; );
+                    }
+                }
+
+                #endif // end of addition during oadStateFromDisk implementation
+
+
                 if( jmb.count( world_id ) ==  0 ) {
                     __Composer__pose_assember_thread_posedebug(
                     cout << "jmb[" << world_id << "] = vector<Matrix4d>() \n";

src/NodeDataManager.cpp

@@ -832,6 +832,23 @@ const ros::Time NodeDataManager::last_kidnap_started() const
     }
 }
 
+
+void NodeDataManager::mark_as_kidnapped_and_signal_end_of_world() //this is needed to be done when CTRL+C is called ie. when saveStateToDisk is to be performed
+{
+    cout << "[NodeDataManager::mark_as_kidnapped_and_signal_end_of_world]" << endl;
+    ros::Time last_ti_stamp = * ( node_timestamps.rbegin() );
+    mark_as_kidnapped( last_ti_stamp );
+    worlds_handle_raw_ptr->world_ends( last_ti_stamp );
+}
+
+void NodeDataManager::mark_as_unkidnapped_and_signal_start_of_world( ros::Time _t_begin) //this is called when loading data from disk
+{
+    cout << "[NodeDataManager::mark_as_unkidnapped_and_signal_start_of_world] _t_begin=" << _t_begin << endl;
+
+    mark_as_unkidnapped( _t_begin );
+    worlds_handle_raw_ptr->world_starts( _t_begin );
+}
+
 json NodeDataManager::kidnap_data_to_json() const
 {
     std::lock_guard<std::mutex> lk(mutex_kidnap);
@@ -993,14 +1010,44 @@ bool NodeDataManager::load_solved_posegraph_data_from_json( json obj )
         int _worldID = obj.at("SolvedPoseGraph").at(i).at("worldID");
         ros::Time _tnode = ros::Time().fromNSec( obj.at("SolvedPoseGraph").at(i).at("stampNSec") );
 
-        Matrix4d ___w_T_c;
+        Matrix4d ___w_T_c; //< this is actually ws_T_cam (cam position in world-set-id, )
         bool poseloadstatus=RawFileIO::read_eigen_matrix4d_fromjson(  obj.at("SolvedPoseGraph").at(i).at("w_T_c"), ___w_T_c );
         if( poseloadstatus == false )
         {
             cout << TermColor::RED() << "When loading SolvedPoseGraph[" << i << "], RawFileIO::read_eigen_matrix4d_fromjson returned false. This means, I cannot read the pose from json file\n" << TermColor::RESET() ;
             return false;
         }
 
+
+        if( i==0 || i==1 || i==2 || i==n_nodes-1 || i== n_nodes-2) {
+            cout << "i=" << i << "\t";
+            cout << "t=" << _tnode << "\t";
+            cout << "_worldID=" << _worldID << "\t";
+            cout << "_setID_of_worldID=" << _setID_of_worldID << "\t";
+            cout << "wset_T_cam=" << PoseManipUtils::prettyprintMatrix4d( ___w_T_c ) << endl;
+        }
+        if( i==3 ) {
+            cout << ".\n.\n.\n";
+        }
+
+        // For everything to be working correctly, I need to store the wTc and not wsTc
+        // w_T_c := w_T_ws * ws_T_c
+        if( _worldID >= 0 && _worldID != _setID_of_worldID )
+        {
+            Matrix4d wTws = Matrix4d::Identity();
+            if( getWorldsConstPtr()->is_exist(_worldID,_setID_of_worldID) )
+                wTws = getWorldsConstPtr()->getPoseBetweenWorlds( _worldID,_setID_of_worldID );
+            else {
+                cout << "[NodeDataManager::load_solved_posegraph_data_from_json] ERROR";
+                cout << "at i="<< i << "you requesting a pose between the worlds "<<  _worldID << " and " << _setID_of_worldID << " that does not exist. This cannot be happening\n";
+                getWorldsConstPtr()->print_summary();
+                exit(1);
+            }
+            Matrix4d f_wTc = wTws * ___w_T_c;
+            ___w_T_c = f_wTc;
+        }
+
+
         // verify worldID and setID_of_worldID
         #if 1
         if( _worldID >= 0 && _setID_of_worldID>=0 ) {

src/NodeDataManager.h

@@ -159,22 +159,11 @@ class NodeDataManager
     ros::Time stamp_of_kidnap_i_started( int i ) const;
     ros::Time stamp_of_kidnap_i_ended( int i ) const;
     int n_kidnaps() const;
+
+    void mark_as_kidnapped_and_signal_end_of_world(); //this is needed to be done when CTRL+C is called ie. when saveStateToDisk is to be performed
+    void mark_as_unkidnapped_and_signal_start_of_world( ros::Time _t_begin); //this is called when loading data from disk
+
     json kidnap_data_to_json() const;
-    void mark_as_kidnapped_and_signal_end_of_world() //this is needed to be done when CTRL+C is called ie. when saveStateToDisk is to be performed
-    {
-        cout << "[NodeDataManager::mark_as_kidnapped_and_signal_end_of_world]" << endl;
-        ros::Time last_ti_stamp = * ( node_timestamps.rbegin() );
-        mark_as_kidnapped( last_ti_stamp );
-        worlds_handle_raw_ptr->world_ends( last_ti_stamp );
-    }
-
-    void mark_as_unkidnapped_and_signal_start_of_world( ros::Time _t_begin) //this is called when loading data from disk
-    {
-        cout << "[NodeDataManager::mark_as_unkidnapped_and_signal_start_of_world] _t_begin=" << _t_begin << endl;
-
-        mark_as_unkidnapped( _t_begin );
-        worlds_handle_raw_ptr->world_starts( _t_begin );
-    }
     bool load_kidnap_data_from_json( json obj ); //< See comments in the implementation for json input format
     bool load_solved_posegraph_data_from_json( json obj ); //< See comments in the implementation for json input format
 

src/Worlds.cpp

@@ -478,12 +478,13 @@ bool Worlds::loadStateFromDisk( json _objk )
             return false;
         }
 
-
+        cout << "\t" << node_b << "_T_" << node_a ;
         rel_pose_between_worlds__wb_T_wa[ p ] = wb_T_wa;
         rel_pose_between_worlds__wb_T_wa___info_string[ p ] = info_wb_T_wa;
 
 
     }
+    cout << endl;
     cout << "OK...rel_pose_between_worlds__wb_T_wa\n";
 
 

src/keyframe_pose_graph_slam_node.cpp

@@ -320,7 +320,7 @@ int main( int argc, char ** argv)
     //----Pose Composer---//
     Composer * cmpr = new Composer( manager, slam, viz , nh);
 
-    #define __LOAD_STATE__ 1 //set this to 1 to enable, 0 to disable
+    #define __LOAD_STATE__ 0 //set this to 1 to enable, 0 to disable
     #if __LOAD_STATE__
     cmpr->loadStateFromDisk( "/Bulk_Data/chkpts_posegraph_solver" );
     // cout << "PREMATURE EXIT\n";
@@ -423,7 +423,7 @@ int main( int argc, char ** argv)
 
 
     //make this to 1 to save state to file upon exit, 0 to disable saving to file
-    #define __SAVE_STATE__ 0
+    #define __SAVE_STATE__ 1
     #if __SAVE_STATE__
     cmpr->saveStateToDisk( "/Bulk_Data/chkpts_posegraph_solver" );
     #endif