From 81c91514c4cd83a81b15f74a67cb2cc0ce6fc412 Mon Sep 17 00:00:00 2001
From: Sebastian Hahta <joseha@utu.fi>
Date: Mon, 27 Jul 2020 10:36:04 +0300
Subject: [PATCH] more options for calibration
---
.../src/modules/calibration/calibration.cpp | 11 +++-
.../src/modules/calibration/calibration.hpp | 9 ++++
.../src/modules/calibration/extrinsic.cpp | 12 +++++
.../src/modules/calibration/intrinsic.cpp | 27 ++++++++--
.../src/views/calibration/extrinsicview.cpp | 28 ++++++++--
.../src/views/calibration/intrinsicview.cpp | 53 ++++++++++++-------
.../src/views/calibration/visualization.hpp | 44 +++++++++++++++
.../gui2/src/views/calibration/widgets.cpp | 2 +-
.../include/ftl/calibration/extrinsic.hpp | 6 +++
.../include/ftl/calibration/optimize.hpp | 6 +--
components/calibration/src/extrinsic.cpp | 14 ++++-
components/calibration/src/structures.cpp | 6 +--
12 files changed, 178 insertions(+), 40 deletions(-)
diff --git a/applications/gui2/src/modules/calibration/calibration.cpp b/applications/gui2/src/modules/calibration/calibration.cpp
index 2c87f99f4..f892ba61c 100644
--- a/applications/gui2/src/modules/calibration/calibration.cpp
+++ b/applications/gui2/src/modules/calibration/calibration.cpp
@@ -41,6 +41,7 @@ int OpenCVCalibrateFlags::defaultFlags() const { return (
std::vector<int> OpenCVCalibrateFlags::list() const {
return {
cv::CALIB_USE_INTRINSIC_GUESS,
+ cv::CALIB_FIX_FOCAL_LENGTH,
cv::CALIB_FIX_PRINCIPAL_POINT,
cv::CALIB_FIX_ASPECT_RATIO,
cv::CALIB_ZERO_TANGENT_DIST,
@@ -64,6 +65,9 @@ std::string OpenCVCalibrateFlags::name(int i) const {
case CALIB_FIX_INTRINSIC:
return "CALIB_FIX_INTRINSIC";
+ case CALIB_FIX_FOCAL_LENGTH:
+ return "CALIB_FIX_FOCAL_LENGTH";
+
case CALIB_USE_INTRINSIC_GUESS:
return "CALIB_USE_INTRINSIC_GUESS";
@@ -123,8 +127,11 @@ std::string OpenCVCalibrateFlags::explain(int i) const {
using namespace cv;
switch(i) {
case CALIB_FIX_INTRINSIC:
- return "";
-
+ return "Fix all intrinsic paramters.";
+
+ case CALIB_FIX_FOCAL_LENGTH:
+ return "Fix focal length (fx and fy).";
+
case CALIB_USE_INTRINSIC_GUESS:
return "Use valid initial values of fx, fy, cx, cy that are "
"optimized further. Otherwise, (cx, cy) is initially set "
diff --git a/applications/gui2/src/modules/calibration/calibration.hpp b/applications/gui2/src/modules/calibration/calibration.hpp
index 1c05fee5b..991acc206 100644
--- a/applications/gui2/src/modules/calibration/calibration.hpp
+++ b/applications/gui2/src/modules/calibration/calibration.hpp
@@ -172,6 +172,13 @@ public:
cv::Size2d sensorSize();
void setSensorSize(cv::Size2d size);
+ /** Set/get focal length in mm */
+ double focalLength();
+ void setFocalLength(double value, cv::Size2d sensor_size);
+
+ /** Set principal point at image center */
+ void resetPrincipalPoint();
+
/** get current frame */
cv::cuda::GpuMat getFrame();
bool hasFrame();
@@ -417,6 +424,8 @@ public:
ftl::calibration::CalibrationData::Calibration calibration(int camera);
+ double reprojectionError(int camera=-1);
+
void setOptions(ftl::calibration::BundleAdjustment::Options options) { state_.calib.setOptions(options); }
ftl::calibration::BundleAdjustment::Options options() { return state_.calib.options(); }
diff --git a/applications/gui2/src/modules/calibration/extrinsic.cpp b/applications/gui2/src/modules/calibration/extrinsic.cpp
index 9f88e3b6c..18de0f373 100644
--- a/applications/gui2/src/modules/calibration/extrinsic.cpp
+++ b/applications/gui2/src/modules/calibration/extrinsic.cpp
@@ -326,6 +326,18 @@ void ExtrinsicCalibration::run() {
});
}
+double ExtrinsicCalibration::reprojectionError(int camera) {
+ if (camera <= cameraCount()) {
+ return NAN;
+ }
+ if (camera < 0) {
+ return state_.calib.reprojectionError();
+ }
+ else {
+ return state_.calib.reprojectionError(camera);
+ }
+}
+
ftl::calibration::CalibrationData::Calibration ExtrinsicCalibration::getCalibration(CameraID id) {
if (fs_current_ == nullptr) {
throw ftl::exception("No frame");
diff --git a/applications/gui2/src/modules/calibration/intrinsic.cpp b/applications/gui2/src/modules/calibration/intrinsic.cpp
index 44b37bef1..ab9b78486 100644
--- a/applications/gui2/src/modules/calibration/intrinsic.cpp
+++ b/applications/gui2/src/modules/calibration/intrinsic.cpp
@@ -246,7 +246,7 @@ void IntrinsicCalibration::run() {
state_->flags, term);
state_->calib = CalibrationData::Intrinsic(K, distCoeffs, size);
- state_->calib.sensorSize = ssize;
+ state_->calib.sensorSize = ssize; // TODO: save somewhere else
state_->calibrated = true;
}
catch (std::exception &e) {
@@ -295,8 +295,9 @@ cv::cuda::GpuMat IntrinsicCalibration::getFrameUndistort() {
auto im = getMat((*fs_current_)[state_->id.source()].cast<ftl::rgbd::Frame>(),
state_->channel_alt);
- // NOTE: would be faster to use remap() and computing the maps just once.
- // This isn't likely performance critical code.
+ // NOTE: would be faster to use remap() and computing the maps just once if
+ // performance is relevant here
+
cv::Mat im_undistort;
cv::cuda::GpuMat gpu;
cv::undistort(im, im_undistort, state_->calib.matrix(), state_->calib.distCoeffs.Mat(12));
@@ -319,6 +320,26 @@ void IntrinsicCalibration::setSensorSize(cv::Size2d sz) {
state_->calib.sensorSize = sz;
}
+double IntrinsicCalibration::focalLength() {
+ return (sensorSize().width * state_->calib.fx)/(state_->calib.resolution.width);
+}
+
+void IntrinsicCalibration::setFocalLength(double value, cv::Size2d sensor_size) {
+ setSensorSize(sensor_size);
+ double fx = (state_->calib.resolution.width * value)/(sensor_size.width);
+ double fy = (state_->calib.resolution.height * value)/(sensor_size.height);
+ LOG_IF(WARNING, fx != fy) << "fx != fy; is sensor size correct?";
+
+ state_->calib.cx = fx;
+ state_->calib.cy = fy;
+}
+
+void IntrinsicCalibration::resetPrincipalPoint() {
+ auto sz = state_->calib.resolution;
+ state_->calib.cx = double(sz.width)/2.0;
+ state_->calib.cy = double(sz.height)/2.0;
+}
+
bool IntrinsicCalibration::hasChannel(Channel c) {
if (fs_current_) {
return (*fs_current_)[state_->id.source()].hasChannel(c);
diff --git a/applications/gui2/src/views/calibration/extrinsicview.cpp b/applications/gui2/src/views/calibration/extrinsicview.cpp
index 4e583f6c2..d403d7678 100644
--- a/applications/gui2/src/views/calibration/extrinsicview.cpp
+++ b/applications/gui2/src/views/calibration/extrinsicview.cpp
@@ -1,7 +1,9 @@
#include "extrinsicview.hpp"
+#include "visualization.hpp"
#include "../../screen.hpp"
#include "../../widgets/window.hpp"
+#include <nanogui/common.h>
#include <nanogui/window.h>
#include <nanogui/layout.h>
#include <nanogui/button.h>
@@ -79,7 +81,7 @@ void ExtrinsicCalibrationStart::draw(NVGcontext* ctx) {
}
void ExtrinsicCalibrationStart::resetSources() {
- sources_ = 0;
+ sources_ = ~uint64_t(0);
}
bool ExtrinsicCalibrationStart::sourceSelected(unsigned int idx) {
@@ -97,7 +99,10 @@ void ExtrinsicCalibrationStart::removeSource(unsigned int idx) {
std::vector<FrameID> ExtrinsicCalibrationStart::getSources() {
std::vector<FrameID> sources;
- for (unsigned int i = 0; i < 64; i++) {
+ unsigned int nmax = ctrl_->listSources(fsid_, show_all_).size();
+ CHECK(nmax < 64);
+
+ for (unsigned int i = 0; i < nmax; i++) {
if (sourceSelected(i)) {
sources.push_back(FrameID(fsid_, i));
}
@@ -181,13 +186,15 @@ ExtrinsicCalibrationView::ControlWindow::ControlWindow(nanogui::Widget* parent,
bsave_ = new nanogui::Button(buttons, "", ENTYPO_ICON_SAVE);
bsave_->setTooltip("Save input to file (Debug)");
bsave_->setCallback([this](){
- ctrl_->saveInput("./calib.bin");
+ std::string fname = nanogui::file_dialog({{"bin", "Binary file"}}, true);
+ ctrl_->saveInput(fname);
});
bsave_ = new nanogui::Button(buttons, "", ENTYPO_ICON_FOLDER);
bsave_->setTooltip("Load input from file (Debug)");
bsave_->setCallback([this](){
- ctrl_->loadInput("./calib.bin");
+ std::string fname = nanogui::file_dialog({{"bin", "Binary file"}}, true);
+ ctrl_->loadInput(fname);
});
bupload_ = new nanogui::Button(buttons, "", ENTYPO_ICON_UPLOAD);
@@ -372,7 +379,7 @@ void ExtrinsicCalibrationView::CalibrationWindow::draw(NVGcontext* ctx) {
class ExtrinsicCalibrationView::ResultsWindow : public FixedWindow {
public:
ResultsWindow(nanogui::Widget* parent, ExtrinsicCalibrationView* view);
- //virtual void draw(NVGcontext* ctx) override;
+ virtual void draw(NVGcontext* ctx) override;
private:
ExtrinsicCalibrationView* view_;
@@ -393,7 +400,18 @@ ExtrinsicCalibrationView::ResultsWindow::ResultsWindow(nanogui::Widget* parent,
[view = view_]() {
view->setMode(Mode::VIDEO);
});
+ setSize({300, 300});
+}
+
+void ExtrinsicCalibrationView::ResultsWindow::draw(NVGcontext* ctx) {
+ FixedWindow::draw(ctx);
+ std::vector<ftl::calibration::CalibrationData::Extrinsic> calib(ctrl_->cameraCount());
+ for (int i = 0; i < ctrl_->cameraCount(); i++) {
+ calib[i] = ctrl_->calibration(i).extrinsic;
+ }
+ drawFloorPlan(ctx, this, calib);
}
+
////////////////////////////////////////////////////////////////////////////////
static void drawText(NVGcontext* ctx, nanogui::Vector2f &pos, const std::string& text,
diff --git a/applications/gui2/src/views/calibration/intrinsicview.cpp b/applications/gui2/src/views/calibration/intrinsicview.cpp
index d14ff335f..be714ab20 100644
--- a/applications/gui2/src/views/calibration/intrinsicview.cpp
+++ b/applications/gui2/src/views/calibration/intrinsicview.cpp
@@ -94,6 +94,8 @@ private:
nanogui::Button* bcalibrate_;
nanogui::FloatBox<double>* sensor_width_;
nanogui::FloatBox<double>* sensor_height_;
+ nanogui::FloatBox<double>* focal_length_;
+ nanogui::CheckBox* pp_;
bool calibrating_;
public:
@@ -350,7 +352,7 @@ IntrinsicCalibrationView::ControlWindow::ControlWindow(nanogui::Widget* parent,
void IntrinsicCalibrationView::ControlWindow::draw(NVGcontext* ctx) {
if (ctrl_->capturing()) { bpause_->setCaption("Pause"); }
else { bpause_->setCaption("Continue"); }
- bcalibrate_->setEnabled(ctrl_->count() > 0);
+ //bcalibrate_->setEnabled(ctrl_->count() > 0);
bresults_->setEnabled(ctrl_->calibrated());
bsave_->setEnabled(ctrl_->calibrated());
bapply_->setEnabled(ctrl_->calibrated());
@@ -381,7 +383,7 @@ IntrinsicCalibrationView::CalibrationWindow::CalibrationWindow(nanogui::Widget*
});
// sensor size
- new nanogui::Label(this, "Sensor size (optional)");
+ new nanogui::Label(this, "Initial values");
nanogui::GridLayout *grid_layout = new nanogui::GridLayout
(nanogui::Orientation::Horizontal, 2, nanogui::Alignment::Fill, 0, 5);
@@ -389,28 +391,30 @@ IntrinsicCalibrationView::CalibrationWindow::CalibrationWindow(nanogui::Widget*
({nanogui::Alignment::Maximum, nanogui::Alignment::Fill});
grid_layout->setSpacing(0, 10);
- auto* sensor = new nanogui::Widget(this);
- sensor->setLayout(grid_layout);
+ auto* initial_values = new nanogui::Widget(this);
+ initial_values->setLayout(grid_layout);
- new nanogui::Label(sensor, "Width");
- sensor_width_ = new nanogui::FloatBox<double>(sensor, ctrl_->sensorSize().width);
+ new nanogui::Label(initial_values, "Sensor width");
+ sensor_width_ = new nanogui::FloatBox<double>(initial_values, ctrl_->sensorSize().width);
sensor_width_->setEditable(true);
sensor_width_->setFormat("[0-9]*\\.?[0-9]+");
sensor_width_->setUnits("mm");
- sensor_width_->setCallback([ctrl = this->ctrl_](double value) {
- auto size = ctrl->sensorSize();
- ctrl->setSensorSize({value, size.height});
- });
- new nanogui::Label(sensor, "Height");
- sensor_height_ = new nanogui::FloatBox<double>(sensor, ctrl_->sensorSize().height);
+ new nanogui::Label(initial_values, "Sensor height");
+ sensor_height_ = new nanogui::FloatBox<double>(initial_values, ctrl_->sensorSize().height);
sensor_height_->setEditable(true);
sensor_height_->setFormat("[0-9]*\\.?[0-9]+");
sensor_height_->setUnits("mm");
- sensor_height_->setCallback([ctrl = this->ctrl_](double value) {
- auto size = ctrl->sensorSize();
- ctrl->setSensorSize({size.width, value});
- });
+
+ new nanogui::Label(initial_values, "Focal length");
+ focal_length_ = new nanogui::FloatBox<double>(initial_values, ctrl_->sensorSize().height);
+ focal_length_->setEditable(true);
+ focal_length_->setFormat("[0-9]*\\.?[0-9]+");
+ focal_length_->setUnits("mm");
+
+ new nanogui::Label(initial_values, "Reset principal point");
+ pp_ = new nanogui::CheckBox(initial_values, "");
+ pp_->setChecked(false);
// flags
new nanogui::Label(this, "Flags");
@@ -419,15 +423,24 @@ IntrinsicCalibrationView::CalibrationWindow::CalibrationWindow(nanogui::Widget*
bcalibrate_ = new nanogui::Button(this, "Run");
bcalibrate_->setEnabled(false);
- bcalibrate_->setCallback([&ctrl = ctrl_, &running = calibrating_](){
- if (!ctrl->isBusy()) {
- ctrl->run();
- running = true;
+ bcalibrate_->setCallback([this](){
+ if (!ctrl_->isBusy()) {
+ ctrl_->setSensorSize({sensor_width_->value(), sensor_height_->value()});
+ ctrl_->setFocalLength(focal_length_->value(), ctrl_->sensorSize());
+ if (pp_->checked()) {
+ ctrl_->resetPrincipalPoint();
+ }
+ ctrl_->run();
+ calibrating_ = true;
}
});
}
void IntrinsicCalibrationView::CalibrationWindow::draw(NVGcontext* ctx) {
+ bool use_guess = ctrl_->flags().has(cv::CALIB_USE_INTRINSIC_GUESS);
+ focal_length_->setEnabled(use_guess);
+ pp_->setEnabled(use_guess);
+
if (ctrl_->isBusy()) {
if (calibrating_) {
auto dots = std::string(int(round(glfwGetTime())) % 4, '.');
diff --git a/applications/gui2/src/views/calibration/visualization.hpp b/applications/gui2/src/views/calibration/visualization.hpp
index 274037dd2..de2fe29ab 100644
--- a/applications/gui2/src/views/calibration/visualization.hpp
+++ b/applications/gui2/src/views/calibration/visualization.hpp
@@ -2,6 +2,8 @@
#include "../../widgets/imageview.hpp"
+#include <ftl/calibration/structures.hpp>
+
/** Draw Chessboard Corners with OpenGL to ImageView widget. */
template<typename T>
static void drawChessboardCorners(NVGcontext* ctx, ftl::gui2::ImageView* imview, const std::vector<T>& points) {
@@ -39,3 +41,45 @@ static void drawChessboardCorners(NVGcontext* ctx, ftl::gui2::ImageView* imview,
}
nvgResetScissor(ctx);
}
+
+static void drawFloorPlan(NVGcontext* ctx, nanogui::Widget* parent,
+ const std::vector<ftl::calibration::CalibrationData::Extrinsic>& calib,
+ const std::vector<std::string>& names = {},
+ int origin=0) {
+
+ float minx = INFINITY;
+ float miny = INFINITY;
+ float maxx = -INFINITY;
+ float maxy = -INFINITY;
+
+ std::vector<cv::Point2f> points(calib.size());
+ for (unsigned int i = 0; i < points.size(); i++) {
+ // xz, assume floor on y-plane
+ float x = calib[i].tvec[0];
+ float y = calib[i].tvec[2];
+ points[i] = {x, y};
+ minx = std::min(minx, x);
+ miny = std::min(miny, y);
+ maxx = std::max(maxx, x);
+ maxy = std::max(maxy, y);
+ }
+
+ float w = parent->width();
+ float sx = w/(maxx - minx);
+ float h = parent->height();
+ float sy = h/(maxy - miny);
+ float s = min(sx, sy);
+
+ nanogui::Vector2f apos = parent->absolutePosition().cast<float>() + nanogui::Vector2f{w/2.0f, h/2.0f};
+
+ for (unsigned int i = 0; i < points.size(); i++) {
+ float x = points[i].x*s + apos.x();
+ float y = points[i].y*s + apos.y();
+ // TODO: draw triangles (rotate by camera rotation)
+ nvgBeginPath(ctx);
+ nvgCircle(ctx, x, y, 2.5);
+ nvgStrokeColor(ctx, nvgRGBA(0, 0, 0, 255));
+ nvgStrokeWidth(ctx, 1.0f);
+ nvgStroke(ctx);
+ }
+}
diff --git a/applications/gui2/src/views/calibration/widgets.cpp b/applications/gui2/src/views/calibration/widgets.cpp
index bbf9d81dd..f907b2ce1 100644
--- a/applications/gui2/src/views/calibration/widgets.cpp
+++ b/applications/gui2/src/views/calibration/widgets.cpp
@@ -106,7 +106,7 @@ void IntrinsicDetails::update(const ftl::calibration::CalibrationData::Intrinsic
double ar;
cv::calibrationMatrixValues(K, imsize, sw, sh, fovx, fovy, f, pp, ar);
- new nanogui::Label(params_, "Image size:");
+ new nanogui::Label(params_, "Size (sensor/image):");
if (use_physical) new nanogui::Label(params_, to_string(sw, 1) + std::string("x") + to_string(sh, 1));
new nanogui::Label(params_, std::to_string(imsize.width) + std::string("x") + std::to_string(imsize.height));
diff --git a/components/calibration/include/ftl/calibration/extrinsic.hpp b/components/calibration/include/ftl/calibration/extrinsic.hpp
index 3dbd9556a..3dba4ffc6 100644
--- a/components/calibration/include/ftl/calibration/extrinsic.hpp
+++ b/components/calibration/include/ftl/calibration/extrinsic.hpp
@@ -194,6 +194,9 @@ public:
/** run calibration, returns reprojection error */
double run();
+ double reprojectionError(unsigned int c); // reprojection error rmse
+ double reprojectionError(); // total reprojection error rmse
+
/** debug methods */
bool fromFile(const std::string& fname);
bool toFile(const std::string& fname); // should return new instance...
@@ -224,6 +227,9 @@ private:
//
std::shared_ptr<std::string> status_;
+ std::vector<double> rmse_;
+ double rmse_total_;
+
// Theshold for point to be skipped (m); absdiff between minimum and maximum
// values of each coordinate axis in all triangulated points is calculated
// and l2 norm is compared against threshold value. Optimization uses median
diff --git a/components/calibration/include/ftl/calibration/optimize.hpp b/components/calibration/include/ftl/calibration/optimize.hpp
index 1c620f1d4..dff01ffb9 100644
--- a/components/calibration/include/ftl/calibration/optimize.hpp
+++ b/components/calibration/include/ftl/calibration/optimize.hpp
@@ -47,9 +47,6 @@ double optimizeScale(const std::vector<cv::Point3d>& object, std::vector<cv::Poi
* - rotation and translation rx, ry, rz, tx, ty, tz,
* - focal legth and principal point: f, cx, cy
* - radial distortion (first three cofficients): k1, k2, k3
- *
- * @note: Distortion paramters are used in reprojection error, but they are
- * not not optimized.
*/
class BundleAdjustment {
public:
@@ -80,8 +77,7 @@ public:
/**
* @todo Radial distortion must be monotonic. This constraint is not
- * included in the model, thus distortion parameters are always
- * fixed.
+ * included in the model.
*/
bool fix_distortion = true;
diff --git a/components/calibration/src/extrinsic.cpp b/components/calibration/src/extrinsic.cpp
index 0d88b1804..0ad4fbe73 100644
--- a/components/calibration/src/extrinsic.cpp
+++ b/components/calibration/src/extrinsic.cpp
@@ -644,7 +644,10 @@ double ExtrinsicCalibration::optimize() {
ba.run(options_);
calib_optimized_.resize(calib_.size());
+ rmse_.resize(calib_.size());
+
for (unsigned int i = 0; i < cameras.size(); i++) {
+ rmse_[i] = ba.reprojectionError(i);
auto intr = cameras[i].intrinsic();
auto extr = cameras[i].extrinsic();
calib_optimized_[i] = calib_[i];
@@ -652,7 +655,8 @@ double ExtrinsicCalibration::optimize() {
calib_optimized_[i].extrinsic.set(extr.rvec, extr.tvec);
}
- return ba.reprojectionError();
+ rmse_total_ = ba.reprojectionError();
+ return rmse_total_;
}
double ExtrinsicCalibration::run() {
@@ -672,6 +676,14 @@ const CalibrationData::Calibration& ExtrinsicCalibration::calibrationOptimized(u
return calib_optimized_.at(c);
}
+double ExtrinsicCalibration::reprojectionError(unsigned int c) {
+ return rmse_.at(c);
+}
+
+double ExtrinsicCalibration::reprojectionError() {
+ return rmse_total_;
+}
+
bool ExtrinsicCalibration::toFile(const std::string& fname) {
points_.clear();
std::ofstream ofs(fname, std::ios_base::trunc);
diff --git a/components/calibration/src/structures.cpp b/components/calibration/src/structures.cpp
index 12b5a2f41..1e194bcd9 100644
--- a/components/calibration/src/structures.cpp
+++ b/components/calibration/src/structures.cpp
@@ -103,11 +103,11 @@ void CalibrationData::Extrinsic::set(const cv::Mat& T) {
void CalibrationData::Extrinsic::set(cv::InputArray R, cv::InputArray t) {
if ((t.type() != CV_64FC1) || (R.type() != CV_64FC1)) {
- throw ftl::exception("R and t must be CV_64FC1");
+ throw ftl::exception("Type of R and t must be CV_64FC1");
}
if ((t.size() != cv::Size(3, 1)) && (t.size() != cv::Size(1, 3))) {
- throw ftl::exception("t must be have size (3, 1) or (1, 3");
+ throw ftl::exception("Size of t must be (3, 1) or (1, 3");
}
if (R.isMat()) {
@@ -125,7 +125,7 @@ void CalibrationData::Extrinsic::set(cv::InputArray R, cv::InputArray t) {
rvec[2] = rmat.at<double>(2);
}
else {
- throw ftl::exception("R must be 3x3 rotation matrix or vector");
+ throw ftl::exception("R must be a 3x3 rotation matrix or 3x1/1x3 rotation vector (Rodrigues)");
}
}
--
GitLab