imuTcam (cam extrinsic acquired successfully)

Author: mpkuse

src/NodeDataManager.cpp

@@ -43,7 +43,7 @@ void NodeDataManager::camera_pose_callback( const nav_msgs::Odometry::ConstPtr&
     w_T_cam(2,3) = msg->pose.pose.position.z;
     w_T_cam(3,3) = 1.0;
     __NODEDATAMANAGER_CALLBACKS(
-    cout << "camera_pose_callback: node_pose.size()=" << node_pose.size() << " curr_pose=" << PoseManipUtils::prettyprintMatrix4d( w_T_cam ) << endl;
+    cout << "[NodeDataManager::camera_pose_callback] t=" <<  msg->header.stamp << ": node_pose.size()=" << node_pose.size() << " curr_pose=" << PoseManipUtils::prettyprintMatrix4d( w_T_cam ) << endl;
     )
 
 
@@ -75,56 +75,6 @@ void NodeDataManager::camera_pose_callback( const nav_msgs::Odometry::ConstPtr&
 }
 
 
-// void NodeDataManager::loopclosure_pose_callback( const nap::NapMsg::ConstPtr& msg  )
-// {
-//     // ROS_INFO( "NodeDataManager::loopclosure_pose_callback");
-//     // Add a new edge ( 2 node*)
-//
-//
-//     ros::Time t_c = msg->c_timestamp;
-//     ros::Time t_p = msg->prev_timestamp;
-//     int op_mode = msg->op_mode;
-//     double goodness = (double)msg->goodness;
-//     assert( op_mode == 30 );
-//
-//     // retrive rel-pose  p_T_c
-//     Matrix4d p_T_c = Matrix4d::Zero();
-//     Quaterniond quat( msg->p_T_c.orientation.w, msg->p_T_c.orientation.x, msg->p_T_c.orientation.y, msg->p_T_c.orientation.z );
-//     p_T_c.topLeftCorner<3,3>() = quat.toRotationMatrix();
-//     p_T_c(0,3) = msg->p_T_c.position.x;
-//     p_T_c(1,3) = msg->p_T_c.position.y;
-//     p_T_c(2,3) = msg->p_T_c.position.z;
-//     p_T_c(3,3) = 1.0;
-//
-//
-//     // Lock
-//     node_mutex.lock() ;
-//
-//     // loop up t_c in node_timestamps[]
-//     int index_t_c = find_indexof_node(node_timestamps, t_c );
-//
-//     // loop up t_p in node_timestamps
-//     int index_t_p = find_indexof_node(node_timestamps, t_p );
-//
-//     // Unlock
-//     node_mutex.unlock();
-//
-//     cout << "[NodeDataManager] Rcvd NapMsg " << index_t_c << "<--->" << index_t_p << endl;
-//     assert( t_c > t_p );
-//
-//     std::pair<int,int> closure_edge;
-//     closure_edge.first = index_t_p;
-//     closure_edge.second = index_t_c;
-//
-//     edge_mutex.lock();
-//     loopclosure_edges.push_back( closure_edge );
-//     loopclosure_edges_goodness.push_back( goodness );
-//     loopclosure_p_T_c.push_back( p_T_c );
-//     edge_mutex.unlock();
-//
-//
-//
-// }
 #ifdef __USE_SELF_LOOPEDGE_MSG
 void NodeDataManager::loopclosure_pose_callback( const solve_keyframe_pose_graph::LoopEdge::ConstPtr& msg  )
 #else
@@ -208,6 +158,71 @@ void NodeDataManager::loopclosure_pose_callback(  const cerebro::LoopEdge::Const
 
 }
 
+
+void NodeDataManager::extrinsic_cam_imu_callback( const nav_msgs::Odometry::ConstPtr msg )
+{
+    // __NODEDATAMANAGER_CALLBACKS( cout << TermColor::GREEN() << "[NodeDataManager::extrinsic_cam_imu_callback]" << msg->header.stamp  << TermColor::RESET() << endl; )
+
+    // Acquire lock
+    //      update imu_T_cam,
+    //      update last got timestamp
+    //      set is_imu_cam_extrinsic_available to true
+    {
+        std::lock_guard<std::mutex> lk(imu_cam_mx);
+        PoseManipUtils::geometry_msgs_Pose_to_eigenmat( msg->pose.pose, this->imu_T_cam );
+        this->imu_T_cam_stamp = msg->header.stamp;
+        this->imu_T_cam_available = true;
+
+    }
+
+
+    __NODEDATAMANAGER_CALLBACKS(
+    cout << TermColor::GREEN() << "[NodeDataManager::extrinsic_cam_imu_callback]" << msg->header.stamp  << TermColor::RESET();
+    cout << " imu_T_cam = " << PoseManipUtils::prettyprintMatrix4d(this->imu_T_cam);
+    cout << endl;
+    )
+
+}
+
+
+Matrix4d NodeDataManager::get_imu_T_cam() const
+{
+    std::lock_guard<std::mutex> lk(imu_cam_mx);
+    // Remove this if, once i am confident everythinbg is ok!
+    if( imu_T_cam_available == false )
+    {
+        ROS_ERROR( "[NodeDataManager::get_imu_T_cam] posegraph solver, you requested imu_T_cam aka imu-cam extrinsic calib, but currently it is not available. FATAL ERROR.\n");
+        exit(1);
+    }
+    assert( imu_T_cam_available );
+    return imu_T_cam;
+}
+
+
+void NodeDataManager::get_imu_T_cam( Matrix4d& res, ros::Time& _t ) const
+{
+    std::lock_guard<std::mutex> lk(imu_cam_mx);
+    // Remove this if, once i am confident everythinbg is ok!
+    if( imu_T_cam_available == false )
+    {
+        ROS_ERROR( "[NodeDataManager::get_imu_T_cam] posegraph solver, you requested imu_T_cam aka imu-cam extrinsic calib, but currently it is not available. FATAL ERROR.\n");
+        exit(1);
+    }
+
+    assert( imu_T_cam_available );
+    res = imu_T_cam;
+    _t = imu_T_cam_stamp;
+    return;
+}
+
+bool NodeDataManager::is_imu_T_cam_available() const
+{
+    std::lock_guard<std::mutex> lk(imu_cam_mx);
+    return imu_T_cam_available;
+}
+
+
+
 // internal node queue length info
 void NodeDataManager::print_nodes_lengths() const
 {
@@ -1005,7 +1020,7 @@ int NodeDataManager::n_worlds() const
 
 
 
-void NodeDataManager::print_worlds_info( int verbosity ) const 
+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 ;

src/NodeDataManager.h

@@ -193,5 +193,17 @@ class NodeDataManager
     Worlds * worlds_handle_raw_ptr=NULL;
 
 
+    //////// cam imu extrinsic (this is published by vins estimator node)
+public:
+    void extrinsic_cam_imu_callback( const nav_msgs::Odometry::ConstPtr msg );
+    Matrix4d get_imu_T_cam() const;
+    void get_imu_T_cam( Matrix4d& res, ros::Time& _t ) const;
+    bool is_imu_T_cam_available() const;
+
+private:
+    mutable mutex imu_cam_mx;
+    bool imu_T_cam_available = false;
+    Matrix4d imu_T_cam;
+    ros::Time imu_T_cam_stamp;
 
 };

src/keyframe_pose_graph_slam_node.cpp

@@ -269,8 +269,17 @@ int main( int argc, char ** argv)
     ros::Subscriber  sub_gt_ = nh.subscribe( ground_truth_topic, 100, ground_truth_callback );
     #endif
 
+    //-- subscribe to imu_T_cam : imu camera extrinsic calib. Will store this just in case there is a need
+    //   this is published by vins_estimator node.
+    string extrinsic_cam_imu_topic = string("/vins_estimator/extrinsic");
+    ROS_INFO( "Subscribe to extrinsic_cam_imu_topic: %s", extrinsic_cam_imu_topic.c_str() );
+    ros::Subscriber sub_cam_imu_extrinsic = nh.subscribe( extrinsic_cam_imu_topic, 1000, &NodeDataManager::extrinsic_cam_imu_callback, manager );
 
 
+    // TODO
+    // subscribes to w_T_imu @100hz and publish the pose in my world frame at 100hz
+    // implement this in composer class.
+
     // Setup publishers
     //--- Marker ---//
     string marker_topic = string( "viz/visualization_marker");
@@ -303,7 +312,7 @@ int main( int argc, char ** argv)
     // std::thread th_slam( &PoseGraphSLAM::new_optimize6DOF, slam );
 
     slam->reinit_ceres_problem_onnewloopedge_optimize6DOF_enable();
-    // slam->reinit_ceres_problem_onnewloopedge_optimize6DOF_disable();
+    slam->reinit_ceres_problem_onnewloopedge_optimize6DOF_disable();
     std::thread th_slam( &PoseGraphSLAM::reinit_ceres_problem_onnewloopedge_optimize6DOF, slam );
 
 
@@ -320,33 +329,33 @@ int main( int argc, char ** argv)
     // ++ 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();
+    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();
+    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();
+    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();
+    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();
+    cmpr->disjointset_statusimage_publish_disable();
     std::thread disjointset_monitor_pub_th( &Composer::disjointset_statusimage_publish_thread, cmpr, 1 );
 
 
     //----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.
+    // 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.
 
 
 
@@ -373,7 +382,7 @@ int main( int argc, char ** argv)
     cmpr->disjointset_statusimage_publish_disable();
 
 
-    th4.join();
+    // th4.join();
 
     th_slam.join();
     assember_th.join();