a. save the poses in their own worlds when loading from disk. There some glitches and corner cases still remain, but is usually ok. also load and save can now be done with roslaunch params

Author: mpkuse

src/Composer.cpp

@@ -124,7 +124,7 @@ void Composer::pose_assember_thread( int looprate )
                     w_TM_i = manager->getNodePose( i );
                     #if 1 // added during loadStateFromDisk implementation
                     w_TM_i_from_mgr_or_from_slam = 0;
-                    #endif 
+                    #endif
                 } else {
                     if( world_id >= 0 && ____solvedUntil_worldid == world_id ) {
                         last_idx = ____solvedUntil;}
@@ -296,9 +296,11 @@ void Composer::bf_traj_publish_thread( int looprate ) const
     static thread_local std::mt19937 generator;
     std::uniform_int_distribution<int> distribution(0,100);
 
+    int x_loop_count = 0;
     while( b_bf_traj_publish )
     {
         rate.sleep();
+        x_loop_count++;
         __Composer_bf_traj_publish_thread(
         cout <<  "[Composer::bf_traj_publish_thread] publish : global_lmb.size()=" << global_lmb.size() << "\t n_worlds aka. global_jmb.size()=" <<  global_jmb.size() << endl;
         )
@@ -313,7 +315,7 @@ void Composer::bf_traj_publish_thread( int looprate ) const
                 continue;
             }
 
-            bool publish_all = (distribution(generator) < 5)?true:false;
+            bool publish_all = (distribution(generator) < 5 || x_loop_count < 10 )?true:false;
 
 
             for( auto it=global_jmb.begin() ; it!=global_jmb.end() ; it++ )

src/NodeDataManager.cpp

@@ -179,7 +179,9 @@ void NodeDataManager::loopclosure_pose_callback(  const cerebro::LoopEdge::Const
         loopclosure_edges_timestamps.push_back( std::make_pair( t_a, t_b) );
     }
     else {
-        cout << TermColor::YELLOW() << "[NodeDataManager::loopclosure_pose_callback] This edge's end points cannot be found in vector of nodes. This is not FATAL, I am ignoring this edge candidate as a fix.Ideally this should not be happening.\n" << TermColor::RESET() << endl;
+        cout << TermColor::YELLOW() << "[NodeDataManager::loopclosure_pose_callback] This edge's end points( t_a=" << t_a << ", t_b=" << t_b << ") cannot be found in vector of nodes. This is not FATAL, I am ignoring this edge candidate as a fix.Ideally this should not be happening." << TermColor::RESET() << endl;
+        cout << "t_a=" << t_a << "    t_b=" << t_b << endl;
+        cout << "index_t_a=" << index_t_a << "    index_t_b=" << index_t_b << endl;
     }
 
 
@@ -1027,7 +1029,7 @@ bool NodeDataManager::load_solved_posegraph_data_from_json( json obj )
             cout << "wset_T_cam=" << PoseManipUtils::prettyprintMatrix4d( ___w_T_c ) << endl;
         }
         if( i==3 ) {
-            cout << ".\n.\n.\n";
+            cout << "\t.\n\t.\n\t.\n";
         }
 
         // For everything to be working correctly, I need to store the wTc and not wsTc

src/keyframe_pose_graph_slam_node.cpp

@@ -236,6 +236,51 @@ int main( int argc, char ** argv)
     ros::NodeHandle nh("~");
 
 
