Refactor loggers to allow for different output paths

Dockerfile_18_04

@@ -93,10 +93,10 @@ RUN cd evo-1 && python3.6 -m pip install .
 # Install spark_vio_evaluation
 RUN python3.6 -m pip install ipython prompt_toolkit
 # Hack to avoid Docker's cache when spark_vio_evaluation master branch is updated.
-ADD https://api.github.com/repos/ToniRV/Kimera-VIO-Evaluation/git/refs/heads/master version.json
+ADD https://api.github.com/repos/ToniRV/Kimera-VIO-Evaluation/git/refs/heads/feature/dynamic_log_output_path version.json
 RUN git clone https://github.com/ToniRV/Kimera-VIO-Evaluation.git
 # We use `pip install -e .` so that Jinja2 has access to the webiste template...
-RUN cd Kimera-VIO-Evaluation && python3.6 -m pip install -e .
+RUN cd Kimera-VIO-Evaluation && git checkout feature/dynamic_log_output_path && python3.6 -m pip install -e .
 
 # Install glog, gflags
 RUN apt-get update && apt-get install -y libgflags2.2 libgflags-dev libgoogle-glog0v5 libgoogle-glog-dev

examples/KimeraVIO.cpp

@@ -28,8 +28,10 @@
 #include "kimera-vio/utils/Statistics.h"
 #include "kimera-vio/utils/Timer.h"
 
