PR3: Adding abstract class corresponding to OpenCvVisualizer3D and making other relevant changes

include/kimera-vio/mesh/MeshOptimization.h

@@ -51,7 +51,7 @@ class MeshOptimization {
   MeshOptimization(const MeshOptimizerType& solver_type,
                    const MeshColorType& mesh_color_type,
                    Camera::ConstPtr camera,
-                   OpenCvVisualizer3D::Ptr visualizer = nullptr);
+                   BaseOpenCvVisualizer3D::Ptr visualizer = nullptr);
   virtual ~MeshOptimization() = default;
 
   /**
@@ -176,7 +176,7 @@ class MeshOptimization {
   /// 3D plotting
   // TODO(Toni) this should be done by the display module...
   MeshColorType mesh_color_type_;
-  OpenCvVisualizer3D::Ptr visualizer_;
+  VIO::BaseOpenCvVisualizer3D::Ptr visualizer_;
 };
 
 }  // namespace VIO

include/kimera-vio/playground/EurocPlayground.h

@@ -88,7 +88,7 @@ class EurocPlayground {
   //! Params
   VioParams vio_params_;
 
-  OpenCvVisualizer3D::Ptr visualizer_3d_;
+  BaseOpenCvVisualizer3D::Ptr visualizer_3d_;
 
   //! Stereo Camera to back/project and do stereo dense reconstruction.
   StereoCamera::ConstPtr stereo_camera_;

include/kimera-vio/visualizer/OpenCvVisualizer3D.h

@@ -37,10 +37,11 @@
 #include "kimera-vio/utils/Macros.h"
 #include "kimera-vio/visualizer/Visualizer3D-definitions.h"
 #include "kimera-vio/visualizer/Visualizer3D.h"
+#include "kimera-vio/visualizer/abstract/BaseOpenCvVisualizer3D.h"
 
 namespace VIO {
 
-class OpenCvVisualizer3D : public Visualizer3D {
+class OpenCvVisualizer3D : public BaseOpenCvVisualizer3D, public Visualizer3D {
  public:
   KIMERA_DELETE_COPY_CONSTRUCTORS(OpenCvVisualizer3D);
   KIMERA_POINTER_TYPEDEFS(OpenCvVisualizer3D);
@@ -91,7 +92,7 @@ class OpenCvVisualizer3D : public Visualizer3D {
    * @param current_pose_gtsam Pose to be added
    * @param img Optional img to be displayed at the pose's frustum.
    */
-  void addPoseToTrajectory(const cv::Affine3d& pose);
+  void addPoseToTrajectory(const cv::Affine3d& pose) override;
 
   /**
    * @brief visualizeTrajectory3D
@@ -100,7 +101,7 @@ class OpenCvVisualizer3D : public Visualizer3D {
    * @param frustum_image
    * @param widgets_map
    */
-  void visualizeTrajectory3D(WidgetsMap* widgets_map);
+  void visualizeTrajectory3D(WidgetsMap* widgets_map) override;
 
   /**
    * @brief visualizeTrajectoryWithFrustums
@@ -128,14 +129,16 @@ class OpenCvVisualizer3D : public Visualizer3D {
       WidgetsMap* widgets_map,
       const std::string& widget_id = "Camera Pose with Frustum",
       const cv::Matx33d K =
-          cv::Matx33d(458, 0.0, 360, 0.0, 458, 240, 0.0, 0.0, 1.0));
+          cv::Matx33d(458, 0.0, 360, 0.0, 458, 240, 0.0, 0.0, 1.0))
+      override;
 
   /**
    * @brief visualizePlyMesh Visualize a PLY from filename (absolute path).
    * @param filename Absolute path to ply file
    * @param widgets output
    */
-  void visualizePlyMesh(const std::string& filename, WidgetsMap* widgets);
+  void visualizePlyMesh(const std::string& filename, WidgetsMap* widgets)
+      override;
 
   /**
    * @brief visualizePointCloud Given a cv::Mat with each col being
@@ -151,9 +154,10 @@ class OpenCvVisualizer3D : public Visualizer3D {
                            WidgetsMap* widgets,
                            const cv::Affine3d& pose = cv::Affine3d(),
                            const cv::Mat& colors = cv::Mat(),
-                           const cv::Mat& normals = cv::Mat());
+                           const cv::Mat& normals = cv::Mat()) override;
 
-  void visualizeGlobalFrameOfReference(WidgetsMap* widgets, double scale = 1.0);
+  void visualizeGlobalFrameOfReference(WidgetsMap* widgets, double scale = 1.0)
+      override;
 
   /**
    * @brief visualizeMesh3D
@@ -176,7 +180,7 @@ class OpenCvVisualizer3D : public Visualizer3D {
                        const cv::Mat& tcoords = cv::Mat(),
                        const cv::Mat& texture = cv::Mat(),
                        //! This has to be the same than the id in OpenCvDisplay
-                       const std::string& mesh_id = "Mesh");
+                       const std::string& mesh_id = "Mesh") override;
 
   //! Draw a line in opencv.
   void drawLine(const std::string& line_id,
@@ -198,10 +202,10 @@ class OpenCvVisualizer3D : public Visualizer3D {
   void draw3dMesh(const std::string& id,
                   const Mesh3D& mesh_3d,
                   bool display_as_wireframe = false,
-                  const double& opacity = 1.0);
+                  const double& opacity = 1.0) override;
 
   //! Render the collected visualizations
-  void meshSpinDisplay();
+  void meshSpinDisplay() override;
 
  private:
   //! Create a 2D mesh from 2D corners in an image, coded as a Frame class

include/kimera-vio/visualizer/abstract/BaseOpenCvVisualizer3D.h

@@ -0,0 +1,49 @@
+#include <opencv2/opencv.hpp>
+
+#include "kimera-vio/visualizer/Visualizer3D-definitions.h"
+
+namespace VIO {
+
+  class BaseOpenCvVisualizer3D {
+  public:
+    KIMERA_POINTER_TYPEDEFS(BaseOpenCvVisualizer3D);
+
+    virtual void addPoseToTrajectory(const cv::Affine3d& pose) = 0;
+
+    virtual void visualizeTrajectory3D(WidgetsMap* widgets_map) = 0;
+
+    virtual void visualizePoseWithImgInFrustum(
+      const cv::Mat& frustum_image,
+      const cv::Affine3d& frustum_pose,
+      WidgetsMap* widgets_map,
+      const std::string& widget_id,
+      const cv::Matx33d K) = 0;
+
+    virtual void visualizePlyMesh(const std::string& filename, 
+                                  WidgetsMap* widgets) = 0;
+
+    virtual void visualizePointCloud(const cv::Mat& point_cloud,
+                                     WidgetsMap* widgets,
+                                     const cv::Affine3d& pose,
+                                     const cv::Mat& colors,
+                                     const cv::Mat& normals) = 0;
+
+    virtual void visualizeGlobalFrameOfReference(WidgetsMap* widgets, 
+                                                 double scale) = 0;
+
+    virtual void visualizeMesh3D(const cv::Mat& map_points_3d,
+                                 const cv::Mat& colors,
+                                 const cv::Mat& polygons_mesh,
+                                 WidgetsMap* widgets,
+                                 const cv::Mat& tcoords,
+                                 const cv::Mat& texture,
+                                 const std::string& mesh_id) = 0;
+
+    virtual void draw3dMesh(const std::string& id, const Mesh3D& mesh_3d,
+                            bool display_as_wireframe ,
+                            const double& opacity) = 0;
+
+    virtual void meshSpinDisplay() = 0;
+  };
+
+} // namespace VIO

src/mesh/MeshOptimization.cpp

@@ -50,7 +50,7 @@ constexpr float MeshOptimization::kMaxZ;
 MeshOptimization::MeshOptimization(const MeshOptimizerType& solver_type,
                                    const MeshColorType& mesh_color_type,
                                    Camera::ConstPtr camera,
-                                   OpenCvVisualizer3D::Ptr visualizer)
+                                   BaseOpenCvVisualizer3D::Ptr visualizer)
     : mesh_optimizer_type_(solver_type),
       mono_camera_(camera),
       body_pose_cam_(camera->getBodyPoseCam()),
@@ -279,7 +279,8 @@ MeshOptimizationOutput::UniquePtr MeshOptimization::solveOptimalMesh(
         viz_cloud,
         &output->widgets_,
         UtilsOpenCV::gtsamPose3ToCvAffine3d(body_pose_cam_),
-        colors_pcl);
+        colors_pcl,
+        cv::Mat());
     // draw2dMeshOnImg(img_, mesh_2d);
     // spinDisplay();
   }

src/playground/EurocPlayground.cpp

@@ -85,7 +85,7 @@ void EurocPlayground::visualizeGtData(const bool& viz_traj,
   output->visualization_type_ = VisualizationType::kPointcloud;
 
   // Draw the global frame of reference
-  visualizer_3d_->visualizeGlobalFrameOfReference(&output->widgets_);
+  visualizer_3d_->visualizeGlobalFrameOfReference(&output->widgets_, 1.0);
 
   if (viz_traj) {
     CHECK_GT(vio_params_.camera_params_.size(), 0);
@@ -226,7 +226,8 @@ void EurocPlayground::visualizeGtData(const bool& viz_traj,
 
         LOG(INFO) << "Send pcl to viz.";
         visualizer_3d_->visualizePointCloud(
-            valid_depth, &output->widgets_, left_cam_rect_pose, valid_colors);
+            valid_depth, &output->widgets_, left_cam_rect_pose, valid_colors,
+            cv::Mat());
       }
     }
   }

tests/testEurocPlayground.cpp

@@ -136,7 +136,8 @@ TEST(TestEurocPlayground, DISABLED_basicEurocPlayground) {
       output->visualization_type_ = VisualizationType::kPointcloud;
       CHECK(euroc_playground.visualizer_3d_);
       euroc_playground.visualizer_3d_->visualizeMesh3D(
-          pcl, colors, connect, &output->widgets_, cv::Mat(), cv::Mat());
+          pcl, colors, connect, &output->widgets_, cv::Mat(), cv::Mat(),
+          "Mesh");
       gtsam::Pose3 world_pose_body = it->second.world_pose_body_;
       CHECK_GT(output->widgets_.size(), 0u);
       output->widgets_.rbegin()->second->setPose(

tests/testRgbdCamera.cpp

@@ -101,7 +101,8 @@ class RgbdCameraFixture : public ::testing::Test {
     }
     CHECK(!valid_depth.empty());
     CHECK(visualizer_3d_);
-    visualizer_3d_->visualizePointCloud(valid_depth, &output->widgets_);
+    visualizer_3d_->visualizePointCloud(valid_depth, &output->widgets_, 
+                                        cv::Affine3d(), cv::Mat(), cv::Mat());
     CHECK_GT(output->widgets_.size(), 0u);
     CHECK(display_module_);
     display_module_->spinOnce(std::move(output));
@@ -112,7 +113,7 @@ class RgbdCameraFixture : public ::testing::Test {
   RgbdCamera::UniquePtr rgbd_camera_;
 
   //! For visualization only
-  OpenCvVisualizer3D::Ptr visualizer_3d_;
+  BaseOpenCvVisualizer3D::Ptr visualizer_3d_;
   DisplayModule::UniquePtr display_module_;
   DisplayModule::InputQueue display_input_queue_;
 };