+    //--- loadStateFromDisk, saveStateToDisk ---//
+    string loadStateFromDisk = "";
+    if( nh.getParam( "loadStateFromDisk", loadStateFromDisk ) == true )
+    {
+        if( loadStateFromDisk.compare("") == 0 ) {
+            ROS_WARN( "[keyframe_pose_graph_slam_node] loadStateFromDisk cmdline parameter was found, but with a empty string, so I will not loadStateFromDisk()");
+        } else {
+            // now make sure it is a directory
+            if( RawFileIO::is_path_a_directory(loadStateFromDisk) ) {
+                ROS_INFO( "[keyframe_pose_graph_slam_node] loadStateFromDisk=%s, Directory exists...OK!", loadStateFromDisk.c_str() );
+                cout << TermColor::GREEN() <<  "[keyframe_pose_graph_slam_node] loadStateFromDisk=" << loadStateFromDisk << ", Directory exists...OK!" << TermColor::RESET() << endl;
+            }
+            else {
+                ROS_ERROR( "[keyframe_pose_graph_slam_node] You specified a directory for loadStateFromDisk=`%s`, This path need to exist\n...EXIT\n", loadStateFromDisk.c_str());
+                exit(1);
+            }
+        }
+    } else {
+        ROS_WARN( "[keyframe_pose_graph_slam_node] loadStateFromDisk cmdline parameter was not found, so I will not loadStateFromDisk()");
+    }
+
+    string saveStateToDisk = "";
+    if( nh.getParam( "saveStateToDisk", saveStateToDisk ) == true )
+    {
+        if( saveStateToDisk.compare("") == 0 ) {
+            ROS_WARN( "[keyframe_pose_graph_slam_node] saveStateToDisk cmdline parameter was found, but with a empty string, so I will not saveStateToDisk()");
+        } else {
+            // now make sure it is a directory
+            if( RawFileIO::is_path_a_directory(saveStateToDisk) ) {
+                ROS_INFO( "[keyframe_pose_graph_slam_node] saveStateToDisk=%s, Directory exists...OK!", saveStateToDisk.c_str() );
+                cout << TermColor::GREEN() <<  "[keyframe_pose_graph_slam_node] saveStateToDisk=" << saveStateToDisk << ", Directory exists...OK!" << TermColor::RESET() << endl;
+            }
+            else {
+                ROS_ERROR( "[keyframe_pose_graph_slam_node] You specified a directory for saveStateToDisk=`%s`, This path need to exist and be writable\n...EXIT\n", saveStateToDisk.c_str());
+                exit(1);
+            }
+        }
+    } else {
+        ROS_WARN( "[keyframe_pose_graph_slam_node] saveStateToDisk cmdline parameter was not found, so I will not saveStateToDisk()");
+    }
+
+    //--- END loadStateFromDisk, saveStateToDisk ---//
+    
+
+
     // Setup subscribers and callbacks
     NodeDataManager * manager = new NodeDataManager(nh);
 
@@ -327,6 +372,11 @@ int main( int argc, char ** argv)
     // exit(1);
     #endif
 
+    if( loadStateFromDisk.compare("") != 0 )
+    {
+        cmpr->loadStateFromDisk( loadStateFromDisk );
+    }
+
 
     // ++ start the pose assember thread - This is needed for the following publish threads
     // It queries data from manager, slam and assembles the upto date data. This is threadsafe.
@@ -423,11 +473,16 @@ 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__ 1
+    #define __SAVE_STATE__ 0
     #if __SAVE_STATE__
     cmpr->saveStateToDisk( "/Bulk_Data/chkpts_posegraph_solver" );
     #endif
 
+    if( saveStateToDisk.compare("") != 0 )
+    {
+        cmpr->saveStateToDisk( saveStateToDisk );
+    }
+
     #define __LOGGING__ 0 // make this 1 to enable logging. 0 to disable logging. rememeber to catkin_make after this change
     #if __LOGGING__
     // Note: If using roslaunch to launch this node and when LOGGING is enabled,

src/utils/RawFileIO.cpp

@@ -456,6 +456,18 @@ bool RawFileIO::read_eigen_vector_fromjson( const json str, VectorXd&  output )
 
 
 
+
+bool RawFileIO::is_path_a_directory(const char* path)
+{
+    struct stat buf;
+    stat(path, &buf);
+    return S_ISDIR(buf.st_mode);
+}
+
+bool RawFileIO::is_path_a_directory(const string path) { return is_path_a_directory(path.c_str() ); }
+
+
+
 std::vector<std::string>
 RawFileIO::split( std::string const& original, char separator )
 {

src/utils/RawFileIO.h

@@ -33,6 +33,9 @@ using json = nlohmann::json;
 
 #define __RawFileIO__write_image_debug_dm( msg ) msg;
 
+#include <sys/stat.h> // for is_directory
+
+
 class RawFileIO
 {
 public:
@@ -101,6 +104,10 @@ class RawFileIO
 
     static bool if_file_exist( char * fname );
     static bool if_file_exist( string fname );
+
+    static bool is_path_a_directory(const char* path);
+    static bool is_path_a_directory(const string path);
+
     static int exec_cmd( const string& cmd ); //< Executes a unix command.