diff --git a/applications/gui2/src/modules/calibration/calibration.cpp b/applications/gui2/src/modules/calibration/calibration.cpp index 4a7e0d6844ba3f230e35ab53f379c6df8a7f979c..96ca5a121e42f5c6d68e6f7c261229ee5dd09938 100644 --- a/applications/gui2/src/modules/calibration/calibration.cpp +++ b/applications/gui2/src/modules/calibration/calibration.cpp @@ -25,6 +25,98 @@ using ftl::codecs::Channel; using ftl::data::FrameID; using ftl::data::FrameSetPtr; + +// ==== OpenCVCalibrateFlags =================================================== + +using ftl::gui2::OpenCVCalibrateFlags; +using ftl::gui2::OpenCVCalibrateFlagsStereo; + +int OpenCVCalibrateFlags::defaultFlags() const { return ( + cv::CALIB_FIX_K4 | cv::CALIB_FIX_K5 | cv::CALIB_FIX_K6 | + cv::CALIB_ZERO_TANGENT_DIST | cv::CALIB_FIX_S1_S2_S3_S4 | + cv::CALIB_FIX_ASPECT_RATIO +);} + +std::vector<int> OpenCVCalibrateFlags::list() const { + return { + cv::CALIB_USE_INTRINSIC_GUESS, + cv::CALIB_FIX_PRINCIPAL_POINT, + cv::CALIB_FIX_ASPECT_RATIO, + cv::CALIB_ZERO_TANGENT_DIST, + cv::CALIB_FIX_K1, + cv::CALIB_FIX_K2, + cv::CALIB_FIX_K3, + cv::CALIB_FIX_K4, + cv::CALIB_FIX_K5, + cv::CALIB_FIX_K6, + cv::CALIB_RATIONAL_MODEL, + cv::CALIB_THIN_PRISM_MODEL, + cv::CALIB_FIX_S1_S2_S3_S4, + cv::CALIB_TILTED_MODEL, + cv::CALIB_FIX_TAUX_TAUY + }; +} + +std::string OpenCVCalibrateFlags::name(int i) const { + using namespace cv; + switch(i) { + case CALIB_USE_INTRINSIC_GUESS: + return "CALIB_USE_INTRINSIC_GUESS"; + + case CALIB_USE_EXTRINSIC_GUESS: + return "CALIB_USE_EXTRINSIC_GUESS"; + + case CALIB_FIX_PRINCIPAL_POINT: + return "CALIB_FIX_PRINCIPAL_POINT"; + + case CALIB_FIX_ASPECT_RATIO: + return "CALIB_FIX_ASPECT_RATIO"; + + case CALIB_ZERO_TANGENT_DIST: + return "CALIB_ZERO_TANGENT_DIST"; + + case CALIB_FIX_K1: + return "CALIB_FIX_K1"; + + case CALIB_FIX_K2: + return "CALIB_FIX_K2"; + + case CALIB_FIX_K3: + return "CALIB_FIX_K3"; + + case CALIB_FIX_K4: + return "CALIB_FIX_K4"; + + case CALIB_FIX_K5: + return "CALIB_FIX_K5"; + + case CALIB_FIX_K6: + return "CALIB_FIX_K6"; + + case CALIB_RATIONAL_MODEL: + return "CALIB_RATIONAL_MODEL"; + + case CALIB_THIN_PRISM_MODEL: + return "CALIB_THIN_PRISM_MODEL"; + + case CALIB_FIX_S1_S2_S3_S4: + return "CALIB_FIX_S1_S2_S3_S4"; + + case CALIB_TILTED_MODEL: + return "CALIB_TILTED_MODEL"; + + case CALIB_FIX_TAUX_TAUY: + return "CALIB_FIX_TAUX_TAUY"; + }; + return ""; +} + +std::vector<int> OpenCVCalibrateFlagsStereo::list() const { + auto ls = OpenCVCalibrateFlags::list(); + ls.insert(ls.begin(), cv::CALIB_USE_EXTRINSIC_GUESS); + return ls; +} + // ==== Calibration module ===================================================== // Loads sub-modules and adds buttons to main screen. @@ -161,3 +253,19 @@ void CalibrationModule::setCalibrationMode(ftl::data::Frame& frame, bool value) void CalibrationModule::setCalibrationMode(bool value) { calibration_enabled_ = value; } + +cv::Mat CalibrationModule::getMat(ftl::rgbd::Frame& frame, Channel c) { + auto& vframe = frame.get<ftl::rgbd::VideoFrame>(c); + cv::Mat host; + if (!vframe.isGPU()) { vframe.getGPU().download(host); } + else { host = vframe.getCPU(); } + return host; +} + +cv::cuda::GpuMat CalibrationModule::getGpuMat(ftl::rgbd::Frame& frame, Channel c) { + auto& vframe = frame.get<ftl::rgbd::VideoFrame>(c); + cv::cuda::GpuMat gpu; + if (!vframe.isGPU()) { gpu.upload(vframe.getCPU()); } + else { gpu = vframe.getGPU(); } + return gpu; +} diff --git a/applications/gui2/src/modules/calibration/calibration.hpp b/applications/gui2/src/modules/calibration/calibration.hpp index bf09cae94e8553a9d8a2e82d48f6867f7ca9e4b8..4bc2842fbf3b692334c52d9e187847ac99dbb6da 100644 --- a/applications/gui2/src/modules/calibration/calibration.hpp +++ b/applications/gui2/src/modules/calibration/calibration.hpp @@ -2,6 +2,7 @@ #include "../../module.hpp" +#include <ftl/calibration/object.hpp> #include <ftl/calibration/intrinsic.hpp> #include <ftl/calibration/extrinsic.hpp> #include <ftl/calibration/structures.hpp> @@ -12,6 +13,28 @@ namespace ftl namespace gui2 { +class OpenCVCalibrateFlags { +public: + bool has(unsigned int flag) const { return (flags_ & flag) != 0; } + void set(unsigned int flag) { flags_ |= flag; } + void unset(unsigned int flag) { flags_ &= ~flag; } + void reset() { flags_ = 0; } + int defaultFlags() const; + std::vector<int> list() const; + std::string name(int) const; + std::string explain(int) const; + operator int() { return flags_; } + +private: + int flags_ = 0; +}; + +class OpenCVCalibrateFlagsStereo : public OpenCVCalibrateFlags { +public: + std::vector<int> list() const; + std::string explain(int) const; +}; + /** * Calibration. Loads Intrinsic and Extrinsic calibration modules and * adds buttons to main screen. @@ -52,6 +75,9 @@ protected: */ bool checkFrame(ftl::data::Frame& frame); + cv::cuda::GpuMat getGpuMat(ftl::rgbd::Frame&, ftl::codecs::Channel); + cv::Mat getMat(ftl::rgbd::Frame&, ftl::codecs::Channel); + private: bool calibrationEnabled(ftl::data::Frame& frame); @@ -95,18 +121,21 @@ public: bool hasChannel(ftl::codecs::Channel c); /** select channel */ void setChannel(ftl::codecs::Channel c); - ftl::codecs::Channel channel() { return channel_; } + ftl::codecs::Channel channel() { return state_->channel; } - /** list all calibration flags */ - std::vector<int> listFlags(); - /** get string name for a flag */ - std::string flagName(int); + int count() { return state_->count; } + int calibrated() { return state_->calibrated; } + + OpenCVCalibrateFlagsStereo& flags() { return state_->flags; }; + void resetFlags(); - /** Apply flags, uses class defaults and config options. */ - void setDefaultFlags(); /** Reset calibration instance, discards drops all state. */ void reset(); + void setChessboard(cv::Size, double); + cv::Size chessboardSize(); + double squareSize(); + /** Returns if capture/calibration is still processing in background. * calib() instance must not be modifed while isBusy() is true. */ @@ -115,14 +144,14 @@ public: /** Start/stop capture. After stopping, use isBusy() to check when last * frame is finished. */ - void setCapture(bool v) { capture_ = v; } - bool capturing() { return capture_; } + void setCapture(bool v) { state_->capture = v; } + bool capturing() { return state_->capture; } /** get/set capture frequency: interval between processed frames in * chessboard detection */ - void setFrequency(float v) { frequency_ = v; } - float frequency() { return frequency_; } + void setFrequency(float v) { state_->frequency = v; } + float frequency() { return state_->frequency; } /** Run calibration in another thread. Check status with isBusy(). */ void run(); @@ -144,8 +173,6 @@ public: //std::vector<cv::Point2f> getPoints(int n); //std::vector<cv::Point2f> getProjectedPoints(int n); - ftl::calibration::IntrinsicCalibration& calib() { return calib_; } - /** List sources which can be calibrated. */ std::vector<std::pair<std::string, ftl::data::FrameID>> listSources(bool all=false); @@ -155,21 +182,34 @@ private: /** 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_; - std::atomic_bool running_; - ftl::calibration::IntrinsicCalibration calib_; std::future<void> future_; - float last_; - float frequency_; - std::vector<cv::Point2f> previous_points_; std::mutex mtx_; + + struct State { + cv::Mat gray; + ftl::data::FrameSetPtr fs_current; + ftl::data::FrameSetPtr fs_updated; + + ftl::codecs::Channel channel; + ftl::codecs::Channel channel_alt; + ftl::data::FrameID id; + + std::atomic_bool capture = false; + std::atomic_bool running = false; + float last = 0.0f; + float frequency = 0.0f; + bool calibrated = false; + int count = 0; + + std::vector<std::vector<cv::Point2f>> points; + std::vector<std::vector<cv::Point3f>> points_object; + + std::unique_ptr<ftl::calibration::ChessboardObject> object; + OpenCVCalibrateFlagsStereo flags; + ftl::calibration::CalibrationData::Intrinsic calib; + }; + + std::unique_ptr<State> state_; }; //////////////////////////////////////////////////////////////////////////////// @@ -180,12 +220,6 @@ private: * parameters can be used to calculate paramters for stereo rectification. */ -class CalibrationObject { -public: - virtual int detect(cv::InputArray, const cv::Mat&, const cv::Mat&, std::vector<cv::Point2d>&) = 0; - virtual std::vector<cv::Point3d> object() = 0; -}; - /** Stereo calibration for OpenCV's calibrateStereo() */ class StereoCalibration : public CalibrationModule { @@ -199,16 +233,16 @@ public: bool hasChannel(ftl::codecs::Channel c); - /** list all calibration flags */ - std::vector<int> listFlags(); - /** get string name for a flag */ - std::string flagName(int); + void setChessboard(cv::Size, double); + cv::Size chessboardSize(); + double squareSize(); - /** Apply flags, uses class defaults and config options. */ - void setDefaultFlags(); /** Reset calibration instance, discards drops all state. */ void reset(); + OpenCVCalibrateFlagsStereo& flags() { return state_->flags; }; + void resetFlags(); + /** Returns if capture/calibration is still processing in background. * calib() instance must not be modifed while isBusy() is true. */ @@ -217,14 +251,14 @@ public: /** Start/stop capture. After stopping, use isBusy() to check when last * frame is finished. */ - void setCapture(bool v) { capture_ = v; } - bool capturing() { return capture_; } + void setCapture(bool v); + bool capturing(); /** get/set capture frequency: interval between processed frames in * chessboard detection */ - void setFrequency(float v) { frequency_ = v; } - float frequency() { return frequency_; } + void setFrequency(float v); + float frequency(); /** Run calibration in another thread. Check status with isBusy(). */ void run(); @@ -238,33 +272,44 @@ public: bool hasFrame(); /** get previous points (visualization) */ - std::vector<cv::Point2f> previousPoints(); - // must not be running_ - //std::vector<cv::Point2f> getPoints(int n); - //std::vector<cv::Point2f> getProjectedPoints(int n); - + std::vector<std::vector<cv::Point2d>> previousPoints(); + cv::cuda::GpuMat getLeftPrevious(); + cv::cuda::GpuMat getRightPrevious(); + int count(); /** List sources which can be calibrated. */ std::vector<std::pair<std::string, ftl::data::FrameID>> listSources(bool all=false); private: - cv::cuda::GpuMat getFrame_(ftl::codecs::Channel); bool onFrame_(const ftl::data::FrameSetPtr& fs); - ftl::calibration::CalibrationData calib_; + ftl::codecs::Channel channelLeft_(); + ftl::codecs::Channel channelRight_(); - ftl::data::FrameSetPtr current_fs_; - ftl::data::FrameSetPtr updated_fs_; - - ftl::data::FrameID id_; - bool highres_; - std::atomic_bool capture_; - std::atomic_bool running_; std::future<void> future_; - float last_; - float frequency_; - std::vector<cv::Point2f> previous_points_; std::mutex mtx_; + + struct State { + ftl::calibration::CalibrationData calib; + std::unique_ptr<ftl::calibration::ChessboardObject> object; + ftl::data::FrameID id; + bool highres = false; + cv::Size imsize; + std::atomic_bool capture = false; + std::atomic_bool running = false; + float last = 0.0f; + float frequency = 0.0f; + int count = 0; + OpenCVCalibrateFlagsStereo flags; + + ftl::data::FrameSetPtr fs_current; + ftl::data::FrameSetPtr fs_updated; + ftl::data::FrameSetPtr fs_previous_points; + std::vector<std::vector<cv::Point2d>> points_l; + std::vector<std::vector<cv::Point2d>> points_r; + std::vector<std::vector<cv::Point3d>> points_object; + }; + std::unique_ptr<State> state_; }; class ExtrinsicCalibration : public CalibrationModule { @@ -342,7 +387,7 @@ private: bool onFrameSet_(const ftl::data::FrameSetPtr& fs); - std::unique_ptr<CalibrationObject> calib_object_; + std::unique_ptr<ftl::calibration::CalibrationObject> calib_object_; ftl::calibration::ExtrinsicCalibration calib_; ftl::calibration::CalibrationPoints<double> points_; diff --git a/applications/gui2/src/modules/calibration/extrinsic.cpp b/applications/gui2/src/modules/calibration/extrinsic.cpp index 553aa5c6a0b7fc5689cfb22a3e26fce75bb9d434..86b9e2deba02db3d1aaf300d686f9ce4bc51d336 100644 --- a/applications/gui2/src/modules/calibration/extrinsic.cpp +++ b/applications/gui2/src/modules/calibration/extrinsic.cpp @@ -5,7 +5,6 @@ #include "../../views/calibration/extrinsicview.hpp" #include <opencv2/aruco.hpp> -#include <opencv2/imgproc.hpp> #include <ftl/calibration/optimize.hpp> #include <ftl/calibration/structures.hpp> @@ -21,121 +20,8 @@ using ftl::data::FrameID; using ftl::data::FrameSetPtr; using ftl::gui2::ExtrinsicCalibration; -using ftl::gui2::CalibrationObject; - -/** - * Calibration board with 2 ArUco tags. - */ -class ArucoCalibrationObject : public CalibrationObject { -private: - cv::Mat dl_; - cv::Mat im_gray_; - cv::Ptr<cv::aruco::Dictionary> dict_; - cv::Ptr<cv::aruco::DetectorParameters> params_; - float baseline_; - float tag_size_; - int id1_; - int id2_; - -public: - ArucoCalibrationObject(cv::aruco::PREDEFINED_DICTIONARY_NAME dictionary = cv::aruco::DICT_6X6_100, - float baseline = 0.25f, float tag_size = 0.15, int id1=0, int id2=1) : - baseline_(baseline), tag_size_(tag_size),id1_(id1), id2_(id2) { - - dict_ = cv::aruco::getPredefinedDictionary(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; - } - - virtual std::vector<cv::Point3d> object() override { - return { - cv::Point3d(0.0, 0.0, 0.0), - cv::Point3d(tag_size_, 0.0, 0.0), - cv::Point3d(tag_size_, tag_size_, 0.0), - cv::Point3d(0.0, tag_size_, 0.0), - - cv::Point3d(baseline_, 0.0, 0.0), - cv::Point3d(baseline_ + tag_size_, 0.0, 0.0), - cv::Point3d(baseline_ + tag_size_, tag_size_, 0.0), - cv::Point3d(baseline_, tag_size_, 0.0) - }; - } - - virtual int detect(cv::InputArray im, const cv::Mat& K, const cv::Mat& distCoeffs, std::vector<cv::Point2d>& result) override { - - // note: cv::aruco requires floats - std::vector<std::vector<cv::Point2f>> corners; - std::vector<int> ids; - - if (im.size() == cv::Size(0, 0)) { - return -1; - } - - if (im.isGpuMat()) { - im.getGpuMat().download(dl_); - cv::cvtColor(dl_, im_gray_, cv::COLOR_BGRA2GRAY); - } - else if (im.isMat()) { - cv::cvtColor(im.getMat(), im_gray_, cv::COLOR_BGRA2GRAY); - } - else { - throw ftl::exception("Bad input (not cv::Mat/cv::GpuMat)"); - } - - cv::aruco::detectMarkers(im_gray_, dict_, corners, ids, params_, - cv::noArray(), K, distCoeffs); - - - if (ids.size() < 2) { return 0; } - - const size_t n_corners = 4; - const size_t n_tags = 2; - - std::vector<cv::Point2d> marker1; marker1.reserve(n_corners); - std::vector<cv::Point2d> 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_) { - n++; - for (auto& p : corners[i]) { - marker1.push_back({p.x, p.y}); - } - CHECK(corners[i].size() == n_corners); - } - if (ids[i] == id2_) { - n++; - for (auto& p : corners[i]) { - marker2.push_back({p.x, p.y}); - } - CHECK(corners[i].size() == n_corners); - } - } - - if (marker1.empty() || marker2.empty()) { - return 0; - } - - if (n != 2) { - LOG(WARNING) << "Found more than one marker with same ID"; - return 0; - } - - // add the points to output - result.reserve(n_tags*n_corners + result.size()); - for (size_t i_corner = 0; i_corner < n_corners; i_corner++) { - result.push_back(marker1[i_corner]); - } - for (size_t i_corner = 0; i_corner < n_corners; i_corner++) { - result.push_back(marker2[i_corner]); - } - - return n_tags*n_corners; - } -}; +using ftl::calibration::CalibrationObject; +using ftl::calibration::ArUCoObject; void ExtrinsicCalibration::init() { reset(); @@ -149,7 +35,7 @@ void ExtrinsicCalibration::reset() { fs_current_.reset(); fs_update_.reset(); - calib_object_ = std::unique_ptr<CalibrationObject>(new ArucoCalibrationObject(cv::aruco::DICT_6X6_100)); + calib_object_ = std::unique_ptr<CalibrationObject>(new ArUCoObject(cv::aruco::DICT_6X6_100)); calib_ = ftl::calibration::ExtrinsicCalibration(); points_ = ftl::calibration::CalibrationPoints<double>(); @@ -364,7 +250,7 @@ void ExtrinsicCalibration::updateCalibration() { } void ExtrinsicCalibration::updateCalibration(int c) { - + } void ExtrinsicCalibration::run() { diff --git a/applications/gui2/src/modules/calibration/intrinsic.cpp b/applications/gui2/src/modules/calibration/intrinsic.cpp index 7a119401ab767dc24acedf1d0dbbaaabc203c353..2c883263e02f228722245d60a2495e897ebca8c1 100644 --- a/applications/gui2/src/modules/calibration/intrinsic.cpp +++ b/applications/gui2/src/modules/calibration/intrinsic.cpp @@ -15,14 +15,13 @@ using ftl::gui2::Calibration; using ftl::gui2::IntrinsicCalibration; +using ftl::calibration::ChessboardObject; using ftl::calibration::CalibrationData; using ftl::codecs::Channel; using ftl::data::FrameID; using ftl::data::FrameSetPtr; void IntrinsicCalibration::init() { - frequency_ = 1.0f; - last_ = 0.0f; reset(); } @@ -32,20 +31,30 @@ IntrinsicCalibration::~IntrinsicCalibration() { } } +cv::Size IntrinsicCalibration::chessboardSize() { + return state_->object->chessboardSize(); +} + +double IntrinsicCalibration::squareSize() { + return state_->object->squareSize(); +} + +void IntrinsicCalibration::setChessboard(cv::Size size, double square) { + state_->object = std::make_unique<ChessboardObject>(size.height, size.width, square); +} + void IntrinsicCalibration::reset() { - capture_ = false; - running_ = false; - channel_ = Channel::Left; - channel_alt_ = Channel::Left; - calib_ = ftl::calibration::IntrinsicCalibration(); - previous_points_.clear(); - setDefaultFlags(); + state_ = std::make_unique<State>(); + state_->object = std::make_unique<ChessboardObject>(); + state_->channel = Channel::Left; + state_->channel_alt = Channel::Left; + resetFlags(); } void IntrinsicCalibration::start(ftl::data::FrameID id) { reset(); setCalibrationMode(false); - id_ = id; + state_->id = id; auto* view = new ftl::gui2::IntrinsicCalibrationView(screen, this); auto* filter = io->feed()->filter @@ -58,32 +67,30 @@ void IntrinsicCalibration::start(ftl::data::FrameID id) { view->onClose([filter, this](){ // if calib_ caches images, also reset() here! filter->remove(); - if (current_fs_) { - setCalibrationMode(current_fs_->frames[id_.source()], true); + if (state_->fs_current) { + setCalibrationMode(state_->fs_current->frames[state_->id.source()], true); } reset(); - current_fs_.reset(); - updated_fs_.reset(); + state_->fs_current.reset(); + state_->fs_updated.reset(); }); screen->setView(view); for (auto fs : filter->getLatestFrameSets()) { - if (!(fs->frameset() == id_.frameset()) || - !(fs->hasFrame(id_.source()))) { continue; } + if (!(fs->frameset() == state_->id.frameset()) || + !(fs->hasFrame(state_->id.source()))) { continue; } - // read calibration channel and set channel_alt_ to high res if available + // read calibration channel and set state_->channel_alt to high res if available setChannel_(fs); try { - auto& frame = (*fs)[id_.source()]; + auto& frame = (*fs)[state_->id.source()]; auto calib = frame.get<CalibrationData>(Channel::CalibrationData); - if (calib.hasCalibration(channel_)) { - auto intrinsic = calib.get(channel_).intrinsic; - auto size = frame.cast<ftl::rgbd::Frame>().get<cv::cuda::GpuMat>(channel_alt_).size(); - auto distCoeffs = intrinsic.distCoeffs.Mat(); - auto K = intrinsic.matrix(size); - calib_.setCalibration(K, distCoeffs, size); + if (calib.hasCalibration(state_->channel)) { + auto intrinsic = calib.get(state_->channel).intrinsic; + auto size = frame.get<cv::cuda::GpuMat>(state_->channel_alt).size(); + state_->calib = CalibrationData::Intrinsic(intrinsic.matrix(size), intrinsic.distCoeffs.Mat(), size); } } catch (std::exception& ex) { @@ -95,53 +102,61 @@ void IntrinsicCalibration::start(ftl::data::FrameID id) { } void IntrinsicCalibration::setChannel(Channel channel) { - channel_ = channel; - auto fs = std::atomic_load(¤t_fs_); + state_->channel= channel; + auto fs = std::atomic_load(&state_->fs_current); setChannel_(fs); } void IntrinsicCalibration::setChannel_(FrameSetPtr fs) { - channel_alt_ = channel_; + state_->channel_alt = state_->channel; if (fs == nullptr) { return; } - auto& frame = (*fs)[id_.source()]; - if ((channel_ == Channel::Left) && frame.has(Channel::LeftHighRes)) { - channel_alt_ = Channel::LeftHighRes; + auto& frame = (*fs)[state_->id.source()]; + if ((state_->channel== Channel::Left) && frame.has(Channel::LeftHighRes)) { + state_->channel_alt = Channel::LeftHighRes; return; } - if ((channel_ == Channel::Right) && frame.has(Channel::RightHighRes)) { - channel_alt_ = Channel::RightHighRes; + if ((state_->channel== Channel::Right) && frame.has(Channel::RightHighRes)) { + state_->channel_alt = Channel::RightHighRes; return; } } bool IntrinsicCalibration::onFrame_(const ftl::data::FrameSetPtr& fs) { - std::atomic_store(&updated_fs_, fs); + std::atomic_store(&state_->fs_updated, fs); screen->redraw(); - auto& frame = fs->frames[id_.source()]; + auto& frame = fs->frames[state_->id.source()]; - if (!checkFrame(frame)) { return true; } - if (!capture_) { return true; } - if ((float(glfwGetTime()) - last_) < frequency_) { return true; } - if (running_.exchange(true)) { return true; } + //if (!checkFrame(frame)) { return true; } + if (!state_->capture) { return true; } + if ((float(glfwGetTime()) - state_->last) < state_->frequency) { return true; } + if (state_->running.exchange(true)) { return true; } - future_ = ftl::pool.push( [fs, id = id_, channel = channel_alt_, - &previous_points = previous_points_, &mtx = mtx_, - &calib = calib_, &running = running_, &last = last_] + future_ = ftl::pool.push( [fs, this] (int thread_id) { try { - auto& frame = (*fs)[id.source()].cast<ftl::rgbd::Frame>().get<ftl::rgbd::VideoFrame>(channel); - cv::Mat host; - if (frame.isGPU()) { frame.getGPU().download(host); } - else { host = frame.getCPU(); } - - if (calib.addFrame(host)) { - LOG(INFO) << "Calibration pattern detected"; - std::unique_lock<std::mutex> lk(mtx); - previous_points.clear(); - previous_points = calib.getPoints(calib.count() - 1); + auto& frame = (*fs)[state_->id.source()].cast<ftl::rgbd::Frame>(); + + auto im = getMat(frame, state_->channel_alt); + cv::cvtColor(im, state_->gray, cv::COLOR_BGRA2GRAY); + + std::vector<cv::Point2d> points; + int npoints = state_->object->detect(state_->gray, cv::Mat(), cv::Mat(), points); + + if (npoints > 0) { + auto& new_points = state_->points.emplace_back(); + for (auto p : points) { + new_points.push_back(p); + } + + auto& new_points_obj = state_->points_object.emplace_back(); + for (auto p : state_->object->object()) { + new_points_obj.push_back(p); + } + + state_->count++; } else { LOG(INFO) << "Calibration pattern was not detected"; @@ -149,12 +164,12 @@ bool IntrinsicCalibration::onFrame_(const ftl::data::FrameSetPtr& fs) { } catch (std::exception &e) { LOG(ERROR) << "exception in chesboard detection: " << e.what(); - running = false; + state_->running = false; throw; } - running = false; - last = float(glfwGetTime()); + state_->running = false; + state_->last = float(glfwGetTime()); }); return true; @@ -162,65 +177,71 @@ bool IntrinsicCalibration::onFrame_(const ftl::data::FrameSetPtr& fs) { void IntrinsicCalibration::save() { - auto& frame = current_fs_->frames[id_.source()]; + auto& frame = state_->fs_current->frames[state_->id.source()]; CalibrationData calib_data = CalibrationData(frame.get<CalibrationData>(Channel::CalibrationData)); - auto& calibration = calib_data.get(channel_); - calibration.intrinsic = calib_.calibration(); + auto& calibration = calib_data.get(state_->channel); + calibration.intrinsic = state_->calib; setCalibration(frame, calib_data); } -void IntrinsicCalibration::setDefaultFlags() { +void IntrinsicCalibration::resetFlags() { // reset flags and get class defaults - calib_.removeFlags(unsigned(-1)); - calib_.setFlags(calib_.defaultFlags()); + state_->flags.reset(); + state_->flags.set(state_->flags.defaultFlags()); // load config flags - for (int i : listFlags()) { - auto flag = get<bool>(flagName(i)); + for (int i : state_->flags.list()) { + auto flag = get<bool>(state_->flags.name(i)); if (flag) { - if (*flag) calib_.setFlags(i); - else calib_.removeFlags(i); + if (*flag) state_->flags.set(i); + else state_->flags.unset(i); } } } bool IntrinsicCalibration::isBusy() { - return capture_ || running_; + return state_->capture || state_->running; } void IntrinsicCalibration::run() { - running_ = true; - future_ = ftl::pool.push([&calib = calib_, &running = running_](int id) { + state_->running = true; + future_ = ftl::pool.push([this](int id) { try { - calib.calibrate(); + cv::Mat K; + cv::Mat distCoeffs; + std::vector<cv::Mat> rvecs, tvecs; + cv::calibrateCamera(state_->points_object, state_->points, + state_->calib.resolution, K, distCoeffs, rvecs, tvecs, + state_->flags); + //calib.calibrate(); } catch (std::exception &e) { LOG(ERROR) << "exception in calibration: " << e.what(); - running = false; + state_->running = false; throw; } - running = false; + state_->running = false; }); } bool IntrinsicCalibration::hasFrame() { - return (std::atomic_load(&updated_fs_).get() != nullptr) - && updated_fs_->frames[id_.source()].hasChannel(channel_alt_); + return (std::atomic_load(&state_->fs_updated).get() != nullptr) + && state_->fs_updated->frames[state_->id.source()].hasChannel(state_->channel_alt); }; cv::cuda::GpuMat IntrinsicCalibration::getFrame() { - if (std::atomic_load(&updated_fs_)) { - current_fs_ = updated_fs_; - std::atomic_store(&updated_fs_, {}); + if (std::atomic_load(&state_->fs_updated)) { + state_->fs_current = state_->fs_updated; + std::atomic_store(&state_->fs_updated, {}); } - if (!current_fs_) { + if (!state_->fs_current) { return cv::cuda::GpuMat(); } - auto& frame = (*current_fs_)[id_.source()].cast<ftl::rgbd::Frame> - ().get<ftl::rgbd::VideoFrame>(channel_alt_); + auto& frame = (*state_->fs_current)[state_->id.source()].cast<ftl::rgbd::Frame> + ().get<ftl::rgbd::VideoFrame>(state_->channel_alt); cv::cuda::GpuMat gpu; if (!frame.isGPU()) { gpu.upload(frame.getCPU()); } else { gpu = frame.getGPU(); } @@ -236,8 +257,8 @@ float IntrinsicCalibration::sensorHeight() { }; bool IntrinsicCalibration::hasChannel(Channel c) { - if (current_fs_) { - return (*current_fs_)[id_.source()].hasChannel(c); + if (state_->fs_current) { + return (*state_->fs_current)[state_->id.source()].hasChannel(c); } return false; } @@ -258,78 +279,8 @@ std::vector<std::pair<std::string, FrameID>> IntrinsicCalibration::listSources(b std::vector<cv::Point2f> IntrinsicCalibration::previousPoints() { std::unique_lock<std::mutex> lk(mtx_, std::defer_lock); if (lk.try_lock()) { - return previous_points_; + if (state_->points.size() == 0) { return {}; } + return state_->points.back(); } return {}; } - -std::vector<int> IntrinsicCalibration::listFlags() { - return { - cv::CALIB_USE_INTRINSIC_GUESS, - cv::CALIB_FIX_PRINCIPAL_POINT, - cv::CALIB_FIX_ASPECT_RATIO, - cv::CALIB_ZERO_TANGENT_DIST, - cv::CALIB_FIX_K1, - cv::CALIB_FIX_K2, - cv::CALIB_FIX_K3, - cv::CALIB_FIX_K4, - cv::CALIB_FIX_K5, - cv::CALIB_FIX_K6, - cv::CALIB_RATIONAL_MODEL, - cv::CALIB_THIN_PRISM_MODEL, - cv::CALIB_FIX_S1_S2_S3_S4, - cv::CALIB_TILTED_MODEL, - cv::CALIB_FIX_TAUX_TAUY - }; -} - -std::string IntrinsicCalibration::flagName(int i) { - using namespace cv; - switch(i) { - case CALIB_USE_INTRINSIC_GUESS: - return "CALIB_USE_INTRINSIC_GUESS"; - - case CALIB_FIX_PRINCIPAL_POINT: - return "CALIB_FIX_PRINCIPAL_POINT"; - - case CALIB_FIX_ASPECT_RATIO: - return "CALIB_FIX_ASPECT_RATIO"; - - case CALIB_ZERO_TANGENT_DIST: - return "CALIB_ZERO_TANGENT_DIST"; - - case CALIB_FIX_K1: - return "CALIB_FIX_K1"; - - case CALIB_FIX_K2: - return "CALIB_FIX_K2"; - - case CALIB_FIX_K3: - return "CALIB_FIX_K3"; - - case CALIB_FIX_K4: - return "CALIB_FIX_K4"; - - case CALIB_FIX_K5: - return "CALIB_FIX_K5"; - - case CALIB_FIX_K6: - return "CALIB_FIX_K6"; - - case CALIB_RATIONAL_MODEL: - return "CALIB_RATIONAL_MODEL"; - - case CALIB_THIN_PRISM_MODEL: - return "CALIB_THIN_PRISM_MODEL"; - - case CALIB_FIX_S1_S2_S3_S4: - return "CALIB_FIX_S1_S2_S3_S4"; - - case CALIB_TILTED_MODEL: - return "CALIB_TILTED_MODEL"; - - case CALIB_FIX_TAUX_TAUY: - return "CALIB_FIX_TAUX_TAUY"; - }; - return ""; -} diff --git a/applications/gui2/src/modules/calibration/stereo.cpp b/applications/gui2/src/modules/calibration/stereo.cpp index c67a40cef461c76ac2accb424bd0ce59072d53bd..d57b5d2a4113e87f78a831e4a9c198ba70741570 100644 --- a/applications/gui2/src/modules/calibration/stereo.cpp +++ b/applications/gui2/src/modules/calibration/stereo.cpp @@ -6,6 +6,7 @@ #include "../../views/calibration/stereoview.hpp" #include <opencv2/imgproc.hpp> +#include <opencv2/calib3d.hpp> #include <ftl/calibration/structures.hpp> #include <ftl/threads.hpp> @@ -15,36 +16,66 @@ using ftl::gui2::Calibration; using ftl::gui2::StereoCalibration; +using ftl::calibration::ChessboardObject; using ftl::calibration::CalibrationData; using ftl::codecs::Channel; using ftl::data::FrameID; using ftl::data::FrameSetPtr; +//////////////////////////////////////////////////////////////////////////////// + +void StereoCalibration::setCapture(bool v) { + state_->capture = v; +} + +bool StereoCalibration::capturing() { + return state_->capture; +} + +void StereoCalibration::setFrequency(float v) { + state_->frequency = v; +} + +float StereoCalibration::frequency() { + return state_->frequency; +} + void StereoCalibration::init() { - frequency_ = 1.0f; - last_ = 0.0f; - reset(); + state_ = std::make_unique<State>(); + state_->object = std::unique_ptr<ChessboardObject>(new ChessboardObject()); } StereoCalibration::~StereoCalibration() { + if (state_) { + state_->running = false; + } if(future_.valid()) { future_.wait(); } } void StereoCalibration::reset() { - calib_ = ftl::calibration::CalibrationData(); - capture_ = false; - running_ = false; - highres_ = false; - previous_points_.clear(); - setDefaultFlags(); + while(state_->running) { state_->capture = false; } + state_ = std::make_unique<State>(); + state_->object = std::unique_ptr<ChessboardObject>(new ChessboardObject()); +} + +cv::Size StereoCalibration::chessboardSize() { + return state_->object->chessboardSize(); +} + +double StereoCalibration::squareSize() { + return state_->object->squareSize(); +} + +void StereoCalibration::setChessboard(cv::Size size, double square) { + state_->object = std::make_unique<ChessboardObject>(size.height, size.width, square); } void StereoCalibration::start(ftl::data::FrameID id) { reset(); setCalibrationMode(false); - id_ = id; + state_->id = id; auto* view = new ftl::gui2::StereoCalibrationView(screen, this); auto* filter = io->feed()->filter @@ -55,28 +86,35 @@ void StereoCalibration::start(ftl::data::FrameID id) { filter->on([this](const FrameSetPtr& fs){ return onFrame_(fs); }); view->onClose([filter, this](){ - // if calib_ caches images, also reset() here! + // if state_->calib caches images, also reset() here! filter->remove(); - if (current_fs_) { - setCalibrationMode(current_fs_->frames[id_.source()], true); + if (state_->fs_current) { + setCalibrationMode(state_->fs_current->frames[state_->id.source()], true); } reset(); - current_fs_.reset(); - updated_fs_.reset(); + state_->fs_current.reset(); + state_->fs_updated.reset(); }); screen->setView(view); for (auto fs : filter->getLatestFrameSets()) { - if (!(fs->frameset() == id_.frameset()) || - !(fs->hasFrame(id_.source()))) { continue; } + if (!(fs->frameset() == state_->id.frameset()) || + !(fs->hasFrame(state_->id.source()))) { continue; } // read calibration channel and set channel_alt_ to high res if available - + try { - auto& frame = (*fs)[id_.source()]; - calib_ = frame.get<CalibrationData>(Channel::CalibrationData); - highres_ = frame.hasAll({Channel::LeftHighRes, Channel::RightHighRes}); + auto& frame = (*fs)[state_->id.source()]; + state_->calib = frame.get<CalibrationData>(Channel::CalibrationData); + state_->highres = frame.hasAll({Channel::LeftHighRes, Channel::RightHighRes}); + auto sizel = frame.get<cv::cuda::GpuMat>(channelLeft_()).size(); + auto sizer = frame.get<cv::cuda::GpuMat>(channelLeft_()).size(); + if (sizel != sizer) { + LOG(ERROR) << "Frames have different resolutions"; + // TODO: do not proceed + } + state_->imsize = sizel; } catch (std::exception& ex) { LOG(ERROR) << "Could not read calibration: " << ex.what() @@ -88,36 +126,32 @@ void StereoCalibration::start(ftl::data::FrameID id) { bool StereoCalibration::onFrame_(const ftl::data::FrameSetPtr& fs) { - std::atomic_store(&updated_fs_, fs); + std::atomic_store(&state_->fs_updated, fs); screen->redraw(); - auto& frame = fs->frames[id_.source()]; + auto& frame = fs->frames[state_->id.source()]; if (!checkFrame(frame)) { return true; } - if (!capture_) { return true; } - if ((float(glfwGetTime()) - last_) < frequency_) { return true; } - if (running_.exchange(true)) { return true; } + if (!state_->capture) { return true; } + if ((float(glfwGetTime()) - state_->last) < state_->frequency) { return true; } + if (state_->running.exchange(true)) { return true; } - /* - future_ = ftl::pool.push( [fs, id = id_, channel = channel_alt_, - &previous_points = previous_points_, &mtx = mtx_, - &calib = calib_, &running = running_, &last = last_] - (int thread_id) { + future_ = ftl::pool.push([this, fs] (int thread_id) { try { - auto& frame = (*fs)[id.source()].cast<ftl::rgbd::Frame>().get<ftl::rgbd::VideoFrame>(channel); - cv::Mat host; - if (frame.isGPU()) { frame.getGPU().download(host); } - else { host = frame.getCPU(); } - - if (calib.addFrame(host)) { - LOG(INFO) << "Calibration pattern detected"; - std::unique_lock<std::mutex> lk(mtx); - previous_points.clear(); - previous_points = calib.getPoints(calib.count() - 1); - } - else { - LOG(INFO) << "Calibration pattern was not detected"; + auto& frame = (*fs)[state_->id.source()].cast<ftl::rgbd::Frame>(); + auto left = getMat(frame, channelLeft_()); + auto right = getMat(frame, channelRight_()); + + std::vector<cv::Point2d> pointsl; + std::vector<cv::Point2d> pointsr; + if ((state_->object->detect(left, cv::Mat(), cv::Mat(), pointsl) == 1) && + (state_->object->detect(right, cv::Mat(), cv::Mat(), pointsr) == 1)) { + + std::unique_lock<std::mutex> lk(mtx_); + state_->points_l.push_back(std::move(pointsl)); + state_->points_r.push_back(pointsr); + state_->points_object.push_back(state_->object->object()); } } catch (std::exception &e) { @@ -126,41 +160,52 @@ bool StereoCalibration::onFrame_(const ftl::data::FrameSetPtr& fs) { throw; } - running = false; - last = float(glfwGetTime()); + state_->running = false; + state_->last = float(glfwGetTime()); }); - */ + return true; } void StereoCalibration::save() { - //setCalibration(frame, calib_data); + //setCalibration(frame, state_->calibdata); } -void StereoCalibration::setDefaultFlags() { +void StereoCalibration::resetFlags() { // reset flags and get class defaults - /*calib_.removeFlags(unsigned(-1)); - calib_.setFlags(calib_.defaultFlags()); + state_->flags.reset(); + state_->flags.set(state_->flags.defaultFlags()); // load config flags - for (int i : listFlags()) { - auto flag = get<bool>(flagName(i)); + for (int i : state_->flags.list()) { + auto flag = get<bool>(state_->flags.name(i)); if (flag) { - if (*flag) calib_.setFlags(i); - else calib_.removeFlags(i); + if (*flag) state_->flags.set(i); + else state_->flags.unset(i); } - }*/ + } } bool StereoCalibration::isBusy() { - return capture_ || running_; + return state_->capture || state_->running; } void StereoCalibration::run() { - running_ = true; - future_ = ftl::pool.push([&calib = calib_, &running = running_](int id) { + state_->running = true; + future_ = ftl::pool.push([this](int id) { try { + auto& calib_l = state_->calib.get(Channel::Left); + auto& calib_r = state_->calib.get(Channel::Right); + auto K1 = calib_l.intrinsic.matrix(); + auto distCoeffs1 = calib_l.intrinsic.distCoeffs.Mat(); + auto K2 = calib_l.intrinsic.matrix(); + auto distCoeffs2 = calib_r.intrinsic.distCoeffs.Mat(); + cv::Mat R, T, E, F; + int flags = 0; + cv::stereoCalibrate(state_->points_object, state_->points_l, + state_->points_r, K1, distCoeffs1, K2, distCoeffs2, + state_->imsize, R, T, E, F, flags); // cv::stereoCalibrate(); } catch (std::exception &e) { @@ -174,38 +219,41 @@ void StereoCalibration::run() { } bool StereoCalibration::hasFrame() { - auto cleft = highres_ ? Channel::LeftHighRes : Channel::Left; - auto cright = highres_ ? Channel::RightHighRes : Channel::Right; - return (std::atomic_load(&updated_fs_).get() != nullptr) - && updated_fs_->frames[id_.source()].hasAll({cleft, cright}); + auto cleft = state_->highres ? Channel::LeftHighRes : Channel::Left; + auto cright = state_->highres ? Channel::RightHighRes : Channel::Right; + return (std::atomic_load(&state_->fs_updated).get() != nullptr) + && state_->fs_updated->frames[state_->id.source()].hasAll({cleft, cright}); }; -cv::cuda::GpuMat StereoCalibration::getLeft() { - return getFrame_(highres_ ? Channel::LeftHighRes : Channel::Left); +Channel StereoCalibration::channelLeft_() { + return (state_->highres ? Channel::LeftHighRes : Channel::Left); } -cv::cuda::GpuMat StereoCalibration::getRight() { - return getFrame_(highres_ ? Channel::RightHighRes : Channel::Right); +Channel StereoCalibration::channelRight_() { + return (state_->highres ? Channel::RightHighRes : Channel::Right); } -cv::cuda::GpuMat StereoCalibration::getFrame_(Channel c) { - if (std::atomic_load(&updated_fs_)) { - current_fs_ = updated_fs_; - std::atomic_store(&updated_fs_, {}); +cv::cuda::GpuMat StereoCalibration::getLeft() { + if (std::atomic_load(&state_->fs_updated)) { + state_->fs_current = state_->fs_updated; + std::atomic_store(&state_->fs_updated, {}); } + auto& frame = (*state_->fs_current)[state_->id.source()].cast<ftl::rgbd::Frame>(); + return getGpuMat(frame ,channelLeft_()); +} - auto& frame = (*current_fs_)[id_.source()].cast<ftl::rgbd::Frame> - ().get<ftl::rgbd::VideoFrame>(c); - - cv::cuda::GpuMat gpu; - if (!frame.isGPU()) { gpu.upload(frame.getCPU()); } - else { gpu = frame.getGPU(); } - return gpu; +cv::cuda::GpuMat StereoCalibration::getRight() { + if (std::atomic_load(&state_->fs_updated)) { + state_->fs_current = state_->fs_updated; + std::atomic_store(&state_->fs_updated, {}); + } + auto& frame = (*state_->fs_current)[state_->id.source()].cast<ftl::rgbd::Frame>(); + return getGpuMat(frame ,channelRight_()); } bool StereoCalibration::hasChannel(Channel c) { - if (current_fs_) { - return (*current_fs_)[id_.source()].hasChannel(c); + if (state_->fs_current) { + return (*state_->fs_current)[state_->id.source()].hasChannel(c); } return false; } @@ -223,81 +271,12 @@ std::vector<std::pair<std::string, FrameID>> StereoCalibration::listSources(bool return cameras; } -std::vector<cv::Point2f> StereoCalibration::previousPoints() { +std::vector<std::vector<cv::Point2d>> StereoCalibration::previousPoints() { std::unique_lock<std::mutex> lk(mtx_, std::defer_lock); if (lk.try_lock()) { - return previous_points_; + return { state_->points_l.back(), + state_->points_r.back() + }; } return {}; } - -std::vector<int> StereoCalibration::listFlags() { - return { - cv::CALIB_USE_INTRINSIC_GUESS, - cv::CALIB_FIX_PRINCIPAL_POINT, - cv::CALIB_FIX_ASPECT_RATIO, - cv::CALIB_ZERO_TANGENT_DIST, - cv::CALIB_FIX_K1, - cv::CALIB_FIX_K2, - cv::CALIB_FIX_K3, - cv::CALIB_FIX_K4, - cv::CALIB_FIX_K5, - cv::CALIB_FIX_K6, - cv::CALIB_RATIONAL_MODEL, - cv::CALIB_THIN_PRISM_MODEL, - cv::CALIB_FIX_S1_S2_S3_S4, - cv::CALIB_TILTED_MODEL, - cv::CALIB_FIX_TAUX_TAUY - }; -} - -std::string StereoCalibration::flagName(int i) { - using namespace cv; - switch(i) { - case CALIB_USE_INTRINSIC_GUESS: - return "CALIB_USE_INTRINSIC_GUESS"; - - case CALIB_FIX_PRINCIPAL_POINT: - return "CALIB_FIX_PRINCIPAL_POINT"; - - case CALIB_FIX_ASPECT_RATIO: - return "CALIB_FIX_ASPECT_RATIO"; - - case CALIB_ZERO_TANGENT_DIST: - return "CALIB_ZERO_TANGENT_DIST"; - - case CALIB_FIX_K1: - return "CALIB_FIX_K1"; - - case CALIB_FIX_K2: - return "CALIB_FIX_K2"; - - case CALIB_FIX_K3: - return "CALIB_FIX_K3"; - - case CALIB_FIX_K4: - return "CALIB_FIX_K4"; - - case CALIB_FIX_K5: - return "CALIB_FIX_K5"; - - case CALIB_FIX_K6: - return "CALIB_FIX_K6"; - - case CALIB_RATIONAL_MODEL: - return "CALIB_RATIONAL_MODEL"; - - case CALIB_THIN_PRISM_MODEL: - return "CALIB_THIN_PRISM_MODEL"; - - case CALIB_FIX_S1_S2_S3_S4: - return "CALIB_FIX_S1_S2_S3_S4"; - - case CALIB_TILTED_MODEL: - return "CALIB_TILTED_MODEL"; - - case CALIB_FIX_TAUX_TAUY: - return "CALIB_FIX_TAUX_TAUY"; - }; - return ""; -} diff --git a/applications/gui2/src/views/calibration/intrinsicview.cpp b/applications/gui2/src/views/calibration/intrinsicview.cpp index 7615478e10b4778638c33d80523a746b6644e3b9..cd99a7c99cdcd34b84ee2fa634e5d507d7844bcf 100644 --- a/applications/gui2/src/views/calibration/intrinsicview.cpp +++ b/applications/gui2/src/views/calibration/intrinsicview.cpp @@ -42,11 +42,16 @@ public: virtual void draw(NVGcontext* ctx) override; private: + void update(); IntrinsicCalibrationView* view_; IntrinsicCalibration* ctrl_; nanogui::Widget* channels_; + int width_; + int height_; + double square_size_; + public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; @@ -171,9 +176,18 @@ void IntrinsicCalibrationStart::draw(NVGcontext* ctx) { //////////////////////////////////////////////////////////////////////////////// // Capture Window + +void IntrinsicCalibrationView::CaptureWindow::update() { + ctrl_->setChessboard({width_, height_}, square_size_); +} + IntrinsicCalibrationView::CaptureWindow::CaptureWindow(nanogui::Widget* parent, IntrinsicCalibrationView* view) : FixedWindow(parent, "Capture Options"), view_(view), ctrl_(view->ctrl_) { + width_ = ctrl_->chessboardSize().width; + height_ = ctrl_->chessboardSize().height; + square_size_ = ctrl_->squareSize(); + setLayout(new nanogui::BoxLayout (nanogui::Orientation::Vertical, nanogui::Alignment::Fill, 6, 6)); @@ -220,44 +234,37 @@ IntrinsicCalibrationView::CaptureWindow::CaptureWindow(nanogui::Widget* parent, // width new nanogui::Label(chessboard, "Chessboard width"); - auto* chessboard_size_x = new nanogui::IntBox<int> - (chessboard, ctrl_->calib().chessboardSize().width); + auto* chessboard_size_x = new nanogui::IntBox<int>(chessboard, width_); chessboard_size_x->setEditable(true); chessboard_size_x->setFormat("[1-9][0-9]*"); - chessboard_size_x->setCallback([&calib = this->ctrl_->calib()](int v){ - auto size = calib.chessboardSize(); - size.width = max(0, v); - calib.setParameters(size, calib.squareSize()); + chessboard_size_x->setCallback([this](int v){ + width_ = max(0, v); }); // height new nanogui::Label(chessboard, "Chessboard height"); - auto* chessboard_size_y = new nanogui::IntBox<int> - (chessboard, ctrl_->calib().chessboardSize().height); + auto* chessboard_size_y = new nanogui::IntBox<int>(chessboard, height_); chessboard_size_y->setEditable(true); chessboard_size_y->setFormat("[1-9][0-9]*"); - chessboard_size_y->setCallback([&calib = this->ctrl_->calib()](int v){ - auto size = calib.chessboardSize(); - size.height = max(0, v); - calib.setParameters(size, calib.squareSize()); + chessboard_size_y->setCallback([this](int v){ + height_ = max(0, v); }); // square size new nanogui::Label(chessboard, "Chessboard square size"); - auto* square_size = new nanogui::FloatBox<float> - (chessboard, ctrl_->calib().squareSize() * 1000.0f); + auto* square_size = new nanogui::FloatBox<float>(chessboard, square_size_*1000.0); square_size->setEditable(true); square_size->setFormat("[0-9]*\\.?[0-9]+"); square_size->setUnits("mm"); - square_size->setCallback([&calib = this->ctrl_->calib()](float v){ - v = std::max(0.0f, v/1000.0f); - calib.setParameters(calib.chessboardSize(), v); + square_size->setCallback([this](float v){ + square_size_ = v/1000.0; }); auto* button_start = new nanogui::Button(this, "Start"); - button_start->setCallback([view = view_]() { - view->setMode(Mode::CAPTURE_IMAGES); + button_start->setCallback([this]() { + update(); + view_->setMode(Mode::CAPTURE_IMAGES); }); } @@ -288,7 +295,7 @@ IntrinsicCalibrationView::ControlWindow::ControlWindow(nanogui::Widget* parent, }); bresults_ = new nanogui::Button(buttons, "Show Calibration"); - bresults_->setEnabled(ctrl_->calib().calibrated()); + bresults_->setEnabled(ctrl_->calibrated()); bresults_->setCallback([view = view_, button = bresults_]{ view->setMode(Mode::RESULTS); }); @@ -311,15 +318,15 @@ IntrinsicCalibrationView::ControlWindow::ControlWindow(nanogui::Widget* parent, void IntrinsicCalibrationView::ControlWindow::draw(NVGcontext* ctx) { if (ctrl_->capturing()) { bpause_->setCaption("Pause"); } else { bpause_->setCaption("Continue"); } - bcalibrate_->setEnabled(ctrl_->calib().count() > 0); - bresults_->setEnabled(ctrl_->calib().calibrated()); + bcalibrate_->setEnabled(ctrl_->count() > 0); + bresults_->setEnabled(ctrl_->calibrated()); updateCount(); FixedWindow::draw(ctx); } void IntrinsicCalibrationView::ControlWindow::updateCount() { txtnframes_->setCaption("Detected patterns: " + - std::to_string(ctrl_->calib().count())); + std::to_string(ctrl_->count())); } //////////////////////////////////////////////////////////////////////////////// // Calibration Window @@ -367,25 +374,25 @@ IntrinsicCalibrationView::CalibrationWindow::CalibrationWindow(nanogui::Widget* flags->setLayout(new nanogui::BoxLayout (nanogui::Orientation::Vertical, nanogui::Alignment::Fill, 0, 4)); - for(int flag : ctrl_->listFlags()) { - auto* checkbox = new nanogui::CheckBox(flags, ctrl_->flagName(flag), - [flag, &calib = ctrl_->calib()](bool state){ - if (state) { calib.setFlags(flag); } - else { calib.removeFlags(flag); } + for(int flag : ctrl_->flags().list()) { + auto* checkbox = new nanogui::CheckBox(flags, ctrl_->flags().name(flag), + [flag, this](bool state){ + if (state) { ctrl_->flags().set(flag); } + else { ctrl_->flags().unset(flag); } }); - checkbox->setChecked(ctrl_->calib().hasFlags(flag)); + checkbox->setChecked(ctrl_->flags().has(flag)); } // reset button auto* reset = new nanogui::Button(this, "Reset flags"); reset->setCallback([ctrl = ctrl_, flags](){ - ctrl->setDefaultFlags(); + ctrl->resetFlags(); // update widget - auto all_flags = ctrl->listFlags(); + auto all_flags = ctrl->flags().list(); for(size_t i = 0; i < all_flags.size(); i++) { auto* checkbox = dynamic_cast<nanogui::CheckBox*>(flags->childAt(i)); - checkbox->setChecked(ctrl->calib().hasFlags(all_flags[i])); + checkbox->setChecked(ctrl->flags().has(all_flags[i])); } }); @@ -400,7 +407,7 @@ IntrinsicCalibrationView::CalibrationWindow::CalibrationWindow(nanogui::Widget* } void IntrinsicCalibrationView::CalibrationWindow::draw(NVGcontext* ctx) { - bcalibrate_->setEnabled(!ctrl_->isBusy() && (ctrl_->calib().count() > 0)); + bcalibrate_->setEnabled(!ctrl_->isBusy() && (ctrl_->count() > 0)); if (calibrating_ && !ctrl_->isBusy()) { calibrating_ = false; view_->setMode(Mode::RESULTS); @@ -453,13 +460,13 @@ void IntrinsicCalibrationView::ResultWindow::update() { while (dist_->childCount() > 0) { dist_->removeChild(dist_->childCount() - 1); } - + /* auto sw = view_->wcalibration_->sensorWidth(); auto sh = view_->wcalibration_->sensorHeight(); auto values = ctrl_->calib().values(sw, sh); auto K = ctrl_->calib().matrix(); std::vector<double> D; - ctrl_->calib().distortionCoefficients().copyTo(D); + ctrl_->calib.distCoeffs.Mat(); auto fx = K.at<double>(0, 0); auto fy = K.at<double>(1, 1); @@ -520,7 +527,7 @@ void IntrinsicCalibrationView::ResultWindow::update() { if (!pK.empty()) new nanogui::Label(dist_, pK); if (!pP.empty()) new nanogui::Label(dist_, pP); - if (!pS.empty()) new nanogui::Label(dist_, pS); + if (!pS.empty()) new nanogui::Label(dist_, pS);*/ bsave_->setEnabled(true); bsave_->setCaption("Save"); diff --git a/applications/gui2/src/views/calibration/stereoview.cpp b/applications/gui2/src/views/calibration/stereoview.cpp index d046eaf4bacc36c6a0c2c7e4bf071545a77da6c0..c6b89a8e81623204a40e2bde51c20bd77af91741 100644 --- a/applications/gui2/src/views/calibration/stereoview.cpp +++ b/applications/gui2/src/views/calibration/stereoview.cpp @@ -42,8 +42,12 @@ public: virtual void draw(NVGcontext* ctx) override; private: + void update(); StereoCalibrationView* view_; StereoCalibration* ctrl_; + int width_; + int height_; + double square_size_; public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW @@ -169,15 +173,24 @@ void StereoCalibrationStart::draw(NVGcontext* ctx) { //////////////////////////////////////////////////////////////////////////////// // Capture Window +void StereoCalibrationView::CaptureWindow::update() { + ctrl_->setChessboard({width_, height_}, square_size_); +} + StereoCalibrationView::CaptureWindow::CaptureWindow(nanogui::Widget* parent, StereoCalibrationView* view) : FixedWindow(parent, "Capture Options"), view_(view), ctrl_(view->ctrl_) { + width_ = ctrl_->chessboardSize().width; + height_ = ctrl_->chessboardSize().height; + square_size_ = ctrl_->squareSize(); + setLayout(new nanogui::BoxLayout (nanogui::Orientation::Vertical, nanogui::Alignment::Fill, 6, 6)); (new nanogui::Button(buttonPanel(), "", ENTYPO_ICON_CROSS))->setCallback( - [view = view_]() { - view->setMode(Mode::VIDEO); + [this]() { + update(); + view_->setMode(Mode::VIDEO); }); new nanogui::Label(this, "Capture interval"); @@ -195,45 +208,38 @@ StereoCalibrationView::CaptureWindow::CaptureWindow(nanogui::Widget* parent, Ste (nanogui::Orientation::Vertical, nanogui::Alignment::Fill, 0, 4)); // width - /*new nanogui::Label(chessboard, "Chessboard width"); - auto* chessboard_size_x = new nanogui::IntBox<int> - (chessboard, ctrl_->calib().chessboardSize().width); + new nanogui::Label(chessboard, "Chessboard width"); + auto* chessboard_size_x = new nanogui::IntBox<int>(chessboard, width_); chessboard_size_x->setEditable(true); chessboard_size_x->setFormat("[1-9][0-9]*"); - chessboard_size_x->setCallback([&calib = this->ctrl_->calib()](int v){ - auto size = calib.chessboardSize(); - size.width = max(0, v); - calib.setParameters(size, calib.squareSize()); + chessboard_size_x->setCallback([this](int v){ + width_ = max(0, v); }); // height new nanogui::Label(chessboard, "Chessboard height"); - auto* chessboard_size_y = new nanogui::IntBox<int> - (chessboard, ctrl_->calib().chessboardSize().height); + auto* chessboard_size_y = new nanogui::IntBox<int>(chessboard, height_); chessboard_size_y->setEditable(true); chessboard_size_y->setFormat("[1-9][0-9]*"); - chessboard_size_y->setCallback([&calib = this->ctrl_->calib()](int v){ - auto size = calib.chessboardSize(); - size.height = max(0, v); - calib.setParameters(size, calib.squareSize()); + chessboard_size_y->setCallback([this](int v){ + height_ = max(0, v); }); // square size new nanogui::Label(chessboard, "Chessboard square size"); - auto* square_size = new nanogui::FloatBox<float> - (chessboard, ctrl_->calib().squareSize() * 1000.0f); + auto* square_size = new nanogui::FloatBox<float>(chessboard, square_size_*1000.0); square_size->setEditable(true); square_size->setFormat("[0-9]*\\.?[0-9]+"); square_size->setUnits("mm"); - square_size->setCallback([&calib = this->ctrl_->calib()](float v){ - v = std::max(0.0f, v/1000.0f); - calib.setParameters(calib.chessboardSize(), v); - });*/ + square_size->setCallback([this](float v){ + square_size_ = v/1000.0; + }); auto* button_start = new nanogui::Button(this, "Start"); - button_start->setCallback([view = view_]() { - view->setMode(Mode::CAPTURE_IMAGES); + button_start->setCallback([this]() { + update(); + view_->setMode(Mode::CAPTURE_IMAGES); }); } @@ -331,25 +337,25 @@ StereoCalibrationView::CalibrationWindow::CalibrationWindow(nanogui::Widget* par flags->setLayout(new nanogui::BoxLayout (nanogui::Orientation::Vertical, nanogui::Alignment::Fill, 0, 4)); - /*for(int flag : ctrl_->listFlags()) { - auto* checkbox = new nanogui::CheckBox(flags, ctrl_->flagName(flag), - [flag, &calib = ctrl_->calib()](bool state){ - if (state) { calib.setFlags(flag); } - else { calib.removeFlags(flag); } + for(int flag : ctrl_->flags().list()) { + auto* checkbox = new nanogui::CheckBox(flags, ctrl_->flags().name(flag), + [flag, this](bool state){ + if (state) { ctrl_->flags().set(flag); } + else { ctrl_->flags().unset(flag); } }); - checkbox->setChecked(ctrl_->calib().hasFlags(flag)); - }*/ + checkbox->setChecked(ctrl_->flags().has(flag)); + } // reset button auto* reset = new nanogui::Button(this, "Reset flags"); reset->setCallback([ctrl = ctrl_, flags](){ - ctrl->setDefaultFlags(); + ctrl->resetFlags(); // update widget - auto all_flags = ctrl->listFlags(); + auto all_flags = ctrl->flags().list(); for(size_t i = 0; i < all_flags.size(); i++) { auto* checkbox = dynamic_cast<nanogui::CheckBox*>(flags->childAt(i)); - //checkbox->setChecked(ctrl->calib().hasFlags(all_flags[i])); + checkbox->setChecked(ctrl->flags().has(all_flags[i])); } }); @@ -497,9 +503,15 @@ void StereoCalibrationView::ResultWindow::update() { StereoCalibrationView::StereoCalibrationView(Screen* parent, StereoCalibration* ctrl) : View(parent), ctrl_(ctrl) { + setLayout(new nanogui::BoxLayout + (nanogui::Orientation::Horizontal, nanogui::Alignment::Fill, 0, 0)); + imviewl_ = new ftl::gui2::FTLImageView(this); imviewl_->setFixedOffset(true); imviewl_->setFixedScale(true); + imviewr_ = new ftl::gui2::FTLImageView(this); + imviewr_->setFixedOffset(true); + imviewr_->setFixedScale(true); int w = 300; wcapture_ = new CaptureWindow(screen(), this); @@ -532,8 +544,11 @@ void StereoCalibrationView::performLayout(NVGcontext *ctx) { wcapture_->center(); wcalibration_->center(); wresults_->center(); - imviewl_->setSize(size()); - imviewl_->fit(); + + imviewl_->setWidth(width()/2); + imviewr_->setWidth(width()/2); + + View::performLayout(ctx); } void StereoCalibrationView::draw(NVGcontext *ctx) { @@ -541,7 +556,9 @@ void StereoCalibrationView::draw(NVGcontext *ctx) { auto l = ctrl_->getLeft(); auto r = ctrl_->getRight(); imviewl_->copyFrom(l); - performLayout(ctx); + imviewr_->copyFrom(r); + imviewl_->fit(); + imviewr_->fit(); } View::draw(ctx); //drawChessboardCorners(ctx, imview_, ctrl_->previousPoints()); diff --git a/applications/gui2/src/views/calibration/visualization.hpp b/applications/gui2/src/views/calibration/visualization.hpp index 57e86accd0c4bf113a2796ff7772ceeaada80da4..274037dd2f1e98d4cc3ad7cb36569e4960c30918 100644 --- a/applications/gui2/src/views/calibration/visualization.hpp +++ b/applications/gui2/src/views/calibration/visualization.hpp @@ -3,7 +3,8 @@ #include "../../widgets/imageview.hpp" /** Draw Chessboard Corners with OpenGL to ImageView widget. */ -static void drawChessboardCorners(NVGcontext* ctx, ftl::gui2::ImageView* imview, const std::vector<cv::Point2f>& points) { +template<typename T> +static void drawChessboardCorners(NVGcontext* ctx, ftl::gui2::ImageView* imview, const std::vector<T>& points) { if (points.size() == 0) { return; } nanogui::Vector2f wpos = imview->absolutePosition().cast<float>(); @@ -21,7 +22,7 @@ static void drawChessboardCorners(NVGcontext* ctx, ftl::gui2::ImageView* imview, nvgStrokeColor(ctx, nvgRGBA(255, 32, 32, 192)); nvgStrokeWidth(ctx, 1.0f); nvgStroke(ctx); - + for (unsigned int i = 0; i < points.size(); i++) { apos = imview->positionForCoordinate({points[i].x, points[i].y}) + wpos; nvgBeginPath(ctx); @@ -34,7 +35,7 @@ static void drawChessboardCorners(NVGcontext* ctx, ftl::gui2::ImageView* imview, nvgStrokeColor(ctx, nvgRGBA(255, 255, 255, 255)); nvgStrokeWidth(ctx, 1.0f); nvgStroke(ctx); - + } nvgResetScissor(ctx); } diff --git a/components/calibration/CMakeLists.txt b/components/calibration/CMakeLists.txt index 34ece6f2e767d0191acfc0c88bc5fa688bc80f65..a34297f92d5dafcaa28f248e1c37e725971ff60d 100644 --- a/components/calibration/CMakeLists.txt +++ b/components/calibration/CMakeLists.txt @@ -4,6 +4,7 @@ set(CALIBSRC src/extrinsic.cpp src/structures.cpp src/visibility.cpp + src/object.cpp ) if (WITH_CERES) diff --git a/components/calibration/include/ftl/calibration.hpp b/components/calibration/include/ftl/calibration.hpp index f3918343919ce7a3ad4381d9ece3e553cd0e884b..d4610d911cbad6f76ec6041cf5613180e1cedf0b 100644 --- a/components/calibration/include/ftl/calibration.hpp +++ b/components/calibration/include/ftl/calibration.hpp @@ -1,5 +1,7 @@ + #include "calibration/parameters.hpp" #include "calibration/optimize.hpp" #include "calibration/extrinsic.hpp" #include "calibration/intrinsic.hpp" #include "calibration/structures.hpp" +#Include "calibration/object.hpp" diff --git a/components/calibration/include/ftl/calibration/object.hpp b/components/calibration/include/ftl/calibration/object.hpp new file mode 100644 index 0000000000000000000000000000000000000000..539010a4956b641de8a02fb83b5a9661a8ca1f65 --- /dev/null +++ b/components/calibration/include/ftl/calibration/object.hpp @@ -0,0 +1,54 @@ +#pragma once + +#include <opencv2/core/mat.hpp> +#include <opencv2/aruco.hpp> + +/** Calibration objects */ + +namespace ftl +{ +namespace calibration +{ + +class CalibrationObject { +public: + virtual int detect(cv::InputArray, const cv::Mat&, const cv::Mat&, std::vector<cv::Point2d>&) = 0; + virtual int detect(cv::InputArray im, std::vector<cv::Point2d>& points) { return detect(im, cv::Mat(), cv::Mat(), points); } + virtual std::vector<cv::Point3d> object() = 0; +}; + +class ChessboardObject : public CalibrationObject { +public: + ChessboardObject(); + ChessboardObject(int rows, int cols, double square_size); + int detect(cv::InputArray, const cv::Mat&, const cv::Mat&, std::vector<cv::Point2d>&) override; + std::vector<cv::Point3d> object() override; + + cv::Size chessboardSize(); + double squareSize(); + +private: + void init(); + cv::Size chessboard_size_; + double square_size_; + int flags_; + std::vector<cv::Point3d> object_points_; +}; + +class ArUCoObject : public CalibrationObject { +public: + ArUCoObject(cv::aruco::PREDEFINED_DICTIONARY_NAME dictionary = cv::aruco::DICT_6X6_100, float baseline = 0.25f, float tag_size = 0.15, int id1=0, int id2=1); + int detect(cv::InputArray, const cv::Mat&, const cv::Mat&, std::vector<cv::Point2d>&) override; + std::vector<cv::Point3d> object() override; + +private: + cv::Ptr<cv::aruco::Dictionary> dict_; + cv::Ptr<cv::aruco::DetectorParameters> params_; + float baseline_; + float tag_size_; + int id1_; + int id2_; +}; + +} // namespace calibration +} // namespace ft diff --git a/components/calibration/src/extrinsic.cpp b/components/calibration/src/extrinsic.cpp index b2b6e6d7b68b7b38816b672db2bb8fd0a5f348eb..5677b1c0854fee62f23afb41d1be366248bab851 100644 --- a/components/calibration/src/extrinsic.cpp +++ b/components/calibration/src/extrinsic.cpp @@ -239,6 +239,23 @@ std::vector<std::vector<cv::Point_<T>>> CalibrationPoints<T>::getPoints(const st return points; } +/* uint64_t required = 0; + for (const auto c : cameras) { setOne(required, c); } + + std::vector<typename ftl::calibration::CalibrationPointsHelper<T>::Points> points; + for (const auto& set : points_) { + if (!hasAll(set.cameras, required)) { continue; } + std::vector<std::vector<cv::Point_<T>> const*> image; + + for (auto &[c, p] : set.points) { + if (hasOne(required, c)) { image.push_back(&p); } + } + points.push_back({set.object, image}); + } + return points; +*/ + + template class CalibrationPoints<float>; template class CalibrationPoints<double>; @@ -355,20 +372,3 @@ double calibratePair(const cv::Mat &K1, const cv::Mat &D1, } } - -// -/* uint64_t required = 0; - for (const auto c : cameras) { setOne(required, c); } - - std::vector<typename ftl::calibration::CalibrationPointsHelper<T>::Points> points; - for (const auto& set : points_) { - if (!hasAll(set.cameras, required)) { continue; } - std::vector<std::vector<cv::Point_<T>> const*> image; - - for (auto &[c, p] : set.points) { - if (hasOne(required, c)) { image.push_back(&p); } - } - points.push_back({set.object, image}); - } - return points; -*/ diff --git a/components/calibration/src/object.cpp b/components/calibration/src/object.cpp new file mode 100644 index 0000000000000000000000000000000000000000..c9ec219d888dcd21a927c454c139c0660cb69f78 --- /dev/null +++ b/components/calibration/src/object.cpp @@ -0,0 +1,166 @@ +#include <loguru.hpp> + +#include <ftl/exception.hpp> +#include <ftl/calibration/object.hpp> + +#include <opencv2/core/cuda.hpp> +#include <opencv2/calib3d.hpp> +#include <opencv2/imgproc.hpp> + +using ftl::calibration::ArUCoObject; +using ftl::calibration::ChessboardObject; + + +ArUCoObject::ArUCoObject(cv::aruco::PREDEFINED_DICTIONARY_NAME dictionary, + float baseline, float tag_size, int id1, int id2) : + baseline_(baseline), tag_size_(tag_size),id1_(id1), id2_(id2) { + + dict_ = cv::aruco::getPredefinedDictionary(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; +} + +std::vector<cv::Point3d> ArUCoObject::object() { + return { + cv::Point3d(0.0, 0.0, 0.0), + cv::Point3d(tag_size_, 0.0, 0.0), + cv::Point3d(tag_size_, tag_size_, 0.0), + cv::Point3d(0.0, tag_size_, 0.0), + + cv::Point3d(baseline_, 0.0, 0.0), + cv::Point3d(baseline_ + tag_size_, 0.0, 0.0), + cv::Point3d(baseline_ + tag_size_, tag_size_, 0.0), + cv::Point3d(baseline_, tag_size_, 0.0) + }; +} + +int ArUCoObject::detect(cv::InputArray im, const cv::Mat& K, const cv::Mat& distCoeffs, std::vector<cv::Point2d>& result) { + + // note: cv::aruco requires floats + std::vector<std::vector<cv::Point2f>> corners; + std::vector<int> ids; + cv::Mat im_gray; + + if (im.size() == cv::Size(0, 0)) { + return -1; + } + if (im.isGpuMat()) { + cv::Mat dl; + im.getGpuMat().download(dl); + cv::cvtColor(dl, im_gray, cv::COLOR_BGRA2GRAY); + } + else if (im.isMat()) { + cv::cvtColor(im.getMat(), im_gray, cv::COLOR_BGRA2GRAY); + } + else { + throw ftl::exception("Bad input (not cv::Mat/cv::GpuMat)"); + } + + cv::aruco::detectMarkers(im_gray, dict_, corners, ids, params_, + cv::noArray(), K, distCoeffs); + + + if (ids.size() < 2) { return 0; } + + const size_t n_corners = 4; + const size_t n_tags = 2; + + std::vector<cv::Point2d> marker1; marker1.reserve(n_corners); + std::vector<cv::Point2d> 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_) { + n++; + for (auto& p : corners[i]) { + marker1.push_back({p.x, p.y}); + } + CHECK(corners[i].size() == n_corners); + } + if (ids[i] == id2_) { + n++; + for (auto& p : corners[i]) { + marker2.push_back({p.x, p.y}); + } + CHECK(corners[i].size() == n_corners); + } + } + + if (marker1.empty() || marker2.empty()) { + return 0; + } + + if (n != 2) { + LOG(WARNING) << "Found more than one marker with same ID"; + return 0; + } + + // add the points to output + result.reserve(n_tags*n_corners + result.size()); + for (size_t i_corner = 0; i_corner < n_corners; i_corner++) { + result.push_back(marker1[i_corner]); + } + for (size_t i_corner = 0; i_corner < n_corners; i_corner++) { + result.push_back(marker2[i_corner]); + } + + return n_tags*n_corners; +} + +//////////////////////////////////////////////////////////////////////////////// + +cv::Size ChessboardObject::chessboardSize() { + return {chessboard_size_.width + 1, chessboard_size_.height + 1}; +} + +double ChessboardObject::squareSize() { + return square_size_; +} + +ChessboardObject::ChessboardObject() : + chessboard_size_(24, 17), square_size_(0.0015), + flags_(cv::CALIB_CB_NORMALIZE_IMAGE|cv::CALIB_CB_ACCURACY) { + + init(); +} + +ChessboardObject::ChessboardObject(int rows, int cols, double square_size) : + chessboard_size_(cols - 1, rows - 1), square_size_(square_size), + flags_(cv::CALIB_CB_NORMALIZE_IMAGE|cv::CALIB_CB_ACCURACY) { + + init(); +} + +void ChessboardObject::init() { + object_points_.reserve(chessboard_size_.width * chessboard_size_.height); + for (int row = 0; row < chessboard_size_.height; ++row) { + for (int col = 0; col < chessboard_size_.width; ++col) { + object_points_.push_back({col * square_size_, row * square_size_, 0}); + }} +} + +int ChessboardObject::detect(cv::InputArray im, const cv::Mat& K, const cv::Mat& D, std::vector<cv::Point2d>& points) { + cv::Mat tmp; + + if (im.isMat()) { + tmp = im.getMat(); + } + else if (im.isGpuMat()) { + im.getGpuMat().download(tmp); + } + else { + throw ftl::exception("Image not cv::Mat or cv::GpuMat"); + } + + if (cv::findChessboardCornersSB(tmp, chessboard_size_, points, flags_)) { + return 1; + } + return 0; +} + +std::vector<cv::Point3d> ChessboardObject::object() { + return object_points_; +}