From 93ce01063a60e75612717101eeb667027fc8f0c8 Mon Sep 17 00:00:00 2001
From: Sebastian Hahta <joseha@utu.fi>
Date: Thu, 23 Jul 2020 13:25:54 +0300
Subject: [PATCH] more calibration
---
.../src/modules/calibration/extrinsic.cpp | 22 ++---
applications/gui2/src/screen.cpp | 12 ++-
applications/gui2/src/screen.hpp | 5 +-
applications/gui2/src/view.cpp | 5 +-
.../include/ftl/calibration/extrinsic.hpp | 7 ++
.../include/ftl/calibration/structures.hpp | 2 +
components/calibration/src/extrinsic.cpp | 80 ++++++++++++++++---
components/calibration/src/optimize.cpp | 7 +-
components/calibration/src/parameters.cpp | 2 +-
components/calibration/src/structures.cpp | 11 ++-
.../cpp/include/ftl/utility/msgpack.hpp | 5 +-
11 files changed, 118 insertions(+), 40 deletions(-)
diff --git a/applications/gui2/src/modules/calibration/extrinsic.cpp b/applications/gui2/src/modules/calibration/extrinsic.cpp
index a12aa4b93..6b006ab2a 100644
--- a/applications/gui2/src/modules/calibration/extrinsic.cpp
+++ b/applications/gui2/src/modules/calibration/extrinsic.cpp
@@ -299,23 +299,23 @@ void ExtrinsicCalibration::run() {
try {
state_.calib.run();
LOG(INFO) << "Before:"; // DEBUG INFO
- for (int c1 = 0; c1 < cameraCount(); c1++) {
- for (int c2 = c1; c2 < cameraCount(); c2++) {
+ for (int c1 = 0; c1 < cameraCount(); c1 += 2) {
+
auto t1 = state_.calib.calibration(c1).extrinsic.tvec;
- auto t2 = state_.calib.calibration(c2).extrinsic.tvec;
+ auto t2 = state_.calib.calibration(c1+1).extrinsic.tvec;
- LOG(INFO) << "baseline (" << c1 << ", " << c2 << "): "
+ LOG(INFO) << "baseline (" << c1 << ", " << c1+1 << "): "
<< cv::norm(t1, t2);
- }}
+ }
LOG(INFO) << "After:"; // DEBUG INFO
- for (int c1 = 0; c1 < cameraCount(); c1++) {
- for (int c2 = c1; c2 < cameraCount(); c2++) {
- auto t1 = state_.calib.calibrationOptimized(c1).extrinsic.tvec;
- auto t2 = state_.calib.calibrationOptimized(c2).extrinsic.tvec;
+ for (int c1 = 0; c1 < cameraCount(); c1 += 2) {
- LOG(INFO) << "baseline (" << c1 << ", " << c2 << "): "
+ auto t1 = state_.calib.calibration(c1).extrinsic.tvec;
+ auto t2 = state_.calib.calibration(c1+1).extrinsic.tvec;
+
+ LOG(INFO) << "baseline (" << c1 << ", " << c1+1 << "): "
<< cv::norm(t1, t2);
- }}
+ }
// Rectification maps for visualization; stereo cameras assumed
// if non-stereo cameras added visualization/grouping (by index)
diff --git a/applications/gui2/src/screen.cpp b/applications/gui2/src/screen.cpp
index 5ec71e4c2..fe7747306 100644
--- a/applications/gui2/src/screen.cpp
+++ b/applications/gui2/src/screen.cpp
@@ -45,8 +45,7 @@ Screen::Screen() :
toolbar_->setFixedSize({toolbar_->width(), s[1]});
toolbar_->setPosition({0, 0});
if (active_view_) {
- active_view_->setFixedSize(viewSize());
- // active_view_->setFixedSize({s[0], s[1]});
+ active_view_->setSize(viewSize(s));
}
performLayout();
});
@@ -98,10 +97,15 @@ void Screen::redraw() {
glfwPostEmptyEvent();
}
+nanogui::Vector2i Screen::viewSize(const nanogui::Vector2i &ws) {
+ return {ws.x() - toolbar_->width(), ws.y()};
+}
+
nanogui::Vector2i Screen::viewSize() {
- return {width() - toolbar_->width(), height()};
+ return viewSize(size());
}
+
void Screen::showError(const std::string&title, const std::string& msg) {
if (msgerror_) { return; }
msgerror_ = new nanogui::MessageDialog
@@ -208,4 +212,4 @@ void Screen::drawAll() {
}
last_draw_time_ = now;
nanogui::Screen::drawAll();
-}
\ No newline at end of file
+}
diff --git a/applications/gui2/src/screen.hpp b/applications/gui2/src/screen.hpp
index b46f6dc46..a5e47d6a3 100644
--- a/applications/gui2/src/screen.hpp
+++ b/applications/gui2/src/screen.hpp
@@ -73,13 +73,14 @@ public:
nanogui::Color getColor(const std::string &name);
void setColor(const std::string &name, const nanogui::Color &c);
- nanogui::Vector2i viewSize();
-
// Implement in View or Screen? Add ID (address of creating instance)
// to each error to prevent spam?
/** Show error message popup */
void showError(const std::string& title, const std::string &msg);
+ nanogui::Vector2i viewSize(const nanogui::Vector2i &ws);
+ nanogui::Vector2i viewSize();
+
private:
Module* addModule_(const std::string &name, Module* ptr);
diff --git a/applications/gui2/src/view.cpp b/applications/gui2/src/view.cpp
index 9b1466019..67297b6b4 100644
--- a/applications/gui2/src/view.cpp
+++ b/applications/gui2/src/view.cpp
@@ -5,7 +5,6 @@
using ftl::gui2::View;
-View::View(Screen* screen) : nanogui::Widget(screen) {
- setFixedSize(screen->viewSize());
- screen_ = screen;
+View::View(Screen* screen) : nanogui::Widget(screen), screen_(screen) {
+ setSize(screen_->viewSize());
}
diff --git a/components/calibration/include/ftl/calibration/extrinsic.hpp b/components/calibration/include/ftl/calibration/extrinsic.hpp
index f68ad4f7f..58f954fa2 100644
--- a/components/calibration/include/ftl/calibration/extrinsic.hpp
+++ b/components/calibration/include/ftl/calibration/extrinsic.hpp
@@ -1,5 +1,7 @@
#pragma once
+#include <ftl/utility/msgpack.hpp>
+
#include <ftl/calibration/visibility.hpp>
#include <ftl/calibration/structures.hpp>
#include <ftl/calibration/optimize.hpp>
@@ -89,6 +91,8 @@ public:
/** Get all points. See Points struct. */
const std::vector<Points>& all() { return points_; }
+
+
protected:
bool hasCamera(unsigned int c);
void setCamera(unsigned int c);
@@ -208,6 +212,9 @@ private:
// TODO: add map {c1,c2} for existing calibration which is used if available.
//
std::shared_ptr<std::string> status_;
+
+ float threshold_bad_ = 0.25; // theshold for point to be skipped (cm)
+ float threhsold_warning_ = 0.1; // theshold for warning message (%)
};
diff --git a/components/calibration/include/ftl/calibration/structures.hpp b/components/calibration/include/ftl/calibration/structures.hpp
index ab8544763..e0eeb2fce 100644
--- a/components/calibration/include/ftl/calibration/structures.hpp
+++ b/components/calibration/include/ftl/calibration/structures.hpp
@@ -77,6 +77,8 @@ struct CalibrationData {
void set(const cv::Mat &T);
void set(cv::InputArray R, cv::InputArray t);
+ Extrinsic inverse() const;
+
/** get as a 4x4 matrix */
cv::Mat matrix() const;
/** get 3x3 rotation matrix */
diff --git a/components/calibration/src/extrinsic.cpp b/components/calibration/src/extrinsic.cpp
index e3c3fbf80..971ca74ec 100644
--- a/components/calibration/src/extrinsic.cpp
+++ b/components/calibration/src/extrinsic.cpp
@@ -318,7 +318,6 @@ double calibratePair(const cv::Mat &K1, const cv::Mat &D1,
double s = ftl::calibration::optimizeScale(object_points, points_out);
t = t * s;
- //transform::inverse(R, t);
auto params1 = Camera(K1, D1, cv::Mat::eye(3, 3, CV_64FC1), cv::Mat::zeros(3, 1, CV_64FC1), {0, 0});
auto params2 = Camera(K2, D2, R, t, {0, 0});
@@ -416,8 +415,10 @@ void ExtrinsicCalibration::calculatePairPoses() {
auto object = points().getObject(0);
cv::Mat R, t;
std::vector<cv::Point3d> points3d;
- calibratePair(K1, distCoeffs1, K2, distCoeffs2, pts[0], pts[1], object,
- R, t, points3d, true);
+ double rms = calibratePair(K1, distCoeffs1, K2, distCoeffs2,
+ pts[0], pts[1], object, R, t, points3d, true);
+
+ LOG(INFO) << "Pair error " << rms;
points().setTriangulatedPoints(c1, c2, points3d);
@@ -487,6 +488,28 @@ static std::vector<bool> visibility(unsigned int ncameras, uint64_t visible) {
return res;
}
+static cv::Point3d absdiff(const std::vector<double> &xs, const std::vector<double> &ys, const std::vector<double> &zs) {
+ double minx = INFINITY;
+ double maxx = -INFINITY;
+ for (auto x : xs) {
+ minx = std::min(minx, x);
+ maxx = std::max(maxx, x);
+ }
+ double miny = INFINITY;
+ double maxy = -INFINITY;
+ for (auto y : ys) {
+ miny = std::min(miny, y);
+ maxy = std::max(maxy, y);
+ }
+ double minz = INFINITY;
+ double maxz = -INFINITY;
+ for (auto z : zs) {
+ minz = std::min(minz, z);
+ maxz = std::max(maxz, z);
+ }
+ return {abs(minx - maxx), abs(miny - maxy), abs(minz - maxz)};
+}
+
double ExtrinsicCalibration::optimize() {
BundleAdjustment ba;
@@ -495,8 +518,9 @@ double ExtrinsicCalibration::optimize() {
unsigned int ncameras = calib_.size();
for (const auto& c : calib_) {
- cameras.push_back(Camera(c));
- cameras.back().setDistortion(cv::Mat(cv::Size{12, 1}, CV_64FC1, 0.0));
+ auto cam = c;
+ cameras.push_back(Camera(cam));
+ //cameras.back().setDistortion(cv::Mat(cv::Size{12, 1}, CV_64FC1, 0.0));
}
for (auto &c : cameras ) {
ba.addCamera(c);
@@ -507,7 +531,6 @@ double ExtrinsicCalibration::optimize() {
// inverse transformations, as points are transformed from camera to world
// (instead the other way around by parameters in cameras[]).
- std::vector<cv::Point3d> pointsw;
std::vector<cv::Mat> T;
for (const auto &c : cameras) {
T.push_back(c.extrinsicMatrixInverse());
@@ -521,13 +544,21 @@ double ExtrinsicCalibration::optimize() {
// suited as they can be easily interpreted as flat arrays or
// multi-dimensional.
+ int n_points_bad = 0;
+ int n_points_missing = 0;
+ int n_points = 0;
+
for (const auto& itm : points_.all()) {
auto sz = points_.getObject(itm.object).size();
auto vis = visibility(ncameras, itm.cameras);
for (unsigned int i = 0; i < sz; i++) {
- // observation and triangulated coordinates
- std::vector<cv::Point2d> obs(ncameras, {0, 0});
+ n_points++;
+
+ // observation and triangulated coordinates; Use {NAN, NAN} for
+ // non-visible points (if those are used by mistake, Ceres will
+ // fail with error message).
+ std::vector<cv::Point2d> obs(ncameras, {NAN, NAN});
std::vector<double> px;
std::vector<double> py;
std::vector<double> pz;
@@ -547,10 +578,12 @@ double ExtrinsicCalibration::optimize() {
std::sort(px.begin(), px.end());
std::sort(py.begin(), py.end());
std::sort(pz.begin(), pz.end());
+ cv::Point3d p;
unsigned int n = px.size();
unsigned int m = n / 2;
if (m == 0) {
+ n_points_missing++;
break;
// not triangulated (see earlier steps)
// TODO: triangulate here
@@ -559,18 +592,42 @@ double ExtrinsicCalibration::optimize() {
// mean of two points if number of points even
cv::Point3d p1 = {px[m - 1], py[m - 1], pz[m - 1]};
cv::Point3d p2 = {px[m], py[m], pz[m]};
- pointsw.push_back((p1 + p2)/2.0);
+ p = (p1 + p2)/2.0;
}
else {
- pointsw.push_back({px[m], py[m], pz[m]});
+ p = {px[m], py[m], pz[m]};
}
- ba.addPoint(vis, obs, pointsw.back());
+ if (cv::norm(absdiff(px, py, pz)) > threshold_bad_) {
+ n_points_bad++;
+ continue;
+ }
+
+ ba.addPoint(vis, obs, p);
}
}
+ if (float(n_points_bad)/float(n_points - n_points_missing) > threhsold_warning_) {
+ // print wanrning message; calibration possibly fails if triangulation
+ // was very low quality (more than 10% bad points)
+ LOG(ERROR) << "Large variation in "<< n_points_bad << " "
+ "triangulated points. Are initial intrinsic parameters "
+ "good?";
+ }
+
+ if (float(n_points_missing)/float(n_points - n_points_bad) > threhsold_warning_) {
+ // low number of points; most points only visible in pairs?
+ LOG(WARNING) << "Large number of points skipped. Are there enough "
+ "visible points between stereo camera pairs? (TODO: "
+ "implement necessary triangulation after pair "
+ "calibration)";
+ }
+
updateStatus_("Bundle adjustment");
options_.verbose = true;
+ for (unsigned int i = 0; i < cameras.size(); i++) {
+ LOG(INFO) << "RMS (camera " << i << ": " << ba.reprojectionError(i);
+ }
ba.run(options_);
calib_optimized_.resize(calib_.size());
@@ -580,6 +637,7 @@ double ExtrinsicCalibration::optimize() {
calib_optimized_[i] = calib_[i];
calib_optimized_[i].intrinsic.set(intr.matrix(), intr.distCoeffs.Mat(), intr.resolution);
calib_optimized_[i].extrinsic.set(extr.rvec, extr.tvec);
+ LOG(INFO) << "RMS (camera " << i << ": " << ba.reprojectionError(i);
}
return ba.reprojectionError();
diff --git a/components/calibration/src/optimize.cpp b/components/calibration/src/optimize.cpp
index 7b3030363..da7680e7e 100644
--- a/components/calibration/src/optimize.cpp
+++ b/components/calibration/src/optimize.cpp
@@ -38,6 +38,7 @@ struct ReprojectionError {
*
* 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) {}
@@ -78,8 +79,6 @@ struct ReprojectionError {
const T r4 = r2*r2;
const T r6 = r4*r2;
- // see also https://github.com/opencv/opencv/blob/b698d0a6ee12342a87b8ad739d908fd8d7ff1fca/modules/calib3d/src/calibration.cpp#L774
-
// Radial distortion
const T cdist = T(1.0) + k1*r2 + k2*r4 + k3*r6;
// Radial distortion: rational model
@@ -91,8 +90,8 @@ struct ReprojectionError {
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_x = f*x/*xd*/ + cx;
+ T predicted_y = f*y/*yd*/ + cy;
// Error: the difference between the predicted and observed position
residuals[0] = predicted_x - T(observed_x);
diff --git a/components/calibration/src/parameters.cpp b/components/calibration/src/parameters.cpp
index 5549f75bd..2c9c2f1a3 100644
--- a/components/calibration/src/parameters.cpp
+++ b/components/calibration/src/parameters.cpp
@@ -169,7 +169,7 @@ Mat Camera::extrinsicMatrix() const {
}
Mat Camera::extrinsicMatrixInverse() const {
- return extrinsicMatrix().inv();
+ return transform::inverse(extrinsicMatrix());
}
////////////////////////////////////////////////////////////////////////////////
diff --git a/components/calibration/src/structures.cpp b/components/calibration/src/structures.cpp
index ce67daec3..a7c498860 100644
--- a/components/calibration/src/structures.cpp
+++ b/components/calibration/src/structures.cpp
@@ -91,15 +91,14 @@ void CalibrationData::Extrinsic::set(const cv::Mat& T) {
if (T.type() != CV_64FC1) {
throw ftl::exception("Input must be CV_64FC1");
}
- if (ftl::calibration::validate::pose(T)) {
+ if (!ftl::calibration::validate::pose(T)) {
throw ftl::exception("T is not a valid transform matrix");
}
cv::Rodrigues(T(cv::Rect(0, 0, 3, 3)), rvec);
tvec[0] = T.at<double>(0, 3);
tvec[1] = T.at<double>(1, 3);
- tvec[2] = T.at<double>(3, 3);
- tvec[3] = 1.0;
+ tvec[2] = T.at<double>(2, 3);
}
void CalibrationData::Extrinsic::set(cv::InputArray R, cv::InputArray t) {
@@ -156,6 +155,12 @@ cv::Mat CalibrationData::Extrinsic::matrix() const {
return T;
}
+ CalibrationData::Extrinsic CalibrationData::Extrinsic::inverse() const {
+ cv::Mat T = matrix();
+ ftl::calibration::transform::inverse(T);
+ return CalibrationData::Extrinsic(T);
+}
+
cv::Mat CalibrationData::Extrinsic::rmat() const {
cv::Mat R(cv::Size(3, 3), CV_64FC1, cv::Scalar(0));
cv::Rodrigues(rvec, R);
diff --git a/components/common/cpp/include/ftl/utility/msgpack.hpp b/components/common/cpp/include/ftl/utility/msgpack.hpp
index ca95fd90d..3e13df144 100644
--- a/components/common/cpp/include/ftl/utility/msgpack.hpp
+++ b/components/common/cpp/include/ftl/utility/msgpack.hpp
@@ -149,7 +149,7 @@ struct object_with_zone<cv::Vec<T, SIZE>> {
};
////////////////////////////////////////////////////////////////////////////////
-// cv::Point_ and cv::Point3_
+// cv::Point_
template<typename T>
struct pack<cv::Point_<T>> {
@@ -190,6 +190,9 @@ struct object_with_zone<cv::Point_<T>> {
}
};
+////////////////////////////////////////////////////////////////////////////////
+// cv::Point3_
+
template<typename T>
struct pack<cv::Point3_<T>> {
template <typename Stream>
--
GitLab