Merge pull request #131 from SPARK/feature/mono_slam/base

Feature/mono slam/base
MIT-SPARK · Mar 11, 2021 · fcb588e · fcb588e
2 parents 9b17dfe + 4b9fea5
commit fcb588e
Show file tree

Hide file tree

Showing 14 changed files with 101 additions and 62 deletions.
diff --git a/include/kimera_vio_ros/RosLoopClosureVisualizer.h b/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>

diff --git a/include/kimera_vio_ros/RosOnlineDataProvider.h b/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 {
 

diff --git a/include/kimera_vio_ros/RosVisualizer.h b/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;

diff --git a/launch/gt_logger.launch b/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">

diff --git a/launch/kimera_vio_ros.launch b/launch/kimera_vio_ros.launch
@@ -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

diff --git a/launch/kimera_vio_ros_euroc.launch b/launch/kimera_vio_ros_euroc.launch
@@ -3,7 +3,7 @@
   <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 KimeraVIO will log output of all modules to
@@ -25,7 +25,7 @@
   <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 -->

diff --git a/output_logs/EurocMono/.gitignore b/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
diff --git a/rviz/kimera_vio_euroc.rviz b/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

diff --git a/src/KimeraVioRos.cpp b/src/KimeraVioRos.cpp
@@ -19,8 +19,8 @@
 #include <std_srvs/TriggerResponse.h>
 
 // Dependencies from VIO
-#include <kimera-vio/pipeline/Pipeline-definitions.h>
-#include <kimera-vio/pipeline/Pipeline.h>
+#include <kimera-vio/pipeline/MonoImuPipeline.h>
+#include <kimera-vio/pipeline/StereoImuPipeline.h>
 #include <kimera-vio/utils/Timer.h>
 
 // Dependencies from this repository
@@ -80,8 +80,24 @@ bool KimeraVioRos::runKimeraVio() {
   // Then, create Kimera-VIO from scratch.
   VLOG(1) << "Creating Kimera-VIO.";
   CHECK(ros_display_);
-  vio_pipeline_ = VIO::make_unique<VIO::Pipeline>(
-      *vio_params_, std::move(ros_visualizer_), std::move(ros_display_));
+
+  vio_pipeline_ = nullptr;
+  switch (vio_params_->frontend_type_) {
+    case VIO::FrontendType::kMonoImu: {
+      vio_pipeline_ = VIO::make_unique<VIO::MonoImuPipeline>(
+          *vio_params_, std::move(ros_visualizer_), std::move(ros_display_));
+    } break;
+    case VIO::FrontendType::kStereoImu: {
+      vio_pipeline_ = VIO::make_unique<VIO::StereoImuPipeline>(
+          *vio_params_, std::move(ros_visualizer_), std::move(ros_display_));
+    } break;
+    default: {
+      LOG(FATAL) << "Unrecognized frontend type: "
+                 << VIO::to_underlying(vio_params_->frontend_type_)
+                 << ". 0: Mono, 1: Stereo.";
+    } break;
+  }
+
   CHECK(vio_pipeline_) << "Vio pipeline construction failed.";
 
   // Finally, connect data_provider and vio_pipeline
@@ -105,11 +121,15 @@ bool KimeraVioRos::spin() {
     std::future<bool> data_provider_handle =
         std::async(std::launch::async,
                    &VIO::RosDataProviderInterface::spin,
-                   data_provider_.get());
-    std::future<bool> vio_viz_handle = std::async(
-        std::launch::async, &VIO::Pipeline::spinViz, vio_pipeline_.get());
-    std::future<bool> vio_pipeline_handle = std::async(
-        std::launch::async, &VIO::Pipeline::spin, vio_pipeline_.get());
+                   std::ref(*CHECK_NOTNULL(data_provider_.get())));
+    std::future<bool> vio_viz_handle =
+        std::async(std::launch::async,
+                   &VIO::Pipeline::spinViz,
+                   std::ref(*CHECK_NOTNULL(vio_pipeline_.get())));
+    std::future<bool> vio_pipeline_handle =
+        std::async(std::launch::async,
+                   &VIO::Pipeline::spin,
+                   std::ref(*CHECK_NOTNULL(vio_pipeline_.get())));
     // Run while ROS is ok and vio pipeline is not shutdown.
     ros::Rate rate(20);  // 20 Hz
     while (ros::ok() && !restart_vio_pipeline_) {
@@ -186,29 +206,39 @@ void KimeraVioRos::connectVIO() {
   // shutsdown.
   CHECK(data_provider_);
   CHECK(vio_pipeline_);
-  vio_pipeline_->registerShutdownCallback(std::bind(
-      &VIO::DataProviderInterface::shutdown, std::ref(*data_provider_)));
+  vio_pipeline_->registerShutdownCallback(
+      std::bind(&VIO::DataProviderInterface::shutdown,
+                std::ref(*CHECK_NOTNULL(data_provider_.get()))));
 
   // Register Data Provider callbacks
   data_provider_->registerImuSingleCallback(
       std::bind(&VIO::Pipeline::fillSingleImuQueue,
-                std::ref(*vio_pipeline_),
+                std::ref(*CHECK_NOTNULL(vio_pipeline_.get())),
                 std::placeholders::_1));
 
   data_provider_->registerImuMultiCallback(
       std::bind(&VIO::Pipeline::fillMultiImuQueue,
-                std::ref(*vio_pipeline_),
+                std::ref(*CHECK_NOTNULL(vio_pipeline_.get())),
                 std::placeholders::_1));
 
   data_provider_->registerLeftFrameCallback(
       std::bind(&VIO::Pipeline::fillLeftFrameQueue,
-                std::ref(*vio_pipeline_),
+                std::ref(*CHECK_NOTNULL(vio_pipeline_.get())),
                 std::placeholders::_1));
 
-  data_provider_->registerRightFrameCallback(
-      std::bind(&VIO::Pipeline::fillRightFrameQueue,
-                std::ref(*vio_pipeline_),
-                std::placeholders::_1));
+  if (vio_params_->frontend_type_ == VIO::FrontendType::kStereoImu) {
+    VIO::StereoImuPipeline::UniquePtr stereo_pipeline =
+        VIO::safeCast<VIO::Pipeline, VIO::StereoImuPipeline>(
+            std::move(vio_pipeline_));
+
+    data_provider_->registerRightFrameCallback(
+        std::bind(&VIO::StereoImuPipeline::fillRightFrameQueue,
+                  std::ref(*CHECK_NOTNULL(stereo_pipeline.get())),
+                  std::placeholders::_1));
+
+    vio_pipeline_ = VIO::safeCast<VIO::StereoImuPipeline, VIO::Pipeline>(
+        std::move(stereo_pipeline));
+  }
 }
 
 bool KimeraVioRos::restartKimeraVio(std_srvs::Trigger::Request& request,

diff --git a/src/RosBagDataProvider.cpp b/src/RosBagDataProvider.cpp
@@ -136,13 +136,15 @@ bool RosbagDataProvider::spin() {
             readRosImage(rosbag_data_.left_imgs_.at(k_))));
 
         // Send right frame data to Kimera:
-        CHECK(right_frame_callback_)
-            << "Did you forget to register the right frame callback?";
-        right_frame_callback_(VIO::make_unique<Frame>(
-            k_,
-            timestamp_frame_k,
-            right_cam_info,
-            readRosImage(rosbag_data_.right_imgs_.at(k_))));
+        if (vio_params_.frontend_type_ == VIO::FrontendType::kStereoImu) {
+          CHECK(right_frame_callback_)
+              << "Did you forget to register the right frame callback?";
+          right_frame_callback_(VIO::make_unique<Frame>(
+              k_,
+              timestamp_frame_k,
+              right_cam_info,
+              readRosImage(rosbag_data_.right_imgs_.at(k_))));
+        }
 
         VLOG(10) << "Sent left/right images to VIO for frame k = " << k_;
 

diff --git a/src/RosDisplay.cpp b/src/RosDisplay.cpp
@@ -22,7 +22,7 @@
 namespace VIO {
 
 RosDisplay::RosDisplay()
-    : DisplayBase(), nh_private_("~"), image_publishers_(nullptr) {
+    : DisplayBase(VIO::DisplayType::kOpenCV), nh_private_("~"), image_publishers_(nullptr) {
   image_publishers_ = VIO::make_unique<ImagePublishers>(nh_private_);
 }
 

diff --git a/src/RosOnlineDataProvider.cpp b/src/RosOnlineDataProvider.cpp
@@ -285,18 +285,20 @@ void RosOnlineDataProvider::callbackStereoImages(
   const Timestamp& timestamp_left = left_msg->header.stamp.toNSec();
   const Timestamp& timestamp_right = right_msg->header.stamp.toNSec();
 
-  CHECK(left_frame_callback_)
-      << "Did you forget to register the left frame callback?";
-  CHECK(right_frame_callback_)
-      << "Did you forget to register the right frame callback?";
-
   if (!shutdown_) {
+    CHECK(left_frame_callback_)
+        << "Did you forget to register the left frame callback?";
     left_frame_callback_(VIO::make_unique<Frame>(
         frame_count_, timestamp_left, left_cam_info, readRosImage(left_msg)));
-    right_frame_callback_(VIO::make_unique<Frame>(frame_count_,
-                                                  timestamp_right,
-                                                  right_cam_info,
-                                                  readRosImage(right_msg)));
+
+    if (vio_params_.frontend_type_ == VIO::FrontendType::kStereoImu) {
+      CHECK(right_frame_callback_)
+          << "Did you forget to register the right frame callback?";
+      right_frame_callback_(VIO::make_unique<Frame>(frame_count_,
+                                                    timestamp_right,
+                                                    right_cam_info,
+                                                    readRosImage(right_msg)));
+    }
     frame_count_++;
   }
 }

diff --git a/src/RosVisualizer.cpp b/src/RosVisualizer.cpp
@@ -24,9 +24,10 @@
 #include <tf/transform_broadcaster.h>
 #include <tf2/buffer_core.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/mesh/Mesh.h>
 #include <kimera-vio/mesh/Mesher-definitions.h>
 #include <kimera-vio/pipeline/QueueSynchronizer.h>
 #include <kimera-vio/visualizer/Visualizer3D.h>
@@ -37,7 +38,9 @@ namespace VIO {
 
 RosVisualizer::RosVisualizer(const VioParams& vio_params)
     // I'm not sure we use this flag in ROS?
-    : Visualizer3D(VisualizationType::kMesh2dTo3dSparse),
+    : Visualizer3D(vio_params.frontend_type_ == FrontendType::kMonoImu
+                   ? VisualizationType::kNone
+                   : VisualizationType::kMesh2dTo3dSparse),
       nh_(),
       nh_private_("~"),
       image_size_(vio_params.camera_params_.at(0).image_size_),
@@ -91,7 +94,7 @@ void RosVisualizer::publishBackendOutput(
 }
 
 void RosVisualizer::publishFrontendOutput(
-    const FrontendOutput::ConstPtr& output) const {
+    const FrontendOutputPacketBase::ConstPtr& output) const {
   CHECK(output);
   if (frontend_stats_pub_.getNumSubscribers() > 0) {
     publishFrontendStats(output);
@@ -219,7 +222,7 @@ void RosVisualizer::publishPerFrameMesh3D(
     // Returns indices of points in the 3D mesh corresponding to the
     // vertices
     // in the 2D mesh.
-    int p0_id, p1_id, p2_id;
+    Mesh3D::VertexId p0_id, p1_id, p2_id;
     Mesh3D::VertexType vtx0, vtx1, vtx2;
     if (mesh_3d.getVertex(lmk0_id, &vtx0, &p0_id) &&
         mesh_3d.getVertex(lmk1_id, &vtx1, &p1_id) &&
@@ -364,7 +367,7 @@ void RosVisualizer::publishState(const BackendOutput::ConstPtr& output) const {
 }
 
 void RosVisualizer::publishFrontendStats(
-    const FrontendOutput::ConstPtr& output) const {
+    const FrontendOutputPacketBase::ConstPtr& output) const {
   CHECK(output);
 
   // Get frontend data for resiliency output
@@ -392,15 +395,15 @@ void RosVisualizer::publishFrontendStats(
   frontend_stats_msg.layout.dim[0].size = frontend_stats_msg.data.size();
   frontend_stats_msg.layout.dim[0].stride = 1;
   frontend_stats_msg.layout.dim[0].label =
-      "FrontEnd: nrDetFeat, nrTrackFeat, nrMoIn, nrMoPu, nrStIn, nrStPu, "
+      "Frontend: nrDetFeat, nrTrackFeat, nrMoIn, nrMoPu, nrStIn, nrStPu, "
       "moRaIt, stRaIt, nrVaRKP, nrNoLRKP, nrNoRRKP, nrNoDRKP nrFaARKP";
 
   // Publish Message
   frontend_stats_pub_.publish(frontend_stats_msg);
 }
 
 void RosVisualizer::publishResiliency(
-    const FrontendOutput::ConstPtr& frontend_output,
+    const FrontendOutputPacketBase::ConstPtr& frontend_output,
     const BackendOutput::ConstPtr& backend_output) const {
   CHECK(frontend_output);
   CHECK(backend_output);

diff --git a/src/utils/UtilsRos.cpp b/src/utils/UtilsRos.cpp
@@ -88,10 +88,6 @@ void msgCamInfoToCameraParams(const sensor_msgs::CameraInfoConstPtr& cam_info,
   VIO::CameraParams::convertIntrinsicsVectorToMatrix(cam_params->intrinsics_,
                                                      &cam_params->K_);
 
-  VIO::CameraParams::createGtsamCalibration(cam_params->distortion_coeff_,
-                                            cam_params->intrinsics_,
-                                            &cam_params->calibration_);
-
   // Get extrinsics from the TF tree:
   tf2_ros::Buffer t_buffer;
   static constexpr size_t kTfLookupTimeout = 5u;