Merge pull request #135 from MIT-SPARK/update/master

Update/master

.github/ISSUE_TEMPLATE/error-report.md

@@ -23,7 +23,7 @@ about: Fill this out and upload your data (!!)
 Please attach all the files needed to reproduce the error.
 
 Please give also the following information:
-* SparkVio branch, tag or commit used
+* KimeraVIO branch, tag or commit used
 * GTSAM version used:
 * OpenGV version used:
 * OpenCV version used: type `opencv_version`

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
 )

README.md

@@ -59,9 +59,10 @@ Install [Intel Threaded Building Blocks (TBB)](http://www.threadingbuildingblock
 mkdir -p ~/catkin_ws/src
 cd ~/catkin_ws/
 catkin init
-catkin config --cmake-args -DCMAKE_BUILD_TYPE=Release
+catkin config --cmake-args -DCMAKE_BUILD_TYPE=Release -DGTSAM_TANGENT_PREINTEGRATION=OFF
 # On Ubuntu 16.04:
-# catkin config --cmake-args -DCMAKE_BUILD_TYPE=Release -DGTSAM_USE_SYSTEM_EIGEN=ON
+# catkin config --cmake-args -DCMAKE_BUILD_TYPE=Release -DGTSAM_USE_SYSTEM_EIGEN=ON -DGTSAM_TANGENT_PREINTEGRATION=OFF
+
 catkin config --merge-devel
 
 # Add workspace to bashrc for automatic sourcing of workspace.
@@ -82,8 +83,12 @@ wstool merge Kimera-VIO-ROS/install/kimera_vio_ros_ssh.rosinstall
 # For https
 # wstool merge Kimera-VIO-ROS/install/kimera_vio_ros_https.rosinstall
 
-# Finally, download and update repos:
+# download and update repos:
 wstool update
+
+# Optionally install all dependencies that you might have missed:
+# Some packages may report errors, this is expected
+# rosdep install --from-paths . --ignore-src -r -y
 ```
 
 Finally, compile:

include/kimera_vio_ros/RosLoopClosureVisualizer.h

@@ -25,8 +25,8 @@
 #include <pose_graph_tools/PoseGraphEdge.h>
 #include <pose_graph_tools/PoseGraphNode.h>
 
-#include <kimera-vio/backend/VioBackEnd-definitions.h>
-#include <kimera-vio/frontend/StereoVisionFrontEnd-definitions.h>
+#include <kimera-vio/backend/VioBackend-definitions.h>
+#include <kimera-vio/frontend/StereoVisionImuFrontend-definitions.h>
 #include <kimera-vio/loopclosure/LoopClosureDetector-definitions.h>
 #include <kimera-vio/loopclosure/LoopClosureDetector.h>
 #include <kimera-vio/mesh/Mesher-definitions.h>

include/kimera_vio_ros/RosOnlineDataProvider.h

@@ -17,6 +17,7 @@
 #include <std_msgs/Bool.h>
 
 #include "kimera_vio_ros/RosDataProviderInterface.h"
+#include "kimera-vio/frontend/StereoImuSyncPacket.h"
 
 namespace VIO {
 

include/kimera_vio_ros/RosVisualizer.h

@@ -25,8 +25,8 @@
 #include <pose_graph_tools/PoseGraphEdge.h>
 #include <pose_graph_tools/PoseGraphNode.h>
 
-#include <kimera-vio/backend/VioBackEnd-definitions.h>
-#include <kimera-vio/frontend/StereoVisionFrontEnd-definitions.h>
+#include <kimera-vio/backend/VioBackend-definitions.h>
+#include <kimera-vio/frontend/FrontendOutputPacketBase.h>
 #include <kimera-vio/loopclosure/LoopClosureDetector-definitions.h>
 #include <kimera-vio/mesh/Mesher-definitions.h>
 #include <kimera-vio/visualizer/Visualizer3D.h>
@@ -69,8 +69,7 @@ class RosVisualizer : public Visualizer3D {
   // Publish VIO outputs.
   virtual void publishBackendOutput(const BackendOutput::ConstPtr& output);
 
-  virtual void publishFrontendOutput(
-      const FrontendOutput::ConstPtr& output) const;
+  virtual void publishFrontendOutput(const FrontendOutputPacketBase::ConstPtr& output) const;
 
   virtual void publishMesherOutput(const MesherOutput::ConstPtr& output) const;
 
@@ -84,9 +83,9 @@ class RosVisualizer : public Visualizer3D {
 
   void publishState(const BackendOutput::ConstPtr& output) const;
 
-  void publishFrontendStats(const FrontendOutput::ConstPtr& output) const;
+  void publishFrontendStats(const FrontendOutputPacketBase::ConstPtr& output) const;
 
-  void publishResiliency(const FrontendOutput::ConstPtr& frontend_output,
+  void publishResiliency(const FrontendOutputPacketBase::ConstPtr& frontend_output,
                          const BackendOutput::ConstPtr& backend_output) const;
 
   void publishImuBias(const BackendOutput::ConstPtr& output) const;

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,24 +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,
-                            VioNavState* vio_navstate);
+void rosOdometryToVioNavState(const nav_msgs::Odometry& gt_odom,
+                              const ros::NodeHandle& node_handle,
+                              VioNavState* vio_navstate);
 
 }  // namespace utils
 

install/kimera_vio_ros_https.rosinstall

@@ -13,8 +13,7 @@
 - git:
     local-name: gtsam
     uri: https://github.com/borglab/gtsam.git
-    version: master
-    # Last tested commit: 342f30d148fae84c92ff71705c9e50e0a3683bda
+    version: develop
 - git:
     local-name: opencv3_catkin
     uri: https://github.com/ethz-asl/opencv3_catkin.git

install/kimera_vio_ros_ssh.rosinstall

@@ -13,7 +13,7 @@
 - git:
     local-name: gtsam
     uri: [email protected]:borglab/gtsam.git
-    version: master
+    version: develop
 - git:
     local-name: opencv3_catkin
     uri: [email protected]:ethz-asl/opencv3_catkin.git

launch/gt_logger.launch

@@ -1,6 +1,6 @@
 <launch>
   <arg name="gt_topic" default="/tesse/odom"/>
-  <arg name="output_dir" default="$(find kimera_vio_ros)/output_logs/Tesse/"/>
+  <arg name="output_dir" default="$(find kimera_vio_ros)/output_logs/"/>
 
   <node name="gt_logger_node" pkg="kimera_vio_ros" type="gt_logger_node.py"
         output="screen">

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>

launch/kimera_vio_ros.launch

@@ -13,7 +13,7 @@
   <!-- Use CameraInfo topics to get parameters, instead of yaml files -->
   <arg name="use_online_cam_params" default="false"/>
 
-  <!-- If true, SparkVio will log output of all modules to the
+  <!-- If true, KimeraVIO will log output of all modules to the
        'log_output_path' location. -->
   <arg name="log_output" default="false"/>
   <arg name="log_output_path"
@@ -24,7 +24,7 @@
   <arg name="log_gt_data" default="false"/>
   <arg name="gt_topic"    default="ground_truth_odometry_topic"/>
 
-  <!-- If true, SparkVio will enable the LoopClosureDetector module. -->
+  <!-- If true, KimeraVIO will enable the LoopClosureDetector module. -->
   <arg name="use_lcd" default="false"/>
 
   <!-- Parameters -->
@@ -70,8 +70,8 @@
         args="--use_lcd=$(arg use_lcd)
               --vocabulary_path=$(arg path_to_vocab)
               --flagfile=$(arg params_folder)/flags/Mesher.flags
-              --flagfile=$(arg params_folder)/flags/VioBackEnd.flags
-              --flagfile=$(arg params_folder)/flags/RegularVioBackEnd.flags
+              --flagfile=$(arg params_folder)/flags/VioBackend.flags
+              --flagfile=$(arg params_folder)/flags/RegularVioBackend.flags
               --flagfile=$(arg params_folder)/flags/Visualizer3D.flags
               --logtostderr=1
               --colorlogtostderr=1

launch/kimera_vio_ros_euroc.launch

@@ -3,10 +3,10 @@
   <arg name="verbosity"    default="0" />
 
   <!-- Set online to true if you use rosbag play or sensor stream -->
-  <arg name="online" default="true" />
+  <arg name="online" default="false" />
   <!-- Set use_sim_time to true if you use rosbag with clock argument -->
   <param name="use_sim_time" value="true"/>
-  <!-- Set log_output to true and SparkVio will log output of all modules to
+  <!-- Set log_output to true and KimeraVIO will log output of all modules to
        the log_output_path location. -->
   <arg name="log_output" default="false"/>
   <arg name="log_output_path"
@@ -16,13 +16,16 @@
   <arg name="log_gt_data" default="false"/>
   <arg name="gt_topic" default="/vicon/firefly_sbx/firefly_sbx"/>
 
+  <rosparam param="gt_gyro_bias"> [-0.002229, 0.0207, 0.07635] </rosparam>
+  <rosparam param="gt_accel_bias"> [-0.012492, 0.547666, 0.069073] </rosparam>
+
   <arg name="use_lcd"   default="false"/>
   <arg name="visualize" default="true"/>
 
   <arg name="use_online_cam_params" value="false"/>
 
   <!-- Change rosbag path if online argument is false -->
-  <arg name="rosbag_path" default="/home/tonirv/datasets/EuRoC/V1_01_easy/V1_01_easy_retimestamped.bag"
+  <arg name="rosbag_path" default=""
     unless="$(arg online)"/>
 
   <!-- Frame IDs -->

output_logs/EurocMono/.gitignore

@@ -0,0 +1,5 @@
+# Ignore all pipeline results, but not this folder
+# We will store all output logs in this folder
+*
+*/
+!.gitignore

rviz/kimera_vio_euroc.rviz

@@ -5,8 +5,6 @@ Panels:
     Property Tree Widget:
       Expanded:
         - /Global Options1
-        - /PointCloud Time Horizon1
-        - /PoseGraph1
       Splitter Ratio: 0.5
     Tree Height: 493
   - Class: rviz/Selection
@@ -46,6 +44,7 @@ Visualization Manager:
           Value: true
         world:
           Value: true
+      Marker Alpha: 1
       Marker Scale: 1
       Name: TF
       Show Arrows: true
@@ -253,6 +252,7 @@ Visualization Manager:
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
+      Field of View: 0.7853981852531433
       Focal Point:
         X: 0.217152834
         Y: 0.697479725
@@ -274,6 +274,7 @@ Visualization Manager:
           Stereo Focal Distance: 1
           Swap Stereo Eyes: false
           Value: false
+        Field of View: 0.7853981852531433
         Focal Point:
           X: 0
           Y: 0