diff --git a/components/calibration/include/ftl/calibration/optimize.hpp b/components/calibration/include/ftl/calibration/optimize.hpp
index 77fd1b6d64b67fbbdc714db04e5ec52d38803630..5a2911732d76f31bf3f53ea868ed8b5cc9c7e753 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 with cast to double*
+// BundleAdjustment uses Point3d instances via 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() { data[Parameter::Q1] = 1.0; }
+ 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);
@@ -66,6 +66,8 @@ 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 {
@@ -259,7 +261,6 @@ 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 cb8e82b33c0c65450250900b49a9f6ced45ea3bb..a1381fd476da5cb798423382efabfbe415b548fe 100644
--- a/components/calibration/src/extrinsic.cpp
+++ b/components/calibration/src/extrinsic.cpp
@@ -635,23 +635,10 @@ double ExtrinsicCalibration::optimize() {
cameras.reserve(calib_.size());
unsigned int ncameras = calib_.size();
- 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 (const auto& c : calib_) {
+ auto camera = c;
+ T.push_back(c.extrinsic.inverse().matrix());
+ cameras.push_back(Camera(camera));
}
for (auto &c : cameras) {
// BundleAdjustment stores pointers; do not resize cameras vector
@@ -761,8 +748,6 @@ 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";
@@ -782,15 +767,7 @@ 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);
-
- 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());
- }
+ 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 8f8089cb614fa18d8bc6ac009eba8f739aac1877..40005872504cdcb01c0a46907b84b1bb636c3b5d 100644
--- a/components/calibration/src/optimize.cpp
+++ b/components/calibration/src/optimize.cpp
@@ -99,14 +99,12 @@ 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));
}
@@ -188,67 +186,21 @@ 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 */
+ /**
+ * 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
+ */
explicit ReprojectionError(double observed_x, double observed_y)
: observed_x(observed_x), observed_y(observed_y) {}
@@ -257,12 +209,53 @@ struct ReprojectionError {
const T* const point,
T* residuals) const {
- T predicted[2];
- projectPoint_(camera, point, predicted);
+ 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;
// Error: the difference between the predicted and observed position
- residuals[0] = predicted[0] - T(observed_x);
- residuals[1] = predicted[1] - T(observed_y);
+ residuals[0] = predicted_x - T(observed_x);
+ residuals[1] = predicted_y - T(observed_y);
return true;
}
@@ -278,60 +271,15 @@ struct ReprojectionError {
new ReprojectionError(observed.x, observed.y)));
}
- const double observed_x;
- const double observed_y;
+ double observed_x;
+ double observed_y;
};
-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;
-};
+static ReprojectionError reproject_ = ReprojectionError(0.0, 0.0);
cv::Point2d ftl::calibration::projectPoint(const Camera& camera, const cv::Point3d& point) {
cv::Point2d out;
- projectPoint_<double>(
- static_cast<const double* const>(camera.data),
- reinterpret_cast<const double* const>(&(point.x)),
- reinterpret_cast<double*>(&(out.x)));
+ reproject_(static_cast<const double* const>(camera.data), reinterpret_cast<const double* const>(&(point.x)), reinterpret_cast<double*>(&(out.x)));
return out;
}
@@ -619,6 +567,7 @@ void BundleAdjustment::_setCameraParametrization(ceres::Problem &problem, const
camera_parameterization =
new ceres::SubsetParameterization(Camera::n_parameters, params);
}
+
problem.SetParameterization(getCameraPtr(i), camera_parameterization);
}
}
@@ -636,35 +585,19 @@ 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 < cameras_.size(); i++) {
+ for (size_t i = 0; i < point.observations.size(); i++) {
+ if (!point.visibility[i]) { continue; }
ceres::CostFunction* cost_function =
- ReprojectionErrorStereo::Create(point.observations[i]);
+ ReprojectionError::Create(point.observations[i]);
- 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.AddResidualBlock(cost_function,
+ loss_function,
+ getCameraPtr(i),
+ &(point.point.x)
+ );
}
}
- problem.SetParameterBlockConstant(&(dummy_camera_.data[0]));
_setCameraParametrization(problem, options);
@@ -687,7 +620,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; // http://ceres-solver.org/solving_faqs.html
+ options.linear_solver_type = ceres::DENSE_SCHUR;
options.minimizer_progress_to_stdout = bundle_adjustment_options.verbose;
if (bundle_adjustment_options.max_iter > 0) {
@@ -735,8 +668,7 @@ int BundleAdjustment::removeObservations(double threshold) {
}
error_total /= n_points;
- // should remove (counted in mean error if left)
- if (n_points <= 1) { continue; }
+ if (n_points <= 1) { continue; } // TODO: remove observation completely
for (unsigned int c = 0; c < cameras_.size(); c++) {
if (!point.visibility[c]) { continue; }