diff --git a/applications/gui2/src/modules/calibration.cpp b/applications/gui2/src/modules/calibration.cpp
index a28963c235f0409bb5c34d1b5c9337610c809361..c74dc889047d36ddf0dce84e20ff2d4a4de63eea 100644
--- a/applications/gui2/src/modules/calibration.cpp
+++ b/applications/gui2/src/modules/calibration.cpp
@@ -163,6 +163,7 @@ void IntrinsicCalibration::reset() {
capture_ = false;
running_ = false;
channel_ = Channel::Left;
+ channel_alt_ = Channel::Left;
calib_ = ftl::calibration::IntrinsicCalibration();
setDefaultFlags();
}
@@ -178,7 +179,7 @@ void IntrinsicCalibration::start(ftl::data::FrameID id) {
{Channel::Left, Channel::Right});
- filter->on([this](const FrameSetPtr& fs){ return onFrame(fs); });
+ filter->on([this](const FrameSetPtr& fs){ return onFrame_(fs); });
view->onClose([filter, this](){
// if calib_ caches images, also reset() here!
@@ -192,9 +193,34 @@ void IntrinsicCalibration::start(ftl::data::FrameID id) {
});
screen->setView(view);
+ for (auto fs : filter->getLatestFrameSets()) {
+ if ((fs->frameset() == id_.frameset()) && (fs->hasFrame(id_.source()))) {
+ setChannel_(fs);
+ }
+ }
+}
+
+void IntrinsicCalibration::setChannel(Channel channel) {
+ channel_ = channel;
+ auto fs = std::atomic_load(¤t_fs_);
+ setChannel_(fs);
+}
+
+void IntrinsicCalibration::setChannel_(FrameSetPtr fs) {
+ channel_alt_ = channel_;
+ if (fs == nullptr) { return; }
+ auto& frame = (*fs)[id_.source()];
+ if ((channel_ == Channel::Left) && frame.has(Channel::LeftHighRes)) {
+ channel_alt_ = Channel::LeftHighRes;
+ return;
+ }
+ if ((channel_ == Channel::Right) && frame.has(Channel::RightHighRes)) {
+ channel_alt_ = Channel::LeftHighRes;
+ return;
+ }
}
-bool IntrinsicCalibration::onFrame(const ftl::data::FrameSetPtr& fs) {
+bool IntrinsicCalibration::onFrame_(const ftl::data::FrameSetPtr& fs) {
std::atomic_store(&updated_fs_, fs);
screen->redraw();
@@ -281,7 +307,7 @@ void IntrinsicCalibration::run() {
bool IntrinsicCalibration::hasFrame() {
return (std::atomic_load(&updated_fs_).get() != nullptr)
- && updated_fs_->frames[id_.source()].hasChannel(channel_);
+ && updated_fs_->frames[id_.source()].hasChannel(channel_alt_);
};
cv::cuda::GpuMat IntrinsicCalibration::getFrame() {
@@ -294,7 +320,7 @@ cv::cuda::GpuMat IntrinsicCalibration::getFrame() {
return cv::cuda::GpuMat();
}
- return (*current_fs_)[id_.source()].cast<ftl::rgbd::Frame>().get<cv::cuda::GpuMat>(channel_);
+ return (*current_fs_)[id_.source()].cast<ftl::rgbd::Frame>().get<cv::cuda::GpuMat>(channel_alt_);
}
float IntrinsicCalibration::sensorWidth() {
@@ -315,7 +341,9 @@ bool IntrinsicCalibration::hasChannel(Channel c) {
std::vector<std::pair<std::string, FrameID>> IntrinsicCalibration::listSources(bool all) {
std::vector<std::pair<std::string, FrameID>> cameras;
for (auto id : io->feed()->listFrames()) {
- if (all || io->feed()->availableChannels(id).count(Channel::CalibrationData)) {
+ auto channels = io->feed()->availableChannels(id);
+ // TODO: doesn't work
+ if (all || (channels.count(Channel::CalibrationData) == 1)) {
auto name = io->feed()->getURI(id.frameset()) + "#" + std::to_string(id.source());
cameras.push_back({name, id});
}
@@ -323,7 +351,6 @@ std::vector<std::pair<std::string, FrameID>> IntrinsicCalibration::listSources(b
return cameras;
}
-
std::vector<int> IntrinsicCalibration::listFlags() {
return {
cv::CALIB_USE_INTRINSIC_GUESS,
@@ -469,22 +496,34 @@ public:
const size_t n_corners = 4;
const size_t n_tags = 2;
- cv::Point2f* marker1 = nullptr;
- cv::Point2f* marker2 = nullptr;
+ std::vector<cv::Point2f> marker1; marker1.reserve(n_corners);
+ std::vector<cv::Point2f> marker2; marker2.reserve(n_corners);
+ int n = 0;
// find the right markers
for (unsigned int i = 0; i < ids.size(); i++) {
if (ids[i] == id1_) {
- marker1 = corners[i].data();
+ n++;
+ for (auto& p : corners[i]) {
+ marker1.push_back(cv::Point2f(p.x, p.y));
+ }
CHECK(corners[i].size() == n_corners);
}
if (ids[i] == id2_) {
- marker2 = corners[i].data();
+ n++;
+ for (auto& p : corners[i]) {
+ marker2.push_back(cv::Point2f(p.x, p.y));
+ }
CHECK(corners[i].size() == n_corners);
}
}
- if ((marker1 == nullptr) || (marker2 == nullptr)) {
+ if (marker1.empty() || marker2.empty()) {
+ return 0;
+ }
+
+ if (n != 2) {
+ LOG(WARNING) << "Found more than one marker with same ID";
return 0;
}
@@ -513,13 +552,14 @@ void ExtrinsicCalibration::reset() {
fs_current_.reset();
fs_update_.reset();
- calib_object_ = std::unique_ptr<CalibrationObject>(new CalibrationObject(cv::aruco::DICT_4X4_100));
+ calib_object_ = std::unique_ptr<CalibrationObject>(new CalibrationObject());
cb_detect_ = std::bind(&CalibrationObject::detect, calib_object_.get(),
std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4);
calib_ = ftl::calibration::ExtrinsicCalibration();
points_ = ftl::calibration::CalibrationPoints<float>();
points_.setObject(calib_object_->object());
+ min_cameras_ = 2;
}
ExtrinsicCalibration::~ExtrinsicCalibration() {
@@ -551,6 +591,16 @@ void ExtrinsicCalibration::start(unsigned int fsid, std::vector<FrameID> sources
capture_ = true;
+ auto fss = filter->getLatestFrameSets();
+ if (fss.size() == 1) {
+ fs_current_ = fss.front();
+ for (auto& [id, calib] : cameras_) {
+ calib = getCalibration(id);
+ }
+ }
+ else {
+ LOG(WARNING) << "Couldn't get most recent FrameSet";
+ }
screen->setView(view);
}
@@ -594,7 +644,7 @@ bool ExtrinsicCalibration::onFrameSet(const FrameSetPtr& fs) {
LOG(ERROR) << ex.what();
}
}
- points_.next();
+ points_.next(min_cameras_);
running_ = false;
});
@@ -664,3 +714,56 @@ std::vector<ExtrinsicCalibration::CameraID> ExtrinsicCalibration::cameras() {
}
return res;
}
+
+bool ExtrinsicCalibration::isBusy() {
+ return running_;
+}
+
+template<typename T>
+std::vector<T> flatten(const std::vector<typename std::vector<T>> &in) {
+ std::vector<T> flat;
+ for (const auto& i : in) {
+ flat.insert(flat.end(), i.begin(), i.end());
+ }
+ return flat;
+}
+
+void ExtrinsicCalibration::run() {
+ auto path = points_.visibility().shortestPath(0);
+ auto points = points_.getPoints({0, 1});
+
+ cv::Mat R, t, E, F;
+ auto K1 = cameras_[0].calib.intrinsic.matrix();
+ auto K2 = cameras_[1].calib.intrinsic.matrix();
+ auto dist1 = cameras_[0].calib.intrinsic.distCoeffs.Mat();
+ auto dist2 = cameras_[1].calib.intrinsic.distCoeffs.Mat();
+ auto size = cameras_[0].calib.intrinsic.resolution;
+ std::vector<cv::Point3f> points3d;
+ try {
+ cv::stereoCalibrate(points.object, points.image[0], points.image[1],
+ K1, dist1, K2, dist2, size, R, t, E, F, cv::CALIB_FIX_INTRINSIC,
+ cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, 30, 1.0e-6));
+
+ LOG(INFO) << R;
+ LOG(INFO) << t;
+ //cv::stereoCalibrate(points.object, points.image[0], points.image[1],
+ // K1, dist1, K2, dist2, size, R, t, E, F, cv::CALIB_FIX_INTRINSIC);
+ }
+ catch (std::exception &ex) {
+ LOG(ERROR) << ex.what();
+ }
+ return;
+}
+
+ftl::calibration::CalibrationData::Calibration ExtrinsicCalibration::getCalibration(CameraID id) {
+ if (fs_current_ == nullptr) {
+ throw ftl::exception("No frame");
+ }
+
+ auto calib = (*fs_current_)[id.source()].get<CalibrationData>(Channel::CalibrationData);
+ if (!calib.hasCalibration(id.channel)) {
+ throw ftl::exception("Calibration missing for requierd channel");
+ }
+
+ return calib.get(id.channel);
+}
diff --git a/applications/gui2/src/modules/calibration.hpp b/applications/gui2/src/modules/calibration.hpp
index 5cfa4a225c3d7da41c1e1eac97edaddadd6b0b58..75541c5cf9e5897f1f22bc5792b8332688d7fcef 100644
--- a/applications/gui2/src/modules/calibration.hpp
+++ b/applications/gui2/src/modules/calibration.hpp
@@ -97,7 +97,7 @@ public:
bool hasChannel(ftl::codecs::Channel c);
/** select channel */
- void setChannel(ftl::codecs::Channel c) { channel_ = c; }
+ void setChannel(ftl::codecs::Channel c);
ftl::codecs::Channel channel() { return channel_; }
/** list all calibration flags */
@@ -148,12 +148,15 @@ public:
std::vector<std::pair<std::string, ftl::data::FrameID>> listSources(bool all=false);
private:
- bool onFrame(const ftl::data::FrameSetPtr& fs);
+ bool onFrame_(const ftl::data::FrameSetPtr& fs);
+ /** Set actual channel (channel_alt_) to high res if found in fs */
+ void setChannel_(ftl::data::FrameSetPtr fs);
ftl::data::FrameSetPtr current_fs_;
ftl::data::FrameSetPtr updated_fs_;
ftl::codecs::Channel channel_;
+ ftl::codecs::Channel channel_alt_;
ftl::data::FrameID id_;
std::atomic_bool capture_;
@@ -221,6 +224,17 @@ public:
/** list selected (active) cameras */
std::vector<CameraID> cameras();
+ /** Run calibration in another thread. Check status with isBusy(). */
+ void run();
+
+ /** Returns if capture/calibration is still processing in background.
+ * calib() instance must not be modifed while isBusy() is true.
+ */
+ bool isBusy();
+
+protected:
+ ftl::calibration::CalibrationData::Calibration getCalibration(CameraID id);
+
private:
bool onFrameSet(const ftl::data::FrameSetPtr& fs);
@@ -242,6 +256,7 @@ private:
std::function<int(cv::InputArray, const cv::Mat&, const cv::Mat&, std::vector<cv::Point2f>&)> cb_detect_;
std::future<void> future_;
std::atomic_bool running_;
+ int min_cameras_;
};
diff --git a/applications/gui2/src/views/calibration/extrinsic.cpp b/applications/gui2/src/views/calibration/extrinsic.cpp
index a215716c3f84b265f9e9da5e5f308255f122d54c..65472e44ba51fc991fb76f1aebdaae3104d6637e 100644
--- a/applications/gui2/src/views/calibration/extrinsic.cpp
+++ b/applications/gui2/src/views/calibration/extrinsic.cpp
@@ -268,6 +268,11 @@ void ExtrinsicCalibrationView::CalibrationWindow::build() {
auto* button = new nanogui::Button(wcameras, std::to_string(i));
button->setFlags(nanogui::Button::Flags::RadioButton);
}
+
+ auto* brun = new nanogui::Button(this, "Run");
+ brun->setCallback([this](){
+ ctrl_->run();
+ });
}
diff --git a/components/calibration/include/ftl/calibration/extrinsic.hpp b/components/calibration/include/ftl/calibration/extrinsic.hpp
index ff48c9873ed7b5fa117488a894f4e226eb2e1f01..ca77afd8f3a14fbcc535678b33deed536d11d586 100644
--- a/components/calibration/include/ftl/calibration/extrinsic.hpp
+++ b/components/calibration/include/ftl/calibration/extrinsic.hpp
@@ -45,8 +45,9 @@ public:
void addPoints(unsigned int c, const std::vector<cv::Point_<T>>& points);
/** Continue next set of images. Target must be set. If no points were added
- * next() is no-op. */
- void next();
+ * next() is no-op. If less than min points are set, buffer is reset and
+ * points are discarded. */
+ void next(int min=2);
/** Get count (how many sets) for camera(s). */
int getCount(unsigned int c);
@@ -85,6 +86,28 @@ private:
std::vector<std::vector<cv::Point3_<T>>> objects_;
};
+/**
+ * Same as OpenCV's recoverPose(), but does not assume same intrinsic paramters
+ * for both cameras.
+ *
+ * @todo Write unit tests to check that intrinsic parameters work as expected.
+ */
+int recoverPose(const cv::Mat &E, const std::vector<cv::Point2d> &_points1,
+ const std::vector<cv::Point2d> &_points2, const cv::Mat &_cameraMatrix1,
+ const cv::Mat &_cameraMatrix2, cv::Mat &_R, cv::Mat &_t,
+ double distanceThresh, cv::Mat &triangulatedPoints);
+
+/**
+ * Find camera rotation and translation from first to second camera. Uses
+ * OpenCV's recoverPose() (with modifications) to estimate camera pose and
+ * triangulate point locations. Scale is estimated from object_points. 8 point
+ * algorithm (OpenCV) is used to estimate fundamental matrix at beginning.
+ */
+double computeExtrinsicParameters(const cv::Mat &K1, const cv::Mat &D1,
+ const cv::Mat &K2, const cv::Mat &D2, const std::vector<cv::Point2d> &points1,
+ const std::vector<cv::Point2d> &points2, const std::vector<cv::Point3d> &object_points,
+ cv::Mat &R, cv::Mat &t, std::vector<cv::Point3d> &points_out);
+
class ExtrinsicCalibration {
public:
diff --git a/components/calibration/include/ftl/calibration/parameters.hpp b/components/calibration/include/ftl/calibration/parameters.hpp
index f0fd0cf291f56f30dbe4c2c332cb8cac742adefc..0bbb6d5637db8cd9fb891fbb7d2168b9bd28863d 100644
--- a/components/calibration/include/ftl/calibration/parameters.hpp
+++ b/components/calibration/include/ftl/calibration/parameters.hpp
@@ -8,7 +8,7 @@ namespace ftl {
namespace calibration {
/**
- * Camera paramters
+ * Camera paramters (Ceres)
*/
struct Camera {
Camera() {}
diff --git a/components/calibration/src/extrinsic.cpp b/components/calibration/src/extrinsic.cpp
index 99aa5364034e982628d6037053c7635b3d7f4780..80d0f097588e1e32fe32fe06ba9bc2235c9a9b9a 100644
--- a/components/calibration/src/extrinsic.cpp
+++ b/components/calibration/src/extrinsic.cpp
@@ -1,7 +1,12 @@
+#include <loguru.hpp>
+
#include <ftl/exception.hpp>
+#include <ftl/calibration/optimize.hpp>
#include <ftl/calibration/extrinsic.hpp>
+#include <opencv2/calib3d.hpp>
+
// ==== CalibrationPoints ================================================
/** check bit i in a */
@@ -29,6 +34,16 @@ inline int hbit(uint64_t a) {
return v;
}
+inline int popcount(uint64_t bits) {
+ #if defined(_MSC_VER)
+ return __popcnt64(bits);
+ #elif defined(__GNUC__)
+ return __builtin_popcountl(bits);
+ #else
+ static_assert(false, "unsupported compiler (popcount intrinsic)");
+ #endif
+}
+
namespace ftl {
namespace calibration {
@@ -43,7 +58,7 @@ void CalibrationPoints<T>::addPoints(unsigned int c, const std::vector<cv::Point
}
if (objects_[current_.object].size() != points.size()) {
- throw ftl::exception("Number of points must match object points");
+ throw ftl::exception("Number of points must cv::Match object points");
}
std::vector<cv::Point_<T>> p(points.begin(), points.end());
@@ -80,17 +95,18 @@ void CalibrationPoints<T>::setObject(const std::vector<cv::Point_<T>> &object) {
}
template<typename T>
-void CalibrationPoints<T>::next() {
+void CalibrationPoints<T>::next(int min) {
if (objects_.empty()) {
throw ftl::exception("object must be set before calling next()");
}
if (current_.cameras == uint64_t(0)) {
return;
}
-
- count_ += objects_[current_.object].size();
- points_.push_back(current_);
- visibility_.update(current_.cameras);
+ if (popcount(current_.cameras) >= min) {
+ count_ += objects_[current_.object].size();
+ points_.push_back(current_);
+ visibility_.update(current_.cameras);
+ }
current_ = {uint64_t(0), {}, {}, (unsigned int)(objects_.size()) - 1u};
}
@@ -167,6 +183,122 @@ CalibrationPoints<T>::getPoints(const std::vector<unsigned int>& cameras) {
template class CalibrationPoints<float>;
template class CalibrationPoints<double>;
+////////////////////////////////////////////////////////////////////////////////
+
+int recoverPose(const cv::Mat &E, const std::vector<cv::Point2d> &_points1,
+ const std::vector<cv::Point2d> &_points2, const cv::Mat &_cameraMatrix1,
+ const cv::Mat &_cameraMatrix2, cv::Mat &_R, cv::Mat &_t, double distanceThresh,
+ cv::Mat &triangulatedPoints) {
+
+ cv::Mat cameraMatrix1;
+ cv::Mat cameraMatrix2;
+ cv::Mat cameraMatrix;
+
+ cv::Mat points1(_points1.size(), 2, CV_64FC1);
+ cv::Mat points2(_points2.size(), 2, CV_64FC1);
+
+ CHECK(points1.size() == points2.size());
+
+ for (size_t i = 0; i < _points1.size(); i++) {
+ auto p1 = points1.ptr<double>(i);
+ p1[0] = _points1[i].x;
+ p1[1] = _points1[i].y;
+
+ auto p2 = points2.ptr<double>(i);
+ p2[0] = _points2[i].x;
+ p2[1] = _points2[i].y;
+ }
+
+ _cameraMatrix1.convertTo(cameraMatrix1, CV_64F);
+ _cameraMatrix2.convertTo(cameraMatrix2, CV_64F);
+ cameraMatrix = cv::Mat::eye(cv::Size(3, 3), CV_64FC1);
+
+ double fx1 = cameraMatrix1.at<double>(0,0);
+ double fy1 = cameraMatrix1.at<double>(1,1);
+ double cx1 = cameraMatrix1.at<double>(0,2);
+ double cy1 = cameraMatrix1.at<double>(1,2);
+
+ double fx2 = cameraMatrix2.at<double>(0,0);
+ double fy2 = cameraMatrix2.at<double>(1,1);
+ double cx2 = cameraMatrix2.at<double>(0,2);
+ double cy2 = cameraMatrix2.at<double>(1,2);
+
+ points1.col(0) = (points1.col(0) - cx1) / fx1;
+ points1.col(1) = (points1.col(1) - cy1) / fy1;
+
+ points2.col(0) = (points2.col(0) - cx2) / fx2;
+ points2.col(1) = (points2.col(1) - cy2) / fy2;
+
+ // TODO mask
+ // cameraMatrix = I (for details, see OpenCV's recoverPose() source code)
+ // modules/calib3d/src/five-point.cpp (461)
+ //
+ // https://github.com/opencv/opencv/blob/371bba8f54560b374fbcd47e7e02f015ac4969ad/modules/calib3d/src/five-point.cpp#L461
+
+ return cv::recoverPose(E, points1, points2, cameraMatrix, _R, _t, distanceThresh, cv::noArray(), triangulatedPoints);
+}
+
+double computeExtrinsicParameters(const cv::Mat &K1, const cv::Mat &D1,
+ const cv::Mat &K2, const cv::Mat &D2, const std::vector<cv::Point2d> &points1,
+ const std::vector<cv::Point2d> &points2, const std::vector<cv::Point3d> &object_points, cv::Mat &R,
+ cv::Mat &t, std::vector<cv::Point3d> &points_out) {
+
+ cv::Mat F = cv::findFundamentalMat(points1, points2, cv::noArray(), cv::FM_8POINT);
+ cv::Mat E = K2.t() * F * K1;
+
+ cv::Mat points3dh;
+ // distanceThresh unit?
+ recoverPose(E, points1, points2, K1, K2, R, t, 1000.0, points3dh);
+
+ points_out.clear();
+ points_out.reserve(points3dh.cols);
+
+ for (int col = 0; col < points3dh.cols; col++) {
+ CHECK(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))
+ / points3dh.at<double>(3, col);
+ points_out.push_back(p);
+ }
+
+ double s = ftl::calibration::optimizeScale(object_points, points_out);
+ t = t * s;
+
+ auto params1 = Camera(K1, D1, cv::Mat::eye(3, 3, CV_64FC1), cv::Mat::zeros(3, 1, CV_64FC1));
+ auto params2 = Camera(K2, D2, R, t);
+
+ auto ba = BundleAdjustment();
+ ba.addCamera(params1);
+ ba.addCamera(params2);
+
+ for (size_t i = 0; i < points_out.size(); i++) {
+ ba.addPoint({points1[i], points2[i]}, points_out[i]);
+ }
+
+ ba.addObject(object_points);
+
+ double error_before = ba.reprojectionError();
+
+ BundleAdjustment::Options options;
+ options.optimize_intrinsic = false;
+ options.fix_camera_extrinsic = {0};
+ ba.run(options);
+
+ double error_after = ba.reprojectionError();
+
+ // bundle adjustment didn't work correctly if these checks fail
+ if (error_before < error_after) {
+ LOG(WARNING) << "error before < error_after (" << error_before << " <" << error_after << ")";
+ }
+ CHECK((cv::countNonZero(params1.rvec()) == 0) && (cv::countNonZero(params1.tvec()) == 0));
+
+ R = params2.rmat();
+ t = params2.tvec();
+
+ return sqrt(error_after);
+}
+
}
}
diff --git a/components/renderers/cpp/src/colouriser.cpp b/components/renderers/cpp/src/colouriser.cpp
index 7ca8870e948a7f62c8a397624b298b2a536c2df8..5f4f25d5ddf46d08c74ac602f8cae74946afdf6c 100644
--- a/components/renderers/cpp/src/colouriser.cpp
+++ b/components/renderers/cpp/src/colouriser.cpp
@@ -121,6 +121,7 @@ TextureObject<uchar4> &Colouriser::colourise(ftl::rgbd::Frame &f, Channel c, cud
switch (c) {
case Channel::Overlay : return f.createTexture<uchar4>(c);
case Channel::ColourHighRes :
+ case Channel::RightHighRes :
case Channel::Colour :
case Channel::Colour2 : return _processColour(f,c,stream);
case Channel::GroundTruth :