Add option to use OpenCV calibration

gui/resources/resources.qrc

@@ -3,5 +3,6 @@
         <file>icons/icon.jpg</file>
         <file>icons/frame_target.png</file>
         <file>icons/frame_camera.png</file>
+        <file>icons/opencv.svg</file>
     </qresource>
 </RCC>

gui/src/camera_intrinsic_calibration_widget.cpp

@@ -50,6 +50,77 @@ static void error(QTreeWidgetItem* item, const QString& message)
 
 namespace industrial_calibration
 {
+static CameraIntrinsicResult optimizeOpenCV(const CameraIntrinsicProblem& params, const cv::Size& image_size)
+{
+  std::vector<std::vector<cv::Vec3f>> object_points;
+  std::vector<std::vector<cv::Vec2f>> image_points;
+
+  for (const auto& o : params.image_observations)
+  {
+    std::vector<cv::Vec3f> op;
+    std::vector<cv::Vec2f> ip;
+
+    for (const auto& pair : o)
+    {
+      op.push_back(cv::Vec3f(pair.in_target(0), pair.in_target(1), pair.in_target(2)));
+      ip.push_back(cv::Vec2f(pair.in_image(0), pair.in_image(1)));
+    }
+
+    object_points.push_back(op);
+    image_points.push_back(ip);
+  }
+
+  // Convert intrinsics to OpenCV
+  cv::Mat camera_matrix(3, 3, cv::DataType<double>::type);
+  cv::setIdentity(camera_matrix);
+  camera_matrix.at<double>(0, 0) = params.intrinsics_guess.fx();
+  camera_matrix.at<double>(1, 1) = params.intrinsics_guess.fy();
+  camera_matrix.at<double>(0, 2) = params.intrinsics_guess.cx();
+  camera_matrix.at<double>(1, 2) = params.intrinsics_guess.cy();
+
+  cv::Mat dist_coeffs;
+  std::vector<cv::Mat> rvecs;
+  std::vector<cv::Mat> tvecs;
+  double rms_error =
+      cv::calibrateCamera(object_points, image_points, image_size, camera_matrix, dist_coeffs, rvecs, tvecs);
+
+  // Populate the result
+  CameraIntrinsicResult result;
+  result.converged = true;
+  result.initial_cost_per_obs = std::numeric_limits<double>::infinity();
+  result.final_cost_per_obs = rms_error;
+
+  // Intrinsics
+  result.intrinsics.fx() = camera_matrix.at<double>(0, 0);
+  result.intrinsics.fy() = camera_matrix.at<double>(1, 1);
+  result.intrinsics.cx() = camera_matrix.at<double>(0, 2);
+  result.intrinsics.cy() = camera_matrix.at<double>(1, 2);
+
+  // Distortion
+  result.distortions[0] = dist_coeffs.at<double>(0);
+  result.distortions[1] = dist_coeffs.at<double>(1);
+  result.distortions[2] = dist_coeffs.at<double>(2);
+  result.distortions[3] = dist_coeffs.at<double>(3);
+  result.distortions[4] = dist_coeffs.at<double>(4);
+
+  // Target positions
+  result.target_transforms.reserve(params.image_observations.size());
+  for (std::size_t i = 0; i < params.image_observations.size(); ++i)
+  {
+    // Convert to rotation matrix
+    cv::Mat rotation(3, 3, CV_64F);
+    cv::Rodrigues(rvecs[i], rotation);
+
+    Eigen::Isometry3d pose;
+    pose.linear() = Eigen::Matrix<double, 3, 3, Eigen::RowMajor>::Map(reinterpret_cast<double*>(rotation.data));
+    pose.translation() = Eigen::Vector3d::Map(reinterpret_cast<double*>(tvecs[i].data));
+
+    result.target_transforms.push_back(pose);
+  }
+
+  return result;
+}
+
 CameraIntrinsicCalibrationWidget::CameraIntrinsicCalibrationWidget(QWidget* parent)
   : QMainWindow(parent)
   , ui_(new Ui::CameraIntrinsicCalibration())
@@ -359,15 +430,18 @@ void CameraIntrinsicCalibrationWidget::calibrate()
 
     // Solve the calibration problem
     result_ = std::make_shared<CameraIntrinsicResult>();
-    *result_ = optimize(problem);
+    if (ui_->action_use_opencv->isChecked())
+      *result_ = optimizeOpenCV(problem, image_size);
+    else
+      *result_ = optimize(problem);
 
     // Report results
     std::stringstream ss;
     ss << *result_ << std::endl;
 
-    if (result_->covariance.covariances.empty())
+    if (result_->covariance.covariances.empty() && !ui_->action_use_opencv->isChecked())
       ss << "Failed to compute covariance" << std::endl;
-    else
+    else if (!ui_->action_use_opencv->isChecked())
       ss << result_->covariance.printCorrelationCoeffAboveThreshold(0.5) << std::endl;
 
     ui_->text_edit_results->clear();

gui/src/camera_intrinsic_calibration_widget.ui

@@ -184,13 +184,15 @@
    <addaction name="action_edit_target_finder"/>
    <addaction name="action_camera_intrinsics"/>
    <addaction name="action_use_extrinsic_guesses"/>
+   <addaction name="action_use_opencv"/>
    <addaction name="separator"/>
    <addaction name="action_calibrate"/>
    <addaction name="action_save"/>
   </widget>
   <action name="action_load_configuration">
    <property name="icon">
-    <iconset theme="document-open"/>
+    <iconset theme="document-open">
+     <normaloff>.</normaloff>.</iconset>
    </property>
    <property name="text">
     <string>Load configuration</string>
@@ -201,7 +203,8 @@
   </action>
   <action name="action_edit_target_finder">
    <property name="icon">
-    <iconset theme="edit-find"/>
+    <iconset theme="edit-find">
+     <normaloff>.</normaloff>.</iconset>
    </property>
    <property name="text">
     <string>Target finder</string>
@@ -212,7 +215,8 @@
   </action>
   <action name="action_load_data">
    <property name="icon">
-    <iconset theme="document-new"/>
+    <iconset theme="document-new">
+     <normaloff>.</normaloff>.</iconset>
    </property>
    <property name="text">
     <string>Load observations</string>
@@ -223,7 +227,8 @@
   </action>
   <action name="action_calibrate">
    <property name="icon">
-    <iconset theme="media-playback-start"/>
+    <iconset theme="media-playback-start">
+     <normaloff>.</normaloff>.</iconset>
    </property>
    <property name="text">
     <string>Calibrate</string>
@@ -234,7 +239,8 @@
   </action>
   <action name="action_save">
    <property name="icon">
-    <iconset theme="document-save"/>
+    <iconset theme="document-save">
+     <normaloff>.</normaloff>.</iconset>
    </property>
    <property name="text">
     <string>Save calibration</string>
@@ -248,7 +254,8 @@
     <bool>true</bool>
    </property>
    <property name="icon">
-    <iconset theme="user-available"/>
+    <iconset theme="user-available">
+     <normaloff>.</normaloff>.</iconset>
    </property>
    <property name="text">
     <string>Use target pose guesses</string>
@@ -259,7 +266,8 @@
   </action>
   <action name="action_camera_intrinsics">
    <property name="icon">
-    <iconset theme="camera-photo"/>
+    <iconset theme="camera-photo">
+     <normaloff>.</normaloff>.</iconset>
    </property>
    <property name="text">
     <string>Camera intrinsics</string>
@@ -268,6 +276,24 @@
     <string>Edit camera intrinsics</string>
    </property>
   </action>
+  <action name="action_use_opencv">
+   <property name="checkable">
+    <bool>true</bool>
+   </property>
+   <property name="icon">
+    <iconset resource="../resources/resources.qrc">
+     <normaloff>:/icons/opencv.svg</normaloff>:/icons/opencv.svg</iconset>
+   </property>
+   <property name="text">
+    <string>Use OpenCV calibration</string>
+   </property>
+   <property name="toolTip">
+    <string>Use OpenCV calibration algorithm</string>
+   </property>
+   <property name="menuRole">
+    <enum>QAction::NoRole</enum>
+   </property>
+  </action>
  </widget>
  <customwidgets>
   <customwidget>
@@ -281,6 +307,8 @@
   <tabstop>double_spin_box_homography_threshold</tabstop>
   <tabstop>text_edit_results</tabstop>
  </tabstops>
- <resources/>
+ <resources>
+  <include location="../resources/resources.qrc"/>
+ </resources>
  <connections/>
 </ui>