From 0be3156b36bb8051c2b9f026e3fbcbff43a3c5a3 Mon Sep 17 00:00:00 2001
From: Sebastian Hahta <joseha@utu.fi>
Date: Tue, 28 Jul 2020 19:01:29 +0300
Subject: [PATCH] working rectification in calibration visualizatio
---
.../src/modules/calibration/calibration.hpp | 1 +
.../src/modules/calibration/extrinsic.cpp | 32 +++++++++----------
.../src/views/calibration/extrinsicview.cpp | 21 +++++++++---
.../include/ftl/calibration/optimize.hpp | 5 +--
components/calibration/src/extrinsic.cpp | 5 ++-
components/calibration/src/optimize.cpp | 14 +++++---
components/calibration/src/structures.cpp | 4 +--
7 files changed, 51 insertions(+), 31 deletions(-)
diff --git a/applications/gui2/src/modules/calibration/calibration.hpp b/applications/gui2/src/modules/calibration/calibration.hpp
index a30a67e4d..4986689b1 100644
--- a/applications/gui2/src/modules/calibration/calibration.hpp
+++ b/applications/gui2/src/modules/calibration/calibration.hpp
@@ -323,6 +323,7 @@ public:
FIX_PRINCIPAL_POINT = 16,
FIX_DISTORTION = 32,
LOSS_CAUCHY = 64,
+ NONMONOTONIC_STEP = 128,
};
void setFlags(int flags);
diff --git a/applications/gui2/src/modules/calibration/extrinsic.cpp b/applications/gui2/src/modules/calibration/extrinsic.cpp
index d7847b4a2..d9f60cc13 100644
--- a/applications/gui2/src/modules/calibration/extrinsic.cpp
+++ b/applications/gui2/src/modules/calibration/extrinsic.cpp
@@ -94,8 +94,15 @@ void ExtrinsicCalibration::start(unsigned int fsid, std::vector<FrameID> sources
running_ = false;
filter->remove();
if (fs_current_ == nullptr) { return; }
+
+ // change mode only once per frame (cameras contain same frame twice)
+ std::unordered_set<uint32_t> fids;
for (const auto camera : state_.cameras) {
- setCalibrationMode((*fs_current_)[camera.id.source()], true);
+ fids.insert(camera.id.source());
+ }
+
+ for (const auto i : fids) {
+ setCalibrationMode((*fs_current_)[i], true);
}
});
state_.capture = true;
@@ -272,7 +279,7 @@ void ExtrinsicCalibration::stereoRectify(int cl, int cr,
CHECK(l.intrinsic.resolution == r.intrinsic.resolution);
auto size = l.intrinsic.resolution;
- cv::Mat T = l.extrinsic.matrix() * inverse(r.extrinsic.matrix());
+ cv::Mat T = r.extrinsic.matrix() * inverse(l.extrinsic.matrix());
cv::Mat R, t, R1, R2, P1, P2, Q, map1, map2;
getRotationAndTranslation(T, R, t);
@@ -280,7 +287,7 @@ void ExtrinsicCalibration::stereoRectify(int cl, int cr,
cv::stereoRectify(
l.intrinsic.matrix(), l.intrinsic.distCoeffs.Mat(),
r.intrinsic.matrix(), r.intrinsic.distCoeffs.Mat(), size,
- R, t, R1, R2, P1, P2, Q, 0, 1.0);
+ R, t, R1, R2, P1, P2, Q, cv::CALIB_ZERO_DISPARITY, 1.0);
cv::initUndistortRectifyMap(l.intrinsic.matrix(), l.intrinsic.distCoeffs.Mat(),
R1, P1, size, CV_32FC1, map1, map2);
@@ -288,7 +295,7 @@ void ExtrinsicCalibration::stereoRectify(int cl, int cr,
state_.maps2[cl].upload(map2);
cv::initUndistortRectifyMap(r.intrinsic.matrix(), r.intrinsic.distCoeffs.Mat(),
- R1, P1, size, CV_32FC1, map1, map2);
+ R2, P2, size, CV_32FC1, map1, map2);
state_.maps1[cr].upload(map1);
state_.maps2[cr].upload(map2);
}
@@ -298,14 +305,6 @@ void ExtrinsicCalibration::run() {
future_ = ftl::pool.push([this](int id) {
try {
- for (int c = 0; c < cameraCount(); c += 2) {
- auto t1 = state_.calib.calibration(c).extrinsic.tvec;
- auto t2 = state_.calib.calibration(c + 1).extrinsic.tvec;
-
- LOG(INFO) << "baseline (" << c << ", " << c + 1 << "): "
- << cv::norm(t1, t2);
- }
-
auto opt = state_.calib.options();
opt.optimize_intrinsic = !(state_.flags & Flags::FIX_INTRINSIC);
opt.rational_model = state_.flags & Flags::RATIONAL_MODEL;
@@ -316,15 +315,11 @@ void ExtrinsicCalibration::run() {
opt.loss = (state_.flags & Flags::LOSS_CAUCHY) ?
ftl::calibration::BundleAdjustment::Options::Loss::CAUCHY :
ftl::calibration::BundleAdjustment::Options::Loss::SQUARED;
+ opt.use_nonmonotonic_steps = state_.flags & Flags::NONMONOTONIC_STEP;
state_.calib.setOptions(opt);
state_.calib.run();
- for (int c = 0; c < cameraCount(); c += 2) {
- auto t1 = state_.calib.calibrationOptimized(c).extrinsic.tvec;
- auto t2 = state_.calib.calibrationOptimized(c + 1).extrinsic.tvec;
- }
-
// Rectification maps for visualization; stereo cameras assumed
// if non-stereo cameras added visualization/grouping (by index)
// has to be different.
@@ -336,6 +331,9 @@ void ExtrinsicCalibration::run() {
auto l = state_.calib.calibrationOptimized(c);
auto r = state_.calib.calibrationOptimized(c + 1);
stereoRectify(c, c + 1, l, r);
+
+ LOG(INFO) << "baseline (" << c << ", " << c + 1 << "): "
+ << cv::norm(l.extrinsic.tvec, r.extrinsic.tvec);
}
}
catch (ftl::exception &ex) {
diff --git a/applications/gui2/src/views/calibration/extrinsicview.cpp b/applications/gui2/src/views/calibration/extrinsicview.cpp
index 91b3b946d..00a3713a7 100644
--- a/applications/gui2/src/views/calibration/extrinsicview.cpp
+++ b/applications/gui2/src/views/calibration/extrinsicview.cpp
@@ -303,9 +303,19 @@ void ExtrinsicCalibrationView::CalibrationWindow::build() {
else { flags &= ~ExtrinsicCalibration::Flags::LOSS_CAUCHY; }
});
+ auto* nstep = new nanogui::CheckBox(wfreeze, "Non-monotonic step");
+ nstep->setChecked(flags_ & ExtrinsicCalibration::Flags::NONMONOTONIC_STEP);
+ nstep->setCallback([&flags = flags_](bool v) {
+ if (v) { flags |= ExtrinsicCalibration::Flags::NONMONOTONIC_STEP; }
+ else { flags &= ~ExtrinsicCalibration::Flags::NONMONOTONIC_STEP; }
+ });
+
auto* fall = new nanogui::CheckBox(wfreeze, "Freeze all intrinsic paramters");
fall->setChecked(flags_ & ExtrinsicCalibration::Flags::FIX_INTRINSIC);
- fall->setCallback([&flags = flags_](bool v) {
+ fall->setCallback([&flags = flags_, wfreeze](bool v) {
+ for (int i = 3; i < wfreeze->childCount(); i++) {
+ wfreeze->childAt(i)->setEnabled(!v);
+ }
if (v) { flags |= ExtrinsicCalibration::Flags::FIX_INTRINSIC; }
else { flags &= ~ExtrinsicCalibration::Flags::FIX_INTRINSIC; }
});
@@ -338,10 +348,11 @@ void ExtrinsicCalibrationView::CalibrationWindow::build() {
else { flags &= ~ExtrinsicCalibration::Flags::ZERO_DISTORTION; }
});
- fall->setCallback([wfreeze](bool value){
- for (int i = 2; i < wfreeze->childCount(); i++) {
- wfreeze->childAt(i)->setEnabled(!value);
- }
+ auto* rdist = new nanogui::CheckBox(wfreeze, "Rational distortion model");
+ rdist->setChecked(flags_ & ExtrinsicCalibration::Flags::RATIONAL_MODEL);
+ rdist->setCallback([&flags = flags_](bool v) {
+ if (v) { flags |= ExtrinsicCalibration::Flags::RATIONAL_MODEL; }
+ else { flags &= ~ExtrinsicCalibration::Flags::RATIONAL_MODEL; }
});
/* Needs thinking: visualize visibility graph? Use earlier alignment (if
diff --git a/components/calibration/include/ftl/calibration/optimize.hpp b/components/calibration/include/ftl/calibration/optimize.hpp
index eeaf6d9de..17dd1b42c 100644
--- a/components/calibration/include/ftl/calibration/optimize.hpp
+++ b/components/calibration/include/ftl/calibration/optimize.hpp
@@ -129,6 +129,8 @@ public:
Loss loss = Loss::SQUARED;
+ bool use_nonmonotonic_steps = false;
+
// fix_camera_extrinsic and fix_camera_intrinsic overlap with some of
// the generic options. The more generic setting is always used, the
// specific extrinsic/intrinsic options are applied on top of those.
@@ -150,7 +152,7 @@ public:
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
+ /// distortion set to zero
bool zero_distortion = false;
bool optimize_intrinsic = true;
@@ -217,7 +219,6 @@ protected:
void _buildProblem(ceres::Problem& problem, const BundleAdjustment::Options& options);
void _buildBundleAdjustmentProblem(ceres::Problem& problem, const BundleAdjustment::Options& options);
- void _buildLengthProblem(ceres::Problem& problem, const BundleAdjustment::Options& options);
private:
// point to be optimized and corresponding observations
diff --git a/components/calibration/src/extrinsic.cpp b/components/calibration/src/extrinsic.cpp
index 615b13034..4e59a5466 100644
--- a/components/calibration/src/extrinsic.cpp
+++ b/components/calibration/src/extrinsic.cpp
@@ -613,7 +613,7 @@ double ExtrinsicCalibration::optimize() {
p = {px[m], py[m], pz[m]};
}
- // TODO: desgin more meaningful check
+ // TODO: desgin better check
if (cv::norm(absdiff(px, py, pz)) > threshold_bad_) {
n_points_bad++;
continue;
@@ -641,6 +641,8 @@ double ExtrinsicCalibration::optimize() {
updateStatus_("Bundle adjustment");
options_.verbose = true;
+ options_.max_iter = 250; // should converge much earlier
+
LOG(INFO) << "fix intrinsics: " << (options_.optimize_intrinsic ? "no" : "yes");
LOG(INFO) << "fix focal: " << (options_.fix_focal ? "yes" : "no");
LOG(INFO) << "fix principal point: " << (options_.fix_principal_point ? "yes" : "no");
@@ -661,6 +663,7 @@ double ExtrinsicCalibration::optimize() {
}
rmse_total_ = ba.reprojectionError();
+ LOG(INFO) << "reprojection error (all cameras): " << rmse_total_;
return rmse_total_;
}
diff --git a/components/calibration/src/optimize.cpp b/components/calibration/src/optimize.cpp
index 04722774d..76db14c07 100644
--- a/components/calibration/src/optimize.cpp
+++ b/components/calibration/src/optimize.cpp
@@ -154,6 +154,7 @@ Mat Camera::distortionCoefficients() const {
Mat Camera::rvec() const {
cv::Mat rvec(cv::Size(3, 1), CV_64FC1);
+ CHECK(rvec.step1() == 3);
ceres::QuaternionToAngleAxis(data + Parameter::ROTATION,
(double*)(rvec.data));
return rvec;
@@ -164,8 +165,11 @@ Mat Camera::tvec() const {
}
Mat Camera::rmat() const {
- Mat R;
- cv::Rodrigues(rvec(), R);
+ cv::Mat R(cv::Size(3, 3), CV_64FC1);
+ CHECK(R.step1() == 3);
+ ceres::QuaternionToRotation<double>(data + Parameter::ROTATION,
+ ceres::RowMajorAdapter3x3<double>((double*)(R.data)));
+
return R;
}
@@ -213,8 +217,8 @@ struct ReprojectionError {
p[1] += camera[Camera::Parameter::TY];
p[2] += camera[Camera::Parameter::TZ];
- T x = T(p[0]) / p[2];
- T y = T(p[1]) / p[2];
+ T x = p[0] / p[2];
+ T y = p[1] / p[2];
// Intrinsic parameters
const T& f = camera[Camera::Parameter::F];
@@ -605,6 +609,8 @@ void BundleAdjustment::run(const BundleAdjustment::Options &bundle_adjustment_op
options.num_threads = bundle_adjustment_options.num_threads;
}
+ options.use_nonmonotonic_steps = bundle_adjustment_options.use_nonmonotonic_steps;
+
ceres::Solver::Summary summary;
ceres::Solve(options, &problem, &summary);
diff --git a/components/calibration/src/structures.cpp b/components/calibration/src/structures.cpp
index 2c44a6b02..c464e80b9 100644
--- a/components/calibration/src/structures.cpp
+++ b/components/calibration/src/structures.cpp
@@ -146,7 +146,7 @@ CalibrationData::Extrinsic::Extrinsic(cv::InputArray R, cv::InputArray t) {
}
cv::Mat CalibrationData::Extrinsic::matrix() const {
- cv::Mat T(cv::Size(4, 4), CV_64FC1, cv::Scalar(0));
+ cv::Mat T(cv::Size(4, 4), CV_64FC1, cv::Scalar(0.0));
cv::Rodrigues(rvec, T(cv::Rect(0, 0, 3, 3)));
T.at<double>(0, 3) = tvec[0];
T.at<double>(1, 3) = tvec[1];
@@ -160,7 +160,7 @@ cv::Mat CalibrationData::Extrinsic::matrix() const {
}
cv::Mat CalibrationData::Extrinsic::rmat() const {
- cv::Mat R(cv::Size(3, 3), CV_64FC1, cv::Scalar(0));
+ cv::Mat R(cv::Size(3, 3), CV_64FC1, cv::Scalar(0.0));
cv::Rodrigues(rvec, R);
return R;
}
--
GitLab