handling of some corner cases and World info function in datamanager

mpkuse · mpkuse · commit 9e7e68fdb8cb · 2019-03-23T18:11:18.000+08:00
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -26,8 +26,8 @@ find_package(OpenCV 3 REQUIRED)
 
 
 
-set(CMAKE_BUILD_TYPE "Release")
-#set(CMAKE_BUILD_TYPE Debug)
+#set(CMAKE_BUILD_TYPE "Release")
+set(CMAKE_BUILD_TYPE Debug)
 #set(CMAKE_BUILD_TYPE RelWithDebInfo)
 set( CMAKE_CXX_FLAGS "-fpermissive -std=c++11 -O3" )
 #-DEIGEN_USE_MKL_ALL")
diff --git a/src/NodeDataManager.cpp b/src/NodeDataManager.cpp
@@ -996,3 +996,108 @@ int NodeDataManager::n_worlds() const
         // return kidnap_
     return kidnap_ends.size() + 1;
 }
+
+
+
+
+void NodeDataManager::print_worlds_info( int verbosity )
+{
+
+    bool start_ends_u_of_worlds=false, rel_pose_between_worlds=false, kidnap_info=false, which_world_each_node_belong_to=false ;
+
+    switch( verbosity )
+    {
+        case 0:
+        start_ends_u_of_worlds=true;
+        rel_pose_between_worlds=false;
+        kidnap_info=false;
+        which_world_each_node_belong_to=false;
+        break;
+        case 1:
+        start_ends_u_of_worlds=true;
+        rel_pose_between_worlds=true;
+        kidnap_info=true;
+        which_world_each_node_belong_to=false;
+        break;
+        case 2:
+        start_ends_u_of_worlds=true;
+        rel_pose_between_worlds=true;
+        kidnap_info=true;
+        which_world_each_node_belong_to=true;
+        break;
+        default:
+        cout << "[NodeDataManager::print_worlds_info] ERROR invalid verbosity.\n";
+        exit(10);
+    }
+
+    cout << "---------------------!!!!!  NodeDataManager::print_worlds_info verbosity = " << verbosity << "----------------\n";
+
+    if( start_ends_u_of_worlds ) {
+    //// Info on worlds
+    cout << TermColor::YELLOW() ;
+    cout << "Info on worlds start and end times from NodeDataManager\n";
+    cout << "#worlds = " << this->n_worlds()  << "\t";
+    cout << "\tWorlds::n_worlds = " << this->getWorldsConstPtr()->n_worlds() << "\t";
+    cout << "\tWorlds::n_sets = " << this->getWorldsConstPtr()->n_sets() << "\t";
+    cout << endl;
+    for( int i=0 ; i<this->n_worlds() ; i++ ) {
+        cout << "world#" << std::setw(2) << i;
+        cout << "  start_u=" <<  std::setw(5) << this->nodeidx_of_world_i_started(i);
+        cout << "  end_u=" <<  std::setw(5) << this->nodeidx_of_world_i_ended(i);
+        // cout << "  startstamp=" <<  manager->getNodeTimestamp( manager->nodeidx_of_world_i_started(i) );
+        // cout << "  endstamp  =" << manager->getNodeTimestamp( manager->nodeidx_of_world_i_ended(i) );
+        cout << " (" <<  this->getNodeTimestamp( this->nodeidx_of_world_i_started(i) );
+        cout << " to " << this->getNodeTimestamp( this->nodeidx_of_world_i_ended(i) ) << ")";
+        cout << "  sec=" << this->getNodeTimestamp( this->nodeidx_of_world_i_ended(i) ) - this->getNodeTimestamp( this->nodeidx_of_world_i_started(i) );
+        cout << "  setID=" << std::setw(2) << this->getWorldsConstPtr()->find_setID_of_world_i( i );
+        cout << endl;
+    }
+    cout << TermColor::RESET() << endl;
+    }
+
+    if( rel_pose_between_worlds ) {
+    //// Relative transforms between worlds
+    this->getWorldsPtr()->print_summary(2);
+    } else {
+    this->getWorldsPtr()->print_summary(0);
+    }
+
+
+    if( kidnap_info ) {
+    //// When was I kidnaped
+    cout << TermColor::BLUE();
+    cout << "Info on Kidnap starts and ends\n";
+    cout << "There were a total of " << this->n_kidnaps() << " kidnaps\n";
+    for( int i=0 ; i<this->n_kidnaps() ; i++ )
+    {
+        cout << "kidnap#" << std::setw(2)  << i ;
+        cout << "\tstart=" << this->stamp_of_kidnap_i_started(i);
+        cout << "\tends =" << this->stamp_of_kidnap_i_ended(i);
+        cout << "\tduration=" << this->stamp_of_kidnap_i_ended(i) - this->stamp_of_kidnap_i_started(i) ;
+        cout << endl;
+    }
+
+    cout << " manager->last_kidnap_started() : "  << this->last_kidnap_started() << endl;
+    cout << " manager->last_kidnap_ended()   : " <<  this->last_kidnap_ended() << endl;
+    cout << TermColor::RESET();
+    }
+
+
+
+    if( which_world_each_node_belong_to ) {
+    //// Which world each of the nodes belong to
+    cout << "Info on all the Nodes\n";
+    int r=0;
+    for( int r=0 ; r<this->getNodeLen() ; r++ )
+    {
+        ros::Time _t = this->getNodeTimestamp(r);
+        cout << "node#" <<  std::setw(5) << r << " t=" << _t  << " world=" <<  std::setw(3) << this->which_world_is_this( _t ) ;
+
+        if( r%3 == 0  )
+            cout << endl;
+        else
+            cout << "\t\t";
+    }
+    cout << endl;
+    }
+}
diff --git a/src/NodeDataManager.h b/src/NodeDataManager.h
@@ -166,6 +166,7 @@ class NodeDataManager
     const Worlds * getWorldsConstPtr() const { return (const Worlds*) worlds_handle_raw_ptr; }
     // int worlds__n_worlds() const { return worlds_handle.n_worlds(); }
 
