Merge pull request #1 from mpkuse/drone-master

Making it independent of cerebro.

Author: mpkuse

CMakeLists.txt

@@ -14,7 +14,7 @@ find_package(catkin REQUIRED COMPONENTS
   image_transport
   cv_bridge
   #nap
-  cerebro
+  #cerebro
   nav_msgs
 )
 
@@ -34,24 +34,35 @@ set( CMAKE_CXX_FLAGS "-fpermissive -std=c++11 -O3" )
 
 
 
+
+
+
+
+add_message_files(
+  FILES
+  LoopEdge.msg
+)
+
+
 generate_messages(
   DEPENDENCIES
   std_msgs
   sensor_msgs
   geometry_msgs
   nav_msgs
   #nap
-  cerebro
+  #cerebro
 )
 
 
-
 catkin_package(
   CATKIN_DEPENDS roscpp rospy std_msgs message_runtime image_transport cv_bridge
   #nap
-  cerebro
+  #cerebro
 )
 
+
+
 include_directories(
   ${catkin_INCLUDE_DIRS}
   ${CERES_INCLUDE_DIRS}
@@ -74,7 +85,8 @@ add_executable( keyframe_pose_graph_slam
 
 
 # this is needed to indicate this this executable depends on the messages of the pkg.
-add_dependencies(keyframe_pose_graph_slam cerebro_generate_messages_cpp)
+#add_dependencies(keyframe_pose_graph_slam cerebro_generate_messages_cpp)
+#add_dependencies(keyframe_pose_graph_slam )
 
 
 #add_executable( test_disjointset_imp src/test_disjointset.cpp )

msg/LoopEdge.msg

@@ -0,0 +1,5 @@
+time timestamp0
+time timestamp1
+geometry_msgs/Pose pose_1T0 # pose of 0 as observed from 1. 
+float32 weight
+string description

package.xml

@@ -44,20 +44,22 @@
   <build_depend>roscpp</build_depend>
   <build_depend>rospy</build_depend>
   <build_depend>std_msgs</build_depend>
+  <build_depend>geometry_msgs</build_depend>
   <build_depend>message_generation</build_depend>
   <build_depend>image_transport</build_depend>
   <build_depend>cv_bridge</build_depend>
 
-  <build_depend>cerebro</build_depend>
+  <!-- <build_depend>cerebro</build_depend> -->
 
   <run_depend>roscpp</run_depend>
   <run_depend>rospy</run_depend>
   <run_depend>std_msgs</run_depend>
+  <run_depend>geometry_msgs</run_depend>
   <run_depend>message_runtime</run_depend>
   <run_depend>image_transport</run_depend>
   <run_depend>cv_bridge</run_depend>
 
-  <run_depend>cerebro</run_depend>
+  <!-- <run_depend>cerebro</run_depend> -->
 
   <!-- The export tag contains other, unspecified, tags -->
   <export>

src/NodeDataManager.cpp

@@ -18,6 +18,8 @@ worlds_handle_raw_ptr = new Worlds();
 
 }
 
+#define __NODEDATAMANAGER_CALLBACKS( msg ) msg;
+// #define __NODEDATAMANAGER_CALLBACKS(msg) ;
 void NodeDataManager::camera_pose_callback( const nav_msgs::Odometry::ConstPtr& msg )
 {
     // ROS_INFO( "NodeDataManager::camera_pose_callback");
@@ -40,7 +42,9 @@ void NodeDataManager::camera_pose_callback( const nav_msgs::Odometry::ConstPtr&
     w_T_cam(1,3) = msg->pose.pose.position.y;
     w_T_cam(2,3) = msg->pose.pose.position.z;
     w_T_cam(3,3) = 1.0;
-    // cout << "camera_pose_callback: " << node_pose.size() << " " << PoseManipUtils::prettyprintMatrix4d( w_T_cam ) << endl;
+    __NODEDATAMANAGER_CALLBACKS(
+    cout << "camera_pose_callback: node_pose.size()=" << node_pose.size() << " curr_pose=" << PoseManipUtils::prettyprintMatrix4d( w_T_cam ) << endl;
+    )
 
 
     // co-variance
@@ -121,8 +125,11 @@ void NodeDataManager::camera_pose_callback( const nav_msgs::Odometry::ConstPtr&
 //
 //
 // }
-
+#ifdef __USE_SELF_LOOPEDGE_MSG
+void NodeDataManager::loopclosure_pose_callback( const solve_keyframe_pose_graph::LoopEdge::ConstPtr& msg  )
+#else
 void NodeDataManager::loopclosure_pose_callback(  const cerebro::LoopEdge::ConstPtr& msg  )
+#endif
 {
     // Add a new edge ( 2 node*)
 
@@ -134,11 +141,11 @@ void NodeDataManager::loopclosure_pose_callback(  const cerebro::LoopEdge::Const
     // assert( op_mode == 30 );
     string description = msg->description;
 
-    #if 0
+    __NODEDATAMANAGER_CALLBACKS(
     cout <<  TermColor::iYELLOW() << "[NodeDataManager::loopclosure_pose_callback]";
     cout << "t_a=" << t_a << "   t_b=" << t_b << "   wt=" << msg->weight;
     cout << "  description=" << msg->description << TermColor::RESET() << endl;
-    #endif
+    )
 
 
     // retrive rel-pose  p_T_c

src/NodeDataManager.h

@@ -39,7 +39,13 @@
 // ros messages
 #include <nav_msgs/Path.h>
 // #include <nap/NapMsg.h>
+#define __USE_SELF_LOOPEDGE_MSG
+
+#ifdef __USE_SELF_LOOPEDGE_MSG
+#include <solve_keyframe_pose_graph/LoopEdge.h>
+#else
 #include <cerebro/LoopEdge.h>
+#endif
 #include <nav_msgs/Odometry.h>
 #include <visualization_msgs/Marker.h>
 #include <std_msgs/ColorRGBA.h>
@@ -75,7 +81,11 @@ class NodeDataManager
     // Callbacks core
     void camera_pose_callback( const nav_msgs::Odometry::ConstPtr& msg );
     // void loopclosure_pose_callback( const nap::NapMsg::ConstPtr& msg  );
+    #ifdef __USE_SELF_LOOPEDGE_MSG
+    void loopclosure_pose_callback( const solve_keyframe_pose_graph::LoopEdge::ConstPtr& msg  );
+    #else
     void loopclosure_pose_callback( const cerebro::LoopEdge::ConstPtr& msg  );
+    #endif
 
     void print_nodes_lengths() const;
 

src/PoseGraphSLAM.cpp

@@ -1103,8 +1103,8 @@ bool PoseGraphSLAM::saveAsJSON(const string base_path)
 
 // This thread wakes up when there are new loopedges. This flag controls the printing on trigger.
 // When debugging you want to keep the headers 'on'.
-// #define ___trigger_header( msg ) msg;
-#define ___trigger_header( msg ) ;
+#define ___trigger_header( msg ) msg;
+// #define ___trigger_header( msg ) ;
 
 void PoseGraphSLAM::reinit_ceres_problem_onnewloopedge_optimize6DOF()
 {

src/keyframe_pose_graph_slam_node.cpp

@@ -838,7 +838,7 @@ void opt_traj_publisher_colored_by_world( const NodeDataManager * manager, const
 
 
 
-    ros::Rate loop_rate(20);
+    ros::Rate loop_rate(10);
     // 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.
@@ -881,7 +881,8 @@ void opt_traj_publisher_colored_by_world( const NodeDataManager * manager, const
         }
         #endif
 
-
+        ElapsedTime _time_jmb;
+        _time_jmb.tic();
         for( int i=0 ; i<manager->getNodeLen() ; i++ )
         {
             int world_id = manager->which_world_is_this( manager->getNodeTimestamp(i) );
@@ -1021,13 +1022,16 @@ void opt_traj_publisher_colored_by_world( const NodeDataManager * manager, const
 
         }
 
+        // 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 )
@@ -1232,7 +1236,7 @@ void opt_traj_publisher_colored_by_world( const NodeDataManager * manager, const
 
         }
 
-
+        // 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();
@@ -1443,6 +1447,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();
     std::thread th_slam( &PoseGraphSLAM::reinit_ceres_problem_onnewloopedge_optimize6DOF, slam );
 
 
@@ -1455,7 +1460,7 @@ int main( int argc, char ** argv)
 
     // 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 th4( periodic_publish_odoms, manager, viz );
     std::thread th5( monitor_disjoint_set_datastructure, manager, viz );
 
     opt_traj_publisher_options options;
@@ -1484,7 +1489,7 @@ int main( int argc, char ** argv)
     // th1.join();
     // th2.join();
     // th3.join();
-    th4.join();
+    // th4.join();
     th5.join();
     th6.join();