From 4ce7a502b3395020e10be0a7ee64ddeef1c43190 Mon Sep 17 00:00:00 2001 From: Sarika Ramroop Date: Thu, 5 Dec 2024 17:05:28 -0400 Subject: [PATCH 1/3] Adding Color class to utils folder and replace cv::viz::Color calls with these --- include/kimera-vio/frontend/RgbdCamera.h | 1 + include/kimera-vio/mesh/Mesh.h | 6 ++--- include/kimera-vio/mesh/MeshOptimization.h | 5 +++-- include/kimera-vio/mesh/MeshUtils.h | 10 ++++----- .../kimera-vio/playground/EurocPlayground.h | 1 + src/frontend/RgbdCamera.cpp | 2 +- src/mesh/Mesh.cpp | 4 ++-- src/mesh/MeshOptimization.cpp | 21 +++++++++--------- src/playground/EurocPlayground.cpp | 2 +- tests/testCamera.cpp | 4 ++-- tests/testEurocPlayground.cpp | 4 ++-- tests/testOpticalFlowPredictor.cpp | 19 ++++++++-------- tests/testStereoCamera.cpp | 4 ++-- tests/testTracker.cpp | 22 +++++++++---------- 14 files changed, 55 insertions(+), 50 deletions(-) diff --git a/include/kimera-vio/frontend/RgbdCamera.h b/include/kimera-vio/frontend/RgbdCamera.h index 557cacbde..7d3693c0b 100644 --- a/include/kimera-vio/frontend/RgbdCamera.h +++ b/include/kimera-vio/frontend/RgbdCamera.h @@ -18,6 +18,7 @@ #include "kimera-vio/backend/VioBackend-definitions.h" #include "kimera-vio/frontend/Camera.h" #include "kimera-vio/frontend/RgbdFrame.h" +#include "kimera-vio/utils/ColorUtils.h" #include "kimera-vio/utils/Macros.h" namespace VIO { diff --git a/include/kimera-vio/mesh/Mesh.h b/include/kimera-vio/mesh/Mesh.h index 30bc8fc77..a954ade3b 100644 --- a/include/kimera-vio/mesh/Mesh.h +++ b/include/kimera-vio/mesh/Mesh.h @@ -20,9 +20,9 @@ #include #include #include -#include // Just for color type. #include +#include "kimera-vio/utils/ColorUtils.h" #include "kimera-vio/utils/Macros.h" #include "kimera-vio/utils/UtilsOpenCV.h" @@ -78,11 +78,11 @@ class Mesh { : lmk_id_(-1), vertex_position_(), vertex_normal_(), - vertex_color_(cv::viz::Color::white()) {} + vertex_color_(Color::White()) {} Vertex(const LandmarkId& lmk_id, const VertexPosition& vertex_position, - const VertexColorRGB& vertex_color = cv::viz::Color::white(), + const VertexColorRGB& vertex_color = cv::Vec3b(Color::White()), const VertexNormal& vertex_normal = VertexNormal()) : lmk_id_(lmk_id), vertex_position_(vertex_position), diff --git a/include/kimera-vio/mesh/MeshOptimization.h b/include/kimera-vio/mesh/MeshOptimization.h index 898fab971..ad87a2aec 100644 --- a/include/kimera-vio/mesh/MeshOptimization.h +++ b/include/kimera-vio/mesh/MeshOptimization.h @@ -30,6 +30,7 @@ #include "kimera-vio/mesh/Mesh.h" #include "kimera-vio/mesh/MeshOptimization-definitions.h" #include "kimera-vio/mesh/Mesher-definitions.h" +#include "kimera-vio/utils/ColorUtils.h" #include "kimera-vio/utils/Macros.h" #include "kimera-vio/visualizer/OpenCvVisualizer3D.h" @@ -65,7 +66,7 @@ class MeshOptimization { static void draw2dMeshOnImg( const Mesh2D& mesh_2d, cv::Mat* img, - const cv::viz::Color& color = cv::viz::Color::red(), + const cv::Scalar& color = Color::Red(), const size_t& thickness = 1u, const int line_type = CV_AA); @@ -148,7 +149,7 @@ class MeshOptimization { void drawPixelOnImg(const cv::Point2f& pixel, const cv::Mat& img, - const cv::viz::Color& color = cv::viz::Color::red(), + const cv::Scalar& color = Color::Red(), const size_t& pixel_size = 5u); public: diff --git a/include/kimera-vio/mesh/MeshUtils.h b/include/kimera-vio/mesh/MeshUtils.h index 07ced60f4..e57561e5f 100644 --- a/include/kimera-vio/mesh/MeshUtils.h +++ b/include/kimera-vio/mesh/MeshUtils.h @@ -18,7 +18,7 @@ #include -#include +#include #include "kimera-vio/common/vio_types.h" @@ -174,8 +174,8 @@ inline bool rayTriangleIntersect(const Vec3f& orig, * Maps an input h from a value between 0.0 and 1.0 into a rainbow. Copied from * OctomapProvider in octomap. Copied from voxblox itself. */ -inline cv::viz::Color rainbowColorMap(double h) { - cv::viz::Color color; +inline cv::Scalar rainbowColorMap(double h) { + cv::Scalar color; // blend over HSV-values (more colors) double s = 1.0; @@ -221,12 +221,12 @@ inline cv::viz::Color rainbowColorMap(double h) { } /// Maps an input h from a value between 0.0 and 1.0 into a grayscale color. -inline cv::viz::Color grayColorMap(double h) { +inline cv::Scalar grayColorMap(double h) { auto x = round(h * 255); return cv::Scalar(x, x, x); } -inline cv::viz::Color randomColor() { +inline cv::Scalar randomColor() { return cv::Scalar(rand() % 256, rand() % 256, rand() % 256, 255); } diff --git a/include/kimera-vio/playground/EurocPlayground.h b/include/kimera-vio/playground/EurocPlayground.h index 17333e41c..3e809ced7 100644 --- a/include/kimera-vio/playground/EurocPlayground.h +++ b/include/kimera-vio/playground/EurocPlayground.h @@ -28,6 +28,7 @@ #include "kimera-vio/frontend/StereoCamera.h" #include "kimera-vio/frontend/StereoMatcher.h" #include "kimera-vio/frontend/feature-detector/FeatureDetector.h" +#include "kimera-vio/utils/ColorUtils.h" #include "kimera-vio/visualizer/Display.h" #include "kimera-vio/visualizer/DisplayFactory.h" #include "kimera-vio/visualizer/DisplayModule.h" diff --git a/src/frontend/RgbdCamera.cpp b/src/frontend/RgbdCamera.cpp index 761567e77..fcdfa2894 100644 --- a/src/frontend/RgbdCamera.cpp +++ b/src/frontend/RgbdCamera.cpp @@ -50,7 +50,7 @@ void convertToPcl(const cv::Mat& intensity_img, cv::Mat_ cloud_out = cv::Mat(img_rows, img_cols, CV_32FC3, cv::Scalar(0.0, 0.0, 0.0)); cv::Mat colors_out = - cv::Mat(img_rows, img_cols, CV_8UC3, cv::viz::Color::red()); + cv::Mat(img_rows, img_cols, CV_8UC3, Color::Red()); for (int v = 0u; v < img_rows; ++v) { for (int u = 0u; u < img_cols; ++u) { diff --git a/src/mesh/Mesh.cpp b/src/mesh/Mesh.cpp index 66b47f828..7ebc5816f 100644 --- a/src/mesh/Mesh.cpp +++ b/src/mesh/Mesh.cpp @@ -33,7 +33,7 @@ Mesh::Mesh(const size_t& polygon_dimension) vertices_mesh_(0, 1, CV_32FC3), vertices_mesh_normal_(), normals_computed_(false), - vertices_mesh_color_(0, 0, CV_8UC3, cv::viz::Color::blue()), + vertices_mesh_color_(0, 0, CV_8UC3, Color::Blue()), polygons_mesh_(0, 1, CV_32SC1), adjacency_matrix_(1, 1, CV_8UC1, cv::Scalar(0u)), face_hashes_(), @@ -490,7 +490,7 @@ template void Mesh::clearMesh() { vertices_mesh_ = cv::Mat(0, 1, CV_32FC3); vertices_mesh_normal_ = VertexNormals(); - vertices_mesh_color_ = cv::Mat(0, 0, CV_8UC3, cv::viz::Color::blue()); + vertices_mesh_color_ = cv::Mat(0, 0, CV_8UC3, Color::Blue()); polygons_mesh_ = cv::Mat(0, 1, CV_32SC1); adjacency_matrix_ = cv::Mat(1, 1, CV_8UC1, cv::Scalar(0u)); face_hashes_.clear(); diff --git a/src/mesh/MeshOptimization.cpp b/src/mesh/MeshOptimization.cpp index b5571ba35..4295a673e 100644 --- a/src/mesh/MeshOptimization.cpp +++ b/src/mesh/MeshOptimization.cpp @@ -31,6 +31,7 @@ #include "kimera-vio/mesh/MeshOptimization-definitions.h" #include "kimera-vio/mesh/MeshUtils.h" #include "kimera-vio/mesh/Mesher-definitions.h" +#include "kimera-vio/utils/ColorUtils.h" #include "kimera-vio/utils/Macros.h" #include "kimera-vio/utils/UtilsOpenCV.h" #include "kimera-vio/visualizer/OpenCvVisualizer3D.h" @@ -65,7 +66,7 @@ MeshOptimizationOutput::UniquePtr MeshOptimization::spinOnce( void MeshOptimization::draw2dMeshOnImg(const Mesh2D& mesh_2d, cv::Mat* img, - const cv::viz::Color& color, + const cv::Scalar& color, const size_t& thickness, const int line_type) { CHECK_NOTNULL(img); @@ -211,7 +212,7 @@ void MeshOptimization::collectTriangleDataPoints( CHECK_NEAR(left_pixel.y, static_cast(v), 0.001); if (visualizer_) { - // drawPixelOnImg(left_pixel, img_, cv::viz::Color::green(), 1u); + // drawPixelOnImg(left_pixel, img_, Color::Green(), 1u); } // 2. Generate correspondences btw points and triangles. @@ -258,7 +259,7 @@ MeshOptimizationOutput::UniquePtr MeshOptimization::solveOptimalMesh( if (visualizer_) { // Flatten and get colors for pcl cv::Mat viz_cloud(0, 1, CV_32FC3, cv::Scalar(0)); - cv::Mat colors_pcl = cv::Mat(0, 0, CV_8UC3, cv::viz::Color::red()); + cv::Mat colors_pcl = cv::Mat(0, 0, CV_8UC3, Color::Red()); CHECK_EQ(img_.type(), CV_8UC1); if (noisy_pcl.rows != 1u || noisy_pcl.cols != 1u) { LOG(ERROR) << "Reshaping noisy_pcl!"; @@ -574,25 +575,25 @@ MeshOptimizationOutput::UniquePtr MeshOptimization::solveOptimalMesh( //! Add new vertex to polygon //! Color with covariance bgr: static constexpr double kScaleStdDeviation = 0.1; - cv::viz::Color vtx_color = cv::viz::Color::black(); + cv::Scalar vtx_color = Color::Black(); switch (mesh_color_type_) { case MeshColorType::kVertexFlatColor: { // Use color of each pixel where the landmark is switch (mesh_count_ % 5) { case 0: - vtx_color = cv::viz::Color::red(); + vtx_color = Color::Red(); break; case 1: - vtx_color = cv::viz::Color::apricot(); + vtx_color = Color::Apricot(); break; case 2: - vtx_color = cv::viz::Color::purple(); + vtx_color = Color::Purple(); break; case 3: - vtx_color = cv::viz::Color::brown(); + vtx_color = Color::Brown(); break; case 4: - vtx_color = cv::viz::Color::pink(); + vtx_color = Color::Pink(); break; } } break; @@ -721,7 +722,7 @@ bool MeshOptimization::pointInTriangle(const cv::Point2f& pt, void MeshOptimization::drawPixelOnImg(const cv::Point2f& pixel, const cv::Mat& img, - const cv::viz::Color& color, + const cv::Scalar& color, const size_t& pixel_size) { // Draw the pixel on the image cv::circle(img, pixel, pixel_size, color, -1); diff --git a/src/playground/EurocPlayground.cpp b/src/playground/EurocPlayground.cpp index a716c8006..ab97eb399 100644 --- a/src/playground/EurocPlayground.cpp +++ b/src/playground/EurocPlayground.cpp @@ -203,7 +203,7 @@ void EurocPlayground::visualizeGtData(const bool& viz_traj, // Depth image contains INFs. We have to remove them: CHECK_EQ(left_frame->img_.type(), CV_8UC1); // for color cv::Mat_ valid_depth = cv::Mat(1, 0, CV_32FC3); - cv::Mat valid_colors = cv::Mat(1, 0, CV_8UC3, cv::viz::Color::red()); + cv::Mat valid_colors = cv::Mat(1, 0, CV_8UC3, Color::Red()); for (int32_t v = 0; v < depth_map.rows; ++v) { for (int32_t u = 0; u < depth_map.cols; ++u) { const cv::Point3f& xyz = depth_map(v, u); diff --git a/tests/testCamera.cpp b/tests/testCamera.cpp index 4ccc55ede..e9996242c 100644 --- a/tests/testCamera.cpp +++ b/tests/testCamera.cpp @@ -112,7 +112,7 @@ class CameraFixture : public ::testing::Test { /** Visualization **/ // void drawPixelOnImg(const cv::Point2f& pixel, // cv::Mat& img, - // const cv::viz::Color& color = cv::viz::Color::red(), + // const cv::Scalar& color = Color::Red(), // const size_t& pixel_size = 5u, // const uint8_t& alpha = 255u) { // // Draw the pixel on the image @@ -123,7 +123,7 @@ class CameraFixture : public ::testing::Test { // void drawPixelsOnImg(const std::vector& pixels, // cv::Mat& img, - // const cv::viz::Color& color = cv::viz::Color::red(), + // const cv::Scalar& color = Color::Red(), // const size_t& pixel_size = 5u, // const uint8_t& alpha = 255u) { // // Draw the pixel on the image diff --git a/tests/testEurocPlayground.cpp b/tests/testEurocPlayground.cpp index 99431aa6c..398185a82 100644 --- a/tests/testEurocPlayground.cpp +++ b/tests/testEurocPlayground.cpp @@ -90,7 +90,7 @@ TEST(TestEurocPlayground, DISABLED_basicEurocPlayground) { cv::circle(mesh_2d_viz, keypoint, 1, - cv::viz::Color::blue(), + Color::Blue(), CV_FILLED, CV_AA, 0); @@ -125,7 +125,7 @@ TEST(TestEurocPlayground, DISABLED_basicEurocPlayground) { // LOG(INFO) << connect; // mesh_opt.draw3dMesh( - // "Mesh 3D before opt", cv::viz::Color::blue(), input.mesh_3d); + // "Mesh 3D before opt", Color::Blue(), input.mesh_3d); input.pcl = ordered_pcl; MeshOptimizationOutput::UniquePtr out_ptr = mesh_opt.spinOnce(input); diff --git a/tests/testOpticalFlowPredictor.cpp b/tests/testOpticalFlowPredictor.cpp index cabba9cd9..88f91f981 100644 --- a/tests/testOpticalFlowPredictor.cpp +++ b/tests/testOpticalFlowPredictor.cpp @@ -20,6 +20,7 @@ #include "kimera-vio/frontend/optical-flow/OpticalFlowPredictor.h" #include "kimera-vio/frontend/optical-flow/OpticalFlowPredictorFactory.h" #include "kimera-vio/pipeline/Pipeline-definitions.h" +#include "kimera-vio/utils/ColorUtils.h" DECLARE_string(test_data_path); DECLARE_bool(display); @@ -152,7 +153,7 @@ class OpticalFlowPredictorFixture : public ::testing::Test { /** Visualization **/ void drawPixelOnImg(const cv::Point2f& pixel, cv::Mat& img, - const cv::viz::Color& color = cv::viz::Color::red(), + const Scalar& color = Color::Red(), const size_t& pixel_size = 5u, const uint8_t& alpha = 255u) { // Draw the pixel on the image @@ -163,7 +164,7 @@ class OpticalFlowPredictorFixture : public ::testing::Test { void drawPixelsOnImg(const std::vector& pixels, cv::Mat& img, - const cv::viz::Color& color = cv::viz::Color::red(), + const Scalar& color = Color::Red(), const size_t& pixel_size = 5u, const uint8_t& alpha = 255u) { // Draw the pixel on the image @@ -176,7 +177,7 @@ class OpticalFlowPredictorFixture : public ::testing::Test { const std::string& id, const cv::Point3f& cam_world_origin, const double& text_thickness = 0.2, - const cv::viz::Color& color = cv::viz::Color::blue(), + const Scalar& color = Color::Blue(), const bool& display_text = false) { CHECK(window_); // Display 3D rays from cam origin to lmks. @@ -191,7 +192,7 @@ class OpticalFlowPredictorFixture : public ::testing::Test { void visualizePointCloud(const std::string& id, const cv::Mat& pointcloud) { CHECK(window_); - cv::viz::WCloud cloud(pointcloud, cv::viz::Color::red()); + cv::viz::WCloud cloud(pointcloud, Color::Red()); cloud.setRenderingProperty(cv::viz::POINT_SIZE, 6); window_->showWidget(id, cloud); } @@ -200,7 +201,7 @@ class OpticalFlowPredictorFixture : public ::testing::Test { const KeypointsCV& predicted_kpts) { if (FLAGS_display) { window_ = std::make_unique(test_name); - window_->setBackgroundColor(cv::viz::Color::white()); + window_->setBackgroundColor(Color::White()); cv::Matx33d K = UtilsOpenCV::gtsamMatrix3ToCvMat(simulated_calib_.K()); cv::Mat cam_1_img = cv::Mat(camera_params_.image_size_, @@ -236,7 +237,7 @@ class OpticalFlowPredictorFixture : public ::testing::Test { "lmk-cam2" + std::to_string(i), cam_2_position, 0.2, - cv::viz::Color::red()); + Color::Red()); pointcloud.push_back(cv::Mat(lmk_cv).reshape(1).t()); } pointcloud = pointcloud.reshape(3, lmks_.size()); @@ -244,16 +245,16 @@ class OpticalFlowPredictorFixture : public ::testing::Test { // Color image 2 with pixel reprojections (the ground-truth) drawPixelsOnImg( - cam_2_kpts_, cam_2_img, cv::viz::Color::green(), 3u, 125u); + cam_2_kpts_, cam_2_img, Color::Green(), 3u, 125u); // Color image 2 with pixel prediction if no prediction is done // Expected result if using NoPredictionOpticalFlow drawPixelsOnImg( - cam_1_kpts_, cam_2_img, cv::viz::Color::brown(), 6u, 125u); + cam_1_kpts_, cam_2_img, Color::Brown(), 6u, 125u); // Show the estimated kpt positions in red and smaller drawPixelsOnImg( - predicted_kpts, cam_2_img, cv::viz::Color::red(), 1u, 125u); + predicted_kpts, cam_2_img, Color::Red(), 1u, 125u); // Camera frustums // cv::viz::WCameraPosition cpw_1_frustum(K, cam_1_img, 2.0); diff --git a/tests/testStereoCamera.cpp b/tests/testStereoCamera.cpp index 42ce0bd9d..42b476d4b 100644 --- a/tests/testStereoCamera.cpp +++ b/tests/testStereoCamera.cpp @@ -126,7 +126,7 @@ class StereoCameraFixture : public ::testing::Test { /** Visualization **/ // void drawPixelOnImg(const cv::Point2f& pixel, // cv::Mat& img, - // const cv::viz::Color& color = cv::viz::Color::red(), + // const Scalar& color = Color::Red(), // const size_t& pixel_size = 5u, // const uint8_t& alpha = 255u) { // // Draw the pixel on the image @@ -137,7 +137,7 @@ class StereoCameraFixture : public ::testing::Test { // void drawPixelsOnImg(const std::vector& pixels, // cv::Mat& img, - // const cv::viz::Color& color = cv::viz::Color::red(), + // const Scalar& color = Color::Red(), // const size_t& pixel_size = 5u, // const uint8_t& alpha = 255u) { // // Draw the pixel on the image diff --git a/tests/testTracker.cpp b/tests/testTracker.cpp index ec8881874..6f8d2d7e6 100644 --- a/tests/testTracker.cpp +++ b/tests/testTracker.cpp @@ -61,7 +61,7 @@ class TestTracker : public ::testing::Test { if (FLAGS_display) { window_ = std::make_unique("Test Tracker"); - window_->setBackgroundColor(cv::viz::Color::black()); + window_->setBackgroundColor(Color::Black()); } } @@ -559,7 +559,7 @@ class TestTracker : public ::testing::Test { /** Visualization **/ void drawPixelOnImg(const cv::Point2f& pixel, cv::Mat& img, - const cv::viz::Color& color = cv::viz::Color::red(), + const cv::Scalar& color = Color::Red(), const size_t& pixel_size = 5u, const uint8_t& alpha = 255u) { // Draw the pixel on the image @@ -570,7 +570,7 @@ class TestTracker : public ::testing::Test { void drawPixelsOnImg(const std::vector& pixels, cv::Mat& img, - const cv::viz::Color& color = cv::viz::Color::red(), + const cv::Scalar& color = Color::Red(), const size_t& pixel_size = 5u, const uint8_t& alpha = 255u) { // Draw the pixel on the image @@ -583,7 +583,7 @@ class TestTracker : public ::testing::Test { const std::string& id, const cv::Point3f& cam_world_origin, const double& text_thickness = 0.2, - const cv::viz::Color& color = cv::viz::Color::blue(), + const cv::Scalar& color = Color::Blue(), const bool& display_text = false) { CHECK(window_); // Display 3D rays from cam origin to lmks. @@ -598,7 +598,7 @@ class TestTracker : public ::testing::Test { void visualizePointCloud(const std::string& id, const cv::Mat& pointcloud) { CHECK(window_); - cv::viz::WCloud cloud(pointcloud, cv::viz::Color::red()); + cv::viz::WCloud cloud(pointcloud, Color::Red()); cloud.setRenderingProperty(cv::viz::POINT_SIZE, 6); window_->showWidget(id, cloud); } @@ -641,7 +641,7 @@ class TestTracker : public ::testing::Test { "lmk-cam2" + std::to_string(i), cam_2_position, 0.2, - cv::viz::Color::red()); + Color::Red()); pointcloud.push_back(cv::Mat(lmk_cv).reshape(1).t()); } pointcloud = pointcloud.reshape(3, lmks.size()); @@ -653,10 +653,10 @@ class TestTracker : public ::testing::Test { camera_2_params.image_size_, CV_8UC3, cv::Scalar(255u, 0u, 0u)); // Color image 1 with pixel projections - drawPixelsOnImg(cam_1_kpts, cam_1_img, cv::viz::Color::brown(), 6u, 225u); + drawPixelsOnImg(cam_1_kpts, cam_1_img, Color::Brown(), 6u, 225u); // Color image 2 with pixel reprojections - drawPixelsOnImg(cam_2_kpts, cam_2_img, cv::viz::Color::green(), 3u, 125u); + drawPixelsOnImg(cam_2_kpts, cam_2_img, Color::Green(), 3u, 125u); cv::imshow("AHA1", cam_1_img); cv::imshow("AHA2", cam_2_img); @@ -665,9 +665,9 @@ class TestTracker : public ::testing::Test { // Camera frustums // cv::viz::WCameraPosition cpw_1_frustum(K, cam_1_img, 2.0); cv::viz::WCameraPosition cpw_1_frustum( - K_1, cam_1_img, 1.0, cv::viz::Color::red()); + K_1, cam_1_img, 1.0, Color::Red()); cv::viz::WCameraPosition cpw_2_frustum( - K_2, cam_2_img, 1.0, cv::viz::Color::green()); + K_2, cam_2_img, 1.0, Color::Green()); window_->showWidget("Cam 1 Frustum", cpw_1_frustum, cam_1_cv_pose); window_->showWidget("Cam 2 Frustum", cpw_2_frustum, cam_2_cv_pose); @@ -1759,7 +1759,7 @@ TEST_F(TestTracker, PnPTracking) { "bearing-cam1" + std::to_string(k++), UtilsOpenCV::gtsamVector3ToCvPoint3(left_cam_pose.translation()), 0.2, - cv::viz::Color::red()); + Color::Red()); } visualizeScene(cam_params_left, From d1875b2af3d59075902cd31cd80d68d5822da81e Mon Sep 17 00:00:00 2001 From: Sarika Ramroop Date: Fri, 6 Dec 2024 15:06:07 -0400 Subject: [PATCH 2/3] Changing Color class to a ColorUtils namespace instead, updating all other changes and fixing some bugs wrt Scalar/Vec3b confusion. Tested defaults; working --- include/kimera-vio/mesh/Mesh.h | 5 ++-- include/kimera-vio/mesh/MeshOptimization.h | 4 +-- include/kimera-vio/utils/ColorUtils.h | 29 ++++++++++++++++++++++ src/frontend/RgbdCamera.cpp | 2 +- src/mesh/Mesh.cpp | 4 +-- src/mesh/MeshOptimization.cpp | 18 ++++++++------ src/playground/EurocPlayground.cpp | 2 +- tests/testEurocPlayground.cpp | 2 +- tests/testOpticalFlowPredictor.cpp | 18 +++++++------- tests/testTracker.cpp | 22 ++++++++-------- 10 files changed, 69 insertions(+), 37 deletions(-) create mode 100644 include/kimera-vio/utils/ColorUtils.h diff --git a/include/kimera-vio/mesh/Mesh.h b/include/kimera-vio/mesh/Mesh.h index a954ade3b..7776c4809 100644 --- a/include/kimera-vio/mesh/Mesh.h +++ b/include/kimera-vio/mesh/Mesh.h @@ -78,11 +78,12 @@ class Mesh { : lmk_id_(-1), vertex_position_(), vertex_normal_(), - vertex_color_(Color::White()) {} + vertex_color_(ColorUtils::ScalarToVec3b(ColorUtils::White())) {} Vertex(const LandmarkId& lmk_id, const VertexPosition& vertex_position, - const VertexColorRGB& vertex_color = cv::Vec3b(Color::White()), + const VertexColorRGB& vertex_color = + ColorUtils::ScalarToVec3b(ColorUtils::White()), const VertexNormal& vertex_normal = VertexNormal()) : lmk_id_(lmk_id), vertex_position_(vertex_position), diff --git a/include/kimera-vio/mesh/MeshOptimization.h b/include/kimera-vio/mesh/MeshOptimization.h index ad87a2aec..c8836e1c2 100644 --- a/include/kimera-vio/mesh/MeshOptimization.h +++ b/include/kimera-vio/mesh/MeshOptimization.h @@ -66,7 +66,7 @@ class MeshOptimization { static void draw2dMeshOnImg( const Mesh2D& mesh_2d, cv::Mat* img, - const cv::Scalar& color = Color::Red(), + const cv::Scalar& color = ColorUtils::Red(), const size_t& thickness = 1u, const int line_type = CV_AA); @@ -149,7 +149,7 @@ class MeshOptimization { void drawPixelOnImg(const cv::Point2f& pixel, const cv::Mat& img, - const cv::Scalar& color = Color::Red(), + const cv::Scalar& color = ColorUtils::Red(), const size_t& pixel_size = 5u); public: diff --git a/include/kimera-vio/utils/ColorUtils.h b/include/kimera-vio/utils/ColorUtils.h new file mode 100644 index 000000000..097473627 --- /dev/null +++ b/include/kimera-vio/utils/ColorUtils.h @@ -0,0 +1,29 @@ +#pragma once + +#include + +namespace VIO { + +/* ColorUtils is used instead of cv::viz::Color in non-visualizer classes to + facilitate building without the visualizer. Color values are taken from + https://github.com/apc-llc/opencv-2.4.10/blob/master/modules/viz/include/opencv2/viz/types.hpp#L191 +*/ +namespace ColorUtils { + inline cv::Scalar Apricot() { return cv::Scalar(177, 206, 251); } + inline cv::Scalar Black() { return cv::Scalar(0, 0, 0); } + inline cv::Scalar Blue() { return cv::Scalar(255, 0, 0); } + inline cv::Scalar Brown() { return cv::Scalar(0, 75, 150); } + inline cv::Scalar Green() { return cv::Scalar(0, 255, 0); } + inline cv::Scalar Pink() { return cv::Scalar(203, 192, 255); } + inline cv::Scalar Purple() { return cv::Scalar(128, 0, 128); } + inline cv::Scalar Red() { return cv::Scalar(0, 0, 255); } + inline cv::Scalar White() { return cv::Scalar(255, 255, 255); } + + inline cv::Vec3b ScalarToVec3b(cv::Scalar value) { + return cv::Vec3b(static_cast(value[0]), + static_cast(value[1]), + static_cast(value[2])); + + } +} //ColorUtils namespace +} // VIO namespace diff --git a/src/frontend/RgbdCamera.cpp b/src/frontend/RgbdCamera.cpp index fcdfa2894..efd86cb9b 100644 --- a/src/frontend/RgbdCamera.cpp +++ b/src/frontend/RgbdCamera.cpp @@ -50,7 +50,7 @@ void convertToPcl(const cv::Mat& intensity_img, cv::Mat_ cloud_out = cv::Mat(img_rows, img_cols, CV_32FC3, cv::Scalar(0.0, 0.0, 0.0)); cv::Mat colors_out = - cv::Mat(img_rows, img_cols, CV_8UC3, Color::Red()); + cv::Mat(img_rows, img_cols, CV_8UC3, ColorUtils::Red()); for (int v = 0u; v < img_rows; ++v) { for (int u = 0u; u < img_cols; ++u) { diff --git a/src/mesh/Mesh.cpp b/src/mesh/Mesh.cpp index 7ebc5816f..6e7150290 100644 --- a/src/mesh/Mesh.cpp +++ b/src/mesh/Mesh.cpp @@ -33,7 +33,7 @@ Mesh::Mesh(const size_t& polygon_dimension) vertices_mesh_(0, 1, CV_32FC3), vertices_mesh_normal_(), normals_computed_(false), - vertices_mesh_color_(0, 0, CV_8UC3, Color::Blue()), + vertices_mesh_color_(0, 0, CV_8UC3, ColorUtils::Blue()), polygons_mesh_(0, 1, CV_32SC1), adjacency_matrix_(1, 1, CV_8UC1, cv::Scalar(0u)), face_hashes_(), @@ -490,7 +490,7 @@ template void Mesh::clearMesh() { vertices_mesh_ = cv::Mat(0, 1, CV_32FC3); vertices_mesh_normal_ = VertexNormals(); - vertices_mesh_color_ = cv::Mat(0, 0, CV_8UC3, Color::Blue()); + vertices_mesh_color_ = cv::Mat(0, 0, CV_8UC3, ColorUtils::Blue()); polygons_mesh_ = cv::Mat(0, 1, CV_32SC1); adjacency_matrix_ = cv::Mat(1, 1, CV_8UC1, cv::Scalar(0u)); face_hashes_.clear(); diff --git a/src/mesh/MeshOptimization.cpp b/src/mesh/MeshOptimization.cpp index 4295a673e..b6db6fc60 100644 --- a/src/mesh/MeshOptimization.cpp +++ b/src/mesh/MeshOptimization.cpp @@ -259,7 +259,7 @@ MeshOptimizationOutput::UniquePtr MeshOptimization::solveOptimalMesh( if (visualizer_) { // Flatten and get colors for pcl cv::Mat viz_cloud(0, 1, CV_32FC3, cv::Scalar(0)); - cv::Mat colors_pcl = cv::Mat(0, 0, CV_8UC3, Color::Red()); + cv::Mat colors_pcl = cv::Mat(0, 0, CV_8UC3, ColorUtils::Red()); CHECK_EQ(img_.type(), CV_8UC1); if (noisy_pcl.rows != 1u || noisy_pcl.cols != 1u) { LOG(ERROR) << "Reshaping noisy_pcl!"; @@ -575,25 +575,25 @@ MeshOptimizationOutput::UniquePtr MeshOptimization::solveOptimalMesh( //! Add new vertex to polygon //! Color with covariance bgr: static constexpr double kScaleStdDeviation = 0.1; - cv::Scalar vtx_color = Color::Black(); + cv::Scalar vtx_color = ColorUtils::Black(); switch (mesh_color_type_) { case MeshColorType::kVertexFlatColor: { // Use color of each pixel where the landmark is switch (mesh_count_ % 5) { case 0: - vtx_color = Color::Red(); + vtx_color = ColorUtils::Red(); break; case 1: - vtx_color = Color::Apricot(); + vtx_color = ColorUtils::Apricot(); break; case 2: - vtx_color = Color::Purple(); + vtx_color = ColorUtils::Purple(); break; case 3: - vtx_color = Color::Brown(); + vtx_color = ColorUtils::Brown(); break; case 4: - vtx_color = Color::Pink(); + vtx_color = ColorUtils::Pink(); break; } } break; @@ -621,7 +621,9 @@ MeshOptimizationOutput::UniquePtr MeshOptimization::solveOptimalMesh( LOG(FATAL) << "Unrecognized mesh color type."; } } - poly_3d.push_back(Mesh3D::VertexType(lmk_id, lmk, vtx_color)); + poly_3d.push_back(Mesh3D::VertexType(lmk_id, lmk, + ColorUtils::ScalarToVec3b( + vtx_color))); } if (add_poly) { reconstructed_mesh.addPolygonToMesh(poly_3d); diff --git a/src/playground/EurocPlayground.cpp b/src/playground/EurocPlayground.cpp index ab97eb399..3910c180b 100644 --- a/src/playground/EurocPlayground.cpp +++ b/src/playground/EurocPlayground.cpp @@ -203,7 +203,7 @@ void EurocPlayground::visualizeGtData(const bool& viz_traj, // Depth image contains INFs. We have to remove them: CHECK_EQ(left_frame->img_.type(), CV_8UC1); // for color cv::Mat_ valid_depth = cv::Mat(1, 0, CV_32FC3); - cv::Mat valid_colors = cv::Mat(1, 0, CV_8UC3, Color::Red()); + cv::Mat valid_colors = cv::Mat(1, 0, CV_8UC3, ColorUtils::Red()); for (int32_t v = 0; v < depth_map.rows; ++v) { for (int32_t u = 0; u < depth_map.cols; ++u) { const cv::Point3f& xyz = depth_map(v, u); diff --git a/tests/testEurocPlayground.cpp b/tests/testEurocPlayground.cpp index 398185a82..4ba16e46d 100644 --- a/tests/testEurocPlayground.cpp +++ b/tests/testEurocPlayground.cpp @@ -90,7 +90,7 @@ TEST(TestEurocPlayground, DISABLED_basicEurocPlayground) { cv::circle(mesh_2d_viz, keypoint, 1, - Color::Blue(), + ColorUtils::Blue(), CV_FILLED, CV_AA, 0); diff --git a/tests/testOpticalFlowPredictor.cpp b/tests/testOpticalFlowPredictor.cpp index 88f91f981..fda1e35a5 100644 --- a/tests/testOpticalFlowPredictor.cpp +++ b/tests/testOpticalFlowPredictor.cpp @@ -153,7 +153,7 @@ class OpticalFlowPredictorFixture : public ::testing::Test { /** Visualization **/ void drawPixelOnImg(const cv::Point2f& pixel, cv::Mat& img, - const Scalar& color = Color::Red(), + const cv::Scalar& color = ColorUtils::Red(), const size_t& pixel_size = 5u, const uint8_t& alpha = 255u) { // Draw the pixel on the image @@ -164,7 +164,7 @@ class OpticalFlowPredictorFixture : public ::testing::Test { void drawPixelsOnImg(const std::vector& pixels, cv::Mat& img, - const Scalar& color = Color::Red(), + const cv::Scalar& color = ColorUtils::Red(), const size_t& pixel_size = 5u, const uint8_t& alpha = 255u) { // Draw the pixel on the image @@ -177,7 +177,7 @@ class OpticalFlowPredictorFixture : public ::testing::Test { const std::string& id, const cv::Point3f& cam_world_origin, const double& text_thickness = 0.2, - const Scalar& color = Color::Blue(), + const cv::Scalar& color = ColorUtils::Blue(), const bool& display_text = false) { CHECK(window_); // Display 3D rays from cam origin to lmks. @@ -192,7 +192,7 @@ class OpticalFlowPredictorFixture : public ::testing::Test { void visualizePointCloud(const std::string& id, const cv::Mat& pointcloud) { CHECK(window_); - cv::viz::WCloud cloud(pointcloud, Color::Red()); + cv::viz::WCloud cloud(pointcloud, cv::viz::Color::red()); cloud.setRenderingProperty(cv::viz::POINT_SIZE, 6); window_->showWidget(id, cloud); } @@ -201,7 +201,7 @@ class OpticalFlowPredictorFixture : public ::testing::Test { const KeypointsCV& predicted_kpts) { if (FLAGS_display) { window_ = std::make_unique(test_name); - window_->setBackgroundColor(Color::White()); + window_->setBackgroundColor(ColorUtils::White()); cv::Matx33d K = UtilsOpenCV::gtsamMatrix3ToCvMat(simulated_calib_.K()); cv::Mat cam_1_img = cv::Mat(camera_params_.image_size_, @@ -237,7 +237,7 @@ class OpticalFlowPredictorFixture : public ::testing::Test { "lmk-cam2" + std::to_string(i), cam_2_position, 0.2, - Color::Red()); + ColorUtils::Red()); pointcloud.push_back(cv::Mat(lmk_cv).reshape(1).t()); } pointcloud = pointcloud.reshape(3, lmks_.size()); @@ -245,16 +245,16 @@ class OpticalFlowPredictorFixture : public ::testing::Test { // Color image 2 with pixel reprojections (the ground-truth) drawPixelsOnImg( - cam_2_kpts_, cam_2_img, Color::Green(), 3u, 125u); + cam_2_kpts_, cam_2_img, ColorUtils::Green(), 3u, 125u); // Color image 2 with pixel prediction if no prediction is done // Expected result if using NoPredictionOpticalFlow drawPixelsOnImg( - cam_1_kpts_, cam_2_img, Color::Brown(), 6u, 125u); + cam_1_kpts_, cam_2_img, ColorUtils::Brown(), 6u, 125u); // Show the estimated kpt positions in red and smaller drawPixelsOnImg( - predicted_kpts, cam_2_img, Color::Red(), 1u, 125u); + predicted_kpts, cam_2_img, ColorUtils::Red(), 1u, 125u); // Camera frustums // cv::viz::WCameraPosition cpw_1_frustum(K, cam_1_img, 2.0); diff --git a/tests/testTracker.cpp b/tests/testTracker.cpp index 6f8d2d7e6..d941efc20 100644 --- a/tests/testTracker.cpp +++ b/tests/testTracker.cpp @@ -61,7 +61,7 @@ class TestTracker : public ::testing::Test { if (FLAGS_display) { window_ = std::make_unique("Test Tracker"); - window_->setBackgroundColor(Color::Black()); + window_->setBackgroundColor(ColorUtils::Black()); } } @@ -559,7 +559,7 @@ class TestTracker : public ::testing::Test { /** Visualization **/ void drawPixelOnImg(const cv::Point2f& pixel, cv::Mat& img, - const cv::Scalar& color = Color::Red(), + const cv::Scalar& color = ColorUtils::Red(), const size_t& pixel_size = 5u, const uint8_t& alpha = 255u) { // Draw the pixel on the image @@ -570,7 +570,7 @@ class TestTracker : public ::testing::Test { void drawPixelsOnImg(const std::vector& pixels, cv::Mat& img, - const cv::Scalar& color = Color::Red(), + const cv::Scalar& color = ColorUtils::Red(), const size_t& pixel_size = 5u, const uint8_t& alpha = 255u) { // Draw the pixel on the image @@ -583,7 +583,7 @@ class TestTracker : public ::testing::Test { const std::string& id, const cv::Point3f& cam_world_origin, const double& text_thickness = 0.2, - const cv::Scalar& color = Color::Blue(), + const cv::Scalar& color = ColorUtils::Blue(), const bool& display_text = false) { CHECK(window_); // Display 3D rays from cam origin to lmks. @@ -598,7 +598,7 @@ class TestTracker : public ::testing::Test { void visualizePointCloud(const std::string& id, const cv::Mat& pointcloud) { CHECK(window_); - cv::viz::WCloud cloud(pointcloud, Color::Red()); + cv::viz::WCloud cloud(pointcloud, cv::viz::Color::red()); cloud.setRenderingProperty(cv::viz::POINT_SIZE, 6); window_->showWidget(id, cloud); } @@ -641,7 +641,7 @@ class TestTracker : public ::testing::Test { "lmk-cam2" + std::to_string(i), cam_2_position, 0.2, - Color::Red()); + ColorUtils::Red()); pointcloud.push_back(cv::Mat(lmk_cv).reshape(1).t()); } pointcloud = pointcloud.reshape(3, lmks.size()); @@ -653,10 +653,10 @@ class TestTracker : public ::testing::Test { camera_2_params.image_size_, CV_8UC3, cv::Scalar(255u, 0u, 0u)); // Color image 1 with pixel projections - drawPixelsOnImg(cam_1_kpts, cam_1_img, Color::Brown(), 6u, 225u); + drawPixelsOnImg(cam_1_kpts, cam_1_img, ColorUtils::Brown(), 6u, 225u); // Color image 2 with pixel reprojections - drawPixelsOnImg(cam_2_kpts, cam_2_img, Color::Green(), 3u, 125u); + drawPixelsOnImg(cam_2_kpts, cam_2_img, ColorUtils::Green(), 3u, 125u); cv::imshow("AHA1", cam_1_img); cv::imshow("AHA2", cam_2_img); @@ -665,9 +665,9 @@ class TestTracker : public ::testing::Test { // Camera frustums // cv::viz::WCameraPosition cpw_1_frustum(K, cam_1_img, 2.0); cv::viz::WCameraPosition cpw_1_frustum( - K_1, cam_1_img, 1.0, Color::Red()); + K_1, cam_1_img, 1.0, ColorUtils::Red()); cv::viz::WCameraPosition cpw_2_frustum( - K_2, cam_2_img, 1.0, Color::Green()); + K_2, cam_2_img, 1.0, ColorUtils::Green()); window_->showWidget("Cam 1 Frustum", cpw_1_frustum, cam_1_cv_pose); window_->showWidget("Cam 2 Frustum", cpw_2_frustum, cam_2_cv_pose); @@ -1759,7 +1759,7 @@ TEST_F(TestTracker, PnPTracking) { "bearing-cam1" + std::to_string(k++), UtilsOpenCV::gtsamVector3ToCvPoint3(left_cam_pose.translation()), 0.2, - Color::Red()); + ColorUtils::Red()); } visualizeScene(cam_params_left, From 11db1d2c17fdcd62612a1982b159c28101d02b01 Mon Sep 17 00:00:00 2001 From: Sarika Ramroop Date: Mon, 9 Dec 2024 15:08:23 -0400 Subject: [PATCH 3/3] Updating commented out code blocks to use ColorUtils as well, in case this is uncommented in future --- tests/testCamera.cpp | 4 ++-- tests/testEurocPlayground.cpp | 2 +- tests/testStereoCamera.cpp | 4 ++-- 3 files changed, 5 insertions(+), 5 deletions(-) diff --git a/tests/testCamera.cpp b/tests/testCamera.cpp index e9996242c..d0235d6c5 100644 --- a/tests/testCamera.cpp +++ b/tests/testCamera.cpp @@ -112,7 +112,7 @@ class CameraFixture : public ::testing::Test { /** Visualization **/ // void drawPixelOnImg(const cv::Point2f& pixel, // cv::Mat& img, - // const cv::Scalar& color = Color::Red(), + // const cv::Scalar& color = ColorUtils::Red(), // const size_t& pixel_size = 5u, // const uint8_t& alpha = 255u) { // // Draw the pixel on the image @@ -123,7 +123,7 @@ class CameraFixture : public ::testing::Test { // void drawPixelsOnImg(const std::vector& pixels, // cv::Mat& img, - // const cv::Scalar& color = Color::Red(), + // const cv::Scalar& color = ColorUtils::Red(), // const size_t& pixel_size = 5u, // const uint8_t& alpha = 255u) { // // Draw the pixel on the image diff --git a/tests/testEurocPlayground.cpp b/tests/testEurocPlayground.cpp index 4ba16e46d..9e9e1da1d 100644 --- a/tests/testEurocPlayground.cpp +++ b/tests/testEurocPlayground.cpp @@ -125,7 +125,7 @@ TEST(TestEurocPlayground, DISABLED_basicEurocPlayground) { // LOG(INFO) << connect; // mesh_opt.draw3dMesh( - // "Mesh 3D before opt", Color::Blue(), input.mesh_3d); + // "Mesh 3D before opt", ColorUtils::Blue(), input.mesh_3d); input.pcl = ordered_pcl; MeshOptimizationOutput::UniquePtr out_ptr = mesh_opt.spinOnce(input); diff --git a/tests/testStereoCamera.cpp b/tests/testStereoCamera.cpp index 42b476d4b..195f35da1 100644 --- a/tests/testStereoCamera.cpp +++ b/tests/testStereoCamera.cpp @@ -126,7 +126,7 @@ class StereoCameraFixture : public ::testing::Test { /** Visualization **/ // void drawPixelOnImg(const cv::Point2f& pixel, // cv::Mat& img, - // const Scalar& color = Color::Red(), + // const Scalar& color = ColorUtils::Red(), // const size_t& pixel_size = 5u, // const uint8_t& alpha = 255u) { // // Draw the pixel on the image @@ -137,7 +137,7 @@ class StereoCameraFixture : public ::testing::Test { // void drawPixelsOnImg(const std::vector& pixels, // cv::Mat& img, - // const Scalar& color = Color::Red(), + // const Scalar& color = ColorUtils::Red(), // const size_t& pixel_size = 5u, // const uint8_t& alpha = 255u) { // // Draw the pixel on the image