+    void print_worlds_info( int verbosity );
 
 private:
     mutable std::mutex mutex_kidnap;
diff --git a/src/PoseGraphSLAM.cpp b/src/PoseGraphSLAM.cpp
@@ -1357,12 +1357,12 @@ bool PoseGraphSLAM::saveAsJSON(const string base_path)
 // #define __reint_odom_cout(msg) msg;
 #define __reint_odom_cout(msg) ;
 
-// #define __reinit_loopedge_cout( msg ) msg;
-#define __reinit_loopedge_cout( msg ) ;
+#define __reinit_loopedge_cout( msg ) msg;
+// #define __reinit_loopedge_cout( msg ) ;
 
 
-// #define __reint_gueses_short_info(msg) msg;
-#define __reint_gueses_short_info(msg) ;
+#define __reint_gueses_short_info(msg) msg;
+// #define __reint_gueses_short_info(msg) ;
 
 
 // Print info on node regularization. I use node regularization to set
@@ -1374,8 +1374,9 @@ bool PoseGraphSLAM::saveAsJSON(const string base_path)
 
 
 // Print Ceres Brief Report and other info on solving
-// #define __reint_ceres_solve_info( msg ) msg;
-#define __reint_ceres_solve_info( msg ) ;
+#define __reint_ceres_solve_info( msg ) msg;
+// #define __reint_ceres_solve_info( msg ) ;
+
 
 
 // This thread wakes up when there are new loopedges. This flag controls the printing on trigger.
@@ -1893,6 +1894,10 @@ void PoseGraphSLAM::reinit_ceres_problem_onnewloopedge_optimize6DOF()
             int ww_setid = manager->getWorldsConstPtr()->find_setID_of_world_i(ww);
             int ww_start = manager->nodeidx_of_world_i_started( ww );
             int ww_end = manager->nodeidx_of_world_i_ended( ww );
