diff --git a/applications/gui2/src/modules/calibration/calibration.cpp b/applications/gui2/src/modules/calibration/calibration.cpp
index f892ba61c1699ae70dcc1ae837ef4f231905a4b7..83627e6fd2ec0b5baccfeaeebf4d818276edf951 100644
--- a/applications/gui2/src/modules/calibration/calibration.cpp
+++ b/applications/gui2/src/modules/calibration/calibration.cpp
@@ -32,11 +32,13 @@ using ftl::data::FrameSetPtr;
using ftl::gui2::OpenCVCalibrateFlags;
using ftl::gui2::OpenCVCalibrateFlagsStereo;
-int OpenCVCalibrateFlags::defaultFlags() const { return (
- cv::CALIB_RATIONAL_MODEL |
- cv::CALIB_THIN_PRISM_MODEL |
- cv::CALIB_FIX_ASPECT_RATIO
-);}
+int OpenCVCalibrateFlags::defaultFlags() const {
+ // For finding distortion coefficients fix focal length and principal point.
+ // Otherwise results might be unreliable.
+ return (cv::CALIB_FIX_FOCAL_LENGTH |
+ cv::CALIB_FIX_PRINCIPAL_POINT |
+ cv::CALIB_FIX_ASPECT_RATIO);
+}
std::vector<int> OpenCVCalibrateFlags::list() const {
return {
diff --git a/applications/gui2/src/modules/calibration/calibration.hpp b/applications/gui2/src/modules/calibration/calibration.hpp
index 4986689b1bc52e2cda19f812944c15280901f55a..00b33b3ec5288c7f49469f8c51002e2a33efa372 100644
--- a/applications/gui2/src/modules/calibration/calibration.hpp
+++ b/applications/gui2/src/modules/calibration/calibration.hpp
@@ -261,6 +261,9 @@ public:
int cameraCount();
+ std::string cameraName(int camera);
+ std::vector<std::string> cameraNames();
+
ftl::calibration::ExtrinsicCalibration& calib();
/** hasFrame(int) must be true before calling getFrame() **/
@@ -340,10 +343,6 @@ protected:
private:
// map frameid+channel to int. used by ExtrinsicCalibration
- struct Camera {
- const CameraID id;
- ftl::calibration::CalibrationData::Calibration calib;
- };
bool onFrameSet_(const ftl::data::FrameSetPtr& fs);
@@ -356,7 +355,7 @@ private:
bool capture = false;
int min_cameras = 2;
int flags = 0;
- std::vector<Camera> cameras;
+ std::vector<CameraID> cameras;
std::unique_ptr<ftl::calibration::CalibrationObject> calib_object;
ftl::calibration::ExtrinsicCalibration calib;
diff --git a/applications/gui2/src/modules/calibration/extrinsic.cpp b/applications/gui2/src/modules/calibration/extrinsic.cpp
index 3228fd2c0fd9a1e17ee5a664503c76f683c582ee..c1e4c07e3764bcfe62f9eadf50952b81f0cc7c8d 100644
--- a/applications/gui2/src/modules/calibration/extrinsic.cpp
+++ b/applications/gui2/src/modules/calibration/extrinsic.cpp
@@ -56,6 +56,8 @@ void ExtrinsicCalibration::start(unsigned int fsid, std::vector<FrameID> sources
reset();
state_.cameras.reserve(sources.size()*2);
+ state_.maps1.resize(sources.size()*2);
+ state_.maps2.resize(sources.size()*2);
auto* filter = io->feed()->filter
(std::unordered_set<uint32_t>{fsid}, {Channel::Left, Channel::Right});
@@ -73,12 +75,19 @@ void ExtrinsicCalibration::start(unsigned int fsid, std::vector<FrameID> sources
auto cr = CameraID(id.frameset(), id.source(), Channel::Right);
const auto& frame = (*fs_current_)[id.source()].cast<ftl::rgbd::Frame>();
auto sz = cv::Size((int) frame.getLeftCamera().width, (int) frame.getLeftCamera().height);
- state_.cameras.push_back({cl, {}});
- state_.cameras.push_back({cr, {}});
-
- state_.calib.addStereoCamera(
- CalibrationData::Intrinsic(getCalibration(cl).intrinsic, sz),
- CalibrationData::Intrinsic(getCalibration(cr).intrinsic, sz));
+ state_.cameras.push_back(cl);
+ state_.cameras.push_back(cr);
+ auto calibl = getCalibration(cl);
+ calibl.intrinsic = CalibrationData::Intrinsic(calibl.intrinsic, sz);
+ auto calibr = getCalibration(cr);
+ calibr.intrinsic = CalibrationData::Intrinsic(calibr.intrinsic, sz);
+
+ // Scale intrinsics
+ state_.calib.addStereoCamera(calibl.intrinsic, calibr.intrinsic);
+
+ // Update rectification
+ unsigned int idx = state_.cameras.size() - 2;
+ stereoRectify(idx, idx + 1, calibl, calibr);
}
// initialize last points structure; can't be resized while running (without
@@ -90,15 +99,20 @@ void ExtrinsicCalibration::start(unsigned int fsid, std::vector<FrameID> sources
}
auto* view = new ftl::gui2::ExtrinsicCalibrationView(screen, this);
- view->onClose([this, filter](){
- running_ = false;
+ view->onClose([this, filter]() {
filter->remove();
+ state_.capture = false;
+
+ if (future_.valid()) {
+ future_.wait();
+ }
+
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) {
- fids.insert(camera.id.source());
+ fids.insert(camera.source());
}
for (const auto i : fids) {
@@ -109,8 +123,23 @@ void ExtrinsicCalibration::start(unsigned int fsid, std::vector<FrameID> sources
screen->setView(view);
}
+std::string ExtrinsicCalibration::cameraName(int c) {
+ const auto& camera = state_.cameras[c];
+ return (*fs_current_)[camera.id].name() + " - " +
+ ((camera.channel == Channel::Left) ? "Left" : "Right");
+}
+
+std::vector<std::string> ExtrinsicCalibration::cameraNames() {
+ std::vector<std::string> names;
+ names.reserve(cameraCount());
+ for (int i = 0; i < cameraCount(); i++) {
+ names.push_back(cameraName(i));
+ }
+ return names;
+}
+
CalibrationData::Calibration ExtrinsicCalibration::calibration(int c) {
- return state_.calib.calibration(c);
+ return state_.calib.calibrationOptimized(c);
}
bool ExtrinsicCalibration::onFrameSet_(const FrameSetPtr& fs) {
@@ -119,9 +148,8 @@ bool ExtrinsicCalibration::onFrameSet_(const FrameSetPtr& fs) {
screen->redraw();
bool all_good = true;
- for (const auto& [id, channel] : state_.cameras) {
- std::ignore = channel;
- all_good &= checkFrame((*fs)[id.source()]);
+ for (const auto& c : state_.cameras) {
+ all_good &= checkFrame((*fs)[c.source()]);
}
//if (!all_good) { return true; }
@@ -136,14 +164,14 @@ bool ExtrinsicCalibration::onFrameSet_(const FrameSetPtr& fs) {
int count = 0;
for (unsigned int i = 0; i < state_.cameras.size(); i++) {
- const auto& [id, calib] = state_.cameras[i];
-
+ const auto& id = state_.cameras[i];
+ const auto& calib = state_.calib.calibration(i).intrinsic;
if (!(*fs)[id.source()].hasChannel(id.channel)) { continue; }
points.clear();
const cv::cuda::GpuMat& im = (*fs)[id.source()].get<cv::cuda::GpuMat>(id.channel);
- K = calib.intrinsic.matrix();
- distCoeffs = calib.intrinsic.distCoeffs.Mat();
+ K = calib.matrix();
+ distCoeffs = calib.distCoeffs.Mat();
try {
int n = state_.calib_object->detect(im, points, K, distCoeffs);
@@ -171,13 +199,13 @@ bool ExtrinsicCalibration::onFrameSet_(const FrameSetPtr& fs) {
}
bool ExtrinsicCalibration::hasFrame(int camera) {
- const auto id = state_.cameras[camera].id;
+ const auto id = state_.cameras[camera];
return (std::atomic_load(&fs_current_).get() != nullptr) &&
((*fs_current_)[id.source()].hasChannel(id.channel));
}
const cv::cuda::GpuMat ExtrinsicCalibration::getFrame(int camera) {
- const auto id = state_.cameras[camera].id;
+ const auto id = state_.cameras[camera];
return (*fs_current_)[id.source()].cast<ftl::rgbd::Frame>().get<cv::cuda::GpuMat>(id.channel);
}
@@ -239,7 +267,7 @@ std::vector<ExtrinsicCalibration::CameraID> ExtrinsicCalibration::cameras() {
std::vector<ExtrinsicCalibration::CameraID> res;
res.reserve(cameraCount());
for (const auto& camera : state_.cameras) {
- res.push_back(camera.id);
+ res.push_back(camera);
}
return res;
}
@@ -254,13 +282,13 @@ void ExtrinsicCalibration::updateCalibration() {
for (unsigned int i = 0; i < state_.cameras.size(); i++) {
auto& c = state_.cameras[i];
- auto frame_id = ftl::data::FrameID(c.id);
+ auto frame_id = ftl::data::FrameID(c);
if (update.count(frame_id) == 0) {
- auto& frame = fs->frames[c.id];
+ auto& frame = fs->frames[c];
update[frame_id] = frame.get<CalibrationData>(Channel::CalibrationData);
}
- update[frame_id].get(c.id.channel) = state_.calib.calibrationOptimized(i);
+ update[frame_id].get(c.channel) = state_.calib.calibrationOptimized(i);
}
for (auto& [fid, calib] : update) {
@@ -276,8 +304,10 @@ void ExtrinsicCalibration::updateCalibration(int c) {
void ExtrinsicCalibration::stereoRectify(int cl, int cr,
const CalibrationData::Calibration& l, const CalibrationData::Calibration& r) {
- CHECK(l.extrinsic.tvec != r.extrinsic.tvec);
- CHECK(l.intrinsic.resolution == r.intrinsic.resolution);
+ CHECK_NE(l.extrinsic.tvec, r.extrinsic.tvec);
+ CHECK_EQ(l.intrinsic.resolution, r.intrinsic.resolution);
+ CHECK_LT(cr, state_.maps1.size());
+ CHECK_LT(cr, state_.maps2.size());
auto size = l.intrinsic.resolution;
cv::Mat T = r.extrinsic.matrix() * inverse(l.extrinsic.matrix());
@@ -290,6 +320,15 @@ void ExtrinsicCalibration::stereoRectify(int cl, int cr,
r.intrinsic.matrix(), r.intrinsic.distCoeffs.Mat(), size,
R, t, R1, R2, P1, P2, Q, cv::CALIB_ZERO_DISPARITY, 1.0);
+ // sanity check: rectification should give same rotation for both cameras
+ // cameras (with certain accuracy). R1 and R2 contain 3x3 rotation matrices
+ // from unrectified to rectified coordinates.
+ cv::Vec3d rvec1;
+ cv::Vec3d rvec2;
+ cv::Rodrigues(R1 * l.extrinsic.matrix()(cv::Rect(0, 0, 3, 3)), rvec1);
+ cv::Rodrigues(R2 * r.extrinsic.matrix()(cv::Rect(0, 0, 3, 3)), rvec2);
+ CHECK_LT(cv::norm(rvec1, rvec2), 0.01);
+
cv::initUndistortRectifyMap(l.intrinsic.matrix(), l.intrinsic.distCoeffs.Mat(),
R1, P1, size, CV_32FC1, map1, map2);
state_.maps1[cl].upload(map1);
@@ -331,6 +370,8 @@ 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) << "baseline (" << c << ", " << c + 1 << "): "
diff --git a/applications/gui2/src/modules/calibration/intrinsic.cpp b/applications/gui2/src/modules/calibration/intrinsic.cpp
index ba754184b06488c9bd41224507573ab2d7fe59cd..9a3dc06bdef71397f48626edf3a02e05a1a3875d 100644
--- a/applications/gui2/src/modules/calibration/intrinsic.cpp
+++ b/applications/gui2/src/modules/calibration/intrinsic.cpp
@@ -246,7 +246,7 @@ void IntrinsicCalibration::run() {
cv::Size size = state_->calib.resolution;
if (state_->flags.has(cv::CALIB_USE_INTRINSIC_GUESS)) {
// OpenCV seems to use these anyways?
- K = state_->calib.matrix();
+ K = state_->calib.matrix(size);
state_->calib.distCoeffs.Mat(12).copyTo(distCoeffs);
}
std::vector<cv::Mat> rvecs, tvecs;
@@ -341,8 +341,8 @@ void IntrinsicCalibration::setFocalLength(double value, cv::Size2d sensor_size)
setSensorSize(sensor_size);
double f = value*(state_->calib.resolution.width/sensor_size.width);
- state_->calib.cx = f;
- state_->calib.cy = f;
+ state_->calib.fx = f;
+ state_->calib.fy = f;
}
void IntrinsicCalibration::resetPrincipalPoint() {
diff --git a/applications/gui2/src/views/calibration/extrinsicview.cpp b/applications/gui2/src/views/calibration/extrinsicview.cpp
index 0c703474eac7ec293a526c013459522cba33fba1..b59f2ffa46b31802a5fa7b60c757439575324f29 100644
--- a/applications/gui2/src/views/calibration/extrinsicview.cpp
+++ b/applications/gui2/src/views/calibration/extrinsicview.cpp
@@ -1,5 +1,7 @@
#include "extrinsicview.hpp"
#include "visualization.hpp"
+#include "widgets.hpp"
+
#include "../../screen.hpp"
#include "../../widgets/window.hpp"
@@ -10,6 +12,7 @@
#include <nanogui/checkbox.h>
#include <nanogui/label.h>
#include <nanogui/formhelper.h>
+#include <nanogui/tabwidget.h>
using ftl::gui2::ExtrinsicCalibrationStart;
using ftl::gui2::ExtrinsicCalibrationView;
@@ -76,7 +79,7 @@ ExtrinsicCalibrationStart::~ExtrinsicCalibrationStart() {
void ExtrinsicCalibrationStart::draw(NVGcontext* ctx) {
window_->center();
- bcontinue_->setEnabled(sources_ != 0);
+ bcontinue_->setEnabled((lssources_->childCount() != 0));
ftl::gui2::View::draw(ctx);
}
@@ -212,6 +215,7 @@ ExtrinsicCalibrationView::ControlWindow::ControlWindow(nanogui::Widget* parent,
bapply_->setFlags(nanogui::Button::Flags::ToggleButton);
bapply_->setPushed(view_->rectify());
bapply_->setChangeCallback([button = bapply_, view = view_](bool v){
+ view->setMode(Mode::VIDEO); // stop capture
view->setRectify(v);
});
@@ -404,14 +408,19 @@ class ExtrinsicCalibrationView::ResultsWindow : public FixedWindow {
public:
ResultsWindow(nanogui::Widget* parent, ExtrinsicCalibrationView* view);
virtual void draw(NVGcontext* ctx) override;
+ virtual void performLayout(NVGcontext* ctx);
+ //virtual nanogui::Vector2i preferredSize(NVGcontext* ctx) const override;
+
+ void update();
private:
ExtrinsicCalibrationView* view_;
ExtrinsicCalibration* ctrl_;
- nanogui::Button* bcalibrate_;
- nanogui::Button* bpause_;
- nanogui::Button* bresults_;
+ std::vector<ftl::calibration::CalibrationData::Calibration> calib_;
+ std::vector<std::string> names_;
+
+ nanogui::TabWidget* tabs_ = nullptr;
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
@@ -420,25 +429,60 @@ public:
ExtrinsicCalibrationView::ResultsWindow::ResultsWindow(nanogui::Widget* parent, ExtrinsicCalibrationView* view) :
FixedWindow(parent, "Results"), view_(view), ctrl_(view->ctrl_) {
+ setLayout(new nanogui::BoxLayout
+ (nanogui::Orientation::Vertical, nanogui::Alignment::Maximum));
+
(new nanogui::Button(buttonPanel(), "", ENTYPO_ICON_CROSS))->setCallback(
[view = view_]() {
view->setMode(Mode::VIDEO);
});
- setSize({300, 300});
+
+ tabs_ = new nanogui::TabWidget(this);
+ tabs_->createTab("Extrinsic");
+}
+
+/*nanogui::Vector2i ExtrinsicCalibrationView::ResultsWindow::preferredSize(NVGcontext* ctx) const {
+ return {600, 400};
+}*/
+
+void ExtrinsicCalibrationView::ResultsWindow::ResultsWindow::performLayout(NVGcontext* ctx) {
+ setFixedSize({600, 400});
+ tabs_->setFixedWidth(width());
+ FixedWindow::performLayout(ctx);
+}
+
+void ExtrinsicCalibrationView::ResultsWindow::ResultsWindow::update() {
+ calib_.resize(ctrl_->cameraCount());
+ while (tabs_->tabCount() > 1) {
+ // bug in nanogui: incorrect assert in removeTab(int).
+ // workaround: use tabLabelAt()
+ tabs_->removeTab(tabs_->tabLabelAt(tabs_->tabCount() - 1));
+ }
+
+ for (int i = 0; i < ctrl_->cameraCount(); i++) {
+ calib_[i] = ctrl_->calibration(i);
+ // nanogui issue: too many tabs/long names header goes outside of widget
+ // use just idx for now
+ auto* tab = tabs_->createTab(std::to_string(i));
+ new nanogui::Label(tab, ctrl_->cameraName(i), "sans-bold", 18);
+ tab->setLayout(new nanogui::BoxLayout
+ (nanogui::Orientation::Vertical, nanogui::Alignment::Middle, 0, 8));
+
+ auto* display = new IntrinsicDetails(tab);
+ display->update(calib_[i].intrinsic);
+ }
}
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;
+ if (tabs_->activeTab() == 0) { // create a widget and move there
+ drawFloorPlan(ctx, tabs_->tab(0), calib_);
}
- drawFloorPlan(ctx, this, calib);
}
////////////////////////////////////////////////////////////////////////////////
-static void drawText(NVGcontext* ctx, nanogui::Vector2f &pos, const std::string& text,
+static void drawText(NVGcontext* ctx, const nanogui::Vector2f &pos, const std::string& text,
float size=12.0f, int align=NVG_ALIGN_MIDDLE|NVG_ALIGN_CENTER) {
nvgFontSize(ctx, size);
nvgFontFace(ctx, "sans-bold");
@@ -449,6 +493,98 @@ static void drawText(NVGcontext* ctx, nanogui::Vector2f &pos, const std::string&
nvgText(ctx, pos.x() + 1, pos.y() + 1, text.c_str(), nullptr);
}
+////////////////////////////////////////////////////////////////////////////////
+
+class StereoCalibrationImageView : public ftl::gui2::StereoImageView {
+public:
+ using ftl::gui2::StereoImageView::StereoImageView;
+
+ virtual bool keyboardCharacterEvent(unsigned int codepoint) override;
+ virtual bool mouseButtonEvent(const nanogui::Vector2i &p, int button, bool down, int modifiers) override;
+ virtual void draw(NVGcontext* ctx) override;
+
+ void reset();
+
+private:
+ std::set<int> rows_;
+ std::map<int, nanogui::Color> colors_;
+
+ int n_colors_ = 16;
+ float alpha_threshold_ = 2.0f;
+
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
+};
+
+void StereoCalibrationImageView::reset() {
+ rows_.clear();
+}
+
+bool StereoCalibrationImageView::keyboardCharacterEvent(unsigned int codepoint) {
+ if (codepoint == 'r') {
+ reset();
+ return true;
+ }
+ return StereoImageView::keyboardCharacterEvent(codepoint);
+}
+
+bool StereoCalibrationImageView::mouseButtonEvent(const nanogui::Vector2i &p, int button, bool down, int modifiers) {
+ nanogui::Widget::mouseButtonEvent(p, button, down, modifiers);
+ if (button == GLFW_MOUSE_BUTTON_1 && !down) {
+ // half a pixel offset to match "square pixel" visualization
+ nanogui::Vector2f offset{left()->scale()/2.0f, left()->scale()/2.0f};
+ float row = round(imageCoordinateAt(p.cast<float>() + offset).y());
+
+ if (rows_.count(row)) { rows_.erase(row); }
+ else { rows_.insert(row); }
+ }
+ return true;
+}
+
+void StereoCalibrationImageView::draw(NVGcontext* ctx) {
+ StereoImageView::draw(ctx);
+ // assumes vertical alignment (horizontal not implemented)
+ CHECK(orientation() == nanogui::Orientation::Vertical);
+
+ int x = position().x();
+ int y = position().y();
+ int w = width();
+ int h = left()->height();
+ float swidth = std::max(1.0f, left()->scale());
+ int c = 0; // color
+
+ for (int row : rows_) {
+ int y_im = y;
+ nanogui::Vector2f l = left()->positionForCoordinate({0.0f, row}) + left()->position().cast<float>();
+ nanogui::Vector2f r = right()->positionForCoordinate({0.0f, row}) + right()->position().cast<float>();
+ auto color = nvgHSLA(float(c%n_colors_)/float(n_colors_), 0.9, 0.5, (swidth < alpha_threshold_) ? 255 : 96);
+
+ for (auto& p : {l, r}) {
+ nvgScissor(ctx, x, y_im, w, h);
+ nvgBeginPath(ctx);
+ nvgMoveTo(ctx, x, p.y() - swidth*0.5f);
+ nvgLineTo(ctx, x + w, p.y() - swidth*0.5f);
+ nvgStrokeColor(ctx, color);
+ nvgStrokeWidth(ctx, swidth);
+ nvgStroke(ctx);
+
+ 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;
+ }
+ c++;
+ }
+}
+
+////////////////////////////////////////////////////////////////////////////////
+
ExtrinsicCalibrationView::ExtrinsicCalibrationView(Screen* widget, ExtrinsicCalibration* ctrl) :
ftl::gui2::View(widget), ctrl_(ctrl), rows_(0) {
@@ -461,7 +597,7 @@ ExtrinsicCalibrationView::ExtrinsicCalibrationView(Screen* widget, ExtrinsicCali
// assumes all cameras stereo cameras, indexed in order
for (int i = 0; i < ctrl_->cameraCount(); i += 2) {
- new StereoImageView(frames_, nanogui::Orientation::Vertical);
+ new StereoCalibrationImageView(frames_, nanogui::Orientation::Vertical);
}
paused_ = false;
wcontrol_ = new ControlWindow(screen(), this);
@@ -483,7 +619,7 @@ void ExtrinsicCalibrationView::performLayout(NVGcontext* ctx) {
nanogui::Vector2i fsize = { width()/(frames_->childCount()), height() };
for (int i = 0; i < frames_->childCount(); i++) {
- auto* stereo = dynamic_cast<StereoImageView*>(frames_->childAt(i));
+ auto* stereo = dynamic_cast<StereoCalibrationImageView*>(frames_->childAt(i));
stereo->setFixedSize(fsize);
stereo->fit();
}
@@ -562,10 +698,30 @@ void ExtrinsicCalibrationView::draw(NVGcontext* ctx) {
drawText(ctx, paths[p], std::to_string(p), 14.0f);
}
}*/
- nanogui::Vector2f cpos = wpos + nanogui::Vector2f{10.0f, 10.0f};
- drawText(ctx, cpos, std::to_string(ctrl_->getFrameCount(i)), 20.0f, NVG_ALIGN_TOP|NVG_ALIGN_LEFT);
+
+ // TODO: move to stereocalibrateimageview
+ nanogui::Vector2f tpos = wpos + nanogui::Vector2f{10.0f, 10.0f};
+ drawText(ctx, tpos, std::to_string(ctrl_->getFrameCount(i)), 20.0f, NVG_ALIGN_TOP|NVG_ALIGN_LEFT);
+
+ tpos = wpos + nanogui::Vector2f{10.0f, wsize.y() - 30.0f};
+ drawText(ctx, tpos, ctrl_->cameraName(i), 20.0f, NVG_ALIGN_TOP|NVG_ALIGN_LEFT);
+
nvgResetScissor(ctx);
}
+
+ {
+ float h = 14.0f;
+ for (const auto& text : {"Left click: draw line",
+ "Right click: pan",
+ "Scroll: zoom",
+ "C center",
+ "F fit",
+ "R clear lines"
+ }) {
+ drawText(ctx, {float(width()) - 60.0, h}, text, 14, NVGalign::NVG_ALIGN_BOTTOM | NVG_ALIGN_MIDDLE);
+ h += 20.0;
+ }
+ }
}
ExtrinsicCalibrationView::~ExtrinsicCalibrationView() {
@@ -602,7 +758,7 @@ void ExtrinsicCalibrationView::setMode(Mode mode) {
wcontrol_->setVisible(false);
wcalibration_->setVisible(false);
wresults_->setVisible(true);
- //wresults_->update();
+ wresults_->update();
break;
}
screen()->performLayout();
diff --git a/applications/gui2/src/views/calibration/visualization.hpp b/applications/gui2/src/views/calibration/visualization.hpp
index de2fe29ab0410ebaf68fadceecd98c1749a94149..026c40dc20bd12197503909d8f41990261f96834 100644
--- a/applications/gui2/src/views/calibration/visualization.hpp
+++ b/applications/gui2/src/views/calibration/visualization.hpp
@@ -42,8 +42,45 @@ static void drawChessboardCorners(NVGcontext* ctx, ftl::gui2::ImageView* imview,
nvgResetScissor(ctx);
}
+static void drawTriangle(NVGcontext* ctx, const ftl::calibration::CalibrationData::Extrinsic &calib,
+ const nanogui::Vector2f &pos, const nanogui::Vector2f offset, float scale, float sz=1.0f) {
+ const int idx_x = 0;
+ const int idx_y = 2;
+
+ cv::Mat T = calib.matrix();
+ cv::Vec4f p1(cv::Mat(T * cv::Vec4d{sz/2.0f, 0.0f, 0.0f, 1.0f}));
+ cv::Vec4f p2(cv::Mat(T * cv::Vec4d{-sz/2.0f, 0.0f, 0.0f, 1.0f}));
+ cv::Vec4f p3(cv::Mat(T * cv::Vec4d{0.0f, 0.0f, -sz*sqrtf(3.0f)/2.0f, 1.0f}));
+
+ p1[idx_x] -= offset.x();
+ p2[idx_x] -= offset.x();
+ p3[idx_x] -= offset.x();
+ p1[idx_y] -= offset.y();
+ p2[idx_y] -= offset.y();
+ p3[idx_y] -= offset.y();
+ p1 *= scale;
+ p2 *= scale;
+ p3 *= scale;
+
+ nvgBeginPath(ctx);
+
+ // NOTE: flip x
+ nvgMoveTo(ctx, pos.x() + p1[idx_x], pos.y() + p1[idx_y]);
+ nvgLineTo(ctx, pos.x() + p2[idx_x], pos.y() + p2[idx_y]);
+ nvgLineTo(ctx, pos.x() + p3[idx_x], pos.y() + p3[idx_y]);
+ nvgLineTo(ctx, pos.x() + p1[idx_x], pos.y() + p1[idx_y]);
+ if (calib.tvec == cv::Vec3d{0.0, 0.0, 0.0}) {
+ nvgStrokeColor(ctx, nvgRGBA(255, 64, 64, 255));
+ }
+ else {
+ nvgStrokeColor(ctx, nvgRGBA(255, 255, 255, 255));
+ }
+ nvgStrokeWidth(ctx, 1.0f);
+ nvgStroke(ctx);
+}
+
static void drawFloorPlan(NVGcontext* ctx, nanogui::Widget* parent,
- const std::vector<ftl::calibration::CalibrationData::Extrinsic>& calib,
+ const std::vector<ftl::calibration::CalibrationData::Calibration>& calib,
const std::vector<std::string>& names = {},
int origin=0) {
@@ -51,35 +88,31 @@ static void drawFloorPlan(NVGcontext* ctx, nanogui::Widget* parent,
float miny = INFINITY;
float maxx = -INFINITY;
float maxy = -INFINITY;
-
+ cv::Vec3f center = {0.0f, 0.0f};
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];
+ const auto& extrinsic = calib[i].extrinsic;
+ // xz, assume floor on y-plane y = 0
+ float x = extrinsic.tvec[0];
+ float y = extrinsic.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);
+ center += extrinsic.tvec;
}
-
+ center /= float(points.size());
float w = parent->width();
- float sx = w/(maxx - minx);
+ float dx = maxx - minx;
float h = parent->height();
- float sy = h/(maxy - miny);
- float s = min(sx, sy);
+ float dy = maxy - miny;
+ float s = min(w/dx, h/dy) * 0.8; // scale
nanogui::Vector2f apos = parent->absolutePosition().cast<float>() + nanogui::Vector2f{w/2.0f, h/2.0f};
+ nanogui::Vector2f off{center[0], center[2]};
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);
+ drawTriangle(ctx, calib[i].extrinsic, apos, off, s, 0.3);
}
}
diff --git a/applications/gui2/src/views/calibration/widgets.cpp b/applications/gui2/src/views/calibration/widgets.cpp
index f907b2ce1a37ecb5f69504b6987f3d186ae361a7..80a4af5d5d4a43ff19b3bf206891af924de456aa 100644
--- a/applications/gui2/src/views/calibration/widgets.cpp
+++ b/applications/gui2/src/views/calibration/widgets.cpp
@@ -126,17 +126,23 @@ void IntrinsicDetails::update(const ftl::calibration::CalibrationData::Intrinsic
"(" + to_string(values.cx) + ", " +
to_string(values.cy) + ")");
+ new nanogui::Widget(params_);
+ new nanogui::Label(params_,
+ "(" + to_string(100.0*(2.0*values.cx/double(imsize.width) - 1.0)) + "% , " +
+ to_string(100.0*(2.0*values.cy/double(imsize.height) - 1.0)) + "%)");
+ if (use_physical) new nanogui::Widget(params_);
+
new nanogui::Label(params_, "Field of View (x):");
new nanogui::Label(params_, to_string(fovx) + "°");
- if (use_physical) new nanogui::Label(params_, "");
+ if (use_physical) new nanogui::Widget(params_);
new nanogui::Label(params_, "Field of View (y):");
new nanogui::Label(params_, to_string(fovy)+ "°");
- if (use_physical) new nanogui::Label(params_, "");
+ if (use_physical) new nanogui::Widget(params_);
new nanogui::Label(params_, "Aspect ratio:");
new nanogui::Label(params_, to_string(ar));
- if (use_physical) new nanogui::Label(params_, "");
+ if (use_physical) new nanogui::Widget(params_);
std::string pK;
std::string pP;
diff --git a/applications/gui2/src/views/thumbnails.cpp b/applications/gui2/src/views/thumbnails.cpp
index 7250e176341d7888d3256e2010cdec4aaab6a7a3..b04a6428da734a5d6bda323c6c9526d5971cc7e2 100644
--- a/applications/gui2/src/views/thumbnails.cpp
+++ b/applications/gui2/src/views/thumbnails.cpp
@@ -165,7 +165,7 @@ void Thumbnails::updateThumbnails() {
continue;
}
- auto* tab = tabwidget_->createTab("Frameset " + std::to_string(fsid));
+ auto* tab = tabwidget_->createTab(framesets[fsid]->name());
tab->setLayout(new nanogui::BoxLayout
(nanogui::Orientation::Vertical, nanogui::Alignment::Middle, 40));
auto* panel = new nanogui::Widget(tab);
diff --git a/applications/gui2/src/widgets/imageview.cpp b/applications/gui2/src/widgets/imageview.cpp
index 3562577975d3d882b3599e6efbbc693811bb5c8f..2137b1ce9f8124c65234952dfaaa93605323bbb5 100644
--- a/applications/gui2/src/widgets/imageview.cpp
+++ b/applications/gui2/src/widgets/imageview.cpp
@@ -538,6 +538,24 @@ StereoImageView::StereoImageView(nanogui::Widget* parent, nanogui::Orientation o
right_->setFixedScale(true);
}
+
+nanogui::Vector2f StereoImageView::imageCoordinateAt(const nanogui::Vector2f& p) const {
+
+ nanogui::Vector2f pos = position().cast<float>();
+ nanogui::Vector2f posr = pos + right_->position().cast<float>();
+
+ bool is_right =
+ ((p.x() >= posr.x()) && (orientation_ == nanogui::Orientation::Horizontal)) ||
+ ((p.y() >= posr.y()) && (orientation_ == nanogui::Orientation::Vertical));
+
+ if (is_right) {
+ return right_->imageCoordinateAt(p - right_->position().cast<float>());
+ }
+ else {
+ return left_->imageCoordinateAt(p);
+ }
+}
+
bool StereoImageView::mouseMotionEvent(const nanogui::Vector2i &p, const nanogui::Vector2i &rel, int button, int modifiers) {
if ((button & (1 << GLFW_MOUSE_BUTTON_RIGHT)) != 0) {
nanogui::Vector2f posl = left_->imageCoordinateAt(p.cast<float>());
@@ -554,7 +572,7 @@ bool StereoImageView::mouseMotionEvent(const nanogui::Vector2i &p, const nanogui
}
return false;
}
-#include <loguru.hpp>
+
bool StereoImageView::scrollEvent(const nanogui::Vector2i& p, const nanogui::Vector2f& rel) {
// synchronized zoom
diff --git a/applications/gui2/src/widgets/imageview.hpp b/applications/gui2/src/widgets/imageview.hpp
index 740152cdf15183a2aed2440c53ea52edb1059fa3..0209636fafd0049e4fcc8304db2b0240f15c8f2d 100644
--- a/applications/gui2/src/widgets/imageview.hpp
+++ b/applications/gui2/src/widgets/imageview.hpp
@@ -217,7 +217,10 @@ public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
-/** Two ImageViews with synchronized zoom and pan. */
+/** Two ImageViews with synchronized zoom and pan. Widget split in two equal
+ * size sections (left and right). With vertical orientation right is the lower
+ * image.
+*/
class StereoImageView : public nanogui::Widget {
public:
StereoImageView(nanogui::Widget* parent, nanogui::Orientation orientation = nanogui::Orientation::Horizontal);
@@ -232,6 +235,11 @@ public:
FTLImageView* left() { return left_; }
FTLImageView* right() { return right_; }
+ /** get image coordinate at given widget coordinate */
+ nanogui::Vector2f imageCoordinateAt(const nanogui::Vector2f& position) const;
+
+ nanogui::Orientation orientation() { return orientation_; }
+
void fit();
void bindLeft(GLuint id) { left_->texture().free(); left_->bindImage(id); }
diff --git a/components/calibration/include/ftl/calibration/extrinsic.hpp b/components/calibration/include/ftl/calibration/extrinsic.hpp
index 5bded2bc3ddf423d2b8ac4ee7ae2e9a0c7395047..a9f7208e39ff158cb830e0e8900b5d263defe2b6 100644
--- a/components/calibration/include/ftl/calibration/extrinsic.hpp
+++ b/components/calibration/include/ftl/calibration/extrinsic.hpp
@@ -158,14 +158,18 @@ double calibratePair(const cv::Mat &K1, const cv::Mat &D1,
class ExtrinsicCalibration {
public:
-
- /** add a single camera. Returns index of camera. */
+ /** add a single (uncalibrated) camera. Returns index of camera. */
unsigned int addCamera(const CalibrationData::Intrinsic &);
+
+ /** add a single calibrated camera (if valid calibration). Returns index of camera. */
+ unsigned int addCamera(const CalibrationData::Calibration &);
+
/** Add a stereo camera pair. Pairs always use other cameras to estimate
* initial pose. Returns index of first camera. */
unsigned int addStereoCamera(const CalibrationData::Intrinsic &, const CalibrationData::Intrinsic &);
- /** Add stereo camera pair with initial pose. Returns index of first camera. */
- unsigned int addStereoCamera(const CalibrationData::Intrinsic &, const CalibrationData::Intrinsic &, cv::Vec3d rvec, cv::Vec3d tvec);
+
+ /** Add calibrated stereo camera (if contains valid calibration) */
+ unsigned int addStereoCamera(const CalibrationData::Calibration &, const CalibrationData::Calibration &);
const CalibrationData::Intrinsic& intrinsic(unsigned int c);
const CalibrationData::Extrinsic& extrinsic(unsigned int c);
@@ -201,7 +205,7 @@ public:
bool fromFile(const std::string& fname);
bool toFile(const std::string& fname); // should return new instance...
- MSGPACK_DEFINE(points_, mask_, pairs_, calib_);
+ MSGPACK_DEFINE(points_, mask_, pairs_, calib_, is_calibrated_);
protected:
/** Initial pairwise calibration and triangulation. */
@@ -222,6 +226,12 @@ private:
CalibrationPoints<double> points_;
std::set<std::pair<unsigned int, unsigned int>> mask_;
std::map<std::pair<unsigned int, unsigned int>, std::tuple<cv::Mat, cv::Mat, double>> pairs_;
+ unsigned int c_ref_;
+
+ // true if camera already has valid calibration; initial pose estimation is
+ // skipped (and points are directly triangulated)
+ std::vector<bool> is_calibrated_;
+
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/include/ftl/calibration/optimize.hpp b/components/calibration/include/ftl/calibration/optimize.hpp
index 17dd1b42c9a33199077f874acddad3ccbb996f87..2e694cd9d7d7d77197543ba8ad7d9e097bf49f53 100644
--- a/components/calibration/include/ftl/calibration/optimize.hpp
+++ b/components/calibration/include/ftl/calibration/optimize.hpp
@@ -59,11 +59,15 @@ struct Camera {
cv::Mat extrinsicMatrix() const;
cv::Mat extrinsicMatrixInverse() const;
+ void toQuaternion();
+ void toAngleAxis();
+
cv::Size size;
const static int n_parameters = 18;
const static int n_distortion_parameters = 8;
+ bool quaternion = false;
double data[n_parameters] = {0.0};
enum Parameter {
@@ -131,6 +135,9 @@ public:
bool use_nonmonotonic_steps = false;
+ // use quaternion rotation
+ bool use_quaternion = 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.
@@ -193,6 +200,11 @@ public:
*/
void run(const BundleAdjustment::Options& options);
+ /** @brief Get optimized points
+ *
+ */
+ std::vector<cv::Point3d> getPoints();
+
/** @brief Perform bundle adjustment using default options
*/
void run();
@@ -205,6 +217,9 @@ public:
*/
double reprojectionError() const;
+ /**/
+ int removeObservations(double threshold);
+
protected:
double* getCameraPtr(int i) { return cameras_.at(i)->data; }
@@ -220,6 +235,9 @@ protected:
void _buildProblem(ceres::Problem& problem, const BundleAdjustment::Options& options);
void _buildBundleAdjustmentProblem(ceres::Problem& problem, const BundleAdjustment::Options& options);
+ // remove?
+ void _buildLengthProblem(ceres::Problem& problem, const BundleAdjustment::Options& options);
+
private:
// point to be optimized and corresponding observations
struct Point {
diff --git a/components/calibration/include/ftl/calibration/structures.hpp b/components/calibration/include/ftl/calibration/structures.hpp
index e0eeb2fcea82b491637b580680d0398813f90137..9766143742977cb171d9e60fddd7c4db607d7731 100644
--- a/components/calibration/include/ftl/calibration/structures.hpp
+++ b/components/calibration/include/ftl/calibration/structures.hpp
@@ -12,6 +12,8 @@ struct CalibrationData {
struct Intrinsic {
friend CalibrationData;
+ /** 12 distortion coefficients. OpenCV also provides tilted camera model
+ * coefficients, but not used here. */
struct DistortionCoefficients {
friend CalibrationData;
@@ -25,10 +27,16 @@ struct CalibrationData {
* 2,3 p1-p2 tangential distortion
* 4,5,6,7 r3-r6 radial distortion
* 8,9,10,11 s1-s4 thin prism distortion
+ *
*/
double& operator[](unsigned i);
double operator[](unsigned i) const;
+ /** are radial distortion values are for rational model */
+ bool rationalModel() const;
+ /** is thin prism model is used (s1-s4 set) */
+ bool thinPrism() const;
+
/**
* Return distortion parameters in cv::Mat. Shares same memory.
*/
@@ -48,6 +56,18 @@ struct CalibrationData {
/** New instance with scaled values for new resolution */
Intrinsic(const Intrinsic& other, cv::Size sz);
+ /* valid values (resolution is non-zero) */
+ bool valid() const;
+
+ /** horizontal field of view in degrees */
+ double fovx() const;
+ /** vertical field of view in degrees */
+ double fovy() const;
+ /** focal length in sensor size units */
+ double focal() const;
+ /** aspect ratio: fx/fy */
+ double aspectRatio() const;
+
/** Replace current values with new ones */
void set(const cv::Mat &K, cv::Size sz);
void set(const cv::Mat &K, const cv::Mat &D, cv::Size sz);
@@ -64,7 +84,7 @@ struct CalibrationData {
double cy;
DistortionCoefficients distCoeffs;
- /** (optional) sensor size */
+ /** (optional) sensor size; Move elsehwere? */
cv::Size2d sensorSize;
MSGPACK_DEFINE(resolution, fx, fy, cx, cy, distCoeffs, sensorSize);
@@ -79,13 +99,16 @@ struct CalibrationData {
Extrinsic inverse() const;
+ /** valid calibration (values not NAN) */
+ bool valid() const;
+
/** get as a 4x4 matrix */
cv::Mat matrix() const;
/** get 3x3 rotation matrix */
cv::Mat rmat() const;
- cv::Vec3d rvec;
- cv::Vec3d tvec;
+ cv::Vec3d rvec = {NAN, NAN, NAN};
+ cv::Vec3d tvec = {NAN, NAN, NAN};
MSGPACK_DEFINE(rvec, tvec);
};
@@ -107,6 +130,7 @@ struct CalibrationData {
bool hasCalibration(ftl::codecs::Channel channel) const;
private:
+ // TODO: identify cameras with unique ID string instead of channel.
std::map<ftl::codecs::Channel, Calibration> data_;
public:
diff --git a/components/calibration/src/extrinsic.cpp b/components/calibration/src/extrinsic.cpp
index 4e59a54665a24384a1abc64d13746eb55116e98f..d32961040338efec217048c1017b84588b12d443 100644
--- a/components/calibration/src/extrinsic.cpp
+++ b/components/calibration/src/extrinsic.cpp
@@ -248,7 +248,7 @@ int recoverPose(const cv::Mat &E, const std::vector<cv::Point2d> &_points1,
cv::Mat points1(_points1.size(), 2, CV_64FC1);
cv::Mat points2(_points2.size(), 2, CV_64FC1);
- CHECK(points1.size() == points2.size());
+ CHECK_EQ(points1.size(), points2.size());
for (size_t i = 0; i < _points1.size(); i++) {
auto p1 = points1.ptr<double>(i);
@@ -309,7 +309,7 @@ double calibratePair(const cv::Mat &K1, const cv::Mat &D1,
// convert from homogenous coordinates
for (int col = 0; col < points3dh.cols; col++) {
- CHECK(points3dh.at<double>(3, col) != 0);
+ CHECK_NE(points3dh.at<double>(3, col), 0);
cv::Point3d p = cv::Point3d(points3dh.at<double>(0, col),
points3dh.at<double>(1, col),
points3dh.at<double>(2, col))
@@ -332,7 +332,7 @@ double calibratePair(const cv::Mat &K1, const cv::Mat &D1,
}
// needs to be implemented correctly: optimize each pose of the target
- ba.addObject(object_points);
+ //ba.addObject(object_points);
double error = ba.reprojectionError();
@@ -345,8 +345,8 @@ double calibratePair(const cv::Mat &K1, const cv::Mat &D1,
ba.run(options);
error = ba.reprojectionError();
}
- CHECK((cv::countNonZero(params1.rvec()) == 0) &&
- (cv::countNonZero(params1.tvec()) == 0));
+ CHECK_EQ(cv::countNonZero(params1.rvec()), 0);
+ CHECK_EQ(cv::countNonZero(params1.tvec()), 0);
return sqrt(error);
}
@@ -356,13 +356,39 @@ double calibratePair(const cv::Mat &K1, const cv::Mat &D1,
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);
+ calib_optimized_.push_back(calib_.back());
+ is_calibrated_.push_back(true);
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});
+ calib_optimized_.push_back(calib_.back());
+ calib_.push_back({c2.intrinsic, c2.extrinsic});
+ calib_optimized_.push_back(calib_.back());
+ is_calibrated_.push_back(c1.extrinsic.valid());
+ is_calibrated_.push_back(c2.extrinsic.valid());
mask_.insert({idx, idx + 1});
return idx;
}
@@ -462,12 +488,12 @@ void ExtrinsicCalibration::calculateInitialPoses() {
}}
// pick optimal camera: most views of calibration pattern
- unsigned int c_ref = visibility.argmax();
+ c_ref_ = visibility.argmax();
- auto paths = visibility.shortestPath(c_ref);
+ auto paths = visibility.shortestPath(c_ref_);
for (unsigned int c = 0; c < camerasCount(); c++) {
- if (c == c_ref) { continue; }
+ if (c == c_ref_) { continue; }
cv::Mat R_chain = cv::Mat::eye(cv::Size(3, 3), CV_64FC1);
cv::Mat t_chain = cv::Mat(cv::Size(1, 3), CV_64FC1, cv::Scalar(0.0));
@@ -490,7 +516,8 @@ void ExtrinsicCalibration::calculateInitialPoses() {
}
while(path.size() > 1);
- // note: direction of chain in the loop, hence inverse()
+ // note: direction of chain in the loop (ref to target transformation)
+
calib_[c].extrinsic =
CalibrationData::Extrinsic(R_chain, t_chain).inverse();
}
@@ -545,6 +572,8 @@ double ExtrinsicCalibration::optimize() {
// BundleAdjustment stores pointers; do not resize cameras vector
ba.addCamera(c);
}
+ // TODO (?) is this good idea?; make optional
+ ba.addObject(points_.getObject(0));
// Transform triangulated points into same coordinates. Poinst which are
// triangulated multiple times: use median values. Note T[] contains
@@ -640,6 +669,7 @@ double ExtrinsicCalibration::optimize() {
}
updateStatus_("Bundle adjustment");
+ options_.fix_camera_extrinsic = {int(c_ref_)};
options_.verbose = true;
options_.max_iter = 250; // should converge much earlier
@@ -648,6 +678,10 @@ double ExtrinsicCalibration::optimize() {
LOG(INFO) << "fix principal point: " << (options_.fix_principal_point ? "yes" : "no");
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_);
calib_optimized_.resize(calib_.size());
@@ -656,13 +690,13 @@ double ExtrinsicCalibration::optimize() {
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];
calib_optimized_[i].intrinsic.set(intr.matrix(), intr.distCoeffs.Mat(), intr.resolution);
- calib_optimized_[i].extrinsic.set(extr.rvec, extr.tvec);
+ calib_optimized_[i].extrinsic.set(cameras[i].rvec(), cameras[i].tvec());
}
rmse_total_ = ba.reprojectionError();
+
LOG(INFO) << "reprojection error (all cameras): " << rmse_total_;
return rmse_total_;
}
diff --git a/components/calibration/src/object.cpp b/components/calibration/src/object.cpp
index 83f70a3b69a8d6160c418efe4b6810e082cd78ba..4236b864a1711fee2d180e3f37c703ba96d4def0 100644
--- a/components/calibration/src/object.cpp
+++ b/components/calibration/src/object.cpp
@@ -19,7 +19,7 @@ ArUCoObject::ArUCoObject(cv::aruco::PREDEFINED_DICTIONARY_NAME dictionary,
params_ = cv::aruco::DetectorParameters::create();
params_->cornerRefinementMinAccuracy = 0.01;
// cv::aruco::CORNER_REFINE_CONTOUR memory issues? intrinsic quality?
- params_->cornerRefinementMethod = cv::aruco::CORNER_REFINE_APRILTAG;
+ params_->cornerRefinementMethod = cv::aruco::CORNER_REFINE_CONTOUR;
}
std::vector<cv::Point3d> ArUCoObject::object() {
@@ -42,7 +42,9 @@ int ArUCoObject::detect(cv::InputArray im, std::vector<cv::Point2d>& result, con
std::vector<std::vector<cv::Point2f>> corners;
std::vector<int> ids;
cv::Mat im_gray;
-
+ // OpenCV bug: detectMarkers consumes all available memory when any
+ // distortion parameters are passes
+ const cv::Mat d;
if (im.size() == cv::Size(0, 0)) {
return -1;
}
@@ -59,7 +61,7 @@ int ArUCoObject::detect(cv::InputArray im, std::vector<cv::Point2d>& result, con
}
cv::aruco::detectMarkers(im_gray, dict_, corners, ids, params_,
- cv::noArray(), K, distCoeffs);
+ cv::noArray(), K, d);
if (ids.size() < 2) { return 0; }
diff --git a/components/calibration/src/optimize.cpp b/components/calibration/src/optimize.cpp
index 76db14c07b5a28e22e0db846e3fb4190b5e55eed..a7ff076238bfdadaf835bc1db5c644f5f0b566d3 100644
--- a/components/calibration/src/optimize.cpp
+++ b/components/calibration/src/optimize.cpp
@@ -154,7 +154,7 @@ Mat Camera::distortionCoefficients() const {
Mat Camera::rvec() const {
cv::Mat rvec(cv::Size(3, 1), CV_64FC1);
- CHECK(rvec.step1() == 3);
+ CHECK_EQ(rvec.step1(), 3);
ceres::QuaternionToAngleAxis(data + Parameter::ROTATION,
(double*)(rvec.data));
return rvec;
@@ -166,7 +166,7 @@ Mat Camera::tvec() const {
Mat Camera::rmat() const {
cv::Mat R(cv::Size(3, 3), CV_64FC1);
- CHECK(R.step1() == 3);
+ CHECK_EQ(R.step1(), 3);
ceres::QuaternionToRotation<double>(data + Parameter::ROTATION,
ceres::RowMajorAdapter3x3<double>((double*)(R.data)));
@@ -284,6 +284,29 @@ cv::Point2d ftl::calibration::projectPoint(const Camera& camera, const cv::Point
}
////////////////////////////////////////////////////////////////////////////////
+// TODO: estimate pose and optimize it instead (?)
+
+struct LengthError {
+ explicit LengthError(const double d) : d(d) {}
+
+ template <typename T>
+ bool operator()(const T* const p1, const T* const p2, T* residual) const {
+ auto x = p1[0] - p2[0];
+ auto y = p1[1] - p2[1];
+ auto z = p1[2] - p2[2];
+ residual[0] = d - sqrt(x*x + y*y + z*z);
+
+ return true;
+ }
+
+ static ceres::CostFunction* Create(const double d) {
+ return (new ceres::AutoDiffCostFunction<LengthError, 1, 3, 3>(new LengthError(d)));
+ }
+
+ double d;
+};
+
+//
struct ScaleError {
ScaleError(const double d, const Point3d& p) : d(d), p(p) {}
@@ -311,8 +334,8 @@ struct ScaleError {
double ftl::calibration::optimizeScale(const vector<Point3d> &object_points, vector<Point3d> &points) {
// use exceptions instead
- CHECK(points.size() % object_points.size() == 0);
- CHECK(points.size() % 2 == 0);
+ CHECK_EQ(points.size() % object_points.size(), 0);
+ CHECK_EQ(points.size() % 2, 0);
// initial scale guess from first two object points
@@ -420,14 +443,9 @@ void BundleAdjustment::addPoints(const vector<vector<Point2d>>& observations, st
}
}
-void BundleAdjustment::addObject(const vector<Point3d> &object_points) {
- if (points_.size() % object_points.size() != 0) { throw ftl::exception("object does match point count"); }
- objects_.push_back(BundleAdjustment::Object {0, (int) points_.size(), object_points});
-}
-
void BundleAdjustment::_setCameraParametrization(ceres::Problem &problem, const BundleAdjustment::Options &options) {
- vector<int> constant_camera_parameters;
+ std::set<int> constant_camera_parameters;
// apply options
for (size_t i = 0; i < cameras_.size(); i++) {
@@ -435,6 +453,9 @@ void BundleAdjustment::_setCameraParametrization(ceres::Problem &problem, const
cameras_[i]->data[Camera::Parameter::K4] = 0.0;
cameras_[i]->data[Camera::Parameter::K5] = 0.0;
cameras_[i]->data[Camera::Parameter::K6] = 0.0;
+ constant_camera_parameters.insert(Camera::Parameter::K4);
+ constant_camera_parameters.insert(Camera::Parameter::K5);
+ constant_camera_parameters.insert(Camera::Parameter::K6);
}
if (options.zero_distortion) {
cameras_[i]->data[Camera::Parameter::K1] = 0.0;
@@ -450,33 +471,33 @@ void BundleAdjustment::_setCameraParametrization(ceres::Problem &problem, const
// set extrinsic paramters constant for all cameras
if (!options.optimize_motion) {
- constant_camera_parameters.push_back(Camera::Parameter::Q1);
- constant_camera_parameters.push_back(Camera::Parameter::Q2);
- constant_camera_parameters.push_back(Camera::Parameter::Q3);
- constant_camera_parameters.push_back(Camera::Parameter::Q4);
- constant_camera_parameters.push_back(Camera::Parameter::TX);
- constant_camera_parameters.push_back(Camera::Parameter::TY);
- constant_camera_parameters.push_back(Camera::Parameter::TZ);
+ constant_camera_parameters.insert(Camera::Parameter::Q1);
+ constant_camera_parameters.insert(Camera::Parameter::Q2);
+ constant_camera_parameters.insert(Camera::Parameter::Q3);
+ constant_camera_parameters.insert(Camera::Parameter::Q4);
+ constant_camera_parameters.insert(Camera::Parameter::TX);
+ constant_camera_parameters.insert(Camera::Parameter::TY);
+ constant_camera_parameters.insert(Camera::Parameter::TZ);
}
// set intrinsic parameters constant for all cameras
if (!options.optimize_intrinsic || options.fix_focal) {
- constant_camera_parameters.push_back(Camera::Parameter::F);
+ constant_camera_parameters.insert(Camera::Parameter::F);
}
if (!options.optimize_intrinsic || options.fix_principal_point) {
- constant_camera_parameters.push_back(Camera::Parameter::CX);
- constant_camera_parameters.push_back(Camera::Parameter::CY);
+ constant_camera_parameters.insert(Camera::Parameter::CX);
+ constant_camera_parameters.insert(Camera::Parameter::CY);
}
if (!options.optimize_intrinsic || options.fix_distortion) {
- constant_camera_parameters.push_back(Camera::Parameter::K1);
- constant_camera_parameters.push_back(Camera::Parameter::K2);
- constant_camera_parameters.push_back(Camera::Parameter::K3);
- constant_camera_parameters.push_back(Camera::Parameter::K4);
- constant_camera_parameters.push_back(Camera::Parameter::K5);
- constant_camera_parameters.push_back(Camera::Parameter::K6);
- constant_camera_parameters.push_back(Camera::Parameter::P1);
- constant_camera_parameters.push_back(Camera::Parameter::P2);
+ constant_camera_parameters.insert(Camera::Parameter::K1);
+ constant_camera_parameters.insert(Camera::Parameter::K2);
+ constant_camera_parameters.insert(Camera::Parameter::K3);
+ constant_camera_parameters.insert(Camera::Parameter::K4);
+ constant_camera_parameters.insert(Camera::Parameter::K5);
+ constant_camera_parameters.insert(Camera::Parameter::K6);
+ constant_camera_parameters.insert(Camera::Parameter::P1);
+ constant_camera_parameters.insert(Camera::Parameter::P2);
}
if (!options.optimize_motion && !options.optimize_intrinsic) {
@@ -486,14 +507,14 @@ void BundleAdjustment::_setCameraParametrization(ceres::Problem &problem, const
}
}
else {
- std::unordered_set<int> fix_extrinsic(
+ std::set<int> fix_extrinsic(
options.fix_camera_extrinsic.begin(), options.fix_camera_extrinsic.end());
- std::unordered_set<int> fix_intrinsic(
+ std::set<int> fix_intrinsic(
options.fix_camera_extrinsic.begin(), options.fix_camera_extrinsic.end());
for (size_t i = 0; i < cameras_.size(); i++) {
- std::unordered_set<int> constant_parameters(
+ std::set<int> constant_parameters(
constant_camera_parameters.begin(),
constant_camera_parameters.end());
@@ -591,6 +612,7 @@ void BundleAdjustment::_buildBundleAdjustmentProblem(ceres::Problem &problem, co
void BundleAdjustment::_buildProblem(ceres::Problem &problem, const BundleAdjustment::Options &options) {
_buildBundleAdjustmentProblem(problem, options);
+ _buildLengthProblem(problem, options);
}
void BundleAdjustment::run(const BundleAdjustment::Options &bundle_adjustment_options) {
@@ -627,6 +649,40 @@ void BundleAdjustment::run() {
run(options);
}
+int BundleAdjustment::removeObservations(double threshold) {
+ int removed = 0;
+ std::vector<double> error(cameras_.size(), 0.0);
+
+ for (auto& point : points_) {
+ double error_total = 0.0;
+ double n_points = 0.0;
+
+ for (unsigned int c = 0; c < cameras_.size(); c++) {
+ if (!point.visibility[c]) { continue; }
+ const auto& obs = point.observations[c];
+ const auto& proj = projectPoint(*(cameras_[c]), point.point);
+ double err = pow(proj.x - obs.x, 2) + pow(proj.y - obs.y, 2);
+ error[c] = err;
+ error_total += err;
+ n_points += 1;
+ }
+ error_total /= n_points;
+
+ if (n_points <= 1) { continue; } // TODO: remove observation completely
+
+ for (unsigned int c = 0; c < cameras_.size(); c++) {
+ if (!point.visibility[c]) { continue; }
+ if ((error[c] - error_total) > threshold) {
+ point.visibility[c] = false;
+ n_points -= 1;
+ removed++;
+ break;
+ }
+ }
+ }
+ return removed;
+}
+
void BundleAdjustment::_reprojectionErrorSE(const int camera, double &error, double &npoints) const {
error = 0.0;
npoints = 0.0;
@@ -658,3 +714,53 @@ double BundleAdjustment::reprojectionError() const {
}
return sqrt(error / npoints);
}
+
+////
+
+void BundleAdjustment::addObject(const std::vector<cv::Point3d> &object_points) {
+ if (points_.size() % object_points.size() != 0) { throw std::exception(); }
+ objects_.push_back(BundleAdjustment::Object {0, (int) points_.size(), object_points});
+}
+
+void BundleAdjustment::_buildLengthProblem(ceres::Problem &problem, const BundleAdjustment::Options &options) {
+
+ // same idea as in scale optimization
+
+ ceres::LossFunction *loss_function = nullptr;
+
+ // should use separate configuration option
+ /*
+ if (options.loss == Options::Loss::HUBER) {
+ loss_function = new ceres::HuberLoss(1.0);
+ }
+ else if (options.loss == Options::Loss::CAUCHY) {
+ loss_function = new ceres::CauchyLoss(1.0);
+ }
+ */
+
+ for (auto &object : objects_) {
+ int npoints = object.object_points.size();
+ auto &object_points = object.object_points;
+
+ vector<double> d;
+ for (int i = 0; i < npoints; i++) {
+ for (int j = i + 1; j < npoints; j++) {
+ d.push_back(norm(object_points[i]-object_points[j]));
+ }
+ }
+
+ for (int p = object.idx_start; p < object.idx_end; p += npoints) {
+ size_t i_d = 0;
+ for (size_t i = 0; i < object_points.size(); i++) {
+ for (size_t j = i + 1; j < object_points.size(); j++) {
+ double* p1 = static_cast<double*>(&(points_[p+i].point.x));
+ double* p2 = static_cast<double*>(&(points_[p+j].point.x));
+
+ auto cost_function = LengthError::Create(d[i_d++]);
+
+ problem.AddResidualBlock(cost_function, loss_function, p1, p2);
+ }
+ }
+ }
+ }
+}
diff --git a/components/calibration/src/structures.cpp b/components/calibration/src/structures.cpp
index c464e80b9d59997edc364af0286fcf69d01097e3..36d3ff4fedab5c28d790d2f3e9750aab28783685 100644
--- a/components/calibration/src/structures.cpp
+++ b/components/calibration/src/structures.cpp
@@ -6,6 +6,8 @@
#include <ftl/calibration/structures.hpp>
#include <ftl/calibration/parameters.hpp>
+#include <cmath>
+
using ftl::calibration::CalibrationData;
CalibrationData::Intrinsic::DistortionCoefficients::DistortionCoefficients() :
@@ -84,8 +86,11 @@ cv::Mat CalibrationData::Intrinsic::matrix(cv::Size size) const {
return ftl::calibration::scaleCameraMatrix(matrix(), resolution, size);
}
-////////////////////////////////////////////////////////////////////////////////
+bool CalibrationData::Intrinsic::valid() const {
+ return (resolution == cv::Size{0, 0});
+}
+////////////////////////////////////////////////////////////////////////////////
void CalibrationData::Extrinsic::set(const cv::Mat& T) {
if (T.type() != CV_64FC1) {
@@ -165,6 +170,12 @@ cv::Mat CalibrationData::Extrinsic::rmat() const {
return R;
}
+bool CalibrationData::Extrinsic::valid() const {
+ return !(
+ std::isnan(tvec[0]) || std::isnan(tvec[1]) || std::isnan(tvec[2]) ||
+ std::isnan(rvec[0]) || std::isnan(rvec[1]) || std::isnan(rvec[2]));
+}
+
////////////////////////////////////////////////////////////////////////////////
CalibrationData CalibrationData::readFile(const std::string &path) {
diff --git a/components/codecs/include/ftl/codecs/shapes.hpp b/components/codecs/include/ftl/codecs/shapes.hpp
index 3fffaee3e209000733bba5f94f4cf84f01cce87a..313381887bee18c8b70eec45127c4fd7c662352b 100644
--- a/components/codecs/include/ftl/codecs/shapes.hpp
+++ b/components/codecs/include/ftl/codecs/shapes.hpp
@@ -21,8 +21,6 @@ enum class Shape3DType {
};
struct Shape3D {
- Shape3D() {};
-
int id;
Shape3DType type;
Eigen::Vector3f size;
diff --git a/components/rgbd-sources/src/sources/stereovideo/rectification.cpp b/components/rgbd-sources/src/sources/stereovideo/rectification.cpp
index f755d5de89c62fc5272fc125d43ab7d2452cecfb..e495ef263e18f7e349b51704582fb2511f7e7689 100644
--- a/components/rgbd-sources/src/sources/stereovideo/rectification.cpp
+++ b/components/rgbd-sources/src/sources/stereovideo/rectification.cpp
@@ -91,6 +91,7 @@ void StereoRectification::updateCalibration_() {
}
void StereoRectification::calculateParameters_() {
+ // cv::stereoRectify() throws an exception if parameters are invalid
if (!valid_) { return; }
cv::Mat K_l = calib_left_.intrinsic.matrix(image_resolution_);
@@ -98,7 +99,6 @@ void StereoRectification::calculateParameters_() {
cv::Mat dc_l = calib_left_.intrinsic.distCoeffs.Mat();
cv::Mat dc_r = calib_right_.intrinsic.distCoeffs.Mat();
- // calculate rectification parameters
cv::stereoRectify( K_l, dc_l, K_r, dc_r, image_resolution_,
R_, t_, R_l_, R_r_, P_l_, P_r_, Q_, 0, 0);
@@ -198,6 +198,8 @@ cv::Mat StereoRectification::cameraMatrix(Channel c) {
else if (c == Channel::Right) {
// Extrinsics are included in P_r_, can't do same as above
throw ftl::exception("Not implemented");
+ // not tested!
+ return cv::Mat(P_r_(cv::Rect(0, 0, 3, 3)) * R_r_.t());
}
}
else {
diff --git a/components/rgbd-sources/src/sources/stereovideo/rectification.hpp b/components/rgbd-sources/src/sources/stereovideo/rectification.hpp
index d326d0d167b6cae7f06bdccd1bad6be1b6bfca40..756747e04d29bc95d66496be111fcd660e3c7709 100644
--- a/components/rgbd-sources/src/sources/stereovideo/rectification.hpp
+++ b/components/rgbd-sources/src/sources/stereovideo/rectification.hpp
@@ -35,12 +35,14 @@ public:
*/
void setInterpolation(int interpolation);
- /**
- * Calculate rectification parameters from given calibration.
+ /** Calculate rectification parameters from given calibration.
*/
void setCalibration(ftl::calibration::CalibrationData &calib);
bool calibrated();
+ /** Rectify image. Valid channels Left and Right. No-op if disabled with
+ * setEnabled() or calibration parameters have not been set.
+ */
void rectify(cv::InputArray im, cv::OutputArray out, ftl::codecs::Channel c);
/** Enable/disable rectification. TODO: move outside (to stereovideo)?
@@ -53,7 +55,7 @@ public:
*/
cv::Mat getPose(ftl::codecs::Channel c = ftl::codecs::Channel::Left);
- /** Get intrinsic matrix. */
+ /** Get intrinsic matrix. Not implemented for right channel. */
cv::Mat cameraMatrix(ftl::codecs::Channel c = ftl::codecs::Channel::Left);
cv::Mat cameraMatrix(cv::Size size, ftl::codecs::Channel c = ftl::codecs::Channel::Left);
@@ -74,17 +76,17 @@ private:
cv::Size image_resolution_;
// rectification parameters
- bool enabled_;
- bool valid_;
+ bool enabled_; // rectification enabled
+ bool valid_; // instance contains valid parameters
int interpolation_;
double baseline_;
cv::Mat R_; // rotation left to right
cv::Mat t_; // translation left to right
- cv::Mat Q_;
- cv::Mat R_l_;
- cv::Mat R_r_;
- cv::Mat P_l_;
- cv::Mat P_r_;
+ cv::Mat Q_; // disparity to depth matrix
+ cv::Mat R_l_; // rotation for left camera: unrectified to rectified
+ cv::Mat R_r_; // rotation for right camera: unrectified to rectified
+ cv::Mat P_l_; // rectified projection matrix for left camera
+ cv::Mat P_r_; // rectified projection matrix for right camera
// rectification maps for cv::remap(); should be CV_16SC2 if remap done on
// CPU and CV_32SC2 for GPU (generated by calculateParameters(), used by
diff --git a/components/rgbd-sources/src/sources/stereovideo/stereovideo.cpp b/components/rgbd-sources/src/sources/stereovideo/stereovideo.cpp
index b7f3d6ce3123335fcbe77d7cf720beb27f9db6dc..ecd902da1e67d28f92523352fafb80d25a2daa26 100644
--- a/components/rgbd-sources/src/sources/stereovideo/stereovideo.cpp
+++ b/components/rgbd-sources/src/sources/stereovideo/stereovideo.cpp
@@ -23,6 +23,7 @@
#include <ftl/operators/mask.hpp>
#include <ftl/rgbd/capabilities.hpp>
+#include <ftl/codecs/shapes.hpp>
#include <ftl/calibration/structures.hpp>
#include "stereovideo.hpp"
@@ -239,9 +240,9 @@ void StereoVideoSource::updateParameters(ftl::rgbd::Frame &frame) {
frame.setPose() = pose;
- cv::Mat K;
+ cv::Mat K = rectification_->cameraMatrix(color_size_);
+ float fx = static_cast<float>(K.at<double>(0,0));
- // same for left and right
float baseline = static_cast<float>(rectification_->baseline());
float doff = rectification_->doff(color_size_);
@@ -249,14 +250,8 @@ void StereoVideoSource::updateParameters(ftl::rgbd::Frame &frame) {
float min_depth = this->host_->getConfig().value<double>("min_depth", 0.45);
float max_depth = this->host_->getConfig().value<double>("max_depth", (lsrc_->isStereo()) ? 12.0 : 1.0);
- // left
-
- K = rectification_->cameraMatrix(color_size_);
- float fx = static_cast<float>(K.at<double>(0,0));
-
if (d_resolution > 0.0) {
- // Learning OpenCV p. 442
- // TODO: remove, should not be used here
+ // Learning OpenCV p. 442; TODO: remove. should not be applied here
float max_depth_new = sqrt(d_resolution * fx * baseline);
max_depth = (max_depth_new > max_depth) ? max_depth : max_depth_new;
}
@@ -275,27 +270,20 @@ void StereoVideoSource::updateParameters(ftl::rgbd::Frame &frame) {
doff
};
+ CHECK(params.fx > 0) << "focal length must be positive";
+ CHECK(params.fy > 0) << "focal length must be positive";
+ CHECK(params.cx < 0) << "bad principal point coordinate (negative value)";
+ CHECK(-params.cx < params.width) << "principal point must be inside image";
+ CHECK(params.cy < 0) << "bad principal point coordinate (negative value)";
+ CHECK(-params.cy < params.height) << "principal point must be inside image";
+ CHECK(params.baseline >= 0.0) << "baseline must be positive";
+
host_->getConfig()["focal"] = params.fx;
host_->getConfig()["centre_x"] = params.cx;
host_->getConfig()["centre_y"] = params.cy;
host_->getConfig()["baseline"] = params.baseline;
host_->getConfig()["doffs"] = params.doffs;
- // right
- /* not used
- K = rectification_->cameraMatrix(color_size_, Channel::Right);
- frame.setRight() = {
- static_cast<float>(K.at<double>(0,0)), // Fx
- static_cast<float>(K.at<double>(1,1)), // Fy
- static_cast<float>(-K.at<double>(0,2)), // Cx
- static_cast<float>(-K.at<double>(1,2)), // Cy
- (unsigned int) color_size_.width,
- (unsigned int) color_size_.height,
- min_depth,
- max_depth,
- baseline,
- doff
- };*/
} else {
Eigen::Matrix4d pose;
auto& params = frame.setLeft();
@@ -337,6 +325,20 @@ bool StereoVideoSource::retrieve(ftl::rgbd::Frame &frame) {
if (!cap_status_) return false;
+ if (host_->value("add_right_pose", false)) {
+ auto shapes = frame.create<std::list<ftl::codecs::Shape3D>>(Channel::Shapes3D);
+ Eigen::Matrix4d pose;
+ cv::cv2eigen(rectification_->getPose(Channel::Right), pose);
+ Eigen::Matrix4f posef = pose.cast<float>();
+ shapes.list.push_back(ftl::codecs::Shape3D{
+ 1,
+ ftl::codecs::Shape3DType::CAMERA,
+ Eigen::Vector3f{0.2, 0.2, 0.2},
+ posef,
+ std::string("Right Camera")
+ });
+ }
+
if (do_update_params_) {
updateParameters(frame);
do_update_params_ = false;