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 :