Open source/feature/rviz display

include/kimera-vio/frontend/Camera.h

@@ -258,8 +258,9 @@ class StereoCamera {
     // Relative pose after rectification
     gtsam::Pose3 camLrect_Pose_calRrect =
         B_Pose_camLrect->between(B_Pose_camRrect);
-    // get baseline
+    // Get baseline (this is after rectification).
     *baseline = camLrect_Pose_calRrect.translation().x();
+    CHECK_GT(*baseline, 0u);
 
     // Sanity check.
     LOG_IF(FATAL,

include/kimera-vio/frontend/StereoVisionFrontEnd-definitions.h

@@ -8,7 +8,7 @@
 
 /**
  * @file   StereoVisionFrontEnd-definitions.h
- * @brief  Definitions for VioBackEnd
+ * @brief  Definitions for StereoVisionFrontEnd
  * @author Antoni Rosinol
  */
 
@@ -64,7 +64,7 @@ struct FrontendOutput : public PipelinePayload {
   const DebugTrackerInfo debug_tracker_info_;
   const cv::Mat feature_tracks_;
 
-  inline DebugTrackerInfo getTrackerInfo() { return debug_tracker_info_; }
+  inline DebugTrackerInfo getTrackerInfo() const { return debug_tracker_info_; }
 };
 
 }  // namespace VIO

include/kimera-vio/frontend/Tracker.h

@@ -37,8 +37,8 @@ namespace VIO {
 // TODO(Toni): Fast-forwarding bcs of an issue wiht includes:
 // if you include here the display-definitions.h, a million errors appear, this
 // should go away after properly cleaning what each file includes.
-class VisualizerOutput;
-typedef ThreadsafeQueue<std::unique_ptr<VisualizerOutput>> DisplayQueue;
+class DisplayInputBase;
+using DisplayQueue = ThreadsafeQueue<std::unique_ptr<DisplayInputBase>>;
 
 class Tracker {
  public:

include/kimera-vio/pipeline/Pipeline.h

@@ -33,7 +33,9 @@
 #include "kimera-vio/mesh/MesherModule.h"
 #include "kimera-vio/pipeline/Pipeline-definitions.h"
 #include "kimera-vio/utils/ThreadsafeQueue.h"
+#include "kimera-vio/visualizer/Display.h" // TODO(Toni): separate ocv display
 #include "kimera-vio/visualizer/DisplayModule.h"
+#include "kimera-vio/visualizer/Visualizer3D.h" // TODO(Toni): separate ocv viz
 #include "kimera-vio/visualizer/Visualizer3DModule.h"
 
 namespace VIO {
@@ -45,7 +47,15 @@ class Pipeline {
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
  public:
-  explicit Pipeline(const VioParams& params);
+  /**
+     * @brief Pipeline
+     * @param params Vio parameters
+     * @param visualizer Optional visualizer for visualizing 3D results
+     * @param displayer Optional displayer for visualizing 2D results
+     */
+  Pipeline(const VioParams& params,
+           Visualizer3D::UniquePtr&& visualizer = nullptr,
+           DisplayBase::UniquePtr&& displayer = nullptr);
 
   virtual ~Pipeline();
 

include/kimera-vio/visualizer/CMakeLists.txt

@@ -1,11 +1,13 @@
 ### Add includes
 target_sources(kimera_vio PRIVATE
-  "${CMAKE_CURRENT_LIST_DIR}/Visualizer3D.h"
   "${CMAKE_CURRENT_LIST_DIR}/Visualizer3D-definitions.h"
   "${CMAKE_CURRENT_LIST_DIR}/Visualizer3DModule.h"
   "${CMAKE_CURRENT_LIST_DIR}/Visualizer3DFactory.h"
+  "${CMAKE_CURRENT_LIST_DIR}/Visualizer3D.h"
+  "${CMAKE_CURRENT_LIST_DIR}/OpenCvVisualizer3D.h"
   "${CMAKE_CURRENT_LIST_DIR}/Display-definitions.h"
-  "${CMAKE_CURRENT_LIST_DIR}/Display.h"
   "${CMAKE_CURRENT_LIST_DIR}/DisplayModule.h"
   "${CMAKE_CURRENT_LIST_DIR}/DisplayFactory.h"
+  "${CMAKE_CURRENT_LIST_DIR}/Display.h"
+  "${CMAKE_CURRENT_LIST_DIR}/OpenCvDisplay.h"
 )

include/kimera-vio/visualizer/Display-definitions.h

@@ -57,14 +57,15 @@ struct WindowData {
  * @param display_queue Threadsafe queue were images will be pushed and that
  * needs to be consumed by a display thread.
  */
-inline void displayImage(const std::string& title,
+inline void displayImage(const Timestamp& timestamp,
+                         const std::string& title,
                          const cv::Mat& img,
                          DisplayQueue* display_queue) {
-  VisualizerOutput::UniquePtr visualizer_output =
-      VIO::make_unique<VisualizerOutput>();
+  DisplayInputBase::UniquePtr visualizer_output =
+      VIO::make_unique<DisplayInputBase>();
+  visualizer_output->timestamp_ = timestamp;
   ImageToDisplay img_to_display(title, img);
   visualizer_output->images_to_display_.push_back(img_to_display);
-  visualizer_output->visualization_type_ = VisualizationType::kNone;
   if (display_queue) {
     display_queue->push(std::move(visualizer_output));
   } else {

include/kimera-vio/visualizer/Display.h

@@ -31,81 +31,12 @@ class DisplayBase {
   DisplayBase() = default;
   virtual ~DisplayBase() = default;
 
-  // Spins the display once to render the visualizer output.
-  virtual void spinOnce(VisualizerOutput::UniquePtr&& viz_output) = 0;
-};
-
-class OpenCv3dDisplay : public DisplayBase {
- public:
-  KIMERA_POINTER_TYPEDEFS(OpenCv3dDisplay);
-  KIMERA_DELETE_COPY_CONSTRUCTORS(OpenCv3dDisplay);
-
-  OpenCv3dDisplay(const ShutdownPipelineCallback& shutdown_pipeline_cb);
-
-  // TODO(Toni): consider using `unregisterAllWindows`
-  ~OpenCv3dDisplay() override = default;
-
-  // Spins renderers to display data using OpenCV imshow and viz3d
-  // Displaying must be done in the main thread.
-  void spinOnce(VisualizerOutput::UniquePtr&& viz_output) override;
-
- private:
-  // Adds 3D widgets to the window, and displays it.
-  void spin3dWindow(VisualizerOutput::UniquePtr&& viz_output);
-
-  void spin2dWindow(const VisualizerOutput& viz_output);
-
-  //! Sets the visualization properties of the 3D mesh.
-  void setMeshProperties(WidgetsMap* widgets);
-
-  //! Sets a 3D Widget Pose, because Widget3D::setPose() doesn't work;
-  void setFrustumPose(const cv::Affine3d& frustum_pose);
-
-  // Keyboard callback.
-  static void keyboardCallback(const cv::viz::KeyboardEvent& event, void* t);
-
-  // Keyboard callback to toggle freezing screen.
-  static void toggleFreezeScreenKeyboardCallback(const uchar& code,
-                                                 WindowData* window_data);
-
-  // Keyboard callback to set mesh representation.
-  static void setMeshRepresentation(const uchar& code, WindowData* window_data);
-
-  // Keyboard callback to set mesh shading.
-  static void setMeshShadingCallback(const uchar& code,
-                                     WindowData* window_data);
-
-  // Keyboard callback to set mesh ambient.
-  static void setMeshAmbientCallback(const uchar& code,
-                                     WindowData* window_data);
-
-  // Keyboard callback to set mesh lighting.
-  static void setMeshLightingCallback(const uchar& code,
-                                      WindowData* window_data);
-
-  // Keyboard callback to get current viewer pose.
-  static void getViewerPoseKeyboardCallback(const uchar& code,
-                                            WindowData* window_data);
-
-  // Keyboard callback to get current screen size.
-  static void getCurrentWindowSizeKeyboardCallback(const uchar& code,
-                                                   WindowData* window_data);
-
-  // Keyboard callback to get screenshot of current windodw.
-  static void getScreenshotCallback(const uchar& code, WindowData* window_data);
-
-  // Record video sequence at a hardcoded directory relative to executable.
-  void recordVideo();
-
-  // Useful for when testing on servers without display screen.
-  void setOffScreenRendering();
-
- private:
-  WindowData window_data_;
-
-  //! We use this callback to shutdown the pipeline gracefully if
-  //! the visualization window is closed.
-  ShutdownPipelineCallback shutdown_pipeline_cb_;
+  /**
+   * @brief spinOnce
+   * Spins the display once to render the visualizer output.
+   * @param viz_output Visualizer output, which is the display input.
+   */
+  virtual void spinOnce(DisplayInputBase::UniquePtr&& viz_output) = 0;
 };
 
 }  // namespace VIO

include/kimera-vio/visualizer/DisplayFactory.h

@@ -18,6 +18,7 @@
 
 #include "kimera-vio/utils/Macros.h"
 #include "kimera-vio/visualizer/Display.h"
+#include "kimera-vio/visualizer/OpenCvDisplay.h"
 
 namespace VIO {
 

include/kimera-vio/visualizer/DisplayModule.h

@@ -31,20 +31,27 @@ namespace VIO {
  * DisplayModule should spin in the main thread to avoid errors.
  */
 class DisplayModule
-    : public SISOPipelineModule<VisualizerOutput, NullPipelinePayload> {
+    : public SISOPipelineModule<DisplayInputBase, NullPipelinePayload> {
  public:
   KIMERA_POINTER_TYPEDEFS(DisplayModule);
   KIMERA_DELETE_COPY_CONSTRUCTORS(DisplayModule);
 
-  using SISO = SISOPipelineModule<VisualizerOutput, NullPipelinePayload>;
+  using SISO = SISOPipelineModule<DisplayInputBase, NullPipelinePayload>;
 
   DisplayModule(DisplayQueue* input_queue,
                 OutputQueue* output_queue,
                 bool parallel_run,
-                DisplayBase::UniquePtr&& display);
+                DisplayBase::UniquePtr&& display)
+      : SISO(input_queue, output_queue, "Display", parallel_run),
+        display_(std::move(display)) {}
+
   virtual ~DisplayModule() = default;
 
-  virtual OutputUniquePtr spinOnce(VisualizerOutput::UniquePtr input);
+  virtual OutputUniquePtr spinOnce(InputUniquePtr input) {
+    CHECK(input);
+    display_->spinOnce(std::move(input));
+    return VIO::make_unique<NullPipelinePayload>();
+  }
 
  private:
   // The renderer used to display the visualizer output.

include/kimera-vio/visualizer/OpenCvDisplay.h

@@ -0,0 +1,117 @@
+/* ----------------------------------------------------------------------------
+ * 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   OpenCvDisplay.h
+ * @brief  Class to display visualizer output using OpenCV
+ * @author Antoni Rosinol
+ */
+
+#pragma once
+
+#include <opencv2/opencv.hpp>
+
+#include "kimera-vio/pipeline/Pipeline-definitions.h"  // Needed for shutdown cb
+#include "kimera-vio/utils/Macros.h"
+#include "kimera-vio/visualizer/Display-definitions.h"
+#include "kimera-vio/visualizer/Display.h"
+#include "kimera-vio/visualizer/Visualizer3D-definitions.h"
+
+namespace VIO {
+
+class OpenCv3dDisplay : public DisplayBase {
+ public:
+  KIMERA_POINTER_TYPEDEFS(OpenCv3dDisplay);
+  KIMERA_DELETE_COPY_CONSTRUCTORS(OpenCv3dDisplay);
+
+  OpenCv3dDisplay(const ShutdownPipelineCallback& shutdown_pipeline_cb);
+
+  // TODO(Toni): consider using `unregisterAllWindows`
+  ~OpenCv3dDisplay() override = default;
+
+  /**
+   * @brief spinOnce
+   * Spins renderers to display data using OpenCV imshow and viz3d
+   * Displaying must be done in the main thread, that it is why it is separated
+   * from Visualizer3D (plus it makes everything faster as displaying and
+   * building 3D graphics is decoupled).
+   * @param viz_output
+   */
+  void spinOnce(DisplayInputBase::UniquePtr&& viz_output) override;
+
+ private:
+  //! Adds 3D widgets to the window, and displays it.
+  void spin3dWindow(VisualizerOutput::UniquePtr&& viz_output);
+
+  //! Visualizes 2D data.
+  void spin2dWindow(const DisplayInputBase& viz_output);
+
+  //! Sets the visualization properties of the 3D mesh.
+  void setMeshProperties(WidgetsMap* widgets);
+
+  //! Sets a 3D Widget Pose, because Widget3D::setPose() doesn't work;
+  void setFrustumPose(const cv::Affine3d& frustum_pose);
+
+  //! Keyboard callback.
+  static void keyboardCallback(const cv::viz::KeyboardEvent& event, void* t);
+
+  //! Keyboard callback to toggle freezing screen.
+  static void toggleFreezeScreenKeyboardCallback(const uchar& code,
+                                                 WindowData* window_data);
+
+  //! Keyboard callback to set mesh representation.
+  static void setMeshRepresentation(const uchar& code, WindowData* window_data);
+
+  //! Keyboard callback to set mesh shading.
+  static void setMeshShadingCallback(const uchar& code,
+                                     WindowData* window_data);
+
+  //! Keyboard callback to set mesh ambient.
+  static void setMeshAmbientCallback(const uchar& code,
+                                     WindowData* window_data);
+
+  //! Keyboard callback to set mesh lighting.
+  static void setMeshLightingCallback(const uchar& code,
+                                      WindowData* window_data);
+
+  //! Keyboard callback to get current viewer pose.
+  static void getViewerPoseKeyboardCallback(const uchar& code,
+                                            WindowData* window_data);
+
+  //! Keyboard callback to get current screen size.
+  static void getCurrentWindowSizeKeyboardCallback(const uchar& code,
+                                                   WindowData* window_data);
+
+  //! Keyboard callback to get screenshot of current windodw.
+  static void getScreenshotCallback(const uchar& code, WindowData* window_data);
+
+  //! Record video sequence at a hardcoded directory relative to executable.
+  void recordVideo();
+
+  //! Useful for when testing on servers without display screen.
+  void setOffScreenRendering();
+
+  /**
+   * @brief safeCast Try to cast display input base to the derived visualizer
+   * output, if unsuccessful, it will return a nullptr.
+   * @param display_input_base
+   * @return
+   */
+  VisualizerOutput::UniquePtr safeCast(
+      DisplayInputBase::UniquePtr display_input_base);
+
+ private:
+  //! Data to visualize (in 3D)
+  WindowData window_data_;
+
+  //! We use this callback to shutdown the pipeline gracefully if
+  //! the visualization window is closed.
+  ShutdownPipelineCallback shutdown_pipeline_cb_;
+};
+
+}  // namespace VIO