From e9170935cee9408ac31782ac7d64035a2c540faa Mon Sep 17 00:00:00 2001
From: Sebastian Hahta <joseha@utu.fi>
Date: Tue, 28 Jul 2020 10:08:36 +0300
Subject: [PATCH] quaternion parametrization
---
.../include/ftl/calibration/optimize.hpp | 72 +++++
.../include/ftl/calibration/parameters.hpp | 66 -----
components/calibration/src/optimize.cpp | 270 +++++++++++++++---
components/calibration/src/parameters.cpp | 165 +----------
4 files changed, 304 insertions(+), 269 deletions(-)
diff --git a/components/calibration/include/ftl/calibration/optimize.hpp b/components/calibration/include/ftl/calibration/optimize.hpp
index dff01ffb9..eeaf6d9de 100644
--- a/components/calibration/include/ftl/calibration/optimize.hpp
+++ b/components/calibration/include/ftl/calibration/optimize.hpp
@@ -24,6 +24,73 @@ static_assert(std::is_standard_layout<cv::Point3d>());
namespace ftl {
namespace calibration {
+/**
+ * Camera paramters (Ceres)
+ */
+struct Camera {
+ Camera() {}
+ Camera(const cv::Mat& K, const cv::Mat& D, const cv::Mat& R, const cv::Mat& tvec, cv::Size size);
+ Camera(const CalibrationData::Calibration& calib);
+
+ CalibrationData::Intrinsic intrinsic() const;
+ CalibrationData::Extrinsic extrinsic() const;
+
+ void setRotation(const cv::Mat& R);
+ void setTranslation(const cv::Mat& tvec);
+ void setExtrinsic(const cv::Mat& R, const cv::Mat& t) {
+ setRotation(R);
+ setTranslation(t);
+ }
+
+ void setIntrinsic(const cv::Mat& K, cv::Size sz);
+ void setDistortion(const cv::Mat &D);
+ void setIntrinsic(const cv::Mat& K, const cv::Mat& D, cv::Size sz) {
+ setIntrinsic(K, sz);
+ setDistortion(D);
+ }
+
+ cv::Mat intrinsicMatrix() const;
+ cv::Mat distortionCoefficients() const;
+
+ cv::Mat rvec() const;
+ cv::Mat tvec() const;
+ cv::Mat rmat() const;
+
+ cv::Mat extrinsicMatrix() const;
+ cv::Mat extrinsicMatrixInverse() const;
+
+ cv::Size size;
+
+ const static int n_parameters = 18;
+ const static int n_distortion_parameters = 8;
+
+ double data[n_parameters] = {0.0};
+
+ enum Parameter {
+ ROTATION = 0,
+ Q1 = 0,
+ Q2 = 1,
+ Q3 = 2,
+ Q4 = 3,
+ TRANSLATION = 4,
+ TX = 4,
+ TY = 5,
+ TZ = 6,
+ F = 7,
+ CX = 8,
+ CY = 9,
+ DISTORTION = 10,
+ K1 = 10,
+ K2 = 11,
+ P1 = 12,
+ P2 = 13,
+ K3 = 14,
+ K4 = 15,
+ K5 = 16,
+ K6 = 17
+ };
+};
+
#ifdef HAVE_CERES
/** Project point using camera model implemented for Ceres */
@@ -79,7 +146,12 @@ public:
* @todo Radial distortion must be monotonic. This constraint is not
* included in the model.
*/
+ /// fix all distortion coefficients to constant (initial values)
bool fix_distortion = true;
+ /// use distortion coefficients k4, k5, and k6; if false, set to zero
+ bool rational_model = true;
+ /// assume zero distortion during optimization
+ bool zero_distortion = false;
bool optimize_intrinsic = true;
bool optimize_motion = true;
diff --git a/components/calibration/include/ftl/calibration/parameters.hpp b/components/calibration/include/ftl/calibration/parameters.hpp
index 12ece270e..d0ed5ec4a 100644
--- a/components/calibration/include/ftl/calibration/parameters.hpp
+++ b/components/calibration/include/ftl/calibration/parameters.hpp
@@ -9,72 +9,6 @@
namespace ftl {
namespace calibration {
-/**
- * Camera paramters (Ceres); TODO: move to optimize.hpp?
- */
-struct Camera {
- Camera() {}
- Camera(const cv::Mat& K, const cv::Mat& D, const cv::Mat& R, const cv::Mat& tvec, cv::Size size);
- Camera(const CalibrationData::Calibration& calib);
-
- CalibrationData::Intrinsic intrinsic() const;
- CalibrationData::Extrinsic extrinsic() const;
-
- void setRotation(const cv::Mat& R);
- void setTranslation(const cv::Mat& tvec);
- void setExtrinsic(const cv::Mat& R, const cv::Mat& t) {
- setRotation(R);
- setTranslation(t);
- }
-
- void setIntrinsic(const cv::Mat& K, cv::Size sz);
- void setDistortion(const cv::Mat &D);
- void setIntrinsic(const cv::Mat& K, const cv::Mat& D, cv::Size sz) {
- setIntrinsic(K, sz);
- setDistortion(D);
- }
-
- cv::Mat intrinsicMatrix() const;
- cv::Mat distortionCoefficients() const;
-
- cv::Mat rvec() const;
- cv::Mat tvec() const;
- cv::Mat rmat() const;
-
- cv::Mat extrinsicMatrix() const;
- cv::Mat extrinsicMatrixInverse() const;
-
- cv::Size size;
-
- const static int n_parameters = 17;
- const static int n_distortion_parameters = 8;
-
- double data[n_parameters] = {0.0};
-
- enum Parameter {
- ROTATION = 0,
- RX = 0,
- RY = 1,
- RZ = 2,
- TRANSLATION = 3,
- TX = 3,
- TY = 4,
- TZ = 5,
- F = 6,
- CX = 7,
- CY = 8,
- DISTORTION = 9,
- K1 = 9,
- K2 = 10,
- P1 = 11,
- P2 = 12,
- K3 = 13,
- K4 = 14,
- K5 = 15,
- K6 = 16
- };
-};
-
namespace validate {
/**
diff --git a/components/calibration/src/optimize.cpp b/components/calibration/src/optimize.cpp
index e683e570f..04722774d 100644
--- a/components/calibration/src/optimize.cpp
+++ b/components/calibration/src/optimize.cpp
@@ -17,7 +17,8 @@ using cv::Mat;
using cv::Point3d;
using cv::Point2d;
-
+using cv::Vec3d;
+using cv::Size;
using cv::Rect;
using ftl::calibration::BundleAdjustment;
@@ -25,16 +26,172 @@ using ftl::calibration::Camera;
////////////////////////////////////////////////////////////////////////////////
+void Camera::setRotation(const Mat& R) {
+ if (((R.size() != Size(3, 3)) &&
+ (R.size() != Size(3, 1)) &&
+ (R.size() != Size(1, 3))) ||
+ (R.type() != CV_64FC1)) {
+
+ throw ftl::exception("bad rotation matrix size/type");
+ }
+
+ Mat rvec;
+ if (R.size() == cv::Size(3, 3)) { cv::Rodrigues(R, rvec); }
+ else { rvec = R; }
+
+ ceres::AngleAxisToQuaternion<double>((double*)(rvec.data), data + Parameter::ROTATION);
+}
+
+void Camera::setTranslation(const Mat& t) {
+ if ((t.type() != CV_64FC1) ||
+ (t.size() != cv::Size(1, 3))) {
+
+ throw ftl::exception("bad translation vector");
+ }
+
+ data[Parameter::TX] = t.at<double>(0);
+ data[Parameter::TY] = t.at<double>(1);
+ data[Parameter::TZ] = t.at<double>(2);
+}
+
+
+void Camera::setIntrinsic(const Mat& K, cv::Size sz) {
+ size = sz;
+ if ((K.type() != CV_64FC1) || (K.size() != cv::Size(3, 3))) {
+ throw ftl::exception("bad intrinsic matrix");
+ }
+
+ data[Parameter::F] = K.at<double>(0, 0);
+ data[Parameter::CX] = K.at<double>(0, 2);
+ data[Parameter::CY] = K.at<double>(1, 2);
+}
+
+void Camera::setDistortion(const Mat& D) {
+ if ((D.type() != CV_64FC1)) {
+ throw ftl::exception("distortion coefficients must be CV_64FC1");
+ }
+ switch(D.total()) {
+ case 12:
+ /*
+ data[Parameter::S1] = D.at<double>(8);
+ data[Parameter::S2] = D.at<double>(9);
+ data[Parameter::S3] = D.at<double>(10);
+ data[Parameter::S4] = D.at<double>(11);
+ */
+ [[fallthrough]];
+
+ case 8:
+ data[Parameter::K4] = D.at<double>(5);
+ data[Parameter::K5] = D.at<double>(6);
+ data[Parameter::K6] = D.at<double>(7);
+ [[fallthrough]];
+
+ case 5:
+ data[Parameter::K3] = D.at<double>(4);
+ [[fallthrough]];
+
+ default:
+ data[Parameter::K1] = D.at<double>(0);
+ data[Parameter::K2] = D.at<double>(1);
+ data[Parameter::P1] = D.at<double>(2);
+ data[Parameter::P2] = D.at<double>(3);
+ }
+}
+
+Camera::Camera(const Mat &K, const Mat &D, const Mat &R, const Mat &tvec, cv::Size sz) {
+ setIntrinsic(K, D, sz);
+ if (!R.empty()) { setRotation(R); }
+ if (!tvec.empty()) { setTranslation(tvec); }
+}
+
+Camera::Camera(const ftl::calibration::CalibrationData::Calibration& calib) {
+ setIntrinsic(calib.intrinsic.matrix(), calib.intrinsic.distCoeffs.Mat(), calib.intrinsic.resolution);
+ setExtrinsic(calib.extrinsic.matrix()(cv::Rect(0, 0, 3, 3)), cv::Mat(calib.extrinsic.tvec));
+}
+
+ftl::calibration::CalibrationData::Intrinsic Camera::intrinsic() const {
+ return ftl::calibration::CalibrationData::Intrinsic(intrinsicMatrix(), distortionCoefficients(), size);
+}
+ftl::calibration::CalibrationData::Extrinsic Camera::extrinsic() const {
+ return ftl::calibration::CalibrationData::Extrinsic(rvec(), tvec());
+}
+
+Mat Camera::intrinsicMatrix() const {
+ Mat K = Mat::eye(3, 3, CV_64FC1);
+ K.at<double>(0, 0) = data[Parameter::F];
+ K.at<double>(1, 1) = data[Parameter::F];
+ K.at<double>(0, 2) = data[Parameter::CX];
+ K.at<double>(1, 2) = data[Parameter::CY];
+ return K;
+}
+
+Mat Camera::distortionCoefficients() const {
+ Mat D;
+ if (Camera::n_distortion_parameters <= 4) { D = Mat::zeros(1, 4, CV_64FC1); }
+ else if (Camera::n_distortion_parameters <= 5) { D = Mat::zeros(1, 5, CV_64FC1); }
+ else if (Camera::n_distortion_parameters <= 8) { D = Mat::zeros(1, 8, CV_64FC1); }
+ else if (Camera::n_distortion_parameters <= 12) { D = Mat::zeros(1, 12, CV_64FC1); }
+ else if (Camera::n_distortion_parameters <= 14) { D = Mat::zeros(1, 14, CV_64FC1); }
+
+ switch(Camera::n_distortion_parameters) {
+ case 14: // not used in OpenCV?
+ case 12:
+ case 8:
+ D.at<double>(5) = data[Parameter::K4];
+ D.at<double>(6) = data[Parameter::K5];
+ D.at<double>(7) = data[Parameter::K6];
+ case 5:
+ D.at<double>(4) = data[Parameter::K3];
+ case 4:
+ D.at<double>(0) = data[Parameter::K1];
+ D.at<double>(1) = data[Parameter::K2];
+ D.at<double>(2) = data[Parameter::P1];
+ D.at<double>(3) = data[Parameter::P2];
+ }
+
+ return D;
+}
+
+Mat Camera::rvec() const {
+ cv::Mat rvec(cv::Size(3, 1), CV_64FC1);
+ ceres::QuaternionToAngleAxis(data + Parameter::ROTATION,
+ (double*)(rvec.data));
+ return rvec;
+}
+
+Mat Camera::tvec() const {
+ return Mat(Vec3d(data[Parameter::TX], data[Parameter::TY], data[Parameter::TZ]));
+}
+
+Mat Camera::rmat() const {
+ Mat R;
+ cv::Rodrigues(rvec(), R);
+ return R;
+}
+
+Mat Camera::extrinsicMatrix() const {
+ Mat T = Mat::eye(4, 4, CV_64FC1);
+ rmat().copyTo(T(Rect(0, 0, 3, 3)));
+ tvec().copyTo(T(Rect(3, 0, 1, 3)));
+ return T;
+}
+
+Mat Camera::extrinsicMatrixInverse() const {
+ return transform::inverse(extrinsicMatrix());
+}
+
+////////////////////////////////////////////////////////////////////////////////
+
struct ReprojectionError {
/**
* Reprojection error.
*
* Camera model has _CAMERA_PARAMETERS parameters:
*
- * - rotation and translation: rx, ry, rz, tx, ty, tx
+ * - rotation and translation: q1, q2, q3, q4, tx, ty, tx
* - focal length: f (fx == fy assumed)
* - pricipal point: cx, cy
- * - distortion coefficients: k1, k2, k3, p1, p2
+ * - distortion coefficients: k1, k2, k3, k4, k5, k6, p1, p2
*
* Camera model documented in
* https://docs.opencv.org/master/d9/d0c/group__calib3d.html
@@ -49,9 +206,8 @@ struct ReprojectionError {
T* residuals) const {
T p[3];
+ ceres::QuaternionRotatePoint(camera + Camera::Parameter::ROTATION, point, p);
- // Apply rotation and translation
- ceres::AngleAxisRotatePoint(camera + Camera::Parameter::ROTATION, point, p);
p[0] += camera[Camera::Parameter::TX];
p[1] += camera[Camera::Parameter::TY];
@@ -123,25 +279,7 @@ cv::Point2d ftl::calibration::projectPoint(const Camera& camera, const cv::Point
return out;
}
-struct LengthError {
- explicit LengthError(const double d) : d(d) {}
-
- template <typename T>
- bool operator()(const T* const p1, const T* const p2, T* residual) const {
- auto x = p1[0] - p2[0];
- auto y = p1[1] - p2[1];
- auto z = p1[2] - p2[2];
- residual[0] = T(d) - sqrt(x*x + y*y + z*z);
-
- return true;
- }
-
- static ceres::CostFunction* Create(const double d) {
- return (new ceres::AutoDiffCostFunction<LengthError, 1, 3, 3>(new LengthError(d)));
- }
-
- double d;
-};
+////////////////////////////////////////////////////////////////////////////////
struct ScaleError {
ScaleError(const double d, const Point3d& p) : d(d), p(p) {}
@@ -287,15 +425,37 @@ void BundleAdjustment::_setCameraParametrization(ceres::Problem &problem, const
vector<int> constant_camera_parameters;
- // extrinsic paramters
- if (!options.optimize_motion) {
- for (int i = 0; i < 3; i++) {
- constant_camera_parameters.push_back(Camera::Parameter::ROTATION + i);
- constant_camera_parameters.push_back(Camera::Parameter::TRANSLATION + i);
+ // apply options
+ for (size_t i = 0; i < cameras_.size(); i++) {
+ if (!options.rational_model) {
+ cameras_[i]->data[Camera::Parameter::K4] = 0.0;
+ cameras_[i]->data[Camera::Parameter::K5] = 0.0;
+ cameras_[i]->data[Camera::Parameter::K6] = 0.0;
}
+ if (options.zero_distortion) {
+ cameras_[i]->data[Camera::Parameter::K1] = 0.0;
+ cameras_[i]->data[Camera::Parameter::K2] = 0.0;
+ cameras_[i]->data[Camera::Parameter::K3] = 0.0;
+ cameras_[i]->data[Camera::Parameter::K4] = 0.0;
+ cameras_[i]->data[Camera::Parameter::K5] = 0.0;
+ cameras_[i]->data[Camera::Parameter::K6] = 0.0;
+ cameras_[i]->data[Camera::Parameter::P1] = 0.0;
+ cameras_[i]->data[Camera::Parameter::P2] = 0.0;
+ }
+ }
+
+ // set extrinsic paramters constant for all cameras
+ if (!options.optimize_motion) {
+ constant_camera_parameters.push_back(Camera::Parameter::Q1);
+ constant_camera_parameters.push_back(Camera::Parameter::Q2);
+ constant_camera_parameters.push_back(Camera::Parameter::Q3);
+ constant_camera_parameters.push_back(Camera::Parameter::Q4);
+ constant_camera_parameters.push_back(Camera::Parameter::TX);
+ constant_camera_parameters.push_back(Camera::Parameter::TY);
+ constant_camera_parameters.push_back(Camera::Parameter::TZ);
}
- // intrinsic parameters
+ // set intrinsic parameters constant for all cameras
if (!options.optimize_intrinsic || options.fix_focal) {
constant_camera_parameters.push_back(Camera::Parameter::F);
}
@@ -329,37 +489,61 @@ void BundleAdjustment::_setCameraParametrization(ceres::Problem &problem, const
options.fix_camera_extrinsic.begin(), options.fix_camera_extrinsic.end());
for (size_t i = 0; i < cameras_.size(); i++) {
- std::unordered_set<int> fixed_parameters(
+ std::unordered_set<int> constant_parameters(
constant_camera_parameters.begin(),
constant_camera_parameters.end());
if (fix_extrinsic.find(i) != fix_extrinsic.end()) {
- fixed_parameters.insert({
- Camera::Parameter::RX, Camera::Parameter::RY,
- Camera::Parameter::RZ, Camera::Parameter::TX,
- Camera::Parameter::TY, Camera::Parameter::TZ
+ constant_parameters.insert({
+ Camera::Parameter::Q1, Camera::Parameter::Q2,
+ Camera::Parameter::Q3, Camera::Parameter::Q4,
+ Camera::Parameter::TX, Camera::Parameter::TY,
+ Camera::Parameter::TZ
});
}
if (fix_intrinsic.find(i) != fix_intrinsic.end()) {
- fixed_parameters.insert({
+ constant_parameters.insert({
Camera::Parameter::F, Camera::Parameter::CX,
Camera::Parameter::CY
});
}
- vector<int> params(fixed_parameters.begin(), fixed_parameters.end());
+ vector<int> params(constant_parameters.begin(), constant_parameters.end());
if (params.size() == Camera::n_parameters) {
- // Ceres crashes if all parameters are set constant using
+ // Ceres crashes if all parameters are set constant with
// SubsetParameterization()
// https://github.com/ceres-solver/ceres-solver/issues/347
// https://github.com/ceres-solver/ceres-solver/commit/27183d661ecae246dbce6d03cacf84f39fba1f1e
problem.SetParameterBlockConstant(getCameraPtr(i));
}
else if (params.size() > 0) {
- problem.SetParameterization(getCameraPtr(i),
- new ceres::SubsetParameterization(Camera::n_parameters, params));
+ ceres::LocalParameterization* camera_parameterization = nullptr;
+
+ if (constant_parameters.count(Camera::Parameter::ROTATION) == 0) {
+ // quaternion parametrization
+ for (auto& v : params) { v -= 4; }
+ camera_parameterization =
+ new ceres::ProductParameterization(
+ new ceres::QuaternionParameterization(),
+ new ceres::SubsetParameterization(Camera::n_parameters - 4, params));
+ }
+ else {
+ // extrinsic parameters constant
+ CHECK(constant_parameters.count(Camera::Parameter::Q1));
+ CHECK(constant_parameters.count(Camera::Parameter::Q2));
+ CHECK(constant_parameters.count(Camera::Parameter::Q3));
+ CHECK(constant_parameters.count(Camera::Parameter::Q4));
+ CHECK(constant_parameters.count(Camera::Parameter::TX));
+ CHECK(constant_parameters.count(Camera::Parameter::TY));
+ CHECK(constant_parameters.count(Camera::Parameter::TZ));
+
+ camera_parameterization =
+ new ceres::SubsetParameterization(Camera::n_parameters, params);
+ }
+
+ problem.SetParameterization(getCameraPtr(i), camera_parameterization);
}
}
}
@@ -367,7 +551,7 @@ void BundleAdjustment::_setCameraParametrization(ceres::Problem &problem, const
void BundleAdjustment::_buildBundleAdjustmentProblem(ceres::Problem &problem, const BundleAdjustment::Options &options) {
- ceres::LossFunction *loss_function = nullptr;
+ ceres::LossFunction *loss_function = nullptr; // squared
if (options.loss == Options::Loss::HUBER) {
loss_function = new ceres::HuberLoss(1.0);
@@ -390,8 +574,6 @@ void BundleAdjustment::_buildBundleAdjustmentProblem(ceres::Problem &problem, co
}
}
- // apply options
-
_setCameraParametrization(problem, options);
if (!options.optmize_structure) {
@@ -412,7 +594,7 @@ void BundleAdjustment::run(const BundleAdjustment::Options &bundle_adjustment_op
_buildProblem(problem, bundle_adjustment_options);
ceres::Solver::Options options;
- options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;
+ options.linear_solver_type = ceres::DENSE_SCHUR;
options.minimizer_progress_to_stdout = bundle_adjustment_options.verbose;
if (bundle_adjustment_options.max_iter > 0) {
diff --git a/components/calibration/src/parameters.cpp b/components/calibration/src/parameters.cpp
index 2c9c2f1a3..fae57d07d 100644
--- a/components/calibration/src/parameters.cpp
+++ b/components/calibration/src/parameters.cpp
@@ -1,9 +1,13 @@
-#include "ftl/calibration/parameters.hpp"
-#include <ftl/exception.hpp>
#include <loguru.hpp>
+
+#include <ftl/calibration/parameters.hpp>
+#include <ftl/exception.hpp>
+
#include <opencv2/calib3d/calib3d.hpp>
+#include <ceres/rotation.h>
+
using cv::Mat;
using cv::Size;
using cv::Point2d;
@@ -15,163 +19,6 @@ using std::vector;
using namespace ftl::calibration;
-using ftl::calibration::Camera;
-
-////////////////////////////////////////////////////////////////////////////////
-
-void Camera::setRotation(const Mat& R) {
- if (((R.size() != Size(3, 3)) &&
- (R.size() != Size(3, 1)) &&
- (R.size() != Size(1, 3))) ||
- (R.type() != CV_64FC1)) {
-
- throw ftl::exception("bad rotation matrix size/type");
- }
-
- Mat rvec;
- if (R.size() == cv::Size(3, 3)) { cv::Rodrigues(R, rvec); }
- else { rvec = R; }
-
- data[Parameter::RX] = rvec.at<double>(0);
- data[Parameter::RY] = rvec.at<double>(1);
- data[Parameter::RZ] = rvec.at<double>(2);
-}
-
-void Camera::setTranslation(const Mat& t) {
- if ((t.type() != CV_64FC1) ||
- (t.size() != cv::Size(1, 3))) {
-
- throw ftl::exception("bad translation vector");
- }
-
- data[Parameter::TX] = t.at<double>(0);
- data[Parameter::TY] = t.at<double>(1);
- data[Parameter::TZ] = t.at<double>(2);
-}
-
-
-void Camera::setIntrinsic(const Mat& K, cv::Size sz) {
- size = sz;
- if ((K.type() != CV_64FC1) || (K.size() != cv::Size(3, 3))) {
- throw ftl::exception("bad intrinsic matrix");
- }
-
- data[Parameter::F] = K.at<double>(0, 0);
- data[Parameter::CX] = K.at<double>(0, 2);
- data[Parameter::CY] = K.at<double>(1, 2);
-}
-
-void Camera::setDistortion(const Mat& D) {
- if ((D.type() != CV_64FC1)) {
- throw ftl::exception("distortion coefficients must be CV_64FC1");
- }
- switch(D.total()) {
- case 12:
- /*
- data[Parameter::S1] = D.at<double>(8);
- data[Parameter::S2] = D.at<double>(9);
- data[Parameter::S3] = D.at<double>(10);
- data[Parameter::S4] = D.at<double>(11);
- */
- [[fallthrough]];
-
- case 8:
- data[Parameter::K4] = D.at<double>(5);
- data[Parameter::K5] = D.at<double>(6);
- data[Parameter::K6] = D.at<double>(7);
- [[fallthrough]];
-
- case 5:
- data[Parameter::K3] = D.at<double>(4);
- [[fallthrough]];
-
- default:
- data[Parameter::K1] = D.at<double>(0);
- data[Parameter::K2] = D.at<double>(1);
- data[Parameter::P1] = D.at<double>(2);
- data[Parameter::P2] = D.at<double>(3);
- }
-}
-
-Camera::Camera(const Mat &K, const Mat &D, const Mat &R, const Mat &tvec, cv::Size sz) {
- setIntrinsic(K, D, sz);
- if (!R.empty()) { setRotation(R); }
- if (!tvec.empty()) { setTranslation(tvec); }
-}
-
-Camera::Camera(const ftl::calibration::CalibrationData::Calibration& calib) {
- setIntrinsic(calib.intrinsic.matrix(), calib.intrinsic.distCoeffs.Mat(), calib.intrinsic.resolution);
- setExtrinsic(calib.extrinsic.matrix()(cv::Rect(0, 0, 3, 3)), cv::Mat(calib.extrinsic.tvec));
-}
-
-ftl::calibration::CalibrationData::Intrinsic Camera::intrinsic() const {
- return ftl::calibration::CalibrationData::Intrinsic(intrinsicMatrix(), distortionCoefficients(), size);
-}
-ftl::calibration::CalibrationData::Extrinsic Camera::extrinsic() const {
- return ftl::calibration::CalibrationData::Extrinsic(rvec(), tvec());
-}
-
-Mat Camera::intrinsicMatrix() const {
- Mat K = Mat::eye(3, 3, CV_64FC1);
- K.at<double>(0, 0) = data[Parameter::F];
- K.at<double>(1, 1) = data[Parameter::F];
- K.at<double>(0, 2) = data[Parameter::CX];
- K.at<double>(1, 2) = data[Parameter::CY];
- return K;
-}
-
-Mat Camera::distortionCoefficients() const {
- Mat D;
- if (Camera::n_distortion_parameters <= 4) { D = Mat::zeros(1, 4, CV_64FC1); }
- else if (Camera::n_distortion_parameters <= 5) { D = Mat::zeros(1, 5, CV_64FC1); }
- else if (Camera::n_distortion_parameters <= 8) { D = Mat::zeros(1, 8, CV_64FC1); }
- else if (Camera::n_distortion_parameters <= 12) { D = Mat::zeros(1, 12, CV_64FC1); }
- else if (Camera::n_distortion_parameters <= 14) { D = Mat::zeros(1, 14, CV_64FC1); }
-
- switch(Camera::n_distortion_parameters) {
- case 14: // not used in OpenCV?
- case 12:
- case 8:
- D.at<double>(5) = data[Parameter::K4];
- D.at<double>(6) = data[Parameter::K5];
- D.at<double>(7) = data[Parameter::K6];
- case 5:
- D.at<double>(4) = data[Parameter::K3];
- case 4:
- D.at<double>(0) = data[Parameter::K1];
- D.at<double>(1) = data[Parameter::K2];
- D.at<double>(2) = data[Parameter::P1];
- D.at<double>(3) = data[Parameter::P2];
- }
-
- return D;
-}
-
-Mat Camera::rvec() const {
- return Mat(Vec3d(data[Parameter::RX], data[Parameter::RY], data[Parameter::RZ]));
-}
-
-Mat Camera::tvec() const {
- return Mat(Vec3d(data[Parameter::TX], data[Parameter::TY], data[Parameter::TZ]));
-}
-
-Mat Camera::rmat() const {
- Mat R;
- cv::Rodrigues(rvec(), R);
- return R;
-}
-
-Mat Camera::extrinsicMatrix() const {
- Mat T = Mat::eye(4, 4, CV_64FC1);
- rmat().copyTo(T(Rect(0, 0, 3, 3)));
- tvec().copyTo(T(Rect(3, 0, 1, 3)));
- return T;
-}
-
-Mat Camera::extrinsicMatrixInverse() const {
- return transform::inverse(extrinsicMatrix());
-}
-
////////////////////////////////////////////////////////////////////////////////
bool validate::translationStereo(const Mat &t) {
--
GitLab