diff --git a/CMakeLists.txt b/CMakeLists.txt index 7583df4..8fd4ef1 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -81,6 +81,7 @@ add_executable( keyframe_pose_graph_slam src/utils/RosMarkerUtils.cpp src/utils/RawFileIO.cpp src/Worlds.cpp + src/Composer.cpp ) diff --git a/src/Composer.cpp b/src/Composer.cpp new file mode 100644 index 0000000..c819056 --- /dev/null +++ b/src/Composer.cpp @@ -0,0 +1,511 @@ +#include "Composer.h" + + +// #define __Composer__pose_assember_thread( msg ) msg; +#define __Composer__pose_assember_thread( msg ) ; + +// #define __Composer__pose_assember_thread_posedebug( msg ) msg; +#define __Composer__pose_assember_thread_posedebug( msg ) ; + +void Composer::pose_assember_thread( int looprate ) +{ + cout << TermColor::GREEN() << "Start `pose_assember_thread` @ " << looprate << " hz" << TermColor::RESET() << endl; + assert( looprate > 0 && looprate < 50 ); + + // After the data is acquired using slam and manager, a deep copy is made into this object's global_jmb and global_lmb + map > jmb; // key: world, value: vector of poses + vector< Matrix4d > lbm_fullpose; // a corrected poses. Same index as the node. These are used for loopedges. + ElapsedTime elp; + + ros::Rate rate( looprate ); + while( b_pose_assember ) + { + + if( manager->getNodeLen() == 0 ) + { + rate.sleep(); + continue; + } + + jmb.clear(); + lbm_fullpose.clear(); + + int latest_pose_worldid = -1; + int ____solvedUntil = slam->solvedUntil(); //note: solvedUntil is the index until which posegraph was solved + int ____solvedUntil_worldid = manager->which_world_is_this( manager->getNodeTimestamp(____solvedUntil) ); + bool ____solvedUntil_worldid_is_neg = false; + if( ____solvedUntil_worldid < 0 ) { /*____solvedUntil_worldid = -____solvedUntil_worldid - 1;*/ ____solvedUntil_worldid_is_neg=true; } + + __Composer__pose_assember_thread( + cout << "____solvedUntil=" << ____solvedUntil << "\t"; + cout << "____solvedUntil_worldid=" << ____solvedUntil_worldid << "\t"; + cout << "__slam->get_reinit_ceres_problem_onnewloopedge_optimize6DOF_status=" << slam->get_reinit_ceres_problem_onnewloopedge_optimize6DOF_status() << "\t"; + cout << endl; + ) + + + + //---------- for i = 0:manager->getNodeLen() -----// + // if( i>=0 i<= ____solvedUntil ) + // ""--do--"" + // if( i>(____solvedUntil) ) + // ""--do--"" + elp.tic(); + for( int i=0 ; igetNodeLen() ; i++ ) + { + int world_id = manager->which_world_is_this( manager->getNodeTimestamp(i) ); + __Composer__pose_assember_thread_posedebug( + cout << "i=" << i << " world#" << world_id << endl; + ) + + if( i>=0 && i<= ____solvedUntil ) + { + 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 + if( world_id >= 0 ) { + if( slam->nodePoseExists(i) /*___slam_nodePoseExists_i*/ ) { + w_T_c = slam->getNodePose( i ); + /*w_T_c = ___slam_getNodePose_i;*/ + // cerr << "w_T_c_optimized=" << PoseManipUtils::prettyprintMatrix4d( w_T_c ) << endl; + from_slam_or_from_odom = 1; + } else { + if( manager->nodePoseExists(i ) ) { + w_T_c = manager->getNodePose( i ); + // cerr << "w_T_c=" << PoseManipUtils::prettyprintMatrix4d( w_T_c ) << endl; + from_slam_or_from_odom = 2; + } + } + } else { + // kidnapped worlds viz, -1, -2 etc. + // use the last pose and add the odometry to it + // TODO + from_slam_or_from_odom = 3; + + int last_idx = manager->nodeidx_of_world_i_ended( -world_id - 1 ); + Matrix4d w_T_last; + w_T_last = *(jmb.at( -world_id - 1 ).rbegin()); + Matrix4d last_M_i = manager->getNodePose( last_idx ).inverse() * manager->getNodePose( i ); + w_T_c = w_T_last * last_M_i; + } + + if( jmb.count( world_id ) == 0 ) + jmb[ world_id ] = vector(); + + jmb[ world_id ].push_back( w_T_c ); + // lbm.push_back( w_T_c.col(3).topRows(3) ); + lbm_fullpose.push_back( w_T_c ); + latest_pose_worldid = world_id; + + __Composer__pose_assember_thread_posedebug( + cout << TermColor::RED() << i << ":" << world_id << " (from_slam_or_from_odom=" << from_slam_or_from_odom << " w_T_c=" << PoseManipUtils::prettyprintMatrix4d( w_T_c ) << endl << TermColor::RESET(); + ) + + + } + + + if( i>(____solvedUntil) ) + { + int last_idx=-1; + Matrix4d w_TM_i; + + if( ____solvedUntil == 0 ) { + w_TM_i = manager->getNodePose( i ); + } else { + 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 ); + } + else if( world_id < 0 ) { + // 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 && igetNodeLen()) ) { + cout << "[Composer::pose_assember_thread]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 && igetNodeLen() ); + } + + w_TM_i = *(jmb[ -world_id-1 ].rbegin()) * ( manager->getNodePose( last_idx ).inverse() * manager->getNodePose( i ) ) ; + last_idx = -1; + + } else { + cout << "\n[Composer::pose_assember_thread] impossivle\n"; + exit(2); + } + + } + + + if( last_idx >= 0 ) { + Matrix4d w_T_last; + if( slam->nodePoseExists(last_idx) ) + w_T_last = slam->getNodePose(last_idx ); + else + w_T_last = manager->getNodePose(last_idx ); + + Matrix4d last_M_i = manager->getNodePose( last_idx ).inverse() * manager->getNodePose( i ); + w_TM_i = w_T_last * last_M_i; + } + + + if( jmb.count( world_id ) == 0 ) { + __Composer__pose_assember_thread_posedebug( + cout << "jmb[" << world_id << "] = vector() \n"; + ) + jmb[ world_id ] = vector(); + } + + jmb[ world_id ].push_back( w_TM_i ); + // lbm.push_back( w_TM_i.col(3).topRows(3) ); + lbm_fullpose.push_back( w_TM_i ); + latest_pose_worldid = world_id; + + __Composer__pose_assember_thread_posedebug( + cout << i << ":" << world_id << " w_TM_i=" << PoseManipUtils::prettyprintMatrix4d( w_TM_i ) << " last_idx="<< last_idx << endl; + ) + + } + + } + + __Composer__pose_assember_thread( + cout << TermColor::BLUE() << "[Composer::pose_assember_thread]main for loop took : " << elp.toc_milli() << " ms" << TermColor::RESET() << endl; + ) + + + elp.tic(); + { + // jmb ===>(data copy) global_jmb. Deep copy needed + __Composer__pose_assember_thread( + cout <<"[Composer::pose_assember_thread]// jmb ===>(data copy) global_jmb. Deep copy needed\n"; + ) + std::lock_guard lk(mx); + + // Deep copy std::map + this->global_jmb.clear(); + for( auto it=jmb.begin() ; it!= jmb.end() ; it++ ) // loop over worlds + { + __Composer__pose_assember_thread( + cout << "\t[Composer::pose_assember_thread]process world#" << it->first << " it has " << it->second.size() << " poses" << endl; + ) + // loop over poses in this world + for( auto vec_it = it->second.begin() ; vec_it != it->second.end() ; vec_it++ ) + { + this->global_jmb[ it->first ].push_back( *vec_it ); + } + } + + // Deep copy std::vector + this->global_lmb.clear(); + __Composer__pose_assember_thread( + cout << "[Composer::pose_assember_thread]lbm_fullpose.size() = " << lbm_fullpose.size() << endl; + ) + for( auto it=lbm_fullpose.begin() ; it!=lbm_fullpose.end() ; it++ ) + { + this->global_lmb.push_back( *it ); + } + + // latest + __Composer__pose_assember_thread( + cout << "[Composer::pose_assember_thread]setting this->global_latest_pose_worldid := " << latest_pose_worldid << endl; + ) + this->global_latest_pose_worldid = latest_pose_worldid; + + } + __Composer__pose_assember_thread( + cout << TermColor::BLUE() << "[Composer::pose_assember_thread]deepcopy took : " << elp.toc_milli() << " ms" << TermColor::RESET() << endl; + ) + + rate.sleep(); + } + + cout << TermColor::RED() << "Finished `pose_assember_thread`\n" << TermColor::RESET(); +} + + + +// #define __Composer_bf_traj_publish_thread( msg ) msg; +#define __Composer_bf_traj_publish_thread( msg ) ; +void Composer::bf_traj_publish_thread( int looprate ) +{ + //--- Options + const int options_line_color_style = 10; // should be either 10 or 12. 10:color by WorldID; 12: //color by world setID + const float options_linewidth_multiplier = 3; // thickness of the line + + //--- + + // Note: global_jmb's keys are `worldIDs` and global_lmb's values are `vector& w_T_ci` + cout << TermColor::GREEN() << "start `Composer::bf_traj_publish_thread` @ " << looprate << " hz" << TermColor::RESET() << endl; + assert( looprate > 0 && looprate < 50 ); + ros::Rate rate(looprate); + + static thread_local std::mt19937 generator; + std::uniform_int_distribution distribution(0,100); + + while( b_bf_traj_publish ) + { + rate.sleep(); + __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; + ) + + { // this braces for mutex + // acquire lock and publish using global_lmb and global_jmb + std::lock_guard lk(mx); + + if( global_jmb.size() == 0 ) + { + // cout << "[Composer::bf_traj_publish_thread] not publishing because jmb.size is zero\n"; + continue; + } + + bool publish_all = (distribution(generator) < 5)?true:false; + + for( auto it=global_jmb.begin() ; it!=global_jmb.end() ; it++ ) + { + // 10% of the times publish all worlds, 90% of the time publish only the newest + if( publish_all == false ) + { + if( it->first == this->global_latest_pose_worldid ) + { + //OK + }else { + continue; + } + } + + string ns = "world#"+to_string( it->first ); + __Composer_bf_traj_publish_thread( + cout << "[Composer::bf_traj_publish_thread] publish for ns=" << ns << endl; + ) + + float c_r=0., c_g=0., c_b=0.; + int rng=-1; + + if( options_line_color_style == 10 ) + rng = it->first; //color by world id WorldID + else if( options_line_color_style == 12 ) + rng = manager->getWorldsConstPtr()->find_setID_of_world_i( it->first ); //color by setID + else { + cout << TermColor::RED() << "[Composer::bf_traj_publish_thread] ERROR. invalid option `opt_traj_publisher_options`. expected either 10 or 12.\n"; + exit( 1 ); + } + + + if( rng >= 0 ) { + cv::Scalar color = FalseColors::randomColor( rng ); + c_r = color[2]/255.; + c_g = color[1]/255.; + c_b = color[0]/255.; + } + + + float offset_x=0., offset_y=0., offset_z=0.; + #if 0 + int curr_set_id = manager->getWorldsConstPtr()->find_setID_of_world_i( it->first ); + if( curr_set_id >= 0 ) { + if ( setids_to_udumbes.count(curr_set_id) > 0 ) { + // cout << "rng=" << rng << "setids_to_udumbes"<< setids_to_udumbes.at( rng ) << endl; + offset_y = setids_to_udumbes.at( curr_set_id )*options.udumbe_offset_y; + } + } else { + curr_set_id = manager->getWorldsConstPtr()->find_setID_of_world_i( -it->first-1); + if ( setids_to_udumbes.count(curr_set_id) > 0 ) { + offset_y = setids_to_udumbes.at( curr_set_id )*options.udumbe_offset_y; + } + } + #endif + + viz->publishNodesAsLineStrip( it->second, ns.c_str(), c_r, c_g, c_b, options_linewidth_multiplier, offset_x, offset_y, offset_z ); + + } + + + } + + } + + cout << TermColor::RED() << "Finished `Composer::bf_traj_publish_thread`\n" << TermColor::RESET() << endl; +} + + + +void Composer::cam_visual_publish_thread( int looprate ) +{ + //--- Options + const int options_linewidth_multiplier = 3; //default 3 + //--- + + + cout << TermColor::GREEN() << "start `Composer::cam_visual_publish_thread` @" << looprate << " hz" << TermColor::RESET() << endl; + assert( looprate > 0 && looprate < 50 ); + + ros::Rate rate(looprate); + while( b_cam_visual_publish ) + { + rate.sleep(); + + { + std::lock_guard lk(mx); + // cout << "[Composer::cam_visual_publish_thread] global_latest_pose_worldid" << global_latest_pose_worldid << endl; + if( global_latest_pose_worldid < 0 ) + continue; + Matrix4d wi_T_latest = *( global_jmb.at( global_latest_pose_worldid ).rbegin() ); + // cout << "\twi_T_latest = " << PoseManipUtils::prettyprintMatrix4d( wi_T_latest ) << endl; + float offset_x=0., offset_y=0., offset_z=0.; + float c_r=0., c_g=0., c_b=0.; + int rng=-1; + rng = global_latest_pose_worldid; + + if( rng >= 0 ) { + cv::Scalar color = FalseColors::randomColor( rng ); + c_r = color[2]/255.; + c_g = color[1]/255.; + c_b = color[0]/255.; + } + + viz->publishCameraVisualMarker( wi_T_latest, "world##", c_r, c_g, c_b, + options_linewidth_multiplier , 20, + offset_x, offset_y, offset_z ); + + } + } + cout << TermColor::RED() << "Finished `Composer::cam_visual_publish_thread`\n" << TermColor::RESET() << endl; +} + + + +// #define __Composer__loopedge_publish_thread(msg) msg; +#define __Composer__loopedge_publish_thread(msg) ; +void Composer::loopedge_publish_thread( int looprate ) +{ + cout << TermColor::GREEN() << "start `Composer::loopedge_publish_thread` @" << looprate << " hz" << TermColor::RESET() << endl; + assert( looprate > 0 && looprate < 50 ); + + //--- Options + const int options_linewidth_multiplier = 3; //default 3 + //--- + + + ros::Rate rate(looprate); + int prev_loopedge_len = 0; + while( b_loopedge_publish ) + { + rate.sleep(); + int nloopedgfes = manager->getEdgeLen(); + __Composer__loopedge_publish_thread( + cout << "[Composer::loopedge_publish_thread] nloopedgfes=" << nloopedgfes << endl; + ) + + + + if( nloopedgfes == prev_loopedge_len ) { + __Composer__loopedge_publish_thread( + cout << "no new loopedges\n"; + ) + } + else { + __Composer__loopedge_publish_thread( + cout << "new from idx=[" << prev_loopedge_len << ", " << nloopedgfes << ")" << endl; + ) + + { + std::lock_guard lk(mx); + // TODO: in the future publish intra-world loopedges in different color and interworld as different color + visualization_msgs::Marker linelist_marker; + RosMarkerUtils::init_line_marker( linelist_marker ); + linelist_marker.ns = "loopedges_on_opt_traj"; linelist_marker.id = 0; + linelist_marker.color.r = 0.42;linelist_marker.color.g = 0.55;linelist_marker.color.b = 0.14;linelist_marker.color.a = 1.; + linelist_marker.scale.x = 0.1*options_linewidth_multiplier; + + + // 5% of the times publish all + // 95% of time publish the newest + // ? TODO + + for( int it=0 ; itgetEdgeIdxInfo( it ); + int __a = pair.first; + int __b = pair.second; + + int __a_worldid = manager->which_world_is_this( manager->getNodeTimestamp(__a) ); + int __a_setid = manager->getWorldsConstPtr()->find_setID_of_world_i( __a_worldid ); + int __b_worldid = manager->which_world_is_this( manager->getNodeTimestamp(__b) ); + int __b_setid = manager->getWorldsConstPtr()->find_setID_of_world_i( __b_worldid ); + #if 0 + cout << TermColor::CYAN() ; + cout << "it=" << it; + cout << "__a=" << __a << " __a_worldid=" << __a_worldid << " __a_setid=" << __a_setid << "\t|"; + cout << "__b=" << __b << " __b_worldid=" << __b_worldid << " __b_setid=" << __b_setid << "\n"; + cout << TermColor::RESET(); + #endif + + + Vector3d ____apose = global_lmb[__a].col(3).topRows(3) ; + Vector3d ____bpose = global_lmb[__b].col(3).topRows(3) ; + + RosMarkerUtils::add_point_to_marker( ____apose, linelist_marker, false ); + RosMarkerUtils::add_point_to_marker( ____bpose, linelist_marker, false ); + } + viz->publishThisVisualMarker( linelist_marker ); + + } + } // else of if( nloopedgfes == prev_loopedge_len ) + + + + + prev_loopedge_len = nloopedgfes; + } + cout << TermColor::RED() << "Finished `Composer::loopedge_publish_thread`\n" << TermColor::RESET() << endl; +} + + + +// #define __Composer__disjointset_statusimage_publish_thread(msg) msg; +#define __Composer__disjointset_statusimage_publish_thread(msg) ; +void Composer::disjointset_statusimage_publish_thread( int looprate ) +{ + cout << TermColor::GREEN() << "start `Composer::disjointset_statusimage_publish_thread` @" << looprate << " hz" << TermColor::RESET() << endl; + assert( looprate > 0 && looprate < 50 ); + + ros::Rate rate(looprate); + string prev_status_str = ""; + while( b_disjointset_statusimage_publish ) + { + rate.sleep(); + + string curr_status_str = manager->getWorldsConstPtr()->disjoint_set_status(); + if( prev_status_str.compare(curr_status_str) == 0 ) { + // same as old, no point publishing + __Composer__disjointset_statusimage_publish_thread( + cout << "[Composer::disjointset_statusimage_publish_thread] No change in status, so dont publish image." << endl; + ) + continue; + } + prev_status_str = curr_status_str; + __Composer__disjointset_statusimage_publish_thread( + cout << "[Composer::disjointset_statusimage_publish_thread]" << curr_status_str << endl; + ) + + cv::Mat im_disp; + // manager->getWorldsConstPtr()->disjoint_set_status_image(im_disp); // will get bubles as well as the text + manager->getWorldsConstPtr()->disjoint_set_status_image(im_disp, true, false); // only bubles + + #if 1 // set this to zero to imshow the image. helpful for debugging. + __Composer__disjointset_statusimage_publish_thread( + cout << "[Composer::disjointset_statusimage_publish_thread] publishImage\n"; + ) + viz->publishImage( im_disp ); + #else + cv::imshow( "disjoint_set_status_image" , im_disp ); + cv::waitKey(30); + #endif + } + + cout << TermColor::RED() << "Finished `Composer::disjointset_statusimage_publish_thread`\n" << TermColor::RESET() << endl; +} diff --git a/src/Composer.h b/src/Composer.h new file mode 100644 index 0000000..cf00683 --- /dev/null +++ b/src/Composer.h @@ -0,0 +1,127 @@ +#pragma once +// This class will run several threads. The ask of each of these is to +// query manager or the slam object and produce the final poses. + + +#include + +#include "NodeDataManager.h" +#include "PoseGraphSLAM.h" +#include "VizPoseGraph.h" + +// Threading +#include +#include +#include + +// Eigen +#include +#include +using namespace Eigen; +#include + +using namespace std; + +#include "utils/PoseManipUtils.h" +#include "utils/RosMarkerUtils.h" +#include "utils/TermColor.h" + +class Composer +{ +public: + Composer( const NodeDataManager * manager, const PoseGraphSLAM * slam, const VizPoseGraph * viz ) + : manager(manager), slam(slam), viz(viz) + { + b_pose_assember = false; + b_bf_traj_publish = false; + b_cam_visual_publish = false; + } + +private: + mutable std::mutex mx; + + //--- External Ptr + // TODO: Better convert to shared_ptr + const NodeDataManager * manager; + const PoseGraphSLAM * slam; + const VizPoseGraph * viz ; + + + + //----------------------// + //--- Assember Thread---// + //----------------------// + // This thread runs say at 30Hz and queries the slam, mgr and forms the jmb. + // it acquires the lock as it write the data to jmb. +public: + void pose_assember_thread( int looprate = 30 ); + void pose_assember_enable() { b_pose_assember = true;} + void pose_assember_disable() {b_pose_assember = false; } + +private: + atomic b_pose_assember; + map > global_jmb; // key: worldID, value: vector of poses + vector< Matrix4d > global_lmb; // a corrected poses. Same index as the node. These are used for loopedges. + int global_latest_pose_worldid = -1; //node-index of the latest pose's worldid + + + + //-------------------------------------------// + //--- A Brute force viz all trajactories ---// + //-------------------------------------------// + // makes use of global_jmb and publish all +public: + void bf_traj_publish_thread( int looprate=15 ); + void bf_traj_publish_enable() { b_bf_traj_publish = true;} + void bf_traj_publish_disable() {b_bf_traj_publish = false; } + +private: + atomic b_bf_traj_publish; + + + + + //---------------------// + //--- Camera visual ---// + //---------------------// + // makes use of global_jmb and publish last cam +public: + void cam_visual_publish_thread( int looprate = 30 ); + void cam_visual_publish_enable() { b_cam_visual_publish = true;} + void cam_visual_publish_disable() {b_cam_visual_publish = false; } + +private: + atomic b_cam_visual_publish; + + + + //-------------------------// + //--- Loop Edges Thread ---// + //-------------------------// + // makes use of manager->getEdgeLen() and global_lmb and publish edges. +public: + void loopedge_publish_thread( int looprate = 10 ); + void loopedge_publish_enable() { b_loopedge_publish = true;} + void loopedge_publish_disable() {b_loopedge_publish = false; } + +private: + atomic b_loopedge_publish; + + + + + //--------------------------------// + //--- Disjointset Status Image ---// + //--------------------------------// + // makes use of manager->getEdgeLen() and global_lmb and publish edges. +public: + void disjointset_statusimage_publish_thread( int looprate = 10 ); + void disjointset_statusimage_publish_enable() { b_disjointset_statusimage_publish = true;} + void disjointset_statusimage_publish_disable() {b_disjointset_statusimage_publish = false; } + +private: + atomic b_disjointset_statusimage_publish; + + + +}; diff --git a/src/NodeDataManager.cpp b/src/NodeDataManager.cpp index af9281b..44d60dc 100644 --- a/src/NodeDataManager.cpp +++ b/src/NodeDataManager.cpp @@ -1005,7 +1005,7 @@ int NodeDataManager::n_worlds() const -void NodeDataManager::print_worlds_info( int verbosity ) +void NodeDataManager::print_worlds_info( int verbosity ) const { bool start_ends_u_of_worlds=false, rel_pose_between_worlds=false, kidnap_info=false, which_world_each_node_belong_to=false ; diff --git a/src/NodeDataManager.h b/src/NodeDataManager.h index 5e28ac9..b30f7f7 100644 --- a/src/NodeDataManager.h +++ b/src/NodeDataManager.h @@ -177,7 +177,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 ); + void print_worlds_info( int verbosity ) const; private: mutable std::mutex mutex_kidnap; diff --git a/src/PoseGraphSLAM.cpp b/src/PoseGraphSLAM.cpp index 8a6b7dd..6fff855 100644 --- a/src/PoseGraphSLAM.cpp +++ b/src/PoseGraphSLAM.cpp @@ -1403,8 +1403,8 @@ void PoseGraphSLAM::reinit_ceres_problem_onnewloopedge_optimize6DOF() get_raw_ptr_to_opt_variable_ypr(paur.first), get_raw_ptr_to_opt_variable_t(paur.first) ); #else - // ceres::CostFunction * cost_function = SixDOFErrorWithSwitchingConstraints::Create( bTa, weight ); - ceres::CostFunction * cost_function = FourDOFErrorWithSwitchingConstraints::Create( bTa, weight ); + ceres::CostFunction * cost_function = SixDOFErrorWithSwitchingConstraints::Create( bTa, weight ); + // ceres::CostFunction * cost_function = FourDOFErrorWithSwitchingConstraints::Create( bTa, weight ); reint_problem.AddResidualBlock( cost_function, NULL, get_raw_ptr_to_opt_variable_q(paur.second), get_raw_ptr_to_opt_variable_t(paur.second), get_raw_ptr_to_opt_variable_q(paur.first), get_raw_ptr_to_opt_variable_t(paur.first), @@ -1482,8 +1482,8 @@ void PoseGraphSLAM::reinit_ceres_problem_onnewloopedge_optimize6DOF() get_raw_ptr_to_opt_variable_ypr(u-f), get_raw_ptr_to_opt_variable_t(u-f) ); #else // cost - // ceres::CostFunction * cost_function = SixDOFError::Create( u_M_umf, odom_edge_weight ); - ceres::CostFunction * cost_function = FourDOFError::Create( u_M_umf, odom_edge_weight ); + ceres::CostFunction * cost_function = SixDOFError::Create( u_M_umf, odom_edge_weight ); + // ceres::CostFunction * cost_function = FourDOFError::Create( u_M_umf, odom_edge_weight ); reint_problem.AddResidualBlock( cost_function, NULL, get_raw_ptr_to_opt_variable_q(u), get_raw_ptr_to_opt_variable_t(u), get_raw_ptr_to_opt_variable_q(u-f), get_raw_ptr_to_opt_variable_t(u-f) ); diff --git a/src/keyframe_pose_graph_slam_node.cpp b/src/keyframe_pose_graph_slam_node.cpp index 0b3c543..2c6ae29 100644 --- a/src/keyframe_pose_graph_slam_node.cpp +++ b/src/keyframe_pose_graph_slam_node.cpp @@ -57,6 +57,8 @@ using namespace Eigen; #include "VizPoseGraph.h" #include "utils/RawFileIO.h" +#include "Composer.h" + void periodic_print_len( const NodeDataManager * manager ) { ros::Rate loop_rate(1); @@ -70,11 +72,14 @@ void periodic_print_len( const NodeDataManager * manager ) -void periodic_publish_odoms( const NodeDataManager * manager, const VizPoseGraph * viz ) +// This thread will publish the odometry data as it is. It takes data from `manager`. +// offsets are used to publish it on rviz as side the main drama. +void periodic_publish_odoms( const NodeDataManager * manager, const VizPoseGraph * viz, + float offset_x=30., float offset_y=0., float offset_z=0. ) { - cout << "Start `periodic_publish`\n"; + cout << "Start `periodic_publish_odoms`\n"; ros::Rate loop_rate(15); - double offset_x = 30., offset_y=0., offset_z=0.; + // double offset_x = 30., offset_y=0., offset_z=0.; // double offset_x = 0., offset_y=0., offset_z=0.; map > jmb; @@ -189,168 +194,10 @@ void periodic_publish_odoms( const NodeDataManager * manager, const VizPoseGraph loop_rate.sleep(); } - cout << "END `periodic_publish`\n"; -} - - -// also looks at slam->solvedUntil() to ot repeatedly update the entire path -void periodic_publish_optimized_poses_smart( const NodeDataManager * manager, const PoseGraphSLAM * slam, const VizPoseGraph * viz ) -{ - ros::Rate loop_rate0(20); - int prev_solved_until = 0, solved_until=0; - - vector optimized_w_T_ci; - while( ros::ok() ) - { - // cout << "periodic_publish_optimized_poses_smart\n"; - if( manager->getNodeLen() == 0 ) { - loop_rate0.sleep(); - continue; - } - solved_until = slam->solvedUntil(); - - - // Solved() just executed, so get the latest values of all the nodes, (ie. clearup existing values) - if( solved_until > prev_solved_until ) - { - optimized_w_T_ci.clear(); - - int L = slam->nNodes(); - for( int i=0 ; igetNodePose( i ); - // slam->opt_pose( i, M ); - // slam->getNodePose( i, M ); - - optimized_w_T_ci.push_back( M ); - } - } - - - // fill in VIO for the rest - if( manager->getNodeLen() > optimized_w_T_ci.size() ) - { - if( optimized_w_T_ci.size() == 0 ) // if ceres::Solve() has not executed even once. - { - for( int a=0 ; agetNodeLen() ; a++ ) - { - Matrix4d w_M_c; // VIO pose of current. - manager->getNodePose( a, w_M_c ); - optimized_w_T_ci.push_back( w_M_c ); // put the VIO poses. - } - } - else - { - Matrix4d w_T_last = optimized_w_T_ci[ optimized_w_T_ci.size() - 1 ]; // optimized pose for last corrected node - Matrix4d w_M_last; - manager->getNodePose( optimized_w_T_ci.size() - 1, w_M_last ); // ger VIO pose of the last corrected. This is use to get relative pose of current node wrt this node - for( int a= optimized_w_T_ci.size() ; agetNodeLen() ; a++ ) - { - Matrix4d w_M_c; // VIO pose of current. - manager->getNodePose( a, w_M_c ); - - Matrix4d last_M_c; // pose of current wrt last infered from VIO - last_M_c = w_M_last.inverse() * w_M_c; - - Matrix4d int__w_TM_c = w_T_last * last_M_c ; // using the corrected one for 0->last and using VIO for last->current. - optimized_w_T_ci.push_back( int__w_TM_c ); - } - } - } - - - - viz->publishNodesAsLineStrip( optimized_w_T_ci, "opt_kf_pose_linestrip", - 1.0, 0.1, 0.7, - solved_until, 1., 1.0, 0.0 ); - - - // in nav_msgs::Path only publish last 5 - int end = optimized_w_T_ci.size(); - int start = max( 0, end - 5 ); - viz->publishPath( optimized_w_T_ci, start, end ); - viz->publishOdometry( optimized_w_T_ci ); - - - loop_rate0.sleep(); - prev_solved_until = solved_until; - } - return ; - - + cout << "END `periodic_publish_odoms`\n"; } -void monitor_disjoint_set_datastructure( const NodeDataManager * manager, const VizPoseGraph * viz ) -{ - ros::Rate loop_rate(0.5); - - #if 1 - while( ros::ok() ) - { - cv::Mat im_disp; - // manager->getWorldsConstPtr()->disjoint_set_status_image(im_disp); // will get bubles as well as the text - manager->getWorldsConstPtr()->disjoint_set_status_image(im_disp, true, false); // only bubles - - #if 1 // set this to zero to imshow the image. helpful for debugging. - viz->publishImage( im_disp ); - #else - cv::imshow( "disjoint_set_status_image" , im_disp ); - cv::waitKey(30); - #endif - - loop_rate.sleep(); - } - #else - while( ros::ok() ) - { - string info_msg = manager->getWorldsConstPtr()->disjoint_set_status(); - cv::Mat im_disp = cv::Mat::zeros(cv::Size(300,80), CV_8UC3); - - // cout << "[info_nsg]" << info_msg << endl; - - - FalseColors::append_status_image( im_disp, info_msg ); - - #if 1 - int uu = manager->getWorldsConstPtr()->n_worlds() ; - // int uu = manager->n_worlds(); - cout << uu << endl; - - // circles - for( int i=0 ; igetWorldsConstPtr()->find_setID_of_world_i( i ); - cerr << "setID=" << setId << " "; - color = FalseColors::randomColor( setId ); - cerr << color << endl; - pt = cv::Point(20*i+15, 45); - cv::circle( im_disp, pt, 10, color, -1 ); - cv::putText( im_disp, to_string(setId), pt, cv::FONT_HERSHEY_SIMPLEX, - 0.4, cv::Scalar(255,255,255), 1.5 ); - - - } - #endif - - cv::imshow( "win" , im_disp ); - cv::waitKey(30); - - loop_rate.sleep(); - } - #endif -} @@ -382,997 +229,6 @@ struct assoc_s #endif -// plots the corrected trajectories, different worlds will have different colored lines - -#define opt_traj_publisher_colored_by_world_LINE_COLOR_STYLE 10 //< color the line with worldID -// #define opt_traj_publisher_colored_by_world_LINE_COLOR_STYLE 12 //< color the line with setID( worldID ) - -struct opt_traj_publisher_options -{ - // 10 //< color the line with worldID - // 12 //< color the line with setID( worldID ) - int line_color_style=10; - - - // The thickness of the lines - float linewidth_multiplier=1.0; - - - // Udumbe offset_y. When multiple co-ordinates exist, the offset for plotting on rviz. keep it 30.0 - float udumbe_offset_y = 30.0; -}; - -// comment the following #define to remove the code which checks file's exisitance before printing. -// #define __opt_traj_publisher_colored_by_world___print_on_file_exist - -#ifdef __opt_traj_publisher_colored_by_world___print_on_file_exist -#include -#include -#include -#include -inline bool exists_test3 (const std::string& name) { - struct stat buffer; - return (stat (name.c_str(), &buffer) == 0); -} -#endif - -//original code -#if 0 -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 > jmb; - vector< Vector3d > lbm; // a corrected poses. Same index as the node. These are used for loopedges. - vector< Matrix4d > lbm_fullpose; - bool published_axis = true; - while( ros::ok() ) - { - // cout << "[opt_traj_publisher_colored_by_world]---\n"; - if( manager->getNodeLen() == 0 ) { - // cout << "[opt_traj_publisher_colored_by_world]nothing to publish\n"; - loop_rate.sleep(); - continue; - } - - //clear map - jmb.clear(); - lbm.clear(); - lbm_fullpose.clear(); - - // cerr << "[opt_traj_publisher_colored_by_world]i=0 ; i<"<< manager->getNodeLen() << " ; solvedUntil=" << slam->solvedUntil() <<"\n"; - int latest_pose_worldid = -1; - int ____solvedUntil = slam->solvedUntil(); //note: solvedUntil is the index until which posegraph was solved - int ____solvedUntil_worldid = manager->which_world_is_this( manager->getNodeTimestamp(____solvedUntil) ); - bool ____solvedUntil_worldid_is_neg = false; - 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<"<getNodeLen() ; - cout << "____solvedUntil=" << ____solvedUntil << " ____solvedUntil_worldid=" << ____solvedUntil_worldid << endl; - } - #endif - - - for( int i=0 ; igetNodeLen() ; i++ ) - { - int world_id = manager->which_world_is_this( manager->getNodeTimestamp(i) ); - // cout << "i=" << i << " world#" << world_id << endl;// << " w_TM_i=" << PoseManipUtils::prettyprintMatrix4d( w_TM_i ) << endl; - - // i>=0 and i=0 && i<= ____solvedUntil ) { - // Matrix4d w_T_c_optimized; - Matrix4d w_T_c; - - - // If the optimized pose exists use that else use the odometry pose - int from_slam_or_from_odom = -1; - if( world_id >= 0 ) { - if( slam->nodePoseExists(i) ) { - w_T_c = slam->getNodePose( i ); - // cerr << "w_T_c_optimized=" << PoseManipUtils::prettyprintMatrix4d( w_T_c ) << endl; - from_slam_or_from_odom = 1; - } else { - if( manager->nodePoseExists(i ) ) { - w_T_c = manager->getNodePose( i ); - // cerr << "w_T_c=" << PoseManipUtils::prettyprintMatrix4d( w_T_c ) << endl; - from_slam_or_from_odom = 2; - } - } - } else { - // kidnapped worlds viz, -1, -2 etc. - // use the last pose and add the odometry to it - // TODO - from_slam_or_from_odom = 3; - - int last_idx = manager->nodeidx_of_world_i_ended( -world_id - 1 ); - Matrix4d w_T_last; - w_T_last = *(jmb.at( -world_id - 1 ).rbegin()); - Matrix4d last_M_i = manager->getNodePose( last_idx ).inverse() * manager->getNodePose( i ); - w_T_c = w_T_last * last_M_i; - - - } - - - if( jmb.count( world_id ) == 0 ) - jmb[ world_id ] = vector(); - - jmb[ world_id ].push_back( w_T_c ); - lbm.push_back( w_T_c.col(3).topRows(3) ); - lbm_fullpose.push_back( w_T_c ); - latest_pose_worldid = world_id; - - #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 - - } - - - // i>solvedUntil() < manager->getNodeLen() only odometry available here. - - if( i>(____solvedUntil) ) { - int last_idx=-1; - Matrix4d w_TM_i; - - - - if( ____solvedUntil == 0 ) { - w_TM_i = manager->getNodePose( i ); - } else { - 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 ); - } - else if( world_id < 0 ) { - // 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 && igetNodeLen()) ) { - 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 && igetNodeLen() ); - } - - 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); - } - - } - - if( last_idx >= 0 ) { - Matrix4d w_T_last; - if( slam->nodePoseExists(last_idx) ) - w_T_last = slam->getNodePose(last_idx ); - else - w_T_last = manager->getNodePose(last_idx ); - - Matrix4d last_M_i = manager->getNodePose( last_idx ).inverse() * manager->getNodePose( i ); - w_TM_i = w_T_last * last_M_i; - } - - - - if( jmb.count( world_id ) == 0 ) - jmb[ world_id ] = vector(); - - - - jmb[ world_id ].push_back( w_TM_i ); - lbm.push_back( w_TM_i.col(3).topRows(3) ); - lbm_fullpose.push_back( w_TM_i ); - latest_pose_worldid = world_id; - - #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 - } - - - } - - - - // cout << "[opt_traj_publisher_colored_by_world]publish\n"; - // publish jmb - // #if 0 - for( auto it=jmb.begin() ; it!=jmb.end() ; it++ ) { - // if( it->first < 0 ) //skip publishing kidnapped part. - // continue; - string ns = "world#"+to_string( it->first ); - - float c_r=0., c_g=0., c_b=0.; - int rng=-1; - if( options.line_color_style == 10 ) - rng = it->first; //color by world id WorldID - else if( options.line_color_style == 12 ) - rng = manager->getWorldsConstPtr()->find_setID_of_world_i( it->first ); //color by setID - else { - cout << TermColor::RED() << "opt_traj_publisher_colored_by_world ERROR. invalid option `opt_traj_publisher_options`. expected either 10 or 12.\n"; - exit( 1 ); - } - - // #if opt_traj_publisher_colored_by_world_LINE_COLOR_STYLE == 10 - // int rng = it->first; //color by world id WorldID - // #endif - // - // #if opt_traj_publisher_colored_by_world_LINE_COLOR_STYLE == 12 - // int rng = manager->getWorldsConstPtr()->find_setID_of_world_i( it->first ); //color by setID - // #endif - if( rng >= 0 ) { - cv::Scalar color = FalseColors::randomColor( rng ); - c_r = color[2]/255.; - c_g = color[1]/255.; - c_b = color[0]/255.; - } - - viz->publishNodesAsLineStrip( it->second, ns.c_str(), c_r, c_g, c_b, options.linewidth_multiplier ); - } - // #endif - - // Publish Camera visual - Matrix4d wi_T_latest = *( jmb.at( latest_pose_worldid ).rbegin() ); - float c_r=0., c_g=0., c_b=0.; - int rng=-1; - if( options.line_color_style == 10 ) - rng = latest_pose_worldid; - else if( options.line_color_style == 12 ) - rng = manager->getWorldsConstPtr()->find_setID_of_world_i( latest_pose_worldid ); //color by setID - else { - cout << TermColor::RED() << "opt_traj_publisher_colored_by_world ERROR. invalid option `opt_traj_publisher_options`. expected either 10 or 12.\n"; - exit( 1 ); - } - - - // #if opt_traj_publisher_colored_by_world_LINE_COLOR_STYLE == 10 - // int rng = latest_pose_worldid; - // #endif - // - // #if opt_traj_publisher_colored_by_world_LINE_COLOR_STYLE == 12 - // int rng = manager->getWorldsConstPtr()->find_setID_of_world_i( latest_pose_worldid ); //color by setID - // #endif - if( rng >= 0 ) { - cv::Scalar color = FalseColors::randomColor( rng ); - c_r = color[2]/255.; - c_g = color[1]/255.; - c_b = color[0]/255.; - } - viz->publishCameraVisualMarker( wi_T_latest, "world##", c_r, c_g, c_b, options.linewidth_multiplier, 20 ); - - - // Publish loop edges - // TODO: in the future publish intra-world loopedges in different color and interworld as different color - visualization_msgs::Marker linelist_marker; - RosMarkerUtils::init_line_marker( linelist_marker ); - linelist_marker.ns = "loopedges_on_opt_traj"; linelist_marker.id = 0; - linelist_marker.color.r = 0.42;linelist_marker.color.g = 0.55;linelist_marker.color.b = 0.14;linelist_marker.color.a = 1.; - linelist_marker.scale.x = 0.1*options.linewidth_multiplier; - - int nloopedgfes = manager->getEdgeLen(); - for( int it=0 ; itgetEdgeIdxInfo( it ); - int __a = pair.first; - int __b = pair.second; - Vector3d ____apose = lbm[__a]; - Vector3d ____bpose = lbm[__b]; - - RosMarkerUtils::add_point_to_marker( ____apose, linelist_marker, false ); - RosMarkerUtils::add_point_to_marker( ____bpose, linelist_marker, false ); - } - 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(); - } - - - #if __CODE___GT__ - - // Set this to 1 to enable loggin of final poses, - #define __LOGGING___LBM__ 1 - #if __LOGGING___LBM__ - // Log LMB - vector _RESULT_; - cout << "========[opt_traj_publisher_colored_by_world] logging ========\n"; - cout << "loop on gt_map\n"; - int _gt_map_i = 0; - //---a - for( auto it=gt_map.begin() ; it!= gt_map.end() ; it++ ) { - cout << _gt_map_i++ << " : " << it->first << " : " << (it->second).transpose() << endl; - } - - ///---b - cout << "loop on lbm lbm.size() = " << lbm.size() << " lbm_fullpose.size= "<< lbm_fullpose.size() <<"\n"; - assert( lbm.size() == lbm_fullpose.size() ); - for( auto k=0 ; kgetNodeTimestamp(k); - Matrix4d _viopose = manager->getNodePose(k); - cout << k << ": " << _t << ": "<< lbm[k].transpose() << "\t" ; - cout << PoseManipUtils::prettyprintMatrix4d(lbm_fullpose[k]) << endl; - - - assoc_s tmp; - tmp.corrected_pose = lbm_fullpose[k]; - tmp.vio_pose = _viopose; - tmp.node_timestamp = _t; - - - //--------------- - // loop through gt_map and find the gt_pose at this t. - // search for `_t` in gt_map. - { - cout << "\tsearch for `_t`= "<< _t << " in gt_map.\n"; - int it_i = -1; - ros::Duration smallest_diff = ros::Duration( 100000 ); - int smallest_diff_i = -1; - auto gt_map_iterator = gt_map.begin(); - bool found = false; - for( auto it=gt_map.begin() ; it!= gt_map.end() ; it++ ) { - it_i++; - ros::Duration diff = it->first - _t; - if( diff.sec < 0 ) { diff.sec = - diff.sec; diff.nsec = 1000000000 - diff.nsec; } - // cout << "\t\tit_i=" << it_i << " diff=" << diff.sec << " " << diff.nsec << " >>>>>> smallest_diff_i=" << smallest_diff_i << endl; - - if( diff.sec < smallest_diff.sec || (diff.sec == smallest_diff.sec && abs(diff.nsec) < abs(smallest_diff.nsec) ) ) { - smallest_diff = diff; - smallest_diff_i = it_i; - gt_map_iterator = it; - } - if( (diff.sec == 0 && abs(diff.nsec) < 10000000) || (diff.sec == -1 && diff.nsec > (1000000000-10000000) ) ) { - // cout << "NodeDataManager::find_indexof_node " << i << " "<< diff.sec << " " << diff.nsec << endl - cout << TermColor::GREEN() << "\tfound " << it->first << " at idx=" << it_i << endl << TermColor::RESET(); - found = true; - - tmp.gt_pose_timestamp = it->first; - tmp.gt_pose = it->second; - break; - } - - } - - if( found == false ) { - cout << TermColor::RED() << "\tNOT found, however, smallest_diff=" << smallest_diff << " at idx="<< smallest_diff_i; - cout << endl << TermColor::RESET(); - tmp.gt_pose_timestamp = gt_map_iterator->first; - tmp.gt_pose = gt_map_iterator->second; - } - } - //--------- - // END of search onn gt_map - //--------- - - - - // Result:" - _RESULT_.push_back( tmp ); - cout << TermColor::CYAN() << "Result: \n"; - cout << "node_timestamp " << tmp.node_timestamp << endl; - cout << "gt_pose_timestamp " << tmp.gt_pose_timestamp << endl; - cout << "gt_pose " << (tmp.gt_pose).transpose() << endl; - cout << "vio_pose " << PoseManipUtils::prettyprintMatrix4d(tmp.vio_pose) << endl; - cout << "corrected_pose " << PoseManipUtils::prettyprintMatrix4d(tmp.corrected_pose) << endl; - cout << TermColor::RESET(); - - } - - - // write CSV: - std::stringstream buffer_gt; - std::stringstream buffer_corrected; - std::stringstream buffer_vio; - - for( int j=0 ; j<_RESULT_.size() ; j++ ) - { - // stamp, tx, ty, tz - buffer_gt << long(_RESULT_[j].node_timestamp.toSec() * 1E9) << "," << _RESULT_[j].gt_pose(0) << "," << _RESULT_[j].gt_pose(1)<< "," << _RESULT_[j].gt_pose(2) < > jmb; - vector< Vector3d > lbm; // a corrected poses. Same index as the node. These are used for loopedges. - vector< Matrix4d > lbm_fullpose; - bool published_axis = true; - while( ros::ok() ) - { - // cout << "[opt_traj_publisher_colored_by_world]---\n"; - if( manager->getNodeLen() == 0 ) { - // cout << "[opt_traj_publisher_colored_by_world]nothing to publish\n"; - loop_rate.sleep(); - continue; - } - - //clear map - jmb.clear(); - lbm.clear(); - lbm_fullpose.clear(); - - // cerr << "[opt_traj_publisher_colored_by_world]i=0 ; i<"<< manager->getNodeLen() << " ; solvedUntil=" << slam->solvedUntil() <<"\n"; - int latest_pose_worldid = -1; - int ____solvedUntil = slam->solvedUntil(); //note: solvedUntil is the index until which posegraph was solved - int ____solvedUntil_worldid = manager->which_world_is_this( manager->getNodeTimestamp(____solvedUntil) ); - bool ____solvedUntil_worldid_is_neg = false; - 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<"<getNodeLen() ; - cout << "____solvedUntil=" << ____solvedUntil << "\t"; - cout << "____solvedUntil_worldid=" << ____solvedUntil_worldid << "\t"; - cout << "__slam->get_reinit_ceres_problem_onnewloopedge_optimize6DOF_status=" << slam->get_reinit_ceres_problem_onnewloopedge_optimize6DOF_status() << "\t"; - cout << endl; - } - #endif - - ElapsedTime _time_jmb; - _time_jmb.tic(); - for( int i=0 ; igetNodeLen() ; i++ ) - { - int world_id = manager->which_world_is_this( manager->getNodeTimestamp(i) ); - // cout << "i=" << i << " world#" << world_id << endl;// << " w_TM_i=" << PoseManipUtils::prettyprintMatrix4d( w_TM_i ) << endl; - - - /* - if( slam->get_reinit_ceres_problem_onnewloopedge_optimize6DOF_status() == 2 ) { - cout << TermColor::iYELLOW() << "[main::opt_traj_publisher_colored_by_world] I have detected ceres::Solve() is in progress. This is the cause of thread blocking....avoiding this by break" << TermColor::RESET() << endl; - break; - } - */ - - // const Matrix4d ___slam_getNodePose_i = slam->getNodePose( i ); - // bool ___slam_nodePoseExists_i = slam->nodePoseExists(i); - - - // i>=0 and i=0 && i<= ____solvedUntil ) { - // Matrix4d w_T_c_optimized; - Matrix4d w_T_c; - - - // If the optimized pose exists use that else use the odometry pose - int from_slam_or_from_odom = -1; - if( world_id >= 0 ) { - if( slam->nodePoseExists(i) /*___slam_nodePoseExists_i*/ ) { - w_T_c = slam->getNodePose( i ); - /*w_T_c = ___slam_getNodePose_i;*/ - // cerr << "w_T_c_optimized=" << PoseManipUtils::prettyprintMatrix4d( w_T_c ) << endl; - from_slam_or_from_odom = 1; - } else { - if( manager->nodePoseExists(i ) ) { - w_T_c = manager->getNodePose( i ); - // cerr << "w_T_c=" << PoseManipUtils::prettyprintMatrix4d( w_T_c ) << endl; - from_slam_or_from_odom = 2; - } - } - } else { - // kidnapped worlds viz, -1, -2 etc. - // use the last pose and add the odometry to it - // TODO - from_slam_or_from_odom = 3; - - int last_idx = manager->nodeidx_of_world_i_ended( -world_id - 1 ); - Matrix4d w_T_last; - w_T_last = *(jmb.at( -world_id - 1 ).rbegin()); - Matrix4d last_M_i = manager->getNodePose( last_idx ).inverse() * manager->getNodePose( i ); - w_T_c = w_T_last * last_M_i; - - - } - - - if( jmb.count( world_id ) == 0 ) - jmb[ world_id ] = vector(); - - jmb[ world_id ].push_back( w_T_c ); - lbm.push_back( w_T_c.col(3).topRows(3) ); - lbm_fullpose.push_back( w_T_c ); - latest_pose_worldid = world_id; - - #ifdef __opt_traj_publisher_colored_by_world___print_on_file_exist - if( enable_cout ) - cout << TermColor::RED() << i << ":" << world_id << " (from_slam_or_from_odom=" << from_slam_or_from_odom << " w_T_c=" << PoseManipUtils::prettyprintMatrix4d( w_T_c ) << endl << TermColor::RESET(); - #endif - - } - - - // i>solvedUntil() < manager->getNodeLen() only odometry available here. - - if( i>(____solvedUntil) ) { - int last_idx=-1; - Matrix4d w_TM_i; - - - - if( ____solvedUntil == 0 ) { - w_TM_i = manager->getNodePose( i ); - } else { - 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 ); - } - else if( world_id < 0 ) { - // 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 && igetNodeLen()) ) { - 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 && igetNodeLen() ); - } - - 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); - } - - } - - if( last_idx >= 0 ) { - Matrix4d w_T_last; - if( slam->nodePoseExists(last_idx) ) - w_T_last = slam->getNodePose(last_idx ); - else - w_T_last = manager->getNodePose(last_idx ); - - Matrix4d last_M_i = manager->getNodePose( last_idx ).inverse() * manager->getNodePose( i ); - w_TM_i = w_T_last * last_M_i; - } - - - - if( jmb.count( world_id ) == 0 ) - jmb[ world_id ] = vector(); - - - - jmb[ world_id ].push_back( w_TM_i ); - lbm.push_back( w_TM_i.col(3).topRows(3) ); - lbm_fullpose.push_back( w_TM_i ); - latest_pose_worldid = world_id; - - #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 - } - - - } - - // cout << TermColor::BLUE() << "[opt_traj_publisher_colored_by_world] Took " << _time_jmb.toc_milli() << "ms to compute #nodes=" << manager->getNodeLen() << TermColor::RESET() << endl; - //-----------------------------------------------------------------------------------------------// - //------------------------- After this only uses jmb and lmb to publish -------------------------// - //-----------------------------------------------------------------------------------------------// - - //--- - //--- Decide offset's (for plotting) different co-ordinate systems - //--- - ElapsedTime _time_publih; - _time_publih.tic(); - #if 1 - map< int, int > setids_to_udumbes; - if( jmb.size() > 0 ) - { - int h=0; - for( auto it=jmb.begin() ; it!=jmb.end() ; it++ ) { - int setid = manager->getWorldsConstPtr()->find_setID_of_world_i( it->first ); - if( setid < 0 ) //ignore negative setids - continue; - if( setids_to_udumbes.count(setid) == 0 ) { - setids_to_udumbes[ setid ] = h; - h++; - } - } - - #if 0 - cout << TermColor::MAGENTA() << "---\n"; - for( auto it=setids_to_udumbes.begin() ; it!=setids_to_udumbes.end() ; it++ ) { - cout << "setid=" << it->first << "\tudumbe=" << it->second << endl; - } - cout << TermColor::RESET(); - #endif - - } - #endif - - - - - //--- - //--- Publish jmb. - // Note: jmb's keys are `worldIDs` and jmb's values are `vector& w_T_ci` - //--- - if( jmb.size() == 0 ) - { - // cout << "[opt_traj_publisher_colored_by_world] not publishing because jmb.size is zero\n"; - } - else - { - // cout << "[opt_traj_publisher_colored_by_world]publish\n"; - - // collect data to publish - for( auto it=jmb.begin() ; it!=jmb.end() ; it++ ) { - // if( it->first < 0 ) //skip publishing kidnapped part. - // continue; - string ns = "world#"+to_string( it->first ); - - float c_r=0., c_g=0., c_b=0.; - int rng=-1; - - if( options.line_color_style == 10 ) - rng = it->first; //color by world id WorldID - else if( options.line_color_style == 12 ) - rng = manager->getWorldsConstPtr()->find_setID_of_world_i( it->first ); //color by setID - else { - cout << TermColor::RED() << "opt_traj_publisher_colored_by_world ERROR. invalid option `opt_traj_publisher_options`. expected either 10 or 12.\n"; - exit( 1 ); - } - - // #if opt_traj_publisher_colored_by_world_LINE_COLOR_STYLE == 10 - // int rng = it->first; //color by world id WorldID - // #endif - // - // #if opt_traj_publisher_colored_by_world_LINE_COLOR_STYLE == 12 - // int rng = manager->getWorldsConstPtr()->find_setID_of_world_i( it->first ); //color by setID - // #endif - if( rng >= 0 ) { - cv::Scalar color = FalseColors::randomColor( rng ); - c_r = color[2]/255.; - c_g = color[1]/255.; - c_b = color[0]/255.; - } - - int curr_set_id = manager->getWorldsConstPtr()->find_setID_of_world_i( it->first ); - float offset_x=0., offset_y=0., offset_z=0.; - if( curr_set_id >= 0 ) { - if ( setids_to_udumbes.count(curr_set_id) > 0 ) { - // cout << "rng=" << rng << "setids_to_udumbes"<< setids_to_udumbes.at( rng ) << endl; - offset_y = setids_to_udumbes.at( curr_set_id )*options.udumbe_offset_y; - } - } else { - curr_set_id = manager->getWorldsConstPtr()->find_setID_of_world_i( -it->first-1); - if ( setids_to_udumbes.count(curr_set_id) > 0 ) { - offset_y = setids_to_udumbes.at( curr_set_id )*options.udumbe_offset_y; - } - } - - viz->publishNodesAsLineStrip( it->second, ns.c_str(), c_r, c_g, c_b, options.linewidth_multiplier, offset_x, offset_y, offset_z ); - } - // #endif - - // Publish Camera visual - Matrix4d wi_T_latest = *( jmb.at( latest_pose_worldid ).rbegin() ); - float c_r=0., c_g=0., c_b=0.; - int rng=-1; - if( options.line_color_style == 10 ) - rng = latest_pose_worldid; - else if( options.line_color_style == 12 ) - rng = manager->getWorldsConstPtr()->find_setID_of_world_i( latest_pose_worldid ); //color by setID - else { - cout << TermColor::RED() << "opt_traj_publisher_colored_by_world ERROR. invalid option `opt_traj_publisher_options`. expected either 10 or 12.\n"; - exit( 1 ); - } - - - // #if opt_traj_publisher_colored_by_world_LINE_COLOR_STYLE == 10 - // int rng = latest_pose_worldid; - // #endif - // - // #if opt_traj_publisher_colored_by_world_LINE_COLOR_STYLE == 12 - // int rng = manager->getWorldsConstPtr()->find_setID_of_world_i( latest_pose_worldid ); //color by setID - // #endif - if( rng >= 0 ) { - cv::Scalar color = FalseColors::randomColor( rng ); - c_r = color[2]/255.; - c_g = color[1]/255.; - c_b = color[0]/255.; - } - - - int curr_set_id = manager->getWorldsConstPtr()->find_setID_of_world_i( latest_pose_worldid ); - float offset_x=0., offset_y=0., offset_z=0.; - if( curr_set_id >= 0 ) { - if ( setids_to_udumbes.count(curr_set_id) > 0 ) { - // cout << "rng=" << rng << "setids_to_udumbes"<< setids_to_udumbes.at( rng ) << endl; - offset_y = setids_to_udumbes.at( curr_set_id )*options.udumbe_offset_y; - } - } else { - curr_set_id = manager->getWorldsConstPtr()->find_setID_of_world_i( -latest_pose_worldid-1 ); - if ( setids_to_udumbes.count(curr_set_id) > 0 ) { - offset_y = setids_to_udumbes.at( curr_set_id )*options.udumbe_offset_y; - } - } - - viz->publishCameraVisualMarker( wi_T_latest, "world##", c_r, c_g, c_b, - options.linewidth_multiplier, 20, - offset_x, offset_y, offset_z ); - - - // Publish loop edges - // TODO: in the future publish intra-world loopedges in different color and interworld as different color - visualization_msgs::Marker linelist_marker; - RosMarkerUtils::init_line_marker( linelist_marker ); - linelist_marker.ns = "loopedges_on_opt_traj"; linelist_marker.id = 0; - linelist_marker.color.r = 0.42;linelist_marker.color.g = 0.55;linelist_marker.color.b = 0.14;linelist_marker.color.a = 1.; - linelist_marker.scale.x = 0.1*options.linewidth_multiplier; - - int nloopedgfes = manager->getEdgeLen(); - for( int it=0 ; itgetEdgeIdxInfo( it ); - int __a = pair.first; - int __b = pair.second; - - int __a_worldid = manager->which_world_is_this( manager->getNodeTimestamp(__a) ); - int __a_setid = manager->getWorldsConstPtr()->find_setID_of_world_i( __a_worldid ); - int __b_worldid = manager->which_world_is_this( manager->getNodeTimestamp(__b) ); - int __b_setid = manager->getWorldsConstPtr()->find_setID_of_world_i( __b_worldid ); - #if 0 - cout << TermColor::CYAN() ; - cout << "it=" << it; - cout << "__a=" << __a << " __a_worldid=" << __a_worldid << " __a_setid=" << __a_setid << "\t|"; - cout << "__b=" << __b << " __b_worldid=" << __b_worldid << " __b_setid=" << __b_setid << "\n"; - cout << TermColor::RESET(); - #endif - - int __a_udumbe = 0, __b_udumbe=0; - if( setids_to_udumbes.count(__a_setid) > 0 ) - __a_udumbe = setids_to_udumbes.at( __a_setid ); - if( setids_to_udumbes.count(__b_setid) > 0 ) - int __b_udumbe = setids_to_udumbes.at( __b_setid ); - - Vector3d ____apose = lbm[__a] + Vector3d( 0., options.udumbe_offset_y*__a_udumbe , 0.0 ); - Vector3d ____bpose = lbm[__b] + Vector3d( 0., options.udumbe_offset_y*__a_udumbe , 0.0 ); - - RosMarkerUtils::add_point_to_marker( ____apose, linelist_marker, false ); - RosMarkerUtils::add_point_to_marker( ____bpose, linelist_marker, false ); - } - viz->publishThisVisualMarker( linelist_marker ); - - - // if( published_axis || rand() % 100 == 0 ) { - if( published_axis || rand() % 100 == 0 ) { - // 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 ); - - for( int p=0 ; ppublishXYZAxis( _axis_pose, "opt_traj_axis", p, 2.0 ); - } - - if( rand() %1000 == 0 ) { - // once in a while flush the unused co-ordinates - Matrix4d _axis_pose = Matrix4d::Identity(); - for( int p=setids_to_udumbes.size() ; p<20; p++ ) { - viz->publishXYZAxis( _axis_pose, "opt_traj_axis", p, 0.0 ); - } - } - - published_axis = false; - } - - } - - // cout << TermColor::BLUE() << "[opt_traj_publisher_colored_by_world] Publish took " << _time_publih.toc_milli() << " ms" << endl; - // book keeping - // cerr << "\nSLEEP\n"; - loop_rate.sleep(); - } - - - #if __CODE___GT__ - - // Set this to 1 to enable loggin of final poses, - #define __LOGGING___LBM__ 1 - #if __LOGGING___LBM__ - // Log LMB - vector _RESULT_; - cout << "========[opt_traj_publisher_colored_by_world] logging ========\n"; - cout << "loop on gt_map\n"; - int _gt_map_i = 0; - //---a - for( auto it=gt_map.begin() ; it!= gt_map.end() ; it++ ) { - cout << _gt_map_i++ << " : " << it->first << " : " << (it->second).transpose() << endl; - } - - ///---b - cout << "loop on lbm lbm.size() = " << lbm.size() << " lbm_fullpose.size= "<< lbm_fullpose.size() <<"\n"; - assert( lbm.size() == lbm_fullpose.size() ); - for( auto k=0 ; kgetNodeTimestamp(k); - Matrix4d _viopose = manager->getNodePose(k); - cout << k << ": " << _t << ": "<< lbm[k].transpose() << "\t" ; - cout << PoseManipUtils::prettyprintMatrix4d(lbm_fullpose[k]) << endl; - - - assoc_s tmp; - tmp.corrected_pose = lbm_fullpose[k]; - tmp.vio_pose = _viopose; - tmp.node_timestamp = _t; - - - //--------------- - // loop through gt_map and find the gt_pose at this t. - // search for `_t` in gt_map. - { - cout << "\tsearch for `_t`= "<< _t << " in gt_map.\n"; - int it_i = -1; - ros::Duration smallest_diff = ros::Duration( 100000 ); - int smallest_diff_i = -1; - auto gt_map_iterator = gt_map.begin(); - bool found = false; - for( auto it=gt_map.begin() ; it!= gt_map.end() ; it++ ) { - it_i++; - ros::Duration diff = it->first - _t; - if( diff.sec < 0 ) { diff.sec = - diff.sec; diff.nsec = 1000000000 - diff.nsec; } - // cout << "\t\tit_i=" << it_i << " diff=" << diff.sec << " " << diff.nsec << " >>>>>> smallest_diff_i=" << smallest_diff_i << endl; - - if( diff.sec < smallest_diff.sec || (diff.sec == smallest_diff.sec && abs(diff.nsec) < abs(smallest_diff.nsec) ) ) { - smallest_diff = diff; - smallest_diff_i = it_i; - gt_map_iterator = it; - } - if( (diff.sec == 0 && abs(diff.nsec) < 10000000) || (diff.sec == -1 && diff.nsec > (1000000000-10000000) ) ) { - // cout << "NodeDataManager::find_indexof_node " << i << " "<< diff.sec << " " << diff.nsec << endl - cout << TermColor::GREEN() << "\tfound " << it->first << " at idx=" << it_i << endl << TermColor::RESET(); - found = true; - - tmp.gt_pose_timestamp = it->first; - tmp.gt_pose = it->second; - break; - } - - } - - if( found == false ) { - cout << TermColor::RED() << "\tNOT found, however, smallest_diff=" << smallest_diff << " at idx="<< smallest_diff_i; - cout << endl << TermColor::RESET(); - tmp.gt_pose_timestamp = gt_map_iterator->first; - tmp.gt_pose = gt_map_iterator->second; - } - } - //--------- - // END of search onn gt_map - //--------- - - - - // Result:" - _RESULT_.push_back( tmp ); - cout << TermColor::CYAN() << "Result: \n"; - cout << "node_timestamp " << tmp.node_timestamp << endl; - cout << "gt_pose_timestamp " << tmp.gt_pose_timestamp << endl; - cout << "gt_pose " << (tmp.gt_pose).transpose() << endl; - cout << "vio_pose " << PoseManipUtils::prettyprintMatrix4d(tmp.vio_pose) << endl; - cout << "corrected_pose " << PoseManipUtils::prettyprintMatrix4d(tmp.corrected_pose) << endl; - cout << TermColor::RESET(); - - } - - - // write CSV: - std::stringstream buffer_gt; - std::stringstream buffer_corrected; - std::stringstream buffer_vio; - - for( int j=0 ; j<_RESULT_.size() ; j++ ) - { - // stamp, tx, ty, tz - buffer_gt << long(_RESULT_[j].node_timestamp.toSec() * 1E9) << "," << _RESULT_[j].gt_pose(0) << "," << _RESULT_[j].gt_pose(1)<< "," << _RESULT_[j].gt_pose(2) <setPathPublisher( pub_path ); viz->setOdometryPublisher( pub_odometry_opt ); - // setup manager publishers threads - adhoc - // std::thread th3( periodic_publish_optimized_poses_smart, manager, slam, viz ); - // std::thread th4( periodic_publish_odoms, manager, viz ); - std::thread th5( monitor_disjoint_set_datastructure, manager, viz ); + //----Pose Composer---// + Composer * cmpr = new Composer( manager, slam, viz ); + + // ++ 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. + cmpr->pose_assember_enable(); + // cmpr->pose_assember_disable(); + std::thread assember_th( &Composer::pose_assember_thread, cmpr, 30 ); + + // ++ start bf_traj_publish_thread + cmpr->bf_traj_publish_enable(); + // cmpr->bf_traj_publish_disable(); + std::thread bf_pub_th( &Composer::bf_traj_publish_thread, cmpr, 15 ); + + // ++ start camera visual publisher thread + cmpr->cam_visual_publish_enable(); + // cmpr->cam_visual_publish_disable(); + std::thread cam_visual_pub_th( &Composer::cam_visual_publish_thread, cmpr, 30 ); + + // ++ loop edge publish thread + cmpr->loopedge_publish_enable(); + // cmpr->loopedge_publish_disable(); + std::thread loopedge_pub_th( &Composer::loopedge_publish_thread, cmpr, 10 ); + + // ++ disjointset status image + cmpr->disjointset_statusimage_publish_enable(); + // cmpr->disjointset_statusimage_publish_disable(); + std::thread disjointset_monitor_pub_th( &Composer::disjointset_statusimage_publish_thread, cmpr, 1 ); - opt_traj_publisher_options options; - // 10 //< color the line with worldID - // 12 //< color the line with setID( worldID ) - options.line_color_style = 10; - options.linewidth_multiplier = 3; //0.25; //8 - options.udumbe_offset_y = 30.0; - std::thread th6( opt_traj_publisher_colored_by_world, manager, slam, viz, options ); + //----END Pose Composer---// + //--- setup manager publishers threads - adhoc ---// + std::thread th4( periodic_publish_odoms, manager, viz, 30.0, 0.0, 0.0 ); //if you enable this, dont forget to join(), after spin() returns. // while(ros::ok()) publish debug stuff + #if 0 ros::Rate loop_rate(40); while(ros::ok()) { ros::spinOnce(); loop_rate.sleep(); } + #else + ros::spin(); + #endif // slam->new_optimize6DOF_disable(); slam->reinit_ceres_problem_onnewloopedge_optimize6DOF_disable(); - // th1.join(); - // th2.join(); - // th3.join(); - // th4.join(); - th5.join(); - th6.join(); + cmpr->pose_assember_disable(); + cmpr->bf_traj_publish_disable(); + cmpr->cam_visual_publish_disable(); + cmpr->loopedge_publish_disable(); + cmpr->disjointset_statusimage_publish_disable(); + + + th4.join(); th_slam.join(); + assember_th.join(); + bf_pub_th.join(); + cam_visual_pub_th.join(); + loopedge_pub_th.join(); + disjointset_monitor_pub_th.join(); +