Make intrinsic calib plugin independent of greyscale plugin

Author: g3force

src/app/plugins/plugin_camera_intrinsic_calib.cpp

@@ -1,15 +1,18 @@
 #include "plugin_camera_intrinsic_calib.h"
+
 #include <dirent.h>
-#include <iostream>
+
 #include <chrono>
+#include <iostream>
 
-PluginCameraIntrinsicCalibration::PluginCameraIntrinsicCalibration(
-    FrameBuffer *buffer, CameraParameters &_camera_params)
+#include "conversions_greyscale.h"
+
+PluginCameraIntrinsicCalibration::PluginCameraIntrinsicCalibration(FrameBuffer *buffer,
+                                                                   CameraParameters &_camera_params)
     : VisionPlugin(buffer),
       settings(new VarList("Camera Intrinsic Calibration")),
       widget(new CameraIntrinsicCalibrationWidget(_camera_params)),
       camera_params(_camera_params) {
-
   worker = new PluginCameraIntrinsicCalibrationWorker(_camera_params, widget);
 
   chessboard_capture_dt = new VarDouble("chessboard capture dT", 0.2);
@@ -40,42 +43,30 @@ PluginCameraIntrinsicCalibration::~PluginCameraIntrinsicCalibration() {
   delete chessboard_capture_dt;
 }
 
-VarList *PluginCameraIntrinsicCalibration::getSettings() {
-  return settings.get();
-}
+VarList *PluginCameraIntrinsicCalibration::getSettings() { return settings.get(); }
 
-std::string PluginCameraIntrinsicCalibration::getName() {
-  return "Camera Intrinsic Calibration";
-}
+std::string PluginCameraIntrinsicCalibration::getName() { return "Camera Intrinsic Calibration"; }
 
-QWidget *PluginCameraIntrinsicCalibration::getControlWidget() {
-  return static_cast<QWidget *>(worker->widget);
-}
+QWidget *PluginCameraIntrinsicCalibration::getControlWidget() { return static_cast<QWidget *>(worker->widget); }
 
-ProcessResult
-PluginCameraIntrinsicCalibration::process(FrameData *data,
-                                          RenderOptions *options) {
+ProcessResult PluginCameraIntrinsicCalibration::process(FrameData *data, RenderOptions *options) {
   (void)options;
 
-  Image<raw8> *img_greyscale;
-  if ((img_greyscale = reinterpret_cast<Image<raw8> *>(
-           data->map.get("greyscale"))) == nullptr) {
-    std::cerr << "Cannot run camera intrinsic calibration. Greyscale image is "
-                 "not available.\n";
-    return ProcessingFailed;
+  Image<raw8> *img_calibration;
+  if ((img_calibration = reinterpret_cast<Image<raw8> *>(data->map.get("img_calibration"))) == nullptr) {
+    img_calibration = reinterpret_cast<Image<raw8> *>(data->map.insert("img_calibration", new Image<raw8>()));
   }
 
+  ConversionsGreyscale::cvColor2Grey(data->video, img_calibration);
+
   Chessboard *chessboard;
-  if ((chessboard = reinterpret_cast<Chessboard *>(
-           data->map.get("chessboard"))) == nullptr) {
-    chessboard = reinterpret_cast<Chessboard *>(
-        data->map.insert("chessboard", new Chessboard()));
+  if ((chessboard = reinterpret_cast<Chessboard *>(data->map.get("chessboard"))) == nullptr) {
+    chessboard = reinterpret_cast<Chessboard *>(data->map.insert("chessboard", new Chessboard()));
   }
 
   // cv expects row major order and image stores col major.
   // height and width are swapped intentionally!
-  cv::Mat greyscale_mat(img_greyscale->getHeight(), img_greyscale->getWidth(),
-                        CV_8UC1, img_greyscale->getData());
+  cv::Mat greyscale_mat(img_calibration->getHeight(), img_calibration->getWidth(), CV_8UC1, img_calibration->getData());
   worker->imageSize = greyscale_mat.size();
 
   if (widget->should_load_images) {
@@ -88,7 +79,6 @@ PluginCameraIntrinsicCalibration::process(FrameData *data,
   }
 
   if (widget->isCapturing() && chessboard->pattern_was_found) {
-
     double captureDiff = data->video.getTime() - lastChessboardCaptureFrame;
     if (captureDiff < chessboard_capture_dt->getDouble()) {
       return ProcessingOk;
@@ -105,7 +95,7 @@ PluginCameraIntrinsicCalibration::process(FrameData *data,
     }
   }
 
-  if(widget->should_calibrate) {
+  if (widget->should_calibrate) {
     widget->should_calibrate = false;
     emit startCalibration();
   }
@@ -118,10 +108,9 @@ PluginCameraIntrinsicCalibration::process(FrameData *data,
   return ProcessingOk;
 }
 
-PluginCameraIntrinsicCalibrationWorker::PluginCameraIntrinsicCalibrationWorker(
-    CameraParameters &_camera_params, CameraIntrinsicCalibrationWidget *widget)
+PluginCameraIntrinsicCalibrationWorker::PluginCameraIntrinsicCalibrationWorker(CameraParameters &_camera_params,
+                                                                               CameraIntrinsicCalibrationWidget *widget)
     : widget(widget), camera_params(_camera_params) {
-
   corner_sub_pixel_windows_size = new VarInt("window size", 5, 1);
   corner_sub_pixel_max_iterations = new VarInt("max iterations", 30, 1);
   corner_sub_pixel_epsilon = new VarDouble("epsilon", 0.1, 1e-10);
@@ -139,8 +128,7 @@ PluginCameraIntrinsicCalibrationWorker::PluginCameraIntrinsicCalibrationWorker(
   thread->start();
 }
 
-PluginCameraIntrinsicCalibrationWorker::
-    ~PluginCameraIntrinsicCalibrationWorker() {
+PluginCameraIntrinsicCalibrationWorker::~PluginCameraIntrinsicCalibrationWorker() {
   thread->quit();
   thread->deleteLater();
 
@@ -156,7 +144,6 @@ PluginCameraIntrinsicCalibrationWorker::
 }
 
 void PluginCameraIntrinsicCalibrationWorker::calibrate() {
-
   calib_mutex.lock();
   widget->calibrating = true;
   widget->updateConfigurationEnabled();
@@ -174,15 +161,14 @@ void PluginCameraIntrinsicCalibrationWorker::calibrate() {
   if (!initializeCameraMatrix->getBool()) {
     flags |= cv::CALIB_USE_INTRINSIC_GUESS;
   }
-  
+
   try {
     std::cout << "Start calibrating with " << image_points.size() << " samples" << std::endl;
     auto start = std::chrono::high_resolution_clock::now();
 
-    double rms = cv::calibrateCamera(object_points, image_points, imageSize,
-                              camera_params.intrinsic_parameters->camera_mat,
-                              camera_params.intrinsic_parameters->dist_coeffs,
-                              rvecs, tvecs, flags);
+    double rms =
+        cv::calibrateCamera(object_points, image_points, imageSize, camera_params.intrinsic_parameters->camera_mat,
+                            camera_params.intrinsic_parameters->dist_coeffs, rvecs, tvecs, flags);
 
     auto finish = std::chrono::high_resolution_clock::now();
     std::chrono::duration<double> elapsed = finish - start;
@@ -199,19 +185,18 @@ void PluginCameraIntrinsicCalibrationWorker::calibrate() {
   calib_mutex.unlock();
 }
 
-bool PluginCameraIntrinsicCalibrationWorker::addChessboard(
-    const Chessboard *chessboard) {
+bool PluginCameraIntrinsicCalibrationWorker::addChessboard(const Chessboard *chessboard) {
   calib_mutex.lock();
 
   // Check if there is a similar sample already
-  for(const auto& img_points : this->image_points) {
+  for (const auto &img_points : this->image_points) {
     double sq_diff_sum = 0;
-    for(uint i = 0; i < img_points.size(); i++) {
+    for (uint i = 0; i < img_points.size(); i++) {
       double diff = cv::norm(img_points[i] - chessboard->corners[i]);
-      sq_diff_sum += diff*diff;
+      sq_diff_sum += diff * diff;
     }
-    double sq_diff = sq_diff_sum / img_points.size();
-    if(sq_diff < this->corner_diff_sq_threshold->getDouble()) {
+    double sq_diff = sq_diff_sum / (double) img_points.size();
+    if (sq_diff < this->corner_diff_sq_threshold->getDouble()) {
       calib_mutex.unlock();
       return false;
     }
@@ -227,65 +212,51 @@ bool PluginCameraIntrinsicCalibrationWorker::addChessboard(
   }
   this->object_points.push_back(obj);
 
-  this->widget->setNumDataPoints(this->object_points.size());
+  this->widget->setNumDataPoints((int) this->object_points.size());
   calib_mutex.unlock();
   return true;
 }
 
-void PluginCameraIntrinsicCalibrationWorker::detectChessboard(
-    const cv::Mat &greyscale_mat, Chessboard *chessboard) const {
-  chessboard->pattern_size.height =
-      this->camera_params.additional_calibration_information->grid_height
-          ->getDouble();
-  chessboard->pattern_size.width =
-      this->camera_params.additional_calibration_information->grid_width
-          ->getDouble();
+void PluginCameraIntrinsicCalibrationWorker::detectChessboard(const cv::Mat &greyscale_mat,
+                                                              Chessboard *chessboard) const {
+  chessboard->pattern_size.height = this->camera_params.additional_calibration_information->grid_height->getDouble();
+  chessboard->pattern_size.width = this->camera_params.additional_calibration_information->grid_width->getDouble();
   chessboard->corners.clear();
 
   cv::Mat greyscale_mat_low_res;
   double scale_factor = min(1.0, reduced_image_width->getDouble() / greyscale_mat.size().width);
-  cv::resize(greyscale_mat, greyscale_mat_low_res, cv::Size(), scale_factor,
-             scale_factor);
+  cv::resize(greyscale_mat, greyscale_mat_low_res, cv::Size(), scale_factor, scale_factor);
 
   std::vector<cv::Point2f> corners_low_res;
-  chessboard->pattern_was_found = this->findPattern(
-      greyscale_mat_low_res, chessboard->pattern_size, corners_low_res);
+  chessboard->pattern_was_found = this->findPattern(greyscale_mat_low_res, chessboard->pattern_size, corners_low_res);
 
   for (auto &corner : corners_low_res) {
-    chessboard->corners.push_back(
-        cv::Point(corner.x / scale_factor, corner.y / scale_factor));
+    chessboard->corners.push_back(cv::Point(corner.x / scale_factor, corner.y / scale_factor));
   }
 
-  if (chessboard->pattern_was_found &&
-      this->widget->cornerSubPixCorrectionEnabled()) {
+  if (chessboard->pattern_was_found && this->widget->cornerSubPixCorrectionEnabled()) {
     cv::cornerSubPix(
         greyscale_mat, chessboard->corners,
-        cv::Size(corner_sub_pixel_windows_size->getInt(),
-                 corner_sub_pixel_windows_size->getInt()),
-        cv::Size(-1, -1),
-        cv::TermCriteria(cv::TermCriteria::EPS | cv::TermCriteria::MAX_ITER,
-                         corner_sub_pixel_max_iterations->getInt(),
+        cv::Size(corner_sub_pixel_windows_size->getInt(), corner_sub_pixel_windows_size->getInt()), cv::Size(-1, -1),
+        cv::TermCriteria(cv::TermCriteria::EPS | cv::TermCriteria::MAX_ITER, corner_sub_pixel_max_iterations->getInt(),
                          corner_sub_pixel_epsilon->getDouble()));
   }
 }
 
-bool PluginCameraIntrinsicCalibrationWorker::findPattern(
-    const cv::Mat &image, const cv::Size &pattern_size,
-    vector<cv::Point2f> &corners) const {
+bool PluginCameraIntrinsicCalibrationWorker::findPattern(const cv::Mat &image, const cv::Size &pattern_size,
+                                                         vector<cv::Point2f> &corners) const {
   using Pattern = CameraIntrinsicCalibrationWidget::Pattern;
-  int cb_flags = cv::CALIB_CB_ADAPTIVE_THRESH + cv::CALIB_CB_FAST_CHECK +
-                 cv::CALIB_CB_NORMALIZE_IMAGE;
+  int cb_flags = cv::CALIB_CB_ADAPTIVE_THRESH + cv::CALIB_CB_FAST_CHECK + cv::CALIB_CB_NORMALIZE_IMAGE;
 
   switch (widget->getPattern()) {
-  case Pattern::CHECKERBOARD:
-    return cv::findChessboardCorners(image, pattern_size, corners, cb_flags);
-  case Pattern::CIRCLES:
-    return cv::findCirclesGrid(image, pattern_size, corners);
-  case Pattern::ASYMMETRIC_CIRCLES:
-    return cv::findCirclesGrid(image, pattern_size, corners,
-                               cv::CALIB_CB_ASYMMETRIC_GRID);
-  default:
-    return false;
+    case Pattern::CHECKERBOARD:
+      return cv::findChessboardCorners(image, pattern_size, corners, cb_flags);
+    case Pattern::CIRCLES:
+      return cv::findCirclesGrid(image, pattern_size, corners);
+    case Pattern::ASYMMETRIC_CIRCLES:
+      return cv::findCirclesGrid(image, pattern_size, corners, cv::CALIB_CB_ASYMMETRIC_GRID);
+    default:
+      return false;
   }
 }
 
@@ -299,10 +270,10 @@ void PluginCameraIntrinsicCalibrationWorker::loadImages() {
     detectChessboard(mat, &image_chessboard);
     if (image_chessboard.pattern_was_found) {
       bool added = addChessboard(&image_chessboard);
-      if(added) {
-        std::cout << "Added chessboard" <<std::endl;
+      if (added) {
+        std::cout << "Added chessboard" << std::endl;
       } else {
-        std::cout << "Filtered chessboard" <<std::endl;
+        std::cout << "Filtered chessboard" << std::endl;
       }
     } else {
       std::cout << "No chessboard detected" << std::endl;
@@ -326,10 +297,8 @@ void PluginCameraIntrinsicCalibrationWorker::clearData() {
   calib_mutex.unlock();
 }
 
-ImageStorage::ImageStorage(CameraIntrinsicCalibrationWidget *widget)
-    : widget(widget) {
-  image_dir =
-      new VarString("pattern image dir", "test-data/intrinsic_calibration");
+ImageStorage::ImageStorage(CameraIntrinsicCalibrationWidget *widget) : widget(widget) {
+  image_dir = new VarString("pattern image dir", "test-data/intrinsic_calibration");
 
   thread = new QThread();
   thread->setObjectName("IntrinsicCalibrationImageStorage");
@@ -344,7 +313,6 @@ ImageStorage::~ImageStorage() {
 }
 
 void ImageStorage::saveImages() {
-
   image_save_mutex.lock();
   if (images_to_save.empty()) {
     image_save_mutex.unlock();
@@ -357,26 +325,24 @@ void ImageStorage::saveImages() {
   }
 }
 
-void ImageStorage::saveImage(cv::Mat& image) const {
+void ImageStorage::saveImage(cv::Mat &image) const {
   long t_now = (long)(GetTimeSec() * 1e9);
   QString num = QString::number(t_now);
-  QString filename =
-      QString(image_dir->getString().c_str()) + "/" + num + ".png";
+  QString filename = QString(image_dir->getString().c_str()) + "/" + num + ".png";
   cv::imwrite(filename.toStdString(), image);
 }
 
 void ImageStorage::readImages(std::vector<cv::Mat> &images) const {
   DIR *dp;
   if ((dp = opendir(image_dir->getString().c_str())) == nullptr) {
-    std::cerr << "Failed to open directory: " << image_dir->getString()
-              << std::endl;
+    std::cerr << "Failed to open directory: " << image_dir->getString() << std::endl;
     return;
   }
   struct dirent *dirp;
   std::list<std::string> imgs_to_load(0);
   while ((dirp = readdir(dp))) {
     std::string file_name(dirp->d_name);
-    if (file_name[0] != '.') { // not a hidden file or one of '..' or '.'
+    if (file_name[0] != '.') {  // not a hidden file or one of '..' or '.'
       imgs_to_load.push_back(image_dir->getString() + "/" + file_name);
     }
   }

src/shared/CMakeLists.txt.inc

@@ -36,6 +36,7 @@ set (SHARED_SRCS
 	${shared_dir}/util/camera_calibration.cpp
 	${shared_dir}/util/camera_parameters.cpp
 	${shared_dir}/util/conversions.cpp
+	${shared_dir}/util/conversions_greyscale.cpp
 	${shared_dir}/util/global_random.cpp
 	${shared_dir}/util/image.cpp
 	${shared_dir}/util/image_io.cpp