From 2aed08ff83bfc3e43a778e5a6f62994f5e82179a Mon Sep 17 00:00:00 2001
From: Sebastian Hahta <joseha@utu.fi>
Date: Thu, 6 Aug 2020 12:33:14 +0300
Subject: [PATCH] extrinsic can be freezed per camera
---
.../src/modules/calibration/extrinsic.cpp | 8 ++-
.../src/views/calibration/extrinsicview.cpp | 7 +-
.../include/ftl/calibration/extrinsic.hpp | 15 +++--
components/calibration/src/extrinsic.cpp | 66 +++++++++----------
4 files changed, 52 insertions(+), 44 deletions(-)
diff --git a/applications/gui2/src/modules/calibration/extrinsic.cpp b/applications/gui2/src/modules/calibration/extrinsic.cpp
index bf7c892f3..93da1494d 100644
--- a/applications/gui2/src/modules/calibration/extrinsic.cpp
+++ b/applications/gui2/src/modules/calibration/extrinsic.cpp
@@ -370,12 +370,14 @@ void ExtrinsicCalibration::run() {
for (int c = 0; c < cameraCount(); c += 2) {
auto l = state_.calib.calibrationOptimized(c);
auto r = state_.calib.calibrationOptimized(c + 1);
- LOG(INFO) << c << ": rvec " << l.extrinsic.rvec << "; tvec " << l.extrinsic.tvec;
- LOG(INFO) << c + 1 << ": rvec " << r.extrinsic.rvec << "; tvec " << r.extrinsic.tvec;
stereoRectify(c, c + 1, l, r);
+ /*LOG(INFO) << c << ": rvec " << l.extrinsic.rvec
+ << "; tvec " << l.extrinsic.tvec;
+ LOG(INFO) << c + 1 << ": rvec " << r.extrinsic.rvec
+ << "; tvec " << r.extrinsic.tvec;
LOG(INFO) << "baseline (" << c << ", " << c + 1 << "): "
- << cv::norm(l.extrinsic.tvec, r.extrinsic.tvec);
+ << 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 c4071ccf1..c79d52e9b 100644
--- a/applications/gui2/src/views/calibration/extrinsicview.cpp
+++ b/applications/gui2/src/views/calibration/extrinsicview.cpp
@@ -405,6 +405,7 @@ void ExtrinsicCalibrationView::CalibrationWindow::build() {
}
////////////////////////////////////////////////////////////////////////////
+ // TODO: selecting camera should also enable use existing above for same c
new nanogui::Label(wfreeze, "Fix extrinsics for cameras: ");
auto* fix_extrinsics = new nanogui::Widget(wfreeze);
@@ -586,7 +587,7 @@ private:
std::set<int> rows_;
std::map<int, nanogui::Color> colors_;
- int n_colors_ = 16;
+ int n_colors_ = 8;
float alpha_threshold_ = 2.0f;
public:
@@ -645,14 +646,14 @@ void StereoCalibrationImageView::draw(NVGcontext* ctx) {
nvgStrokeWidth(ctx, swidth);
nvgStroke(ctx);
- if (swidth*0.5f > alpha_threshold_) {
+ /*if (swidth*0.5f > alpha_threshold_) {
nvgBeginPath(ctx);
nvgMoveTo(ctx, x, p.y() - swidth*0.5f);
nvgLineTo(ctx, x + w, p.y() - swidth*0.5f);
nvgStrokeColor(ctx, nvgRGBA(0, 0, 0, 196));
nvgStrokeWidth(ctx, 1.0f);
nvgStroke(ctx);
- }
+ }*/
nvgResetScissor(ctx);
y_im += h;
}
diff --git a/components/calibration/include/ftl/calibration/extrinsic.hpp b/components/calibration/include/ftl/calibration/extrinsic.hpp
index ef2e48595..645674ddb 100644
--- a/components/calibration/include/ftl/calibration/extrinsic.hpp
+++ b/components/calibration/include/ftl/calibration/extrinsic.hpp
@@ -206,19 +206,19 @@ public:
MSGPACK_DEFINE(points_, mask_, pairs_, calib_, is_calibrated_);
protected:
- /** Calculate initial pose and triangulate points **/
+ /** Calculate initial pose and triangulate points for two cameras **/
void calculatePairPose(unsigned int c1, unsigned int c2);
/** Only triangulate points using existing calibration */
void triangulate(unsigned int c1, unsigned int c2);
- /** Initial pairwise calibration and triangulation. */
+ /** (1) Initial pairwise calibration and triangulation. */
void calculatePairPoses();
- /** Calculate initial poses from pairs for non calibrated cameras */
+ /** (2) Calculate initial poses from pairs for non calibrated cameras */
void calculateInitialPoses();
- /** Bundle adjustment on initial poses and triangulations. */
+ /** (3) Bundle adjustment on initial poses and triangulations. */
double optimize();
/** Select optimal camera for chains. Optimal camera has most visibility and
@@ -241,7 +241,12 @@ private:
// skipped (and points are directly triangulated)
std::vector<bool> is_calibrated_;
- int min_points_ = 64; // minimum number of points required for pair calibration
+ // prune points which have higher reprojection error than given threshold
+ // and re-run bundle adjustment. (might be able to remove some problematic
+ // observations from extreme angles etc.)
+ std::vector<double> prune_observations_ = {2.5, 2.0, 2.0, 1.0};
+
+ int min_obs_ = 64; // minimum number of observations required for pair calibration
// TODO: add map {c1,c2} for existing calibration which is used if available.
//
std::shared_ptr<std::string> status_;
diff --git a/components/calibration/src/extrinsic.cpp b/components/calibration/src/extrinsic.cpp
index 2857527b3..ea575608b 100644
--- a/components/calibration/src/extrinsic.cpp
+++ b/components/calibration/src/extrinsic.cpp
@@ -358,14 +358,6 @@ double calibratePair(const cv::Mat &K1, const cv::Mat &D1,
// ==== Extrinsic Calibration ==================================================
-/*unsigned int ExtrinsicCalibration::addCamera(const CalibrationData::Intrinsic &c) {
- unsigned int idx = calib_.size();
- calib_.push_back({c, {}});
- calib_optimized_.push_back(calib_.back());
- is_calibrated_.push_back(false);
- return idx;
-}*/
-
unsigned int ExtrinsicCalibration::addCamera(const CalibrationData::Calibration &c) {
unsigned int idx = calib_.size();
calib_.push_back(c);
@@ -374,18 +366,6 @@ unsigned int ExtrinsicCalibration::addCamera(const CalibrationData::Calibration
return idx;
}
-/*unsigned int ExtrinsicCalibration::addStereoCamera(const CalibrationData::Intrinsic &c1, const CalibrationData::Intrinsic &c2) {
- unsigned int idx = calib_.size();
- calib_.push_back({c1, {}});
- calib_optimized_.push_back(calib_.back());
- calib_.push_back({c2, {}});
- calib_optimized_.push_back(calib_.back());
- is_calibrated_.push_back(false);
- is_calibrated_.push_back(false);
- mask_.insert({idx, idx + 1});
- return idx;
-}*/
-
unsigned int ExtrinsicCalibration::addStereoCamera(const CalibrationData::Calibration &c1, const CalibrationData::Calibration &c2) {
unsigned int idx = calib_.size();
calib_.push_back({c1.intrinsic, c1.extrinsic});
@@ -515,9 +495,9 @@ void ExtrinsicCalibration::calculatePairPoses() {
}
// require minimum number of visible points
- if (points().visibility().count(c1, c2) < min_points_) {
+ if (points().visibility().count(c1, c2) < min_obs_) {
LOG(WARNING) << "skipped pair (" << c1 << ", " << c2 << "), not enough points";
- return;
+ continue;
}
if (is_calibrated_[c1] && is_calibrated_[c2]) {
@@ -552,9 +532,7 @@ void ExtrinsicCalibration::calculateInitialPoses() {
// mask stereo cameras (do not pairwise calibrate a stereo pair; unreliable)
auto visibility = points_.visibility();
- for (const auto& m: mask_) {
- visibility.mask(m.first, m.second);
- }
+ for (const auto& [c1, c2]: mask_) { visibility.mask(c1, c2); }
// mask cameras which did not have enough points TODO: triangulation later
// would still be useful (calculate initial poses, then triangulate)
@@ -565,7 +543,9 @@ void ExtrinsicCalibration::calculateInitialPoses() {
}
}}
- // select optimal camera to calculate chains to
+ // select optimal camera to calculate chains to. TODO: if any of the
+ // cameras is already calibrated, use most visible calibrated camera as
+ // target camera.
auto c_ref = selectOptimalCamera();
auto paths = visibility.shortestPath(c_ref);
@@ -637,6 +617,18 @@ static cv::Point3d absdiff(const std::vector<double> &xs, const std::vector<doub
double ExtrinsicCalibration::optimize() {
+ // triangulate points for stereo pairs (all points triangulated after this)
+ updateStatus_("Triangulating remaining points");
+ for (const auto& [c1, c2]: mask_) {
+ if (points().visibility().count(c1, c2) >= min_obs_) {
+ triangulate(c1, c2);
+ }
+ else {
+ LOG(INFO) << "Skipping triangulation for pair " << c1 << ", " << c2;
+ }
+ }
+
+ // Build BA
BundleAdjustment ba;
std::vector<Camera> cameras;
std::vector<cv::Mat> T; // camera to world
@@ -660,7 +652,6 @@ double ExtrinsicCalibration::optimize() {
// triangulated multiple times: use median values. Note T[] contains
// inverse transformations, as points are transformed from camera to world
// (instead the other way around by parameters in cameras[]).
-
updateStatus_("Calculating points in world coordinates");
// NOTE: above CalibrationPoints datastructure not optimal regarding how
@@ -724,7 +715,7 @@ double ExtrinsicCalibration::optimize() {
// TODO: desgin better check
if (cv::norm(absdiff(px, py, pz)) > threshold_bad_) {
n_points_bad++;
- continue;
+ //continue;
}
ba.addPoint(vis, obs, p);
@@ -740,7 +731,7 @@ double ExtrinsicCalibration::optimize() {
}
if (float(n_points_missing)/float(n_points - n_points_bad) > threhsold_warning_) {
- // low number of points; most points only visible in pairs?
+ // this should not happen any more (all points should be triangulated).
LOG(WARNING) << "Large number of points skipped. Are there enough "
"visible points between stereo camera pairs?";
}
@@ -755,10 +746,19 @@ double ExtrinsicCalibration::optimize() {
LOG(INFO) << "fix distortion: " << (options_.fix_distortion ? "yes" : "no");
ba.run(options_);
- LOG(INFO) << "removed points: " << ba.removeObservations(2.0);
- ba.run(options_);
- LOG(INFO) << "removed points: " << ba.removeObservations(1.0);
- ba.run(options_);
+
+ int n_removed = 0;
+ for (const auto& t : prune_observations_) {
+ n_removed += ba.removeObservations(t);
+ if (float(n_removed)/float(n_points) > threhsold_warning_) {
+ LOG(WARNING) << "significant number of observations removed";
+ break;
+ }
+ else {
+ LOG(INFO) << "removed observations: " << n_removed;
+ ba.run(options_);
+ }
+ }
calib_optimized_.resize(calib_.size());
rmse_.resize(calib_.size());
--
GitLab