From 42a68cc03c11332f7acd0fb332c5db43cf3d073b Mon Sep 17 00:00:00 2001
From: Sebastian Hahta <joseha@utu.fi>
Date: Fri, 7 Aug 2020 12:47:23 +0300
Subject: [PATCH] ugly hacks for stereo camera in calibration
---
.../include/ftl/calibration/optimize.hpp | 7 +-
components/calibration/src/extrinsic.cpp | 33 ++-
components/calibration/src/optimize.cpp | 216 ++++++++++++------
3 files changed, 173 insertions(+), 83 deletions(-)
diff --git a/components/calibration/include/ftl/calibration/optimize.hpp b/components/calibration/include/ftl/calibration/optimize.hpp
index 5a2911732..77fd1b6d6 100644
--- a/components/calibration/include/ftl/calibration/optimize.hpp
+++ b/components/calibration/include/ftl/calibration/optimize.hpp
@@ -15,7 +15,7 @@
#include <opencv2/core/core.hpp>
-// BundleAdjustment uses Point3d instances via double*
+// BundleAdjustment uses Point3d instances with cast to double*
static_assert(sizeof(cv::Point2d) == 2*sizeof(double));
static_assert(std::is_standard_layout<cv::Point2d>());
static_assert(sizeof(cv::Point3d) == 3*sizeof(double));
@@ -28,7 +28,7 @@ namespace calibration {
* Camera paramters (Ceres)
*/
struct Camera {
- Camera() {}
+ Camera() { data[Parameter::Q1] = 1.0; }
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);
@@ -66,8 +66,6 @@ struct Camera {
const static int n_parameters = 18;
const static int n_distortion_parameters = 8;
-
- bool quaternion = false;
double data[n_parameters] = {0.0};
enum Parameter {
@@ -261,6 +259,7 @@ private:
std::vector<Camera*> cameras_;
std::vector<BundleAdjustment::Point> points_;
std::vector<BundleAdjustment::Object> objects_;
+ Camera dummy_camera_; // contains identity rotation+translation
};
#endif
diff --git a/components/calibration/src/extrinsic.cpp b/components/calibration/src/extrinsic.cpp
index a1381fd47..cb8e82b33 100644
--- a/components/calibration/src/extrinsic.cpp
+++ b/components/calibration/src/extrinsic.cpp
@@ -635,10 +635,23 @@ double ExtrinsicCalibration::optimize() {
cameras.reserve(calib_.size());
unsigned int ncameras = calib_.size();
- for (const auto& c : calib_) {
- auto camera = c;
- T.push_back(c.extrinsic.inverse().matrix());
- cameras.push_back(Camera(camera));
+ for (unsigned i = 0; i < calib_.size(); i++) {
+ const auto& calib = calib_[i];
+ T.push_back(calib.extrinsic.inverse().matrix());
+ if (i%2 == 1) {
+ // Ugly hack to use stereo camera parametrization in
+ // BundleAdjustment (would require significant changes to implement
+ // properly). Use l->r extrinsics for right camera (always odd index)
+ auto cam = calib;
+ cam.extrinsic = CalibrationData::Extrinsic
+ (calib.extrinsic.matrix() * calib_[i - 1].extrinsic.inverse().matrix());
+
+ cameras.push_back(Camera(cam));
+ }
+ else {
+ // left camera (even index), use common extrinsic parameters
+ cameras.push_back(Camera(calib));
+ }
}
for (auto &c : cameras) {
// BundleAdjustment stores pointers; do not resize cameras vector
@@ -748,6 +761,8 @@ double ExtrinsicCalibration::optimize() {
int n_removed = 0;
for (const auto& t : prune_observations_) {
+ // Doesn't work without using reprojection error to with stereocam
+ // hacks ...
n_removed += ba.removeObservations(t);
if (float(n_removed)/float(n_points) > threhsold_warning_) {
LOG(WARNING) << "significant number of observations removed";
@@ -767,7 +782,15 @@ double ExtrinsicCalibration::optimize() {
auto intr = cameras[i].intrinsic();
calib_optimized_[i] = calib_[i];
calib_optimized_[i].intrinsic.set(intr.matrix(), intr.distCoeffs.Mat(), intr.resolution);
- calib_optimized_[i].extrinsic.set(cameras[i].rvec(), cameras[i].tvec());
+
+ if (i%2 == 1) {
+ // HACK!!
+ cv::Mat T = cameras[i].extrinsicMatrix() * cameras[i - 1].extrinsicMatrix();
+ calib_optimized_[i].extrinsic.set(T);
+ }
+ else {
+ calib_optimized_[i].extrinsic.set(cameras[i].rvec(), cameras[i].tvec());
+ }
}
rmse_total_ = ba.reprojectionError();
diff --git a/components/calibration/src/optimize.cpp b/components/calibration/src/optimize.cpp
index 400058725..8f8089cb6 100644
--- a/components/calibration/src/optimize.cpp
+++ b/components/calibration/src/optimize.cpp
@@ -99,12 +99,14 @@ void Camera::setDistortion(const Mat& D) {
}
Camera::Camera(const Mat &K, const Mat &D, const Mat &R, const Mat &tvec, cv::Size sz) {
+ data[Parameter::Q1] = 1.0;
setIntrinsic(K, D, sz);
if (!R.empty()) { setRotation(R); }
if (!tvec.empty()) { setTranslation(tvec); }
}
Camera::Camera(const ftl::calibration::CalibrationData::Calibration& calib) {
+ data[Parameter::Q1] = 1.0;
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));
}
@@ -186,21 +188,67 @@ Mat Camera::extrinsicMatrixInverse() const {
////////////////////////////////////////////////////////////////////////////////
+/*
+* Camera model has 18 parameters (with quaternion rotation):
+*
+* - 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, k4, k5, k6, p1, p2
+*
+* Camera model documented in
+* https://docs.opencv.org/master/d9/d0c/group__calib3d.html
+* https://github.com/opencv/opencv/blob/b698d0a6ee12342a87b8ad739d908fd8d7ff1fca/modules/calib3d/src/calibration.cpp#L774
+*/
+template<typename T>
+void projectPoint_(const T* const camera, const T* const point, T* const projected) {
+
+ T p[3];
+ ceres::QuaternionRotatePoint(camera + Camera::Parameter::ROTATION, point, p);
+
+ p[0] += camera[Camera::Parameter::TX];
+ p[1] += camera[Camera::Parameter::TY];
+ p[2] += camera[Camera::Parameter::TZ];
+
+ T x = p[0]/p[2];
+ T y = p[1]/p[2];
+
+ // Intrinsic parameters
+ const T& f = camera[Camera::Parameter::F];
+ const T& cx = camera[Camera::Parameter::CX];
+ const T& cy = camera[Camera::Parameter::CY];
+
+ // Distortion parameters
+ const T& k1 = camera[Camera::Parameter::K1];
+ const T& k2 = camera[Camera::Parameter::K2];
+ const T& k3 = camera[Camera::Parameter::K3];
+ const T& k4 = camera[Camera::Parameter::K4];
+ const T& k5 = camera[Camera::Parameter::K5];
+ const T& k6 = camera[Camera::Parameter::K6];
+ const T& p1 = camera[Camera::Parameter::P1];
+ const T& p2 = camera[Camera::Parameter::P2];
+
+ const T r2 = x*x + y*y;
+ const T r4 = r2*r2;
+ const T r6 = r4*r2;
+
+ // Radial distortion
+ const T cdist = T(1.0) + k1*r2 + k2*r4 + k3*r6;
+ // Radial distortion: rational model
+ const T icdist = T(1.0)/(T(1.0) + k4*r2 + k5*r4 + k6*r6);
+ // Tangential distortion
+ const T pdistx = (T(2.0)*x*y)*p1 + (r2 + T(2.0)*x*x)*p2;
+ const T pdisty = (r2 + T(2.0)*y*y)*p1 + (T(2.0)*x*y)*p2;
+ // Apply distortion
+ const T xd = x*cdist*icdist + pdistx;
+ const T yd = y*cdist*icdist + pdisty;
+ // Projected point position
+ projected[0] = f*xd + cx;
+ projected[1] = f*yd + cy;
+}
+
struct ReprojectionError {
- /**
- * Reprojection error.
- *
- * Camera model has _CAMERA_PARAMETERS parameters:
- *
- * - 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, k4, k5, k6, p1, p2
- *
- * Camera model documented in
- * https://docs.opencv.org/master/d9/d0c/group__calib3d.html
- * https://github.com/opencv/opencv/blob/b698d0a6ee12342a87b8ad739d908fd8d7ff1fca/modules/calib3d/src/calibration.cpp#L774
- */
+ /** Reprojection error */
explicit ReprojectionError(double observed_x, double observed_y)
: observed_x(observed_x), observed_y(observed_y) {}
@@ -209,53 +257,12 @@ struct ReprojectionError {
const T* const point,
T* residuals) const {
- T p[3];
- ceres::QuaternionRotatePoint(camera + Camera::Parameter::ROTATION, point, p);
-
-
- p[0] += camera[Camera::Parameter::TX];
- p[1] += camera[Camera::Parameter::TY];
- p[2] += camera[Camera::Parameter::TZ];
-
- T x = p[0] / p[2];
- T y = p[1] / p[2];
-
- // Intrinsic parameters
- const T& f = camera[Camera::Parameter::F];
- const T& cx = camera[Camera::Parameter::CX];
- const T& cy = camera[Camera::Parameter::CY];
-
- // Distortion parameters
- const T& k1 = camera[Camera::Parameter::K1];
- const T& k2 = camera[Camera::Parameter::K2];
- const T& k3 = camera[Camera::Parameter::K3];
- const T& k4 = camera[Camera::Parameter::K4];
- const T& k5 = camera[Camera::Parameter::K5];
- const T& k6 = camera[Camera::Parameter::K6];
- const T& p1 = camera[Camera::Parameter::P1];
- const T& p2 = camera[Camera::Parameter::P2];
-
- const T r2 = x*x + y*y;
- const T r4 = r2*r2;
- const T r6 = r4*r2;
-
- // Radial distortion
- const T cdist = T(1.0) + k1*r2 + k2*r4 + k3*r6;
- // Radial distortion: rational model
- const T icdist = T(1.0)/(T(1.0) + k4*r2 + k5*r4 + k6*r6);
- // Tangential distortion
- const T pdistx = (T(2.0)*x*y)*p1 + (r2 + T(2.0)*x*x)*p2;
- const T pdisty = (r2 + T(2.0)*y*y)*p1 + (T(2.0)*x*y)*p2;
- // Apply distortion
- const T xd = x*cdist*icdist + pdistx;
- const T yd = y*cdist*icdist + pdisty;
- // Projected point position
- T predicted_x = f*xd + cx;
- T predicted_y = f*yd + cy;
+ T predicted[2];
+ projectPoint_(camera, point, predicted);
// Error: the difference between the predicted and observed position
- residuals[0] = predicted_x - T(observed_x);
- residuals[1] = predicted_y - T(observed_y);
+ residuals[0] = predicted[0] - T(observed_x);
+ residuals[1] = predicted[1] - T(observed_y);
return true;
}
@@ -271,15 +278,60 @@ struct ReprojectionError {
new ReprojectionError(observed.x, observed.y)));
}
- double observed_x;
- double observed_y;
+ const double observed_x;
+ const double observed_y;
};
-static ReprojectionError reproject_ = ReprojectionError(0.0, 0.0);
+struct ReprojectionErrorStereo {
+ /** Reprojection error for stereo camera pairs. Second camera's extrinsic
+ * parameters are rotation and translation from first camera to second.
+ */
+
+ explicit ReprojectionErrorStereo(double observed_x, double observed_y)
+ : observed_x(observed_x), observed_y(observed_y) {}
+
+ template<typename T>
+ bool operator()(const T* const camera1, const T* const camera2,
+ const T* const point,
+ T* residuals) const {
+
+ T p[3];
+ T predicted[2];
+ ceres::QuaternionRotatePoint(camera1 + Camera::Parameter::ROTATION, point, p);
+ p[0] += camera1[Camera::Parameter::TX];
+ p[1] += camera1[Camera::Parameter::TY];
+ p[2] += camera1[Camera::Parameter::TZ];
+
+ projectPoint_(camera2, p, predicted);
+
+ // Error: the difference between the predicted and observed position
+ residuals[0] = predicted[0] - T(observed_x);
+ residuals[1] = predicted[1] - T(observed_y);
+
+ return true;
+ }
+
+ static ceres::CostFunction* Create( const double observed_x,
+ const double observed_y) {
+ return (new ceres::AutoDiffCostFunction<ReprojectionErrorStereo, 2, Camera::n_parameters, Camera::n_parameters, 3>(
+ new ReprojectionErrorStereo(observed_x, observed_y)));
+ }
+
+ static ceres::CostFunction* Create( const Point2d &observed) {
+ return (new ceres::AutoDiffCostFunction<ReprojectionErrorStereo, 2, Camera::n_parameters, Camera::n_parameters, 3>(
+ new ReprojectionErrorStereo(observed.x, observed.y)));
+ }
+
+ const double observed_x;
+ const double observed_y;
+};
cv::Point2d ftl::calibration::projectPoint(const Camera& camera, const cv::Point3d& point) {
cv::Point2d out;
- reproject_(static_cast<const double* const>(camera.data), reinterpret_cast<const double* const>(&(point.x)), reinterpret_cast<double*>(&(out.x)));
+ projectPoint_<double>(
+ static_cast<const double* const>(camera.data),
+ reinterpret_cast<const double* const>(&(point.x)),
+ reinterpret_cast<double*>(&(out.x)));
return out;
}
@@ -567,7 +619,6 @@ void BundleAdjustment::_setCameraParametrization(ceres::Problem &problem, const
camera_parameterization =
new ceres::SubsetParameterization(Camera::n_parameters, params);
}
-
problem.SetParameterization(getCameraPtr(i), camera_parameterization);
}
}
@@ -585,19 +636,35 @@ void BundleAdjustment::_buildBundleAdjustmentProblem(ceres::Problem &problem, co
loss_function = new ceres::CauchyLoss(1.0);
}
+ // dummy camera here as a quick hack so both cameras can use same cost
+ // function so setCameraParametrization above works as expected
for (auto& point : points_) {
- for (size_t i = 0; i < point.observations.size(); i++) {
- if (!point.visibility[i]) { continue; }
+ for (size_t i = 0; i < cameras_.size(); i++) {
ceres::CostFunction* cost_function =
- ReprojectionError::Create(point.observations[i]);
+ ReprojectionErrorStereo::Create(point.observations[i]);
- problem.AddResidualBlock(cost_function,
- loss_function,
- getCameraPtr(i),
- &(point.point.x)
- );
+ if (!point.visibility[i]) { continue; }
+ if (i%2 == 0) {
+ // left camera
+ problem.AddResidualBlock(cost_function,
+ loss_function,
+ &(dummy_camera_.data[0]),
+ getCameraPtr(i),
+ &(point.point.x)
+ );
+ }
+ else {
+ // right camera
+ problem.AddResidualBlock(cost_function,
+ loss_function,
+ getCameraPtr(i - 1),
+ getCameraPtr(i),
+ &(point.point.x)
+ );
+ }
}
}
+ problem.SetParameterBlockConstant(&(dummy_camera_.data[0]));
_setCameraParametrization(problem, options);
@@ -620,7 +687,7 @@ void BundleAdjustment::run(const BundleAdjustment::Options &bundle_adjustment_op
_buildProblem(problem, bundle_adjustment_options);
ceres::Solver::Options options;
- options.linear_solver_type = ceres::DENSE_SCHUR;
+ options.linear_solver_type = ceres::DENSE_SCHUR; // http://ceres-solver.org/solving_faqs.html
options.minimizer_progress_to_stdout = bundle_adjustment_options.verbose;
if (bundle_adjustment_options.max_iter > 0) {
@@ -668,7 +735,8 @@ int BundleAdjustment::removeObservations(double threshold) {
}
error_total /= n_points;
- if (n_points <= 1) { continue; } // TODO: remove observation completely
+ // should remove (counted in mean error if left)
+ if (n_points <= 1) { continue; }
for (unsigned int c = 0; c < cameras_.size(); c++) {
if (!point.visibility[c]) { continue; }
--
GitLab