-DEFINE_int32(dataset_type, 0, "Type of parser to use:\n "
-                              "0: Euroc \n 1: Kitti (not supported).");
+DEFINE_int32(dataset_type,
+             0,
+             "Type of parser to use:\n "
+             "0: Euroc \n 1: Kitti (not supported).");
 DEFINE_string(
     params_folder_path,
     "../params/Euroc",
@@ -86,14 +88,14 @@ int main(int argc, char* argv[]) {
   auto tic = VIO::utils::Timer::tic();
   bool is_pipeline_successful = false;
   if (vio_params.parallel_run_) {
-    auto handle = std::async(std::launch::async,
-                             &VIO::DataProviderInterface::spin,
-                             dataset_parser);
+    auto handle = std::async(
+        std::launch::async, &VIO::DataProviderInterface::spin, dataset_parser);
     auto handle_pipeline =
         std::async(std::launch::async, &VIO::Pipeline::spin, &vio_pipeline);
     auto handle_shutdown = std::async(std::launch::async,
                                       &VIO::Pipeline::shutdownWhenFinished,
-                                      &vio_pipeline, 500);
+                                      &vio_pipeline,
+                                      500);
     vio_pipeline.spinViz();
     is_pipeline_successful = !handle.get();
     handle_shutdown.get();
@@ -112,9 +114,9 @@ int main(int argc, char* argv[]) {
   LOG(INFO) << "Pipeline successful? "
             << (is_pipeline_successful ? "Yes!" : "No!");
 
-  if (is_pipeline_successful) {
+  if (is_pipeline_successful && !vio_params.log_output_path_.empty()) {
     // Log overall time of pipeline run.
-    VIO::PipelineLogger logger;
+    VIO::PipelineLogger logger (vio_params.log_output_path_);
     logger.logPipelineOverallTiming(spin_duration);
   }
 

include/kimera-vio/backend/RegularVioBackEnd-definitions.h

@@ -14,25 +14,6 @@
 
 #pragma once
 
-#include <vector>
-
-#include <boost/optional.hpp>
-
-#include <glog/logging.h>
-
-#include <gtsam/geometry/Cal3_S2.h>
-#include <gtsam/geometry/StereoPoint2.h>
-#include <gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h>
-#include <gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h>
-
-#include "kimera-vio/common/vio_types.h"
-#include "kimera-vio/frontend/StereoVisionFrontEnd-definitions.h"
-#include "kimera-vio/frontend/Tracker-definitions.h"
-#include "kimera-vio/imu-frontend/ImuFrontEnd-definitions.h"
-#include "kimera-vio/imu-frontend/ImuFrontEnd.h"
-#include "kimera-vio/utils/Macros.h"
-#include "kimera-vio/utils/UtilsOpenCV.h"
-
 namespace VIO {
 
 enum class RegularBackendModality {

include/kimera-vio/backend/RegularVioBackEnd.h

@@ -36,7 +36,7 @@ class RegularVioBackEnd : public VioBackEnd {
                     const BackendParams& backend_params,
                     const ImuParams& imu_params,
                     const BackendOutputParams& backend_output_params,
-                    const bool& log_output);
+                    const std::string& log_output_path = "");
 
   virtual ~RegularVioBackEnd() = default;
 

include/kimera-vio/backend/VioBackEnd.h

@@ -8,7 +8,11 @@
 
 /**
  * @file   VioBackEnd.h
- * @brief  Visual-Inertial Odometry pipeline, as described in these papers:
+ * @brief  Visual-Inertial backend, as described in these papers:
+ *
+ * A. Rosinol, M. Abate, Y. Chang, and L. Carlone.
+ * Kimera: an Open-Source Library for Real-Time Metric-Semantic Localization and
+ * Mapping. IEEE Intl. Conf. on Robotics and Automation (ICRA), 2020.
  *
  * C. Forster, L. Carlone, F. Dellaert, and D. Scaramuzza.
  * On-Manifold Preintegration Theory for Fast and Accurate Visual-Inertial
@@ -30,19 +34,11 @@
 #include <memory>
 #include <unordered_map>
 
-#include <boost/foreach.hpp>
-
-#include <gtsam/geometry/Cal3DS2.h>
 #include <gtsam/geometry/Cal3_S2.h>
-#include <gtsam/geometry/StereoCamera.h>
 #include <gtsam/geometry/StereoPoint2.h>
-#include <gtsam/navigation/CombinedImuFactor.h>
-#include <gtsam/navigation/ImuFactor.h>
 #include <gtsam/nonlinear/ISAM2.h>
 #include <gtsam/nonlinear/LinearContainerFactor.h>
-#include <gtsam/nonlinear/Marginals.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
-#include <gtsam/slam/BetweenFactor.h>
 #include <gtsam/slam/PriorFactor.h>
 #include <gtsam_unstable/nonlinear/BatchFixedLagSmoother.h>
 #include <gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h>
@@ -51,10 +47,9 @@
 #include "kimera-vio/backend/VioBackEndParams.h"
 #include "kimera-vio/factors/PointPlaneFactor.h"
 #include "kimera-vio/frontend/StereoVisionFrontEnd-definitions.h"
-#include "kimera-vio/imu-frontend/ImuFrontEnd.h"
+#include "kimera-vio/imu-frontend/ImuFrontEnd-definitions.h"
 #include "kimera-vio/logging/Logger.h"
 #include "kimera-vio/utils/Macros.h"
-#include "kimera-vio/utils/ThreadsafeQueue.h"
 #include "kimera-vio/utils/UtilsGTSAM.h"
 #include "kimera-vio/utils/UtilsOpenCV.h"
 
@@ -83,7 +78,7 @@ class VioBackEnd {
              const BackendParams& backend_params,
              const ImuParams& imu_params,
              const BackendOutputParams& backend_output_params,
-             bool log_output);
+             const std::string& log_output_path = "");
   virtual ~VioBackEnd() { LOG(INFO) << "Backend destructor called."; }
 
  public:
@@ -494,7 +489,6 @@ class VioBackEnd {
   size_t counter_of_exceptions_ = 0;
 
   //! Logger.
-  const bool log_output_ = {false};
   std::unique_ptr<BackendLogger> logger_;
 };
 

include/kimera-vio/backend/VioBackEndFactory.h

@@ -29,31 +29,17 @@ class BackEndFactory {
   BackEndFactory() = delete;
   virtual ~BackEndFactory() = default;
 
-  static VioBackEnd::UniquePtr createBackend(
-      const BackendType& backend_type,
-      const Pose3& B_Pose_leftCam,
-      const StereoCalibPtr& stereo_calibration,
-      const BackendParams& backend_params,
-      const ImuParams& imu_params,
-      const BackendOutputParams& backend_output_params,
-      bool log_output) {
+ public:
+  template <typename... Ts>
+  static VioBackEnd::UniquePtr createBackend(const BackendType& backend_type,
+                                             Ts&&... args) {
     switch (backend_type) {
       case BackendType::kStereoImu: {
-        return VIO::make_unique<VioBackEnd>(B_Pose_leftCam,
-                                            stereo_calibration,
-                                            backend_params,
-                                            imu_params,
-                                            backend_output_params,
-                                            log_output);
-      }
+        return VIO::make_unique<VioBackEnd>(std::forward<Ts>(args)...);
+      } break;
       case BackendType::kStructuralRegularities: {
-        return VIO::make_unique<RegularVioBackEnd>(B_Pose_leftCam,
-                                                   stereo_calibration,
-                                                   backend_params,
-                                                   imu_params,
-                                                   backend_output_params,
-                                                   log_output);
-      }
+        return VIO::make_unique<RegularVioBackEnd>(std::forward<Ts>(args)...);
+      } break;
       default: {
         LOG(FATAL) << "Requested backend type is not supported.\n"
                    << "Currently supported backend types:\n"

include/kimera-vio/dataprovider/DataProviderModule.h

@@ -26,6 +26,7 @@
 
 #include <glog/logging.h>
 
+#include "kimera-vio/imu-frontend/ImuData.h"
 #include "kimera-vio/frontend/StereoImuSyncPacket.h"
 #include "kimera-vio/frontend/StereoMatchingParams.h"
 #include "kimera-vio/pipeline/Pipeline-definitions.h"

include/kimera-vio/dataprovider/KittiDataProvider.h

@@ -18,6 +18,7 @@
 #include <functional>
 #include <string>
 
+#include "kimera-vio/imu-frontend/ImuData.h"
 #include "kimera-vio/dataprovider/DataProviderInterface.h"
 #include "kimera-vio/frontend/StereoImuSyncPacket.h"
 

include/kimera-vio/frontend/StereoVisionFrontEnd.h

@@ -58,7 +58,7 @@ class StereoVisionFrontEnd {
                        const FrontendParams& tracker_params,
                        const CameraParams& camera_params,
                        DisplayQueue* display_queue = nullptr,
-                       bool log_output = false);
+                       const std::string& log_output_path = "");
   virtual ~StereoVisionFrontEnd() {
     LOG(INFO) << "StereoVisionFrontEnd destructor called.";
   }

include/kimera-vio/frontend/VisionFrontEndFactory.h

@@ -27,14 +27,24 @@ class VisionFrontEndFactory {
   VisionFrontEndFactory() = delete;
   virtual ~VisionFrontEndFactory() = default;
 
+  template <typename... Ts>
   static StereoVisionFrontEnd::UniquePtr createFrontend(
       const FrontendType& frontend_type,
-      const ImuParams& imu_params,
-      const ImuBias& imu_initial_bias,
-      const FrontendParams& frontend_params,
-      const CameraParams& camera_params,
-      DisplayQueue* display_queue,
-      bool log_output);
+      Ts&&... args) {
+    switch (frontend_type) {
+      case FrontendType::kStereoImu: {
+        return VIO::make_unique<StereoVisionFrontEnd>(
+            std::forward<Ts>(args)...);
+      }
+      default: {
+        LOG(FATAL) << "Requested fronetnd type is not supported.\n"
+                   << "Currently supported frontend types:\n"
+                   << "0: Stereo + IMU \n"
+                   << " but requested frontend: "
+                   << static_cast<int>(frontend_type);
+      }
+    }
+  }
 };
 
 }  // namespace VIO

include/kimera-vio/imu-frontend/CMakeLists.txt

@@ -1,6 +1,7 @@
 ### Add source code for kimera_vio
 target_sources(kimera_vio PRIVATE
   "${CMAKE_CURRENT_LIST_DIR}/ImuFrontEnd-definitions.h"
+  "${CMAKE_CURRENT_LIST_DIR}/ImuData.h"
   "${CMAKE_CURRENT_LIST_DIR}/ImuFrontEnd.h"
   "${CMAKE_CURRENT_LIST_DIR}/ImuFrontEndParams.h"
 )

include/kimera-vio/imu-frontend/ImuData.h

@@ -0,0 +1,44 @@
+/* ----------------------------------------------------------------------------
+ * Copyright 2017, Massachusetts Institute of Technology,
+ * Cambridge, MA 02139
+ * All Rights Reserved
+ * Authors: Luca Carlone, et al. (see THANKS for the full author list)
+ * See LICENSE for the license information
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file   ImuData.h
+ * @brief  Container for IMU data
+ * @author Antoni Rosinol
+ */
+
+#pragma once
+
+#include "kimera-vio/utils/Macros.h"
+#include "kimera-vio/utils/ThreadsafeImuBuffer.h"
+
+namespace VIO {
+
+/**
+ * @brief The ImuData class Contains inertial measurements in a temporal buffer
+ * that is threadsafe.
+ */
+class ImuData {
+ public:
+  KIMERA_POINTER_TYPEDEFS(ImuData);
+  KIMERA_DELETE_COPY_CONSTRUCTORS(ImuData);
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  //! Imu buffer with virtually infinite memory.
+  ImuData() : imu_buffer_(-1) {}
+  ~ImuData() = default;
+
+ public:
+  void print() const;
+
+ public:
+  //! Imu data.
+  utils::ThreadsafeImuBuffer imu_buffer_;
+};
+
+}  // namespace VIO

include/kimera-vio/imu-frontend/ImuFrontEnd-definitions.h

@@ -38,6 +38,9 @@ using ImuGyr = Eigen::Matrix<double, 3, 1>;
 using ImuAccGyrS = Eigen::Matrix<double, 6, Eigen::Dynamic>;
 using ImuBias = gtsam::imuBias::ConstantBias;
 
+/**
+ * @brief The ImuMeasurement struct One single IMU measurement
+ */
 struct ImuMeasurement {
   ImuMeasurement() = default;
   ImuMeasurement(const ImuStamp& timestamp, const ImuAccGyr& imu_data)
@@ -50,7 +53,10 @@ struct ImuMeasurement {
   ImuAccGyr acc_gyr_;
 };
 
-// Multiple Imu measurements, bundled in dynamic matrices.
+/**
+ * @brief The ImuMeasurements struct Multiple Imu measurements, bundled in
+ * dynamic matrices.
+ */
 struct ImuMeasurements {
  public:
   ImuMeasurements() = default;
@@ -64,6 +70,9 @@ struct ImuMeasurements {
   ImuAccGyrS acc_gyr_;
 };
 
+/**
+ * @brief The ImuPreintegrationType enum Types of supported IMU preintegration.
+ */
 enum class ImuPreintegrationType {
   kPreintegratedCombinedMeasurements = 0,
   kPreintegratedImuMeasurements = 1