From 0d71ed53fa514f6c5644e1ecc1404b7a354199ce Mon Sep 17 00:00:00 2001
From: Sebastian Hahta <joseha@utu.fi>
Date: Tue, 28 Jul 2020 10:07:28 +0300
Subject: [PATCH] extrinsic calibration options
---
.../src/modules/calibration/calibration.hpp | 242 +++++++++---------
.../src/modules/calibration/extrinsic.cpp | 34 ++-
.../src/modules/calibration/intrinsic.cpp | 56 ++--
.../src/views/calibration/extrinsicview.cpp | 76 +++---
.../src/views/calibration/intrinsicview.cpp | 27 +-
.../include/ftl/calibration/extrinsic.hpp | 2 +-
components/calibration/src/extrinsic.cpp | 21 +-
components/calibration/src/structures.cpp | 10 +-
.../src/sources/stereovideo/stereovideo.cpp | 2 +-
9 files changed, 264 insertions(+), 206 deletions(-)
diff --git a/applications/gui2/src/modules/calibration/calibration.hpp b/applications/gui2/src/modules/calibration/calibration.hpp
index 991acc206..af27c0f82 100644
--- a/applications/gui2/src/modules/calibration/calibration.hpp
+++ b/applications/gui2/src/modules/calibration/calibration.hpp
@@ -179,6 +179,8 @@ public:
/** Set principal point at image center */
void resetPrincipalPoint();
+ void resetDistortion();
+
/** get current frame */
cv::cuda::GpuMat getFrame();
bool hasFrame();
@@ -239,119 +241,6 @@ private:
* parameters can be used to calculate paramters for stereo rectification.
*/
-/** Stereo calibration for OpenCV's calibrateStereo() */
-
-class StereoCalibration : public CalibrationModule {
-public:
- using CalibrationModule::CalibrationModule;
- virtual void init() override;
- virtual ~StereoCalibration();
-
- /** start calibration process, replaces active view */
- void start(ftl::data::FrameID id);
-
- bool hasChannel(ftl::codecs::Channel c);
-
- void setChessboard(cv::Size, double);
- cv::Size chessboardSize();
- double squareSize();
-
- /** Reset calibration instance, discards drops all state. */
- void reset();
-
- OpenCVCalibrateFlagsStereo& flags() { return state_->flags; };
- void resetFlags();
-
- /** Returns if capture/calibration is still processing in background.
- * calib() instance must not be modifed while isBusy() is true.
- */
- bool isBusy();
-
- /** Start/stop capture. After stopping, use isBusy() to check when last
- * frame is finished.
- */
- void setCapture(bool v);
- bool capturing();
-
- /** get/set capture frequency: interval between processed frames in
- * chessboard detection
- */
- void setFrequency(float v);
- float frequency();
-
- /** Run calibration in another thread. Check status with isBusy(). */
- void run();
-
- /** Save calibration */
- void save();
-
- /** check if calibration valid: baseline > 0 */
- bool calibrated();
-
- /** get current frame */
- cv::cuda::GpuMat getLeft();
- cv::cuda::GpuMat getRight();
- cv::cuda::GpuMat getLeftRectify();
- cv::cuda::GpuMat getRightRectify();
- bool hasFrame();
-
- ftl::calibration::CalibrationData::Calibration calibrationLeft();
- ftl::calibration::CalibrationData::Calibration calibrationRight();
- double baseline();
-
- /** get previous points (visualization) */
- std::vector<std::vector<cv::Point2f>> previousPoints();
- cv::cuda::GpuMat getLeftPrevious();
- cv::cuda::GpuMat getRightPrevious();
- int count() const { return state_->count; }
- /** List sources which can be calibrated.
- */
- std::vector<std::pair<std::string, ftl::data::FrameID>> listSources(bool all=false);
-
-private:
- bool onFrame_(const ftl::data::FrameSetPtr& fs);
- void calculateRectification();
- ftl::rgbd::Frame& frame_();
-
- ftl::codecs::Channel channelLeft_();
- ftl::codecs::Channel channelRight_();
-
- std::future<void> future_;
- std::mutex mtx_;
- ftl::data::FrameSetPtr fs_current_;
- ftl::data::FrameSetPtr fs_update_;
-
- struct State {
- cv::Mat gray_left;
- cv::Mat gray_right;
-
- ftl::calibration::CalibrationData calib;
- std::unique_ptr<ftl::calibration::ChessboardObject> object;
- ftl::data::FrameID id;
- bool highres = false;
- cv::Size imsize;
- std::atomic_bool capture = false;
- std::atomic_bool running = false;
- float last = 0.0f;
- float frequency = 0.5f;
- int count = 0;
- float reprojection_error = NAN;
- OpenCVCalibrateFlagsStereo flags;
-
- // maps for rectification (cv)
- std::pair<cv::Mat, cv::Mat> map_l;
- std::pair<cv::Mat, cv::Mat> map_r;
- cv::Rect validROI_l;
- cv::Rect validROI_r;
-
- ftl::data::FrameSetPtr fs_previous_points;
- std::vector<std::vector<cv::Point2f>> points_l;
- std::vector<std::vector<cv::Point2f>> points_r;
- std::vector<std::vector<cv::Point3f>> points_object;
- };
- std::unique_ptr<State> state_;
-};
-
class ExtrinsicCalibration : public CalibrationModule {
public:
using CalibrationModule::CalibrationModule;
@@ -426,8 +315,17 @@ public:
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(); }
+ enum Flags {
+ ZERO_DISTORTION = 1,
+ FIX_INTRINSIC = 2,
+ FIX_FOCAL = 4,
+ FIX_PRINCIPAL_POINT = 8,
+ FIX_DISTORTION = 16,
+ LOSS_CAUCHY = 32,
+ };
+
+ void setFlags(int flags);
+ int flags() const;
protected:
ftl::calibration::CalibrationData::Calibration getCalibration(CameraID id);
@@ -455,6 +353,7 @@ private:
struct State {
bool capture = false;
int min_cameras = 2;
+ int flags = 0;
std::vector<Camera> cameras;
std::unique_ptr<ftl::calibration::CalibrationObject> calib_object;
@@ -468,5 +367,118 @@ private:
////////////////////////////////////////////////////////////////////////////////
+/** Stereo calibration for OpenCV's calibrateStereo() */
+
+class StereoCalibration : public CalibrationModule {
+public:
+ using CalibrationModule::CalibrationModule;
+ virtual void init() override;
+ virtual ~StereoCalibration();
+
+ /** start calibration process, replaces active view */
+ void start(ftl::data::FrameID id);
+
+ bool hasChannel(ftl::codecs::Channel c);
+
+ void setChessboard(cv::Size, double);
+ cv::Size chessboardSize();
+ double squareSize();
+
+ /** Reset calibration instance, discards drops all state. */
+ void reset();
+
+ OpenCVCalibrateFlagsStereo& flags() { return state_->flags; };
+ void resetFlags();
+
+ /** Returns if capture/calibration is still processing in background.
+ * calib() instance must not be modifed while isBusy() is true.
+ */
+ bool isBusy();
+
+ /** Start/stop capture. After stopping, use isBusy() to check when last
+ * frame is finished.
+ */
+ void setCapture(bool v);
+ bool capturing();
+
+ /** get/set capture frequency: interval between processed frames in
+ * chessboard detection
+ */
+ void setFrequency(float v);
+ float frequency();
+
+ /** Run calibration in another thread. Check status with isBusy(). */
+ void run();
+
+ /** Save calibration */
+ void save();
+
+ /** check if calibration valid: baseline > 0 */
+ bool calibrated();
+
+ /** get current frame */
+ cv::cuda::GpuMat getLeft();
+ cv::cuda::GpuMat getRight();
+ cv::cuda::GpuMat getLeftRectify();
+ cv::cuda::GpuMat getRightRectify();
+ bool hasFrame();
+
+ ftl::calibration::CalibrationData::Calibration calibrationLeft();
+ ftl::calibration::CalibrationData::Calibration calibrationRight();
+ double baseline();
+
+ /** get previous points (visualization) */
+ std::vector<std::vector<cv::Point2f>> previousPoints();
+ cv::cuda::GpuMat getLeftPrevious();
+ cv::cuda::GpuMat getRightPrevious();
+ int count() const { return state_->count; }
+ /** List sources which can be calibrated.
+ */
+ std::vector<std::pair<std::string, ftl::data::FrameID>> listSources(bool all=false);
+
+private:
+ bool onFrame_(const ftl::data::FrameSetPtr& fs);
+ void calculateRectification();
+ ftl::rgbd::Frame& frame_();
+
+ ftl::codecs::Channel channelLeft_();
+ ftl::codecs::Channel channelRight_();
+
+ std::future<void> future_;
+ std::mutex mtx_;
+ ftl::data::FrameSetPtr fs_current_;
+ ftl::data::FrameSetPtr fs_update_;
+
+ struct State {
+ cv::Mat gray_left;
+ cv::Mat gray_right;
+
+ ftl::calibration::CalibrationData calib;
+ std::unique_ptr<ftl::calibration::ChessboardObject> object;
+ ftl::data::FrameID id;
+ bool highres = false;
+ cv::Size imsize;
+ std::atomic_bool capture = false;
+ std::atomic_bool running = false;
+ float last = 0.0f;
+ float frequency = 0.5f;
+ int count = 0;
+ float reprojection_error = NAN;
+ OpenCVCalibrateFlagsStereo flags;
+
+ // maps for rectification (cv)
+ std::pair<cv::Mat, cv::Mat> map_l;
+ std::pair<cv::Mat, cv::Mat> map_r;
+ cv::Rect validROI_l;
+ cv::Rect validROI_r;
+
+ ftl::data::FrameSetPtr fs_previous_points;
+ std::vector<std::vector<cv::Point2f>> points_l;
+ std::vector<std::vector<cv::Point2f>> points_r;
+ std::vector<std::vector<cv::Point3f>> points_object;
+ };
+ std::unique_ptr<State> state_;
+};
+
}
}
diff --git a/applications/gui2/src/modules/calibration/extrinsic.cpp b/applications/gui2/src/modules/calibration/extrinsic.cpp
index 18de0f373..83debcedf 100644
--- a/applications/gui2/src/modules/calibration/extrinsic.cpp
+++ b/applications/gui2/src/modules/calibration/extrinsic.cpp
@@ -242,11 +242,21 @@ bool ExtrinsicCalibration::isBusy() {
void ExtrinsicCalibration::updateCalibration() {
auto fs = std::atomic_load(&fs_current_);
+ std::map<ftl::data::FrameID, ftl::calibration::CalibrationData> update;
+
for (unsigned int i = 0; i < state_.cameras.size(); i++) {
auto& c = state_.cameras[i];
- auto& frame = fs->frames[c.id];
- auto calib = frame.get<CalibrationData>(Channel::CalibrationData);
- calib.get(c.id.channel) = state_.calib.calibration(i);
+ auto frame_id = ftl::data::FrameID(c.id);
+
+ if (update.count(frame_id) == 0) {
+ auto& frame = fs->frames[c.id];
+ update[frame_id] = frame.get<CalibrationData>(Channel::CalibrationData);
+ }
+ update[frame_id].get(c.id.channel) = state_.calib.calibrationOptimized(i);
+ }
+
+ for (auto& [fid, calib] : update) {
+ auto& frame = fs->frames[fid];
setCalibration(frame, calib);
}
}
@@ -296,7 +306,17 @@ void ExtrinsicCalibration::run() {
<< cv::norm(t1, t2);
}
+ auto opt = state_.calib.options();
+ opt.optimize_intrinsic = !(state_.flags & Flags::FIX_INTRINSIC);
+ opt.fix_focal = state_.flags & Flags::FIX_FOCAL;
+ opt.fix_distortion = state_.flags & Flags::FIX_DISTORTION;
+ opt.fix_principal_point = state_.flags & Flags::FIX_PRINCIPAL_POINT;
+ opt.loss = (state_.flags & Flags::LOSS_CAUCHY) ?
+ ftl::calibration::BundleAdjustment::Options::Loss::CAUCHY :
+ ftl::calibration::BundleAdjustment::Options::Loss::SQUARED;
+ 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;
@@ -360,6 +380,14 @@ int ExtrinsicCalibration::getFrameCount(int camera) {
return state_.calib.points().getCount(unsigned(camera));
}
+void ExtrinsicCalibration::setFlags(int flags) {
+ state_.flags = flags;
+}
+
+int ExtrinsicCalibration::flags() const {
+ return state_.flags;
+}
+
// debug method: save state to file (msgpack)
void ExtrinsicCalibration::saveInput(const std::string& filename) {
ftl::pool.push([this, filename](int){
diff --git a/applications/gui2/src/modules/calibration/intrinsic.cpp b/applications/gui2/src/modules/calibration/intrinsic.cpp
index da9ff2199..43c08485d 100644
--- a/applications/gui2/src/modules/calibration/intrinsic.cpp
+++ b/applications/gui2/src/modules/calibration/intrinsic.cpp
@@ -64,6 +64,13 @@ void IntrinsicCalibration::start(ftl::data::FrameID id) {
filter->on([this](const FrameSetPtr& fs){ return onFrame_(fs); });
+ while(fs_current_ == nullptr) {
+ auto fss = filter->getLatestFrameSets();
+ if (fss.size() == 1) { fs_current_ = fss.front(); }
+ }
+ auto fs = std::atomic_load(&fs_current_);
+ setChannel_(fs);
+
auto* view = new ftl::gui2::IntrinsicCalibrationView(screen, this);
view->onClose([filter, this](){
// if calib_ caches images, also reset() here!
@@ -77,14 +84,6 @@ void IntrinsicCalibration::start(ftl::data::FrameID id) {
});
screen->setView(view);
-
-
- while(fs_current_ == nullptr) {
- auto fss = filter->getLatestFrameSets();
- if (fss.size() == 1) { fs_current_ = fss.front(); }
- }
- auto fs = std::atomic_load(&fs_current_);
- setChannel_(fs);
}
void IntrinsicCalibration::setChannel(Channel channel) {
@@ -95,6 +94,9 @@ void IntrinsicCalibration::setChannel(Channel channel) {
void IntrinsicCalibration::setChannel_(FrameSetPtr fs) {
// reset points, find if high res available and find correct resolution
+ // TODO/FIXME: channel might be missing from previous frameset; temporary
+ // fix uses left channel to set resulution (assumes left resolution always
+ // the same as right resolution).
state_->calib = CalibrationData::Intrinsic();
state_->points.clear();
@@ -107,16 +109,27 @@ void IntrinsicCalibration::setChannel_(FrameSetPtr fs) {
}
auto& frame = (*fs)[state_->id.source()].cast<ftl::rgbd::Frame>();
+ cv::Size size;
- if ((state_->channel== Channel::Left) && frame.has(Channel::LeftHighRes)) {
- state_->channel_alt = Channel::LeftHighRes;
+ if (state_->channel== Channel::Left) {
+ if(frame.has(Channel::LeftHighRes)) {
+ state_->channel_alt = Channel::LeftHighRes;
+ size = frame.get<cv::Mat>(state_->channel_alt).size();
+ }
+ else {
+ size = frame.get<cv::Mat>(state_->channel_alt).size();
+ }
}
- if ((state_->channel== Channel::Right) && frame.has(Channel::RightHighRes)) {
- state_->channel_alt = Channel::RightHighRes;
+ else if (state_->channel== Channel::Right) {
+ if (frame.has(Channel::RightHighRes)) {
+ state_->channel_alt = Channel::RightHighRes;
+ size = frame.get<cv::Mat>(Channel::LeftHighRes).size();
+ }
+ else {
+ size = frame.get<cv::Mat>(Channel::Left).size();
+ }
}
- auto size = frame.get<cv::Mat>(state_->channel_alt).size();
-
try {
auto calib = frame.get<CalibrationData>(Channel::CalibrationData);
if (calib.hasCalibration(state_->channel)) {
@@ -321,17 +334,15 @@ void IntrinsicCalibration::setSensorSize(cv::Size2d sz) {
}
double IntrinsicCalibration::focalLength() {
- return (sensorSize().width * state_->calib.fx)/(state_->calib.resolution.width);
+ return (state_->calib.fx)*(sensorSize().width/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?";
+ double f = value*(state_->calib.resolution.width/sensor_size.width);
- state_->calib.cx = fx;
- state_->calib.cy = fy;
+ state_->calib.cx = f;
+ state_->calib.cy = f;
}
void IntrinsicCalibration::resetPrincipalPoint() {
@@ -340,6 +351,11 @@ void IntrinsicCalibration::resetPrincipalPoint() {
state_->calib.cy = double(sz.height)/2.0;
}
+void IntrinsicCalibration::resetDistortion() {
+ state_->calib.distCoeffs = CalibrationData::Intrinsic::DistortionCoefficients();
+}
+
+
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 606464e87..91b3b946d 100644
--- a/applications/gui2/src/views/calibration/extrinsicview.cpp
+++ b/applications/gui2/src/views/calibration/extrinsicview.cpp
@@ -266,7 +266,7 @@ private:
nanogui::Label* status_;
nanogui::Button* brun_;
bool running_; // run button clicked
- ftl::calibration::BundleAdjustment::Options opts_;
+ int flags_;
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
@@ -290,51 +290,53 @@ ExtrinsicCalibrationView::CalibrationWindow::CalibrationWindow(nanogui::Widget*
void ExtrinsicCalibrationView::CalibrationWindow::build() {
+ flags_ = ctrl_->flags();
+
auto* wfreeze = new nanogui::Widget(this);
wfreeze->setLayout(new nanogui::BoxLayout
(nanogui::Orientation::Vertical, nanogui::Alignment::Fill, 0 , 5));
auto* floss = new nanogui::CheckBox(wfreeze, "Cauchy loss");
- floss->setChecked(opts_.loss == ftl::calibration::BundleAdjustment::Options::CAUCHY);
- floss->setCallback([&opts = opts_](bool v) {
- if (v) {
- opts.loss = ftl::calibration::BundleAdjustment::Options::CAUCHY;
- }
- else {
- opts.loss = ftl::calibration::BundleAdjustment::Options::SQUARED;
- }
+ floss->setChecked(flags_ & ExtrinsicCalibration::Flags::LOSS_CAUCHY);
+ floss->setCallback([&flags = flags_](bool v) {
+ if (v) { flags |= ExtrinsicCalibration::Flags::LOSS_CAUCHY; }
+ else { flags &= ~ExtrinsicCalibration::Flags::LOSS_CAUCHY; }
});
- opts_ = ctrl_->options();
auto* fall = new nanogui::CheckBox(wfreeze, "Freeze all intrinsic paramters");
- fall->setChecked(!(opts_.optimize_intrinsic));
- fall->setCallback([&opts = opts_](bool v) {
- opts.optimize_intrinsic = !v;
+ fall->setChecked(flags_ & ExtrinsicCalibration::Flags::FIX_INTRINSIC);
+ fall->setCallback([&flags = flags_](bool v) {
+ if (v) { flags |= ExtrinsicCalibration::Flags::FIX_INTRINSIC; }
+ else { flags &= ~ExtrinsicCalibration::Flags::FIX_INTRINSIC; }
});
auto* ff = new nanogui::CheckBox(wfreeze, "Fix focal length");
- ff->setChecked(opts_.fix_focal);
- ff->setCallback([&opts = opts_](bool v) {
- opts.fix_focal = v;
+ ff->setChecked(flags_ & ExtrinsicCalibration::Flags::FIX_FOCAL);
+ ff->setCallback([&flags = flags_](bool v) {
+ if (v) { flags |= ExtrinsicCalibration::Flags::FIX_FOCAL; }
+ else { flags &= ~ExtrinsicCalibration::Flags::FIX_FOCAL; }
});
auto* fpp = new nanogui::CheckBox(wfreeze, "Fix principal point");
- fpp->setChecked(opts_.fix_principal_point);
- fpp->setCallback([&opts = opts_](bool v) {
- opts.fix_principal_point = v;
+ fpp->setChecked(flags_ & ExtrinsicCalibration::Flags::FIX_PRINCIPAL_POINT);
+ fpp->setCallback([&flags = flags_](bool v) {
+ if (v) { flags |= ExtrinsicCalibration::Flags::FIX_PRINCIPAL_POINT; }
+ else { flags &= ~ExtrinsicCalibration::Flags::FIX_PRINCIPAL_POINT; }
});
auto* fdist = new nanogui::CheckBox(wfreeze, "Fix distortion coefficients");
- fdist->setChecked(opts_.fix_distortion);
- fdist->setCallback([&opts = opts_](bool v) {
- opts.fix_distortion = v;
+ fdist->setChecked(flags_ & ExtrinsicCalibration::Flags::FIX_DISTORTION);
+ fdist->setCallback([&flags = flags_](bool v) {
+ if (v) { flags |= ExtrinsicCalibration::Flags::FIX_DISTORTION; }
+ else { flags &= ~ExtrinsicCalibration::Flags::FIX_DISTORTION; }
});
- auto* fzdist = new nanogui::CheckBox(wfreeze, "Assume zero distortion");
- /*fzdist->setChecked(opts_.fix_distortion);
- fzdist->setCallback([&opts = opts_](bool v) {
- opts.fix_distortion = v;
- });*/
+ auto* zdist = new nanogui::CheckBox(wfreeze, "Assume zero distortion");
+ zdist->setChecked(flags_ & ExtrinsicCalibration::Flags::ZERO_DISTORTION);
+ zdist->setCallback([&flags = flags_](bool v) {
+ if (v) { flags |= ExtrinsicCalibration::Flags::ZERO_DISTORTION; }
+ else { flags &= ~ExtrinsicCalibration::Flags::ZERO_DISTORTION; }
+ });
fall->setCallback([wfreeze](bool value){
for (int i = 2; i < wfreeze->childCount(); i++) {
@@ -344,28 +346,12 @@ void ExtrinsicCalibrationView::CalibrationWindow::build() {
/* Needs thinking: visualize visibility graph? Use earlier alignment (if
* some of the cameras already calibrated), do elsewhere?
-
- new nanogui::Label(this, "Select orgin camera");
- auto* wcameras = new nanogui::Widget(this);
- wcameras->setLayout(new nanogui::BoxLayout
- (nanogui::Orientation::Horizontal, nanogui::Alignment::Middle, 0 , 2));
-
- auto* button = new nanogui::Button(wcameras, "A");
- button->setFlags(nanogui::Button::Flags::RadioButton);
- button->setCallback([ctrl = ctrl_](){});
- button->setTooltip("Automatic");
- button->setPushed(true);
- const auto cameras = ctrl_->cameras();
- for (unsigned i = 0; i < cameras.size(); i++) {
- button = new nanogui::Button(wcameras, std::to_string(i));
- button->setFlags(nanogui::Button::Flags::RadioButton);
- button->setCallback([ctrl = ctrl_](){});
- }*/
+ */
status_ = new nanogui::Label(this, "Ready");
brun_ = new nanogui::Button(this, "Run");
brun_->setCallback([this](){
- ctrl_->setOptions(opts_);
+ ctrl_->setFlags(flags_);
ctrl_->run();
running_ = true;
});
diff --git a/applications/gui2/src/views/calibration/intrinsicview.cpp b/applications/gui2/src/views/calibration/intrinsicview.cpp
index a8708183c..6129ce48d 100644
--- a/applications/gui2/src/views/calibration/intrinsicview.cpp
+++ b/applications/gui2/src/views/calibration/intrinsicview.cpp
@@ -84,6 +84,7 @@ public:
class IntrinsicCalibrationView::CalibrationWindow : public FixedWindow {
public:
CalibrationWindow(nanogui::Widget* parent, IntrinsicCalibrationView* view);
+ void update();
virtual void draw(NVGcontext* ctx) override;
private:
@@ -95,7 +96,8 @@ private:
nanogui::FloatBox<double>* sensor_width_;
nanogui::FloatBox<double>* sensor_height_;
nanogui::FloatBox<double>* focal_length_;
- nanogui::CheckBox* pp_;
+ nanogui::CheckBox* reset_dist_;
+ nanogui::CheckBox* reset_pp_;
bool calibrating_;
public:
@@ -407,14 +409,18 @@ IntrinsicCalibrationView::CalibrationWindow::CalibrationWindow(nanogui::Widget*
sensor_height_->setUnits("mm");
new nanogui::Label(initial_values, "Focal length");
- focal_length_ = new nanogui::FloatBox<double>(initial_values, ctrl_->sensorSize().height);
+ focal_length_ = new nanogui::FloatBox<double>(initial_values, ctrl_->focalLength());
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);
+ reset_pp_ = new nanogui::CheckBox(initial_values, "");
+ reset_pp_->setChecked(false);
+
+ new nanogui::Label(initial_values, "Reset distortion coefficients");
+ reset_dist_ = new nanogui::CheckBox(initial_values, "");
+ reset_dist_->setChecked(false);
// flags
new nanogui::Label(this, "Flags");
@@ -427,19 +433,23 @@ IntrinsicCalibrationView::CalibrationWindow::CalibrationWindow(nanogui::Widget*
if (!ctrl_->isBusy()) {
ctrl_->setSensorSize({sensor_width_->value(), sensor_height_->value()});
ctrl_->setFocalLength(focal_length_->value(), ctrl_->sensorSize());
- if (pp_->checked()) {
- ctrl_->resetPrincipalPoint();
- }
+ if (reset_pp_->checked()) { ctrl_->resetPrincipalPoint(); }
+ if (reset_dist_->checked()) { ctrl_->resetDistortion(); }
ctrl_->run();
calibrating_ = true;
}
});
}
+void IntrinsicCalibrationView::CalibrationWindow::update() {
+ focal_length_->setValue(ctrl_->focalLength());
+}
+
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);
+ reset_pp_->setEnabled(use_guess);
+ reset_dist_->setEnabled(use_guess);
if (ctrl_->isBusy()) {
if (calibrating_) {
@@ -601,6 +611,7 @@ void IntrinsicCalibrationView::setMode(Mode m) {
ctrl_->setCapture(false);
wcapture_->setVisible(false);
wcontrol_->setVisible(false);
+ wcalibration_->update();
wcalibration_->setVisible(true);
wresults_->setVisible(false);
break;
diff --git a/components/calibration/include/ftl/calibration/extrinsic.hpp b/components/calibration/include/ftl/calibration/extrinsic.hpp
index 3dba4ffc6..5bded2bc3 100644
--- a/components/calibration/include/ftl/calibration/extrinsic.hpp
+++ b/components/calibration/include/ftl/calibration/extrinsic.hpp
@@ -221,7 +221,7 @@ private:
CalibrationPoints<double> points_;
std::set<std::pair<unsigned int, unsigned int>> mask_;
- std::map<std::pair<unsigned int, unsigned int>, std::pair<cv::Mat, cv::Mat>> pairs_;
+ std::map<std::pair<unsigned int, unsigned int>, std::tuple<cv::Mat, cv::Mat, double>> pairs_;
int min_points_ = 64; // minimum number of points required for pair calibration
// TODO: add map {c1,c2}Â for existing calibration which is used if available.
//
diff --git a/components/calibration/src/extrinsic.cpp b/components/calibration/src/extrinsic.cpp
index 0ad4fbe73..615b13034 100644
--- a/components/calibration/src/extrinsic.cpp
+++ b/components/calibration/src/extrinsic.cpp
@@ -395,7 +395,8 @@ void ExtrinsicCalibration::calculatePairPoses() {
if (c1 == c2) {
pairs_[{c1, c2}] = { cv::Mat::eye(cv::Size(3, 3), CV_64FC1),
- cv::Mat(cv::Size(1, 3), CV_64FC1, cv::Scalar(0.0)) };
+ cv::Mat(cv::Size(1, 3), CV_64FC1, cv::Scalar(0.0)),
+ 0.0};
continue;
}
@@ -419,21 +420,21 @@ void ExtrinsicCalibration::calculatePairPoses() {
auto object = points().getObject(0);
cv::Mat R, t;
std::vector<cv::Point3d> points3d;
- auto rms = calibratePair(K1, distCoeffs1, K2, distCoeffs2,
+ auto rmse = calibratePair(K1, distCoeffs1, K2, distCoeffs2,
pts[0], pts[1], object, R, t, points3d, true);
// debug info
- LOG(INFO) << "RMSE (cameras " << c1 << " & " << c2 << "): " << rms;
+ LOG(INFO) << "RMSE (cameras " << c1 << " & " << c2 << "): " << rmse;
points().setTriangulatedPoints(c1, c2, points3d);
- pairs_[{c1, c2}] = {R, t};
+ pairs_[{c1, c2}] = {R, t, rmse};
cv::Mat R_i, t_i;
R.copyTo(R_i);
t.copyTo(t_i);
transform::inverse(R_i, t_i);
- pairs_[{c2, c1}] = {R_i, t_i};
+ pairs_[{c2, c1}] = {R_i, t_i, rmse};
}}
}
@@ -453,12 +454,16 @@ void ExtrinsicCalibration::calculateInitialPoses() {
if (pairs_.count({c1, c2}) == 0) {
visibility.mask(c1, c2);
}
+
+ // mask bad pairs (high rmse)
+ /*if (std::get<2>(pairs_.at({c1, c2})) > 16.0) {
+ visibility.mask(c1, c2);
+ }*/
}}
// pick optimal camera: most views of calibration pattern
unsigned int c_ref = visibility.argmax();
- // calculate transformation chains; TODO: use pair RMSE as well?
auto paths = visibility.shortestPath(c_ref);
for (unsigned int c = 0; c < camerasCount(); c++) {
@@ -474,8 +479,8 @@ void ExtrinsicCalibration::calculateInitialPoses() {
path.pop_back();
auto next = path.back();
- cv::Mat R = pairs_.at({prev, next}).first;
- cv::Mat t = pairs_.at({prev, next}).second;
+ cv::Mat R = std::get<0>(pairs_.at({prev, next}));
+ cv::Mat t = std::get<1>(pairs_.at({prev, next}));
CHECK_EQ(R.size(), cv::Size(3, 3));
CHECK_EQ(t.total(), 3);
diff --git a/components/calibration/src/structures.cpp b/components/calibration/src/structures.cpp
index 1e194bcd9..2c44a6b02 100644
--- a/components/calibration/src/structures.cpp
+++ b/components/calibration/src/structures.cpp
@@ -9,15 +9,15 @@
using ftl::calibration::CalibrationData;
CalibrationData::Intrinsic::DistortionCoefficients::DistortionCoefficients() :
- data_(12, 0.0) {
+ data_(14, 0.0) {
}
const cv::Mat CalibrationData::Intrinsic::DistortionCoefficients::Mat(int nparams) const {
if (nparams <= 0) {
return cv::Mat();
}
- if (nparams > 12) {
- nparams = 12;
+ if (nparams > 14) {
+ nparams = 14;
}
return cv::Mat(cv::Size(nparams, 1), CV_64FC1, const_cast<double*>(data_.data()));
}
@@ -26,8 +26,8 @@ cv::Mat CalibrationData::Intrinsic::DistortionCoefficients::Mat(int nparams) {
if (nparams <= 0) {
return cv::Mat();
}
- if (nparams > 12) {
- nparams = 12;
+ if (nparams > 14) {
+ nparams = 14;
}
return cv::Mat(cv::Size(nparams, 1), CV_64FC1, data_.data());
}
diff --git a/components/rgbd-sources/src/sources/stereovideo/stereovideo.cpp b/components/rgbd-sources/src/sources/stereovideo/stereovideo.cpp
index 547e84382..7458f5c53 100644
--- a/components/rgbd-sources/src/sources/stereovideo/stereovideo.cpp
+++ b/components/rgbd-sources/src/sources/stereovideo/stereovideo.cpp
@@ -301,7 +301,7 @@ void StereoVideoSource::updateParameters(ftl::rgbd::Frame &frame) {
params.baseline = 0.1f;
params.width = color_size_.width;
params.height = color_size_.height;;
-
+
float offsetz = host_->value("offset_z", 0.0f);
//state_.setPose(matrix(cv::Vec3d(0.0, 3.14159, 0.0), cv::Vec3d(0.0,0.0,params_.maxDepth+offsetz)));
pose = matrix(cv::Vec3d(0.0, 3.14159, 0.0), cv::Vec3d(0.0,0.0,params.maxDepth + offsetz));
--
GitLab