PR2: Adding ColorUtils namespace with functions to replace cv::viz::color calls in non-visualizer classes.

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 {

include/kimera-vio/mesh/Mesh.h

@@ -20,9 +20,9 @@
 #include <map>
 #include <opencv2/core/core.hpp>
 #include <opencv2/core/mat.hpp>
-#include <opencv2/viz/types.hpp>  // Just for color type.
 #include <vector>
 
+#include "kimera-vio/utils/ColorUtils.h"
 #include "kimera-vio/utils/Macros.h"
 #include "kimera-vio/utils/UtilsOpenCV.h"
 
@@ -78,11 +78,12 @@ class Mesh {
         : lmk_id_(-1),
           vertex_position_(),
           vertex_normal_(),
-          vertex_color_(cv::viz::Color::white()) {}
+          vertex_color_(ColorUtils::ScalarToVec3b(ColorUtils::White())) {}
 
     Vertex(const LandmarkId& lmk_id,
            const VertexPosition& vertex_position,
-           const VertexColorRGB& vertex_color = cv::viz::Color::white(),
+           const VertexColorRGB& vertex_color = 
+            ColorUtils::ScalarToVec3b(ColorUtils::White()),
            const VertexNormal& vertex_normal = VertexNormal())
         : lmk_id_(lmk_id),
           vertex_position_(vertex_position),

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 = ColorUtils::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 = ColorUtils::Red(),
                       const size_t& pixel_size = 5u);
 
  public:

include/kimera-vio/mesh/MeshUtils.h

@@ -18,7 +18,7 @@
 
 #include <glog/logging.h>
 
-#include <opencv2/viz.hpp>
+#include <opencv2/core/core.hpp>
 
 #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);
 }
 

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"

include/kimera-vio/utils/ColorUtils.h

@@ -0,0 +1,29 @@
+#pragma once
+
+#include <opencv2/core/core.hpp>
+
+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<uchar>(value[0]), 
+                     static_cast<uchar>(value[1]),
+                     static_cast<uchar>(value[2]));
+
+  }
+} //ColorUtils namespace
+} // VIO namespace

src/frontend/RgbdCamera.cpp

@@ -50,7 +50,7 @@ void convertToPcl(const cv::Mat& intensity_img,
   cv::Mat_<cv::Point3f> 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, ColorUtils::Red());
 
   for (int v = 0u; v < img_rows; ++v) {
     for (int u = 0u; u < img_cols; ++u) {

src/mesh/Mesh.cpp

@@ -33,7 +33,7 @@ Mesh<VertexPositionType>::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, ColorUtils::Blue()),
       polygons_mesh_(0, 1, CV_32SC1),
       adjacency_matrix_(1, 1, CV_8UC1, cv::Scalar(0u)),
       face_hashes_(),
@@ -490,7 +490,7 @@ template <typename VertexPositionType>
 void Mesh<VertexPositionType>::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, ColorUtils::Blue());
   polygons_mesh_ = cv::Mat(0, 1, CV_32SC1);
   adjacency_matrix_ = cv::Mat(1, 1, CV_8UC1, cv::Scalar(0u));
   face_hashes_.clear();

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<double>(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, ColorUtils::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 = 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 = cv::viz::Color::red();
+                  vtx_color = ColorUtils::Red();
                   break;
                 case 1:
-                  vtx_color = cv::viz::Color::apricot();
+                  vtx_color = ColorUtils::Apricot();
                   break;
                 case 2:
-                  vtx_color = cv::viz::Color::purple();
+                  vtx_color = ColorUtils::Purple();
                   break;
                 case 3:
-                  vtx_color = cv::viz::Color::brown();
+                  vtx_color = ColorUtils::Brown();
                   break;
                 case 4:
-                  vtx_color = cv::viz::Color::pink();
+                  vtx_color = ColorUtils::Pink();
                   break;
               }
             } break;
@@ -620,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);
@@ -721,7 +724,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);

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_<cv::Point3f> 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, 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);

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 = 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<cv::Point2f>& pixels,
   //                      cv::Mat& img,
-  //                      const cv::viz::Color& color = cv::viz::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

tests/testEurocPlayground.cpp

@@ -90,7 +90,7 @@ TEST(TestEurocPlayground, DISABLED_basicEurocPlayground) {
         cv::circle(mesh_2d_viz,
                    keypoint,
                    1,
-                   cv::viz::Color::blue(),
+                   ColorUtils::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", ColorUtils::Blue(), input.mesh_3d);
       input.pcl = ordered_pcl;
 
       MeshOptimizationOutput::UniquePtr out_ptr = mesh_opt.spinOnce(input);

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 cv::Scalar& color = ColorUtils::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<cv::Point2f>& pixels,
                        cv::Mat& img,
-                       const cv::viz::Color& color = cv::viz::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
@@ -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 cv::Scalar& color = ColorUtils::Blue(),
                     const bool& display_text = false) {
     CHECK(window_);
     // Display 3D rays from cam origin to lmks.
@@ -200,7 +201,7 @@ class OpticalFlowPredictorFixture : public ::testing::Test {
                       const KeypointsCV& predicted_kpts) {
     if (FLAGS_display) {
       window_ = std::make_unique<cv::viz::Viz3d>(test_name);
-      window_->setBackgroundColor(cv::viz::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_,
@@ -236,24 +237,24 @@ class OpticalFlowPredictorFixture : public ::testing::Test {
                      "lmk-cam2" + std::to_string(i),
                      cam_2_position,
                      0.2,
-                     cv::viz::Color::red());
+                     ColorUtils::Red());
         pointcloud.push_back(cv::Mat(lmk_cv).reshape(1).t());
       }
       pointcloud = pointcloud.reshape(3, lmks_.size());
       visualizePointCloud("Scene Landmarks", pointcloud);
 
       // 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, 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, cv::viz::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, cv::viz::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);

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 = 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<cv::Point2f>& pixels,
   //                      cv::Mat& img,
-  //                      const cv::viz::Color& color = cv::viz::Color::red(),
+  //                      const Scalar& color = ColorUtils::Red(),
   //                      const size_t& pixel_size = 5u,
   //                      const uint8_t& alpha = 255u) {
   //   // Draw the pixel on the image