diff --git a/gui/resources/icons/opencv.svg b/gui/resources/icons/opencv.svg
new file mode 100644
index 00000000..e6a7a483
--- /dev/null
+++ b/gui/resources/icons/opencv.svg
@@ -0,0 +1 @@
+
\ No newline at end of file
diff --git a/gui/resources/resources.qrc b/gui/resources/resources.qrc
index 59ca0e1c..7a484a88 100644
--- a/gui/resources/resources.qrc
+++ b/gui/resources/resources.qrc
@@ -3,5 +3,6 @@
icons/icon.jpg
icons/frame_target.png
icons/frame_camera.png
+ icons/opencv.svg
diff --git a/gui/src/camera_intrinsic_calibration_widget.cpp b/gui/src/camera_intrinsic_calibration_widget.cpp
index c31ff7bb..16b1cbf0 100644
--- a/gui/src/camera_intrinsic_calibration_widget.cpp
+++ b/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> object_points;
+ std::vector> image_points;
+
+ for (const auto& o : params.image_observations)
+ {
+ std::vector op;
+ std::vector 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::type);
+ cv::setIdentity(camera_matrix);
+ camera_matrix.at(0, 0) = params.intrinsics_guess.fx();
+ camera_matrix.at(1, 1) = params.intrinsics_guess.fy();
+ camera_matrix.at(0, 2) = params.intrinsics_guess.cx();
+ camera_matrix.at(1, 2) = params.intrinsics_guess.cy();
+
+ cv::Mat dist_coeffs;
+ std::vector rvecs;
+ std::vector 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::infinity();
+ result.final_cost_per_obs = rms_error;
+
+ // Intrinsics
+ result.intrinsics.fx() = camera_matrix.at(0, 0);
+ result.intrinsics.fy() = camera_matrix.at(1, 1);
+ result.intrinsics.cx() = camera_matrix.at(0, 2);
+ result.intrinsics.cy() = camera_matrix.at(1, 2);
+
+ // Distortion
+ result.distortions[0] = dist_coeffs.at(0);
+ result.distortions[1] = dist_coeffs.at(1);
+ result.distortions[2] = dist_coeffs.at(2);
+ result.distortions[3] = dist_coeffs.at(3);
+ result.distortions[4] = dist_coeffs.at(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::Map(reinterpret_cast(rotation.data));
+ pose.translation() = Eigen::Vector3d::Map(reinterpret_cast(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();
- *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();
diff --git a/gui/src/camera_intrinsic_calibration_widget.ui b/gui/src/camera_intrinsic_calibration_widget.ui
index 9883eb17..4fecf48f 100644
--- a/gui/src/camera_intrinsic_calibration_widget.ui
+++ b/gui/src/camera_intrinsic_calibration_widget.ui
@@ -184,13 +184,15 @@
+
-
+
+ ..
Load configuration
@@ -201,7 +203,8 @@
-
+
+ ..
Target finder
@@ -212,7 +215,8 @@
-
+
+ ..
Load observations
@@ -223,7 +227,8 @@
-
+
+ ..
Calibrate
@@ -234,7 +239,8 @@
-
+
+ ..
Save calibration
@@ -248,7 +254,8 @@
true
-
+
+ ..
Use target pose guesses
@@ -259,7 +266,8 @@
-
+
+ ..
Camera intrinsics
@@ -268,6 +276,24 @@
Edit camera intrinsics
+
+
+ true
+
+
+
+ :/icons/opencv.svg:/icons/opencv.svg
+
+
+ Use OpenCV calibration
+
+
+ Use OpenCV calibration algorithm
+
+
+ QAction::NoRole
+
+
@@ -281,6 +307,8 @@
double_spin_box_homography_threshold
text_edit_results
-
+
+
+