Merge branch 'master' of github.mit.edu:SPARK/Kimera-VIO-ROS into fea…

…ture/mono_slam/base

.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`

.gitignore

@@ -1,3 +1,6 @@
+# iOS things
+.DS_Store
+
 # Qt things
 *.user*
 
@@ -35,4 +38,4 @@
 *.app
 
 # Vscode files
-.vscode/
+.vscode/

CMakeLists.txt

@@ -24,13 +24,17 @@ 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
   src/RosOnlineDataProvider.cpp
   src/RosDisplay.cpp
   src/RosVisualizer.cpp
+  src/RosLoopClosureVisualizer.cpp
   src/utils/UtilsRos.cpp
+  src/utils/CsvPublisher.cpp
 )
 target_link_libraries(${PROJECT_NAME} kimera_vio)
 
@@ -39,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

@@ -0,0 +1,92 @@
+/**
+ * @file   RosLoopClosureVisualizer.h
+ * @brief  Publishes Loop closure and pose graph data to ROS.
+ * @author Yun Chang
+ */
+
+#pragma once
+
+#include <string>
+#include <vector>
+
+#define PCL_NO_PRECOMPILE  // Define this before you include any PCL headers
+                           // to include the templated algorithms
+#include <pcl/point_types.h>
+#include <pcl_ros/point_cloud.h>
+
+#include <opencv2/opencv.hpp>
+
+#include <glog/logging.h>
+
+#include <ros/ros.h>
+#include <tf/transform_broadcaster.h>
+
+#include <pose_graph_tools/PoseGraph.h>
+#include <pose_graph_tools/PoseGraphEdge.h>
+#include <pose_graph_tools/PoseGraphNode.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/RosPublishers.h"
+
+namespace VIO {
+
+class RosLoopClosureVisualizer {
+ public:
+  KIMERA_POINTER_TYPEDEFS(RosLoopClosureVisualizer);
+  KIMERA_DELETE_COPY_CONSTRUCTORS(RosLoopClosureVisualizer);
+
+ public:
+  RosLoopClosureVisualizer();
+  ~RosLoopClosureVisualizer() = default;
+
+  void publishLcdOutput(const LcdOutput::ConstPtr& lcd_output);
+
+ private:
+  void publishTf(const LcdOutput::ConstPtr& lcd_output);
+
+  void publishOptimizedTrajectory(const LcdOutput::ConstPtr& lcd_output);
+
+  void publishPoseGraph(const LcdOutput::ConstPtr& lcd_output);
+
+  void updateNodesAndEdges(const gtsam::NonlinearFactorGraph& nfg,
+                           const gtsam::Values& values);
+
+  void updateRejectedEdges();
+
+  void publishNewNodesAndEdges(const LcdOutput::ConstPtr& lcd_output);
+
+  pose_graph_tools::PoseGraph getPosegraphMsg();
+
+ private:
+  // ROS handles
+  ros::NodeHandle nh_;
+  ros::NodeHandle nh_private_;
+
+  // ROS publishers
+  ros::Publisher trajectory_pub_;
+  ros::Publisher posegraph_pub_;
+  ros::Publisher odometry_pub_;
+  ros::Publisher posegraph_incremental_pub_;
+
+  //! Define tf broadcaster for world to base_link (IMU) and to map (PGO).
+  tf::TransformBroadcaster tf_broadcaster_;
+
+  //! Stored pose graph related objects
+  std::vector<pose_graph_tools::PoseGraphEdge> loop_closure_edges_;
+  std::vector<pose_graph_tools::PoseGraphEdge> odometry_edges_;
+  std::vector<pose_graph_tools::PoseGraphEdge> inlier_edges_;
+  std::vector<pose_graph_tools::PoseGraphNode> pose_graph_nodes_;
+
+ private:
+  //! Define frame ids for odometry message
+  std::string world_frame_id_;
+  std::string base_link_frame_id_;
+  std::string map_frame_id_;
+};
+
+}  // namespace VIO

include/kimera_vio_ros/RosVisualizer.h

@@ -31,6 +31,7 @@
 #include <kimera-vio/mesh/Mesher-definitions.h>
 #include <kimera-vio/visualizer/Visualizer3D.h>
 
+#include "kimera_vio_ros/RosLoopClosureVisualizer.h"
 #include "kimera_vio_ros/RosPublishers.h"
 
 namespace VIO {
@@ -72,12 +73,9 @@ class RosVisualizer : public Visualizer3D {
 
   virtual void publishMesherOutput(const MesherOutput::ConstPtr& output) const;
 
-  // Publish all outputs for LCD
-  // TODO(marcus): make like other outputs
-  virtual void publishLcdOutput(const LcdOutput::ConstPtr& lcd_output);
-
  private:
-  void publishTimeHorizonPointCloud(const BackendOutput::ConstPtr& output) const;
+  void publishTimeHorizonPointCloud(
+      const BackendOutput::ConstPtr& output) const;
 
   void publishPerFrameMesh3D(const MesherOutput::ConstPtr& output) const;
 
@@ -93,18 +91,6 @@ class RosVisualizer : public Visualizer3D {
   void publishImuBias(const BackendOutput::ConstPtr& output) const;
 
   void publishTf(const BackendOutput::ConstPtr& output);
-  void publishTf(const LcdOutput::ConstPtr& lcd_output);
-
-  void publishOptimizedTrajectory(const LcdOutput::ConstPtr& lcd_output) const;
-
-  void publishPoseGraph(const LcdOutput::ConstPtr& lcd_output);
-
-  void updateNodesAndEdges(const gtsam::NonlinearFactorGraph& nfg,
-                           const gtsam::Values& values);
-
-  void updateRejectedEdges();
-
-  pose_graph_tools::PoseGraph getPosegraphMsg();
 
   void publishDebugImage(const Timestamp& timestamp,
                          const cv::Mat& debug_image) const;
@@ -122,18 +108,10 @@ class RosVisualizer : public Visualizer3D {
   ros::Publisher resiliency_pub_;
   ros::Publisher frontend_stats_pub_;
   ros::Publisher imu_bias_pub_;
-  ros::Publisher trajectory_pub_;
-  ros::Publisher posegraph_pub_;
 
   //! Define tf broadcaster for world to base_link (IMU) and to map (PGO).
   tf::TransformBroadcaster tf_broadcaster_;
 
-  //! Stored pose graph related objects
-  std::vector<pose_graph_tools::PoseGraphEdge> loop_closure_edges_;
-  std::vector<pose_graph_tools::PoseGraphEdge> odometry_edges_;
-  std::vector<pose_graph_tools::PoseGraphEdge> inlier_edges_;
-  std::vector<pose_graph_tools::PoseGraphNode> pose_graph_nodes_;
-
  private:
   //! Define frame ids for odometry message
   std::string world_frame_id_;
@@ -145,8 +123,7 @@ class RosVisualizer : public Visualizer3D {
   //! Define image publishers manager
   std::unique_ptr<ImagePublishers> image_publishers_;
 
-  //! Whether we publish lcd things or not. (TODO:(Toni) Not used, implement...)
-  bool use_lcd_;
+  RosLoopClosureVisualizer lcd_visualizer_;
 
  private:
   // Typedefs

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