+            if( ww_start < 0 ) {
+                __reint_node_regularization_info( cout << TermColor::RED() << "ignore this world since ww_start is negative. Shouldnt be happening but..:(\n" );
+                continue;
+            }
             __reint_node_regularization_info( cout << TermColor::CYAN() << "&&&&&&&&&&&&world#" << ww << " is in setID=" << ww_setid << " with ww_start=" << ww_start << " ww_end=" << ww_end << TermColor::RESET() << endl; )
             if( (ww_setid >= 0 && ww_setid==ww)  )
             {
@@ -1904,7 +1909,7 @@ 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( 2.1, log( 1+ww_end - ww_start )/2. );
+            double regularization_weight = max( 1.1, log( 1+ww_end - ww_start )/2. );
 
             for( int s=0; s<3 ; s++ )
             {
diff --git a/src/keyframe_pose_graph_slam_node.cpp b/src/keyframe_pose_graph_slam_node.cpp
@@ -97,7 +97,7 @@ void periodic_publish_odoms( const NodeDataManager * manager, const VizPoseGraph
                 break;
 
             cout << "[periodic_publish_odoms] sleep() for 100milis. This is done just as a precaution because next world may not be immediately available after unkidnap. It takes usually upto 500milisec for vins to reinitialize. This warning is not very critial." << endl;
-            std::this_thread::sleep_for (std::chrono::milliseconds(100));
+            std::this_thread::sleep_for (std::chrono::milliseconds(200));
             __i__start = manager->nodeidx_of_world_i_started(manager->n_worlds()-1); //0;
         }
 
@@ -364,13 +364,32 @@ struct opt_traj_publisher_options
     int line_color_style=10;
 };
 
+// #define __opt_traj_publisher_colored_by_world___print_on_file_exist
+
+#ifdef __opt_traj_publisher_colored_by_world___print_on_file_exist
+#include <sys/stat.h>
+#include <unistd.h>
+#include <string>
+#include <fstream>
+inline bool exists_test3 (const std::string& name) {
+  struct stat buffer;
+  return (stat (name.c_str(), &buffer) == 0);
+}
+#endif
+
 void opt_traj_publisher_colored_by_world( const NodeDataManager * manager, const PoseGraphSLAM * slam, const VizPoseGraph * viz, const opt_traj_publisher_options& options )
 {
+    #ifdef __opt_traj_publisher_colored_by_world___print_on_file_exist
+    bool enable_cout = false;
+    #endif
+
+
 
     ros::Rate loop_rate(20);
     // ros::Rate loop_rate(5);
     map<int, vector<Matrix4d> > jmb;
     vector< Vector3d > lbm; // a corrected poses. Same index as the node. These are used for loopedges.
+    bool published_axis = true;
     while( ros::ok() )
     {
         // cout << "[opt_traj_publisher_colored_by_world]---\n";
@@ -392,8 +411,19 @@ void opt_traj_publisher_colored_by_world( const NodeDataManager * manager, const
         if( ____solvedUntil_worldid < 0 ) { /*____solvedUntil_worldid = -____solvedUntil_worldid - 1;*/ ____solvedUntil_worldid_is_neg=true; }
         // cerr << "\t[opt_traj_publisher_colored_by_world] slam->solvedUntil=" << ____solvedUntil << "  ____solvedUntil_worldid" << ____solvedUntil_worldid << endl;
 
+        #ifdef __opt_traj_publisher_colored_by_world___print_on_file_exist
+        if( exists_test3( "/app/xxx") )
+            enable_cout = true;
+        else
+            enable_cout = false;
+
+        if( enable_cout ) {
         // cout << "[opt_traj_publisher_colored_by_world] i=0" << " i<"<<manager->getNodeLen() ;
-        // cout << "____solvedUntil=" << ____solvedUntil << "  ____solvedUntil_worldid=" << ____solvedUntil_worldid << endl;
+        cout << "____solvedUntil=" << ____solvedUntil << "  ____solvedUntil_worldid=" << ____solvedUntil_worldid << endl;
+        }
+        #endif
+
+
         for( int i=0 ; i<manager->getNodeLen() ; i++ )
         {
             int world_id = manager->which_world_is_this( manager->getNodeTimestamp(i) );
@@ -449,7 +479,11 @@ void opt_traj_publisher_colored_by_world( const NodeDataManager * manager, const
                 jmb[ world_id ].push_back( w_T_c );
                 lbm.push_back( w_T_c.col(3).topRows(3) );
                 latest_pose_worldid = world_id;
-                // cout << "  (from_slam_or_from_odom=" << from_slam_or_from_odom << " w_T_c=" << PoseManipUtils::prettyprintMatrix4d( w_T_c ) << endl;
+
+                #ifdef __opt_traj_publisher_colored_by_world___print_on_file_exist
+                if( enable_cout )
+                cout << i << ":" <<  world_id << "  (from_slam_or_from_odom=" << from_slam_or_from_odom << " w_T_c=" << PoseManipUtils::prettyprintMatrix4d( w_T_c ) << endl;
+                #endif
 
             }
 
@@ -468,11 +502,22 @@ void opt_traj_publisher_colored_by_world( const NodeDataManager * manager, const
                     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 );}
+                        w_TM_i = manager->getNodePose( i );
+                    }
                     else if( world_id < 0 ) {
-                        // last_idx = ____solvedUntil;
-                        last_idx = manager->nodeidx_of_world_i_ended( -world_id - 1 );
-                        // cout << "last_idx=" << last_idx << endl;
+                        // this is the kidnaped node
+                        last_idx = manager->nodeidx_of_world_i_ended( -world_id - 1 ); // only this in working code
+
+                        if( !( last_idx >= 0 && last_idx < manager->getNodeLen() && i >=0 && i<manager->getNodeLen()) ) {
+                            cout << "ERROR. last_idx=" << last_idx << endl;
+                            manager->getWorldsConstPtr()->print_summary( 2);
+                            manager->print_worlds_info(2);
+                            assert( last_idx >= 0 && last_idx < manager->getNodeLen() && i >=0 && i<manager->getNodeLen() );
+                        }
+
+                        w_TM_i = *(jmb[ -world_id-1 ].rbegin()) * ( manager->getNodePose( last_idx ).inverse() * manager->getNodePose( i ) ) ;
+                        last_idx = -1;
+
                     } else {
                         cout << "\nopt_traj_publisher_colored_by_world impossivle\n";
                         exit(2);
@@ -501,7 +546,11 @@ void opt_traj_publisher_colored_by_world( const NodeDataManager * manager, const
                 jmb[ world_id ].push_back( w_TM_i );
                 lbm.push_back( w_TM_i.col(3).topRows(3) );
                 latest_pose_worldid = world_id;
-                // cout << "  w_TM_i=" << PoseManipUtils::prettyprintMatrix4d( w_TM_i ) << "  last_idx="<<  last_idx << endl;
+
+                #ifdef __opt_traj_publisher_colored_by_world___print_on_file_exist
+                if( enable_cout )
+                cout << i << ":" << world_id << "  w_TM_i=" << PoseManipUtils::prettyprintMatrix4d( w_TM_i ) << "  last_idx="<<  last_idx << endl;
+                #endif
             }
 
 
@@ -598,6 +647,14 @@ void opt_traj_publisher_colored_by_world( const NodeDataManager * manager, const
         viz->publishThisVisualMarker( linelist_marker );
 
 
+        if( published_axis || rand() % 100 == 0 ) {
+            Matrix4d _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( _axis_pose, "opt_traj_axis", 0  );
+            published_axis = false;
+        }
+
+
         // book keeping
         // cerr << "\nSLEEP\n";
         loop_rate.sleep();
