fixed some more corner cases in node_idx_of_worldi_starts function and some more viz related corner cases

Author: mpkuse

CMakeLists.txt

@@ -24,7 +24,11 @@ find_package(Ceres REQUIRED)
 find_package(OpenCV 3 REQUIRED)
 #find_package( ZLIB REQUIRED ) # Needed for cnpy (c++ support for writing .npy and .npz files). Remove this and in target if you dont need it.
 
-#set(CMAKE_BUILD_TYPE "Release")
+
+
+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")
 

src/NodeDataManager.cpp

@@ -356,10 +356,14 @@ bool NodeDataManager::getNodeTimestamp( int i, ros::Time& stamp ) const
     return status;
 }
 
+
+#define __MY_ASSERT__(condition, to_print) { if(!(condition)){ std::cerr << "ASSERT FAILED: " << #condition << " @ " << __FILE__ << " (" << __LINE__ << ")" << "msg=" << to_print << std::endl; } }
 const ros::Time NodeDataManager::getNodeTimestamp( int i ) const
 {
     std::lock_guard<std::mutex> lk(node_mutex);
-    assert( i>=0 && i< node_timestamps.size() );
+    // cout << string( "you requested timestamp at i="+to_string(i)+", but node_timestamps.size="+to_string( node_timestamps.size()) ).c_str() << endl;
+    // assert( i>=0 && i< node_timestamps.size() && string( "you requested timestamp at i="+to_string(i)+", but node_timestamps.size="+to_string( node_timestamps.size()) ).c_str() );
+    __MY_ASSERT__( (i>=0 && i< node_timestamps.size()),  "you requested timestamp at i="+to_string(i)+", but node_timestamps.size="+to_string( node_timestamps.size()) );
     return node_timestamps[i];
 }
 
@@ -753,6 +757,7 @@ ros::Time NodeDataManager::stamp_of_kidnap_i_started( int i ) const
         return kidnap_starts[i];
     }
 
+    assert( false && "[NodeDataManager::stamp_of_kidnap_i_started]no such kidnap" );
     __KIDNAP_START_ENDS___debug( cout << "[NodeDataManager::stamp_of_kidnap_i_started]no such kidnap" << i << " kidnap_starts.size()=" << kidnap_starts.size() << endl; )
     return ros::Time();
 }
@@ -764,6 +769,7 @@ ros::Time NodeDataManager::stamp_of_kidnap_i_ended( int i ) const
         return kidnap_ends[i];
     }
 
+    assert( false && "[NodeDataManager::stamp_of_kidnap_i_ended]no such kidnap" );
     __KIDNAP_START_ENDS___debug( cout << "[NodeDataManager::stamp_of_kidnap_i_ended]no such kidnap" << i << " kidnap_ends.size()=" << kidnap_ends.size() << endl; )
     return ros::Time();
 }
@@ -850,15 +856,16 @@ int NodeDataManager::which_world_is_this( int i ) //given the node idx, gets the
 }
 */
 
-
 // #define __WORLD_START_ENDS___debug( msg ) msg;
 #define __WORLD_START_ENDS___debug( msg ) ;
+
+#define __WORLD_START____errors( msg ) msg;
 int NodeDataManager::nodeidx_of_world_i_started( int i ) const
 {
 
     if( i<0 )
     {
-        __WORLD_START_ENDS___debug( cout << "[NodeDataManager::nodeidx_of_world_i_started] i cant be negative. no such world " << i << " exists\n"; )
+        __WORLD_START____errors( cout << TermColor::RED() << "[NodeDataManager::nodeidx_of_world_i_started] ERROR i cant be negative. no such world " << i << " exists" << TermColor::RESET() << endl; )
         return -3;
     }
     if( i==0 ) {
@@ -876,7 +883,7 @@ int NodeDataManager::nodeidx_of_world_i_started( int i ) const
     }
 
     if( i>=1 && (i-1) <n ) {
-        __WORLD_START_ENDS___debug( cout << "[NodeDataManager::nodeidx_of_world_i_started] return nodeidx of kidnap_ends["<<i-1 <<"] as the start of world" << i << "\n"; )
+        __WORLD_START_ENDS___debug( cout << "[NodeDataManager::nodeidx_of_world_i_started]  return nodeidx of kidnap_ends["<<i-1 <<"] as the start of world" << i << "\n"; )
         std::lock_guard<std::mutex> lk(node_mutex);
 
         int r=0;
@@ -890,10 +897,14 @@ int NodeDataManager::nodeidx_of_world_i_started( int i ) const
     }
 
 
-    __WORLD_START_ENDS___debug( cout << "[NodeDataManager::nodeidx_of_world_i_started] no such world " << i << " exists\n"; )
+    __WORLD_START____errors( cout << TermColor::RED() << "[NodeDataManager::nodeidx_of_world_i_started] ERROR no such world " << i << " exists" << TermColor::RESET() << endl; )
     return -4;
 }
 
+
+
+// #define __WORLD__ENDS___error( msg ) msg;
+#define __WORLD__ENDS___error( msg ) ;
 int NodeDataManager::nodeidx_of_world_i_ended( int i ) const
 {
     // returns a large number if the world i never ended
@@ -904,12 +915,12 @@ int NodeDataManager::nodeidx_of_world_i_ended( int i ) const
     }
 
     if( i<0 ) {
-        __WORLD_START_ENDS___debug( cout << "[NodeDataManager::nodeidx_of_world_i_ended] i cannot be negative. no such world\n");
+        __WORLD__ENDS___error( cout << TermColor::RED() << "[NodeDataManager::nodeidx_of_world_i_ended] ERROR i cannot be negative. no such world" << TermColor::RESET() << endl; );
         return -1;
     }
 
     if( i>n_kidnap_ends ) {
-        __WORLD_START_ENDS___debug( cout << "[NodeDataManager::nodeidx_of_world_i_ended] no such world" << i << "\n"; )
+        __WORLD__ENDS___error( cout << TermColor::RED() << "[NodeDataManager::nodeidx_of_world_i_ended] ERROR no such world" << i << TermColor::RESET() << endl; )
         return -1;
     }
     else {
@@ -937,5 +948,7 @@ int NodeDataManager::nodeidx_of_world_i_ended( int i ) const
 int NodeDataManager::n_worlds() const
 {
     std::lock_guard<std::mutex> lk(mutex_kidnap);
+    // if( current_kidnap_status == true )
+        // return kidnap_
     return kidnap_ends.size() + 1;
 }

src/PoseGraphSLAM.cpp

@@ -1656,7 +1656,9 @@ void PoseGraphSLAM::reinit_ceres_problem_onnewloopedge_optimize6DOF()
 
             // --------------> add odometry residue :  u <---> u-f
             for( int f=1 ; f<5 ; f++ ){
-                int world_of_u_m_f = manager->which_world_is_this( manager->getNodeTimestamp(u-f) );
+                int world_of_u_m_f=-1;
+                if( u-f >= 0 )
+                    world_of_u_m_f = manager->which_world_is_this( manager->getNodeTimestamp(u-f) );
                 int setID_of__world_of_u_m_f = manager->getWorldsConstPtr()->find_setID_of_world_i( world_of_u_m_f );
 
                 if( setID_of__world_of_u < 0 || setID_of__world_of_u_m_f < 0 ) {
@@ -1887,12 +1889,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 = 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<1 ; s++ )
             {
             __reint_node_regularization_info( cout << "s="<< s << " "; )
-            Matrix4d ww_start_pose = this->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) );
@@ -1953,6 +1956,8 @@ void PoseGraphSLAM::reinit_ceres_problem_onnewloopedge_optimize6DOF()
                 n_solve_convergences++;
         }
         // cout << reint_summary.FullReport() << endl;
+        ___trigger_header( cout << TermColor::iGREEN() << "Solve() took (milli-sec) : " << eta.toc_milli() << "\n" << TermColor::RESET() ; )
+
 
         __reint_ceres_solve_info(
         cout << "Solve() took (milli-sec) : " << eta.toc_milli()  << endl;

src/Worlds.cpp

@@ -161,7 +161,8 @@ bool Worlds::setPoseBetweenWorlds( int m, int n, const Matrix4d m_T_n, const str
     rel_pose_between_worlds__wb_T_wa___info_string[ make_pair(m,n) ] +=  ";" + info_string;
 
     // union_sets()
-    assert( disjoint_set.exist( m ) && disjoint_set.exist( n ) && "Either of m of n doesn't seem to exist in the disjoint set. m="+to_string(m)+" n="+to_string(n) );
+    // assert( (disjoint_set.exists( m ) && disjoint_set.exists( n )) && "Either of m of n doesn't seem to exist in the disjoint set. m="+to_string(m)+" n="+to_string(n) );
+    assert( (disjoint_set.exists( m ) && disjoint_set.exists( n )) );
 
     // ideally, disjoint_set.union_sets( m, n ), but doing the min max trick to retain the id of earliest sample.
     disjoint_set.union_sets( max(m,n), min(m,n) );

src/keyframe_pose_graph_slam_node.cpp

@@ -88,9 +88,30 @@ void periodic_publish_odoms( const NodeDataManager * manager, const VizPoseGraph
         jmb.clear();
 
         // collect all
+        int __i__start = manager->nodeidx_of_world_i_started(manager->n_worlds()-1); //0;
+        //
+        // try maximum 5 times. mili-seconds after unkidnapped the next world is not available
+        // which causes a seg fault. This little fix will sleep for say 5ms and try again
+        for( int _j=0 ; _j<25 ; _j++ ) {
+            if( __i__start>=0 )
+                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));
+            __i__start = manager->nodeidx_of_world_i_started(manager->n_worlds()-1); //0;
+        }
+
+        // cout << "__i__start=" << __i__start << endl;
+
         // for( int i=0 ; i<manager->getNodeLen() ; i++ )
-        for( int i=manager->nodeidx_of_world_i_started(manager->n_worlds()-1) ; i<manager->getNodeLen() ; i++ )
+        for( int i=__i__start ; i<manager->getNodeLen() ; i++ )
         {
+            if( i==-3 || i==-4 ) {
+                cout << "break" << endl;
+                break;
+            }
+            if( i< 0 )
+                cout << TermColor::iCYAN() << "manager->getNodeTimestamp(" << i << ")" << TermColor::RESET() << endl;
             int world_id = manager->which_world_is_this( manager->getNodeTimestamp(i) );
             if( manager->nodePoseExists(i )  ) {
                 auto w_T_c = manager->getNodePose( i );
@@ -103,6 +124,7 @@ void periodic_publish_odoms( const NodeDataManager * manager, const VizPoseGraph
                 jmb[ world_id ].push_back( w_T_c );
             }
         }
+        // cout << "Done collecting\n";
 
 
 
@@ -142,7 +164,8 @@ void periodic_publish_odoms( const NodeDataManager * manager, const VizPoseGraph
             if( it->first > to_publish_key &&  it->first >=0 )
                 to_publish_key = it->first;
         }
-        viz->publishNodesAsLineStrip( jmb[to_publish_key] , "latest_odometry", 0.0, 0.7, 0.0 );
+        if( to_publish_key >= 0 )
+            viz->publishNodesAsLineStrip( jmb[to_publish_key] , "latest_odometry", 0.0, 0.7, 0.0 );
 
         // Publish Camera visual
         Matrix4d wi_T_latest = *( jmb.at( to_publish_key ).rbegin() );

src/utils/RosMarkerUtils.cpp

@@ -318,7 +318,7 @@ void RosMarkerUtils::add_points_to_marker( const MatrixXd& X, visualization_msgs
 
 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" );
+    // 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;
 
@@ -334,7 +334,7 @@ void RosMarkerUtils::add_colors_to_marker( const Vector3d& color_rgb, visualizat
 
 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" );
+    // 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;