From 5044546186e43a7e7abdca5b0d0729920136b270 Mon Sep 17 00:00:00 2001
From: mason turner <legostrat@gmail.com>
Date: Wed, 3 Jul 2019 20:30:40 +0000
Subject: [PATCH] Convert to grayscale

---
 camera/camera_balsaq.cc        | 22 ++++++++++++++--------
 camera/camera_image_message.cc |  4 ++--
 2 files changed, 16 insertions(+), 10 deletions(-)

diff --git a/camera/camera_balsaq.cc b/camera/camera_balsaq.cc
index a6d9aba..8786d8d 100644
--- a/camera/camera_balsaq.cc
+++ b/camera/camera_balsaq.cc
@@ -67,10 +67,10 @@ void CameraBq::init(const Config& config) {
 }
 
 void CameraBq::loop() {
-  cv::Mat camera_frame;
+  cv::Mat camera_frame_rgb;
   std::cout << "Camera BQ: trying to get a frame" << std::endl;
 
-  if (cap_.read(camera_frame)) {
+  if (cap_.read(camera_frame_rgb)) {
     // Get the image capture timestamp, which is the time that the first byte was captured, as returned by clock_gettime(). 
     const long int cap_time_msec = cap_.get(cv::CAP_PROP_POS_MSEC);
     const Timestamp cap_time_vehicle = msec_monotonic_to_vehicle_monotonic(cap_time_msec);
@@ -79,15 +79,21 @@ void CameraBq::loop() {
 
     // Pack a message
     CameraImageMessage message;
-    const std::size_t n_elements = camera_frame.rows * camera_frame.cols * 3u;
-    message.image_data.resize(n_elements);
+    cv::Mat image_gray;
+    cv::cvtColor(camera_frame_rgb, image_gray, cv::COLOR_BGR2GRAY);
+
+    constexpr uint8_t IMAGE_DEPTH = 1u;
+    const std::size_t n_elements = image_gray.rows * image_gray.cols * IMAGE_DEPTH;
     constexpr std::size_t SIZE_OF_UCHAR = sizeof(uint8_t);
-    if (camera_frame.isContinuous()) {
-      std::memcpy(message.image_data.data(), camera_frame.data, SIZE_OF_UCHAR * n_elements);
+    message.image_data.resize(n_elements);
+    if (image_gray.isContinuous()) {
+      std::memcpy(message.image_data.data(), image_gray.data, SIZE_OF_UCHAR * n_elements);
+    } else {
+      std::cerr << "Cannot possibly make good frames" << std::endl;
     }
     message.timestamp = cap_time_vehicle;
-    message.height = camera_frame.size().height;
-    message.width = camera_frame.size().width;
+    message.height = image_gray.size().height;
+    message.width = image_gray.size().width;
     message.camera_serial_number = camera_serial_number_;
     publisher_->publish(message);
     std::cout << "Camera BQ: publishes a camera frame " << message.width << " " << message.height << std::endl;
diff --git a/camera/camera_image_message.cc b/camera/camera_image_message.cc
index 2d6852b..aa24544 100644
--- a/camera/camera_image_message.cc
+++ b/camera/camera_image_message.cc
@@ -3,8 +3,8 @@
 namespace jet {
 
 cv::Mat get_image_mat(const CameraImageMessage& message) {
-  cv::Mat result = cv::Mat(message.height, message.width, CV_8UC3);
-  const std::size_t n_elements = message.height * message.width * 3u;
+  cv::Mat result = cv::Mat(message.height, message.width, CV_8UC1);
+  const std::size_t n_elements = message.height * message.width * 1u;
   constexpr std::size_t SIZE_OF_UCHAR = sizeof(uint8_t);
   std::memcpy(result.data, message.image_data.data(),
               SIZE_OF_UCHAR * n_elements);