resolved conflicts

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}
@@ -75,7 +86,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

@@ -229,6 +229,7 @@ struct assoc_s
 #endif
 
 
+
 int main( int argc, char ** argv)
 {
     ros::init(argc, argv, "keyframe_pose_graph_slam_node");
@@ -302,6 +303,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 );