Merge pull request MIT-SPARK#140 from SPARK/feature/csv_publisher

Feature/csv publisher

CMakeLists.txt

@@ -24,6 +24,8 @@ cs_add_library(${PROJECT_NAME}
   include/kimera_vio_ros/RosOnlineDataProvider.h
   include/kimera_vio_ros/RosPublishers.h
   include/kimera_vio_ros/RosVisualizer.h
+  include/kimera_vio_ros/utils/UtilsRos.h
+  include/kimera_vio_ros/utils/CsvPublisher.h
   src/KimeraVioRos.cpp
   src/RosDataProviderInterface.cpp
   src/RosBagDataProvider.cpp
@@ -32,6 +34,7 @@ cs_add_library(${PROJECT_NAME}
   src/RosVisualizer.cpp
   src/RosLoopClosureVisualizer.cpp
   src/utils/UtilsRos.cpp
+  src/utils/CsvPublisher.cpp
 )
 target_link_libraries(${PROJECT_NAME} kimera_vio)
 
@@ -40,6 +43,11 @@ cs_add_executable(${PROJECT_NAME}_node
 )
 target_link_libraries(${PROJECT_NAME}_node ${PROJECT_NAME})
 
+cs_add_executable(csv_publisher_node
+  src/utils/CsvPublisherNode.cpp
+)
+target_link_libraries(csv_publisher_node ${PROJECT_NAME})
+
 catkin_add_gtest(testKimeraVioRos
   test/testKimeraVioRos.cpp
 )

include/kimera_vio_ros/utils/CsvPublisher.h

@@ -0,0 +1,97 @@
+/**
+ * @file   CsvPublisher.h
+ * @brief  Publishes a 3D trajectory provided as a CSV file (Euroc format)
+ * at the same pace than a given odometry topic, and aligns the CSV trajectory
+ * with the odometry.
+ * This is nice to visualize a ground-truth trajectory displayed at the same
+ * time that we get
+ * VIO's results so that we can compare both trajectories visually.
+ * @author Antoni Rosinol
+ */
+
+#pragma once
+
+#include <geometry_msgs/PoseStamped.h>
+#include <nav_msgs/Odometry.h>
+#include <nav_msgs/Path.h>
+#include <ros/ros.h>
+#include <tf/transform_broadcaster.h>
+
+#include <Eigen/Core>
+
+#include <gtsam/geometry/Pose3.h>
+#include <gtsam/geometry/Point3.h>
+
+#include <kimera-vio/utils/Macros.h>
+#include <kimera-vio/common/vio_types.h>
+#include <kimera-vio/common/VioNavState.h>
+#include <kimera-vio/pipeline/Pipeline-definitions.h>
+#include <kimera-vio/dataprovider/EurocDataProvider.h>
+
+namespace VIO {
+
+class EurocDataProvider;
+
+namespace utils {
+
+class CsvPublisher {
+ public:
+  KIMERA_POINTER_TYPEDEFS(CsvPublisher);
+  KIMERA_DELETE_COPY_CONSTRUCTORS(CsvPublisher);
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  using MapIterator = std::map<Timestamp, VioNavState>::iterator;
+
+ public:
+  /**
+  * @brief CsvPublisher
+  * The path to the csv dataset published is given by the dataset_path gflag.
+  */
+  CsvPublisher();
+  virtual ~CsvPublisher() = default;
+
+ protected:
+  void callbackOdometry(const nav_msgs::OdometryConstPtr& odom_msg);
+
+  void fillOdometryMsg(const Timestamp& timestamp,
+                       const gtsam::Pose3& pose,
+                       const gtsam::Vector3& velocity,
+                       nav_msgs::Odometry* odometry) const;
+
+ protected:
+  ros::NodeHandle nh_;
+  ros::NodeHandle nh_private_;
+
+  ros::Publisher aligned_gt_odometry_pub_;
+  ros::Publisher aligned_gt_path_pub_;
+  ros::Publisher gt_path_pub_;
+  ros::Publisher vio_path_pub_;
+
+  ros::Subscriber odometry_sub_;
+
+  nav_msgs::Path aligned_gt_trajectory_path_;
+  nav_msgs::Path gt_trajectory_path_;
+  nav_msgs::Path vio_trajectory_path_;
+
+  /// This label is prepended to the published topics and used as
+  /// the child frame id for the published odometry.
+  std::string trajectory_label_ = "aligned_gt";
+  std::string world_frame_id_ = "world";
+
+  VioParams vio_params_;
+  EurocDataProvider::UniquePtr euroc_data_provider_;
+
+  MapIterator csv_odometry_map_it_;
+
+  Eigen::Matrix3Xd src_;
+  Eigen::Matrix3Xd dst_;
+  std::shared_ptr<gtsam::Pose3> T_viow_gtw_ = nullptr;
+
+  // Align trajectories
+  static constexpr int n_alignment_frames_ = 30;
+  static constexpr int n_discard_frames_ = 20;
+  int odometry_msgs_count_ = 0;
+};
+
+}  // namespace utils
+
+}  // namespace VIO

include/kimera_vio_ros/utils/UtilsRos.h

@@ -19,25 +19,27 @@
 
 #include <gtsam/geometry/Pose3.h>
 
-#include <kimera-vio/frontend/CameraParams.h>
 #include <kimera-vio/common/VioNavState.h>
+#include <kimera-vio/frontend/CameraParams.h>
 
 namespace VIO {
 
 namespace utils {
 
-void msgTFtoPose(const geometry_msgs::Transform& tf, gtsam::Pose3* pose);
+void rosTfToGtsamPose(const geometry_msgs::Transform& tf, gtsam::Pose3* pose);
+
+void gtsamPoseToRosTf(const gtsam::Pose3& pose, geometry_msgs::Transform* tf);
 
-void poseToMsgTF(const gtsam::Pose3& pose, geometry_msgs::Transform* tf);
+void rosOdometryToGtsamPose(const nav_msgs::Odometry& odom, gtsam::Pose3* pose);
 
 void msgCamInfoToCameraParams(const sensor_msgs::CameraInfoConstPtr& cam_info,
                               const std::string& base_link_frame_id,
                               const std::string& cam_frame_id,
                               CameraParams* cam_params);
 
-void msgGtOdomToVioNavState(const nav_msgs::Odometry& gt_odom,
-                            const ros::NodeHandle& node_handle,
-                            VioNavState* vio_navstate);
+void rosOdometryToVioNavState(const nav_msgs::Odometry& gt_odom,
+                              const ros::NodeHandle& node_handle,
+                              VioNavState* vio_navstate);
 
 }  // namespace utils
 

launch/gt_publisher.launch

@@ -0,0 +1,17 @@
+<launch>
+  <arg name="dataset_path"  default="/home/tonirv/datasets/EuRoC/MH_05_difficult"/>
+  <arg name="input_odometry" default="/kimera_vio_ros/odometry"/>
+
+  <!-- Assumes we have the same data directory than Euroc, since we use EurocDataProvider-->
+  <!-- launch-prefix="gdb -ex run -args"-->
+  <node name="ground_truth_publisher" pkg="kimera_vio_ros" type="csv_publisher_node" output="screen"
+    args="--logtostderr=1
+          --colorlogtostderr=1
+          --dataset_path=$(arg dataset_path)
+          --log_prefix=1">
+    <remap from="input_odometry" to="$(arg input_odometry)"/>
+    <!-- Appends this string to the topics and child frame id of the published csv data -->
+    <param name="trajectory_label" value="ground_truth"/>
+    <param name="world_frame_id"   value="world"/>
+  </node>
+</launch>

scripts/retimestamp_rosbag.py

@@ -31,26 +31,26 @@ def main(argv):
     try:
         opts, args = getopt.getopt(argv,"hi:o:",["ifile=","ofile="])
     except getopt.GetoptError:
-        print 'usage: restamp_bag.py -i <inputfile> -o <outputfile>'
+        print('usage: restamp_bag.py -i <inputfile> -o <outputfile>')
         sys.exit(2)
     for opt, arg in opts:
         if opt == '-h':
-            print 'usage: python restamp_bag.py -i <inputfile> -o <outputfile>'
+            print('usage: python restamp_bag.py -i <inputfile> -o <outputfile>')
             sys.exit()
         elif opt in ("-i", "--ifile"):
             inputfile = arg
         elif opt in ("-o", "--ofile"):
             outputfile = arg
 
     # print console header
-    print ""
-    print "restamp_bag"
-    print ""
-    print 'input file:  ', inputfile
-    print 'output file: ', outputfile
-    print ""
-    print "starting restamping (may take a while)"
-    print ""
+    print("")
+    print("restamp_bag")
+    print("")
+    print('input file:  ', inputfile)
+    print('output file: ', outputfile)
+    print("")
+    print("starting restamping (may take a while)")
+    print("")
 
     outbag = rosbag.Bag(outputfile, 'w')
     messageCounter = 0
@@ -70,7 +70,7 @@ def main(argv):
                     # Write message in output bag with input message header stamp
                     outbag.write(topic, msg, msg.header.stamp)
                 except:
-                    print "a message has no header here. Coming from topic: ", topic
+                    print("a message has no header here. Coming from topic: ", topic)
 
             if (messageCounter % kPrintDotReductionFactor) == 0:
                     #print '.',
@@ -80,11 +80,11 @@ def main(argv):
 
     # print console footer
     finally:
-        print ""
-        print ""
-        print "finished iterating through input bag"
-        print "output bag written"
-        print ""
+        print("")
+        print("")
+        print("finished iterating through input bag")
+        print("output bag written")
+        print("")
         outbag.close()
 
 if __name__ == "__main__":

src/RosBagDataProvider.cpp

@@ -359,7 +359,7 @@ VioNavState RosbagDataProvider::getGroundTruthVioNavState(
   CHECK_LT(k_frame, rosbag_data_.gt_odometry_.size());
   nav_msgs::Odometry gt_odometry = *(rosbag_data_.gt_odometry_.at(k_frame));
   VioNavState vio_nav_state;
-  utils::msgGtOdomToVioNavState(gt_odometry, nh_private_, &vio_nav_state);
+  utils::rosOdometryToVioNavState(gt_odometry, nh_private_, &vio_nav_state);
   return vio_nav_state;
 }
 

src/RosLoopClosureVisualizer.cpp

@@ -317,7 +317,7 @@ void RosLoopClosureVisualizer::publishTf(
   map_tf.header.stamp.fromNSec(ts);
   map_tf.header.frame_id = world_frame_id_;
   map_tf.child_frame_id = map_frame_id_;
-  utils::poseToMsgTF(w_Pose_map, &map_tf.transform);
+  utils::gtsamPoseToRosTf(w_Pose_map, &map_tf.transform);
   tf_broadcaster_.sendTransform(map_tf);
 }
 

src/RosOnlineDataProvider.cpp

@@ -363,7 +363,7 @@ void RosOnlineDataProvider::callbackGtOdomOnce(
     const nav_msgs::Odometry::ConstPtr& gt_odom_msg) {
   LOG(WARNING) << "Using initial ground-truth state for initialization.";
   CHECK(gt_odom_msg);
-  utils::msgGtOdomToVioNavState(
+  utils::rosOdometryToVioNavState(
       *gt_odom_msg,
       nh_private_,
       &vio_params_.backend_params_->initial_ground_truth_state_);
@@ -413,7 +413,7 @@ void RosOnlineDataProvider::publishStaticTf(const gtsam::Pose3& pose,
   static_transform_stamped.header.stamp = ros::Time::now();
   static_transform_stamped.header.frame_id = parent_frame_id;
   static_transform_stamped.child_frame_id = child_frame_id;
-  utils::poseToMsgTF(pose, &static_transform_stamped.transform);
+  utils::gtsamPoseToRosTf(pose, &static_transform_stamped.transform);
   static_broadcaster.sendTransform(static_transform_stamped);
 }
 

src/RosVisualizer.cpp

@@ -512,7 +512,7 @@ void RosVisualizer::publishTf(const BackendOutput::ConstPtr& output) {
   odom_tf.header.frame_id = world_frame_id_;
   odom_tf.child_frame_id = base_link_frame_id_;
 
-  utils::poseToMsgTF(pose, &odom_tf.transform);
+  utils::gtsamPoseToRosTf(pose, &odom_tf.transform);
   tf_broadcaster_.sendTransform(odom_tf);
 }