add covariance & disable tf

GITAI · May 27, 2021 · e43216d · e43216d
1 parent 25ac370
commit e43216d
Show file tree

Hide file tree

Showing 2 changed files with 20 additions and 12 deletions.
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -41,6 +41,7 @@ catkin_package(
 )
 
 add_executable(livox_repub src/livox_repub.cpp)
+add_dependencies(livox_repub ${catkin_EXPORTED_TARGETS})
 target_link_libraries(livox_repub ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBS})
 
 add_executable(loam_scanRegistration src/scanRegistration.cpp)

diff --git a/src/laserMapping.cpp b/src/laserMapping.cpp
@@ -1157,21 +1157,28 @@ int main(int argc, char** argv)
             odomAftMapped.pose.pose.position.x = transformAftMapped[3];
             odomAftMapped.pose.pose.position.y = transformAftMapped[4];
             odomAftMapped.pose.pose.position.z = transformAftMapped[5];
+            odomAftMapped.pose.covariance = {
+            0.1,    0.0,    0.0,    0.0,    0.0,    0.0,
+            0.0,    0.1,    0.0,    0.0,    0.0,    0.0,
+            0.0,    0.0,    0.1,    0.0,    0.0,    0.0,
+            0.0,    0.0,    0.0,    0.1,    0.0,    0.0,
+            0.0,    0.0,    0.0,    0.0,    0.1,    0.0,
+            0.0,    0.0,    0.0,    0.0,    0.0,    0.1};
 
             pubOdomAftMapped.publish(odomAftMapped);
 
-            static tf::TransformBroadcaster br;
-            tf::Transform                   transform;
-            tf::Quaternion                  q;
-            transform.setOrigin( tf::Vector3( odomAftMapped.pose.pose.position.x,
-                                                odomAftMapped.pose.pose.position.y,
-                                                odomAftMapped.pose.pose.position.z ) );
-            q.setW( odomAftMapped.pose.pose.orientation.w );
-            q.setX( odomAftMapped.pose.pose.orientation.x );
-            q.setY( odomAftMapped.pose.pose.orientation.y );
-            q.setZ( odomAftMapped.pose.pose.orientation.z );
-            transform.setRotation( q );
-            br.sendTransform( tf::StampedTransform( transform, odomAftMapped.header.stamp, "camera_init", "aft_mapped" ) );
+            // static tf::TransformBroadcaster br;
+            // tf::Transform                   transform;
+            // tf::Quaternion                  q;
+            // transform.setOrigin( tf::Vector3( odomAftMapped.pose.pose.position.x,
+            //                                     odomAftMapped.pose.pose.position.y,
+            //                                     odomAftMapped.pose.pose.position.z ) );
+            // q.setW( odomAftMapped.pose.pose.orientation.w );
+            // q.setX( odomAftMapped.pose.pose.orientation.x );
+            // q.setY( odomAftMapped.pose.pose.orientation.y );
+            // q.setZ( odomAftMapped.pose.pose.orientation.z );
+            // transform.setRotation( q );
+            // br.sendTransform( tf::StampedTransform( transform, odomAftMapped.header.stamp, "camera_init", "aft_mapped" ) );
 
             kfNum++;