PR1: Moving mesh visualization logic from MeshOptimization to OpenCvVisualizer3D.

include/kimera-vio/mesh/MeshOptimization.h

@@ -69,15 +69,6 @@ class MeshOptimization {
       const size_t& thickness = 1u,
       const int line_type = CV_AA);
 
-  //! Visualization functions
-  void draw3dMesh(const std::string& id,
-                  const Mesh3D& mesh_3d,
-                  bool display_as_wireframe = false,
-                  const double& opacity = 1.0);
-
-  //! Render the collected visualizations
-  void spinDisplay();
-
  public:
   // In public only for testing... please remove.
 
@@ -183,7 +174,6 @@ class MeshOptimization {
 
   /// 3D plotting
   // TODO(Toni) this should be done by the display module...
-  cv::viz::Viz3d window_;
   MeshColorType mesh_color_type_;
   OpenCvVisualizer3D::Ptr visualizer_;
 };

include/kimera-vio/visualizer/OpenCvVisualizer3D.h

@@ -194,6 +194,15 @@ class OpenCvVisualizer3D : public Visualizer3D {
                 const cv::Point3d& pt2,
                 WidgetsMap* widgets);
 
+  //! Mesh visualization functions
+  void draw3dMesh(const std::string& id,
+                  const Mesh3D& mesh_3d,
+                  bool display_as_wireframe = false,
+                  const double& opacity = 1.0);
+
+  //! Render the collected visualizations
+  void meshSpinDisplay();
+
  private:
   //! Create a 2D mesh from 2D corners in an image, coded as a Frame class
   static cv::Mat visualizeMesh2D(
@@ -422,6 +431,9 @@ class OpenCvVisualizer3D : public Visualizer3D {
   WidgetIds widget_ids_to_remove_;
   WidgetIds widget_ids_to_remove_in_next_iter_;
 
+  // Used draw3dMesh and meshSpinDisplay functions
+  cv::viz::Viz3d meshWindow_;
+
   //! Colors & Scales
   cv::viz::Color cloud_color_ = cv::viz::Color::white();
 

src/mesh/MeshOptimization.cpp

@@ -53,12 +53,9 @@ MeshOptimization::MeshOptimization(const MeshOptimizerType& solver_type,
     : mesh_optimizer_type_(solver_type),
       mono_camera_(camera),
       body_pose_cam_(camera->getBodyPoseCam()),
-      window_("Mesh Optimization"),
       mesh_color_type_(mesh_color_type),
       visualizer_(visualizer) {
-  CHECK(camera);
-  window_.setBackgroundColor(cv::viz::Color::white());
-  window_.setFullScreen(true);
+  CHECK(camera);  
 }
 
 MeshOptimizationOutput::UniquePtr MeshOptimization::spinOnce(
@@ -87,41 +84,6 @@ void MeshOptimization::draw2dMeshOnImg(const Mesh2D& mesh_2d,
   }
 }
 
-void MeshOptimization::draw3dMesh(const std::string& id,
-                                  const Mesh3D& mesh_3d,
-                                  bool display_as_wireframe,
-                                  const double& opacity) {
-  cv::Mat vertices_mesh;
-  cv::Mat polygons_mesh;
-  mesh_3d.getVerticesMeshToMat(&vertices_mesh);
-  mesh_3d.getPolygonsMeshToMat(&polygons_mesh);
-  cv::Mat colors_mesh = mesh_3d.getColorsMesh().t();  // Note the transpose.
-  if (colors_mesh.empty()) {
-    colors_mesh = cv::Mat(1u,
-                          mesh_3d.getNumberOfUniqueVertices(),
-                          CV_8UC3,
-                          cv::viz::Color::yellow());
-  }
-
-  // Build visual mesh
-  cv::viz::Mesh cv_mesh;
-  cv_mesh.cloud = vertices_mesh.t();
-  cv_mesh.polygons = polygons_mesh;
-  cv_mesh.colors = colors_mesh;
-
-  // Build widget mesh
-  cv::viz::WMesh widget_cv_mesh(cv_mesh);
-  widget_cv_mesh.setRenderingProperty(cv::viz::SHADING, cv::viz::SHADING_FLAT);
-  widget_cv_mesh.setRenderingProperty(cv::viz::AMBIENT, 0);
-  widget_cv_mesh.setRenderingProperty(cv::viz::LIGHTING, 1);
-  widget_cv_mesh.setRenderingProperty(cv::viz::OPACITY, opacity);
-  if (display_as_wireframe) {
-    widget_cv_mesh.setRenderingProperty(cv::viz::REPRESENTATION,
-                                        cv::viz::REPRESENTATION_WIREFRAME);
-  }
-  window_.showWidget(id.c_str(), widget_cv_mesh);
-}
-
 void MeshOptimization::collectTriangleDataPointsFast(
     const cv::Mat& noisy_point_cloud,
     const Mesh2D& mesh_2d,
@@ -677,11 +639,11 @@ MeshOptimizationOutput::UniquePtr MeshOptimization::solveOptimalMesh(
   // Display reconstructed mesh.
   if (visualizer_) {
     LOG(INFO) << "Drawing optimized reconstructed mesh...";
-    draw3dMesh("Reconstructed Mesh " + std::to_string(mesh_count_),
+    visualizer_->draw3dMesh("Reconstructed Mesh " + std::to_string(mesh_count_),
                reconstructed_mesh,
                false,
                0.9);
-    spinDisplay();
+    visualizer_->meshSpinDisplay();
   }
   MeshOptimizationOutput::UniquePtr mesh_output =
       std::make_unique<MeshOptimizationOutput>();
@@ -765,9 +727,4 @@ void MeshOptimization::drawPixelOnImg(const cv::Point2f& pixel,
   cv::circle(img, pixel, pixel_size, color, -1);
 }
 
-void MeshOptimization::spinDisplay() {
-  // Display 3D window
-  window_.spin();
-}
-
 }  // namespace VIO

src/visualizer/OpenCvVisualizer3D.cpp

@@ -88,7 +88,10 @@ namespace VIO {
 
 OpenCvVisualizer3D::OpenCvVisualizer3D(const VisualizationType& viz_type,
                                        const BackendType& backend_type)
-    : Visualizer3D(viz_type), backend_type_(backend_type), logger_(nullptr) {
+    : Visualizer3D(viz_type), backend_type_(backend_type), logger_(nullptr),
+      meshWindow_("Mesh Optimization") {
+  meshWindow_.setBackgroundColor(cv::viz::Color::white());
+  meshWindow_.setFullScreen(true);
   if (FLAGS_log_mesh) {
     logger_ = std::make_unique<VisualizerLogger>();
   }
@@ -1926,6 +1929,46 @@ void OpenCvVisualizer3D::colorMeshByClusters(const std::vector<Plane>& planes,
   }
 }
 
+void OpenCvVisualizer3D::draw3dMesh(const std::string& id,
+                                  const Mesh3D& mesh_3d,
+                                  bool display_as_wireframe,
+                                  const double& opacity) {
+  cv::Mat vertices_mesh;
+  cv::Mat polygons_mesh;
+  mesh_3d.getVerticesMeshToMat(&vertices_mesh);
+  mesh_3d.getPolygonsMeshToMat(&polygons_mesh);
+  cv::Mat colors_mesh = mesh_3d.getColorsMesh().t();  // Note the transpose.
+  if (colors_mesh.empty()) {
+    colors_mesh = cv::Mat(1u,
+                          mesh_3d.getNumberOfUniqueVertices(),
+                          CV_8UC3,
+                          cv::viz::Color::yellow());
+  }
+
+  // Build visual mesh
+  cv::viz::Mesh cv_mesh;
+  cv_mesh.cloud = vertices_mesh.t();
+  cv_mesh.polygons = polygons_mesh;
+  cv_mesh.colors = colors_mesh;
+
+  // Build widget mesh
+  cv::viz::WMesh widget_cv_mesh(cv_mesh);
+  widget_cv_mesh.setRenderingProperty(cv::viz::SHADING, cv::viz::SHADING_FLAT);
+  widget_cv_mesh.setRenderingProperty(cv::viz::AMBIENT, 0);
+  widget_cv_mesh.setRenderingProperty(cv::viz::LIGHTING, 1);
+  widget_cv_mesh.setRenderingProperty(cv::viz::OPACITY, opacity);
+  if (display_as_wireframe) {
+    widget_cv_mesh.setRenderingProperty(cv::viz::REPRESENTATION,
+                                        cv::viz::REPRESENTATION_WIREFRAME);
+  }
+  meshWindow_.showWidget(id.c_str(), widget_cv_mesh);
+}
+
+void OpenCvVisualizer3D::meshSpinDisplay() {
+  // Display 3D window
+  meshWindow_.spin();
+}
+
 void OpenCvVisualizer3D::getColorById(const size_t& id,
                                       cv::viz::Color* color) const {
   CHECK_NOTNULL(color);