diff --git a/include/kimera-vio/mesh/MeshOptimization.h b/include/kimera-vio/mesh/MeshOptimization.h index c8836e1c..cd653606 100644 --- a/include/kimera-vio/mesh/MeshOptimization.h +++ b/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 diff --git a/include/kimera-vio/playground/EurocPlayground.h b/include/kimera-vio/playground/EurocPlayground.h index 3e809ced..820658fa 100644 --- a/include/kimera-vio/playground/EurocPlayground.h +++ b/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_; diff --git a/include/kimera-vio/visualizer/OpenCvVisualizer3D.h b/include/kimera-vio/visualizer/OpenCvVisualizer3D.h index 3b6de6b5..f14f922f 100644 --- a/include/kimera-vio/visualizer/OpenCvVisualizer3D.h +++ b/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 diff --git a/include/kimera-vio/visualizer/abstract/BaseOpenCvVisualizer3D.h b/include/kimera-vio/visualizer/abstract/BaseOpenCvVisualizer3D.h new file mode 100644 index 00000000..36552d41 --- /dev/null +++ b/include/kimera-vio/visualizer/abstract/BaseOpenCvVisualizer3D.h @@ -0,0 +1,49 @@ +#include + +#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 diff --git a/src/mesh/MeshOptimization.cpp b/src/mesh/MeshOptimization.cpp index b6db6fc6..1ab50aef 100644 --- a/src/mesh/MeshOptimization.cpp +++ b/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(); } diff --git a/src/playground/EurocPlayground.cpp b/src/playground/EurocPlayground.cpp index 3910c180..3120cd16 100644 --- a/src/playground/EurocPlayground.cpp +++ b/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()); } } } diff --git a/tests/testEurocPlayground.cpp b/tests/testEurocPlayground.cpp index 9e9e1da1..fa630293 100644 --- a/tests/testEurocPlayground.cpp +++ b/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( diff --git a/tests/testRgbdCamera.cpp b/tests/testRgbdCamera.cpp index 9fecf810..9a844522 100644 --- a/tests/testRgbdCamera.cpp +++ b/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_; };