diff --git a/applications/calibration-multi/src/main.cpp b/applications/calibration-multi/src/main.cpp index 46e2122b2f3a5d2acd8515f98e5e8eee175dfe6c..ddd41ceb1b22f87e4aee84c19e52e9d5b0c8faf0 100644 --- a/applications/calibration-multi/src/main.cpp +++ b/applications/calibration-multi/src/main.cpp @@ -116,7 +116,7 @@ void visualizeCalibration( MultiCameraCalibrationNew &calib, Mat &out, Scalar(64, 255, 64), Scalar(64, 255, 64), }; - + vector<int> markers = {cv::MARKER_SQUARE, cv::MARKER_DIAMOND}; for (size_t c = 0; c < rgb.size(); c++) { @@ -211,7 +211,7 @@ void calibrateRPC( ftl::net::Universe* net, Mat P1, P2, Q; Mat R1, R2; Mat R_c1c2, T_c1c2; - + calculateTransform(R[c], t[c], R[c + 1], t[c + 1], R_c1c2, T_c1c2); cv::stereoRectify(K1, D1, K2, D2, params.size, R_c1c2, T_c1c2, R1, R2, P1, P2, Q, 0, params.alpha); @@ -275,12 +275,12 @@ std::vector<cv::Point2d> findCalibrationTarget(const cv::Mat &im, const cv::Mat if (corners.size() > 2) { LOG(ERROR) << "Too many ArUco tags in image"; } if (corners.size() != 2) { return {}; } - + const size_t ncorners = 4; const size_t ntags = ids.size(); std::vector<cv::Point2d> points; - + if (ids[0] == 1) { std::swap(ids[0], ids[1]); std::swap(corners[0], corners[1]); @@ -314,7 +314,7 @@ void runCameraCalibration( ftl::Configurable* root, net->start(); net->waitConnections(); - + ftl::stream::Muxer *stream = ftl::create<ftl::stream::Muxer>(root, "muxstream"); ftl::stream::Receiver *gen = ftl::create<ftl::stream::Receiver>(root, "receiver"); gen->setStream(stream); @@ -330,10 +330,10 @@ void runCameraCalibration( ftl::Configurable* root, nstream->set("uri", s); nstreams.push_back(nstream); stream->add(nstream); - + ++count; } - + const size_t n_sources = nstreams.size(); const size_t n_cameras = n_sources * 2; size_t reference_camera = 0; @@ -349,7 +349,7 @@ void runCameraCalibration( ftl::Configurable* root, if (fs.frames.size() != (rgb_new.size()/2)) { // nstreams.size() == (rgb_new.size()/2) LOG(ERROR) << "frames.size() != nstreams.size(), " - << fs.frames.size() << " != " << (rgb_new.size()/2); + << fs.frames.size() << " != " << (rgb_new.size()/2); } UNIQUE_LOCK(mutex, CALLBACK); @@ -370,28 +370,28 @@ void runCameraCalibration( ftl::Configurable* root, auto idx = stream->originStream(0, i); CHECK(idx >= 0) << "negative index"; - + fs.frames[i].download(Channel::Left+Channel::Right); Mat &left = fs.frames[i].get<Mat>(Channel::Left); Mat &right = fs.frames[i].get<Mat>(Channel::Right); - + /* - // note: also returns empty sometimes + // note: also returns empty sometimes fs.frames[i].upload(Channel::Left+Channel::Right); Mat left, right; fs.frames[i].get<cv::cuda::GpuMat>(Channel::Left).download(left); fs.frames[i].get<cv::cuda::GpuMat>(Channel::Right).download(right); */ - + CHECK(!left.empty() && !right.empty()); cv::cvtColor(left, rgb_new[2*idx], cv::COLOR_BGRA2BGR); cv::cvtColor(right, rgb_new[2*idx+1], cv::COLOR_BGRA2BGR); - + camera_parameters[2*idx] = ftl::calibration::scaleCameraMatrix(createCameraMatrix(fs.frames[i].getLeftCamera()), - rgb_new[2*idx].size(), Size(fs.frames[i].getLeftCamera().width, fs.frames[i].getLeftCamera().height)); + Size(fs.frames[i].getLeftCamera().width, fs.frames[i].getLeftCamera().height), rgb_new[2*idx].size()); camera_parameters[2*idx+1] = ftl::calibration::scaleCameraMatrix(createCameraMatrix(fs.frames[i].getRightCamera()), - rgb_new[2*idx].size(), Size(fs.frames[i].getRightCamera().width, fs.frames[i].getRightCamera().height)); + Size(fs.frames[i].getRightCamera().width, fs.frames[i].getRightCamera().height), rgb_new[2*idx].size()); if (res.empty()) res = rgb_new[2*idx].size(); } @@ -410,7 +410,7 @@ void runCameraCalibration( ftl::Configurable* root, stream->begin(); ftl::timer::start(false); - + while(true) { if (!res.empty()) { params.size = res; @@ -455,7 +455,7 @@ void runCameraCalibration( ftl::Configurable* root, while(calib.getMinVisibility() < static_cast<size_t>(n_views)) { loop: cv::waitKey(10); - + while (true) { if (new_frames) { UNIQUE_LOCK(mutex, LOCK); @@ -465,7 +465,7 @@ void runCameraCalibration( ftl::Configurable* root, } cv::waitKey(10); } - + for (Mat &im : rgb) { if (im.empty()) { LOG(ERROR) << "EMPTY"; @@ -488,10 +488,10 @@ void runCameraCalibration( ftl::Configurable* root, n_found++; } } - + if (n_found >= min_visible) { calib.addPoints(points, visible); - + if (save_input) { for (size_t i = 0; i < n_cameras; i++) { cv::imwrite(path + std::to_string(i) + "_" + std::to_string(iter) + ".jpg", rgb[i]); @@ -508,13 +508,13 @@ void runCameraCalibration( ftl::Configurable* root, cv::FONT_HERSHEY_COMPLEX_SMALL, 1.0, Scalar(64, 64, 255), 1); } } - + // index cv::putText(rgb[i], "Camera " + std::to_string(i), Point2i(10, 30), cv::FONT_HERSHEY_COMPLEX_SMALL, 1.0, Scalar(64, 64, 255), 1); - + // resolution cv::putText(rgb[i], "[" + std::to_string(rgb[i].size().width) + "x" + std::to_string(rgb[i].size().height) + "]", @@ -542,7 +542,7 @@ void runCameraCalibration( ftl::Configurable* root, cv::imshow("Cameras", show); } cv::destroyWindow("Cameras"); - + for (size_t i = 0; i < nstreams.size(); i++) { while(true) { try { @@ -558,7 +558,7 @@ void runCameraCalibration( ftl::Configurable* root, catch (...) {} } } - + Mat out; vector<Mat> map1, map2; vector<cv::Rect> roi; @@ -614,7 +614,7 @@ void runCameraCalibrationPath( ftl::Configurable* root, net->start(); net->waitConnections(); - + ftl::stream::Muxer *stream = ftl::create<ftl::stream::Muxer>(root, "muxstream"); ftl::stream::Receiver *gen = ftl::create<ftl::stream::Receiver>(root, "receiver"); gen->setStream(stream); @@ -630,10 +630,10 @@ void runCameraCalibrationPath( ftl::Configurable* root, nstream->set("uri", s); nstreams.push_back(nstream); stream->add(nstream); - + ++count; } - + const size_t n_sources = nstreams.size(); const size_t n_cameras = n_sources * 2; size_t reference_camera = 0; @@ -649,7 +649,7 @@ void runCameraCalibrationPath( ftl::Configurable* root, if (fs.frames.size() != (rgb_new.size()/2)) { // nstreams.size() == (rgb_new.size()/2) LOG(ERROR) << "frames.size() != nstreams.size(), " - << fs.frames.size() << " != " << (rgb_new.size()/2); + << fs.frames.size() << " != " << (rgb_new.size()/2); } UNIQUE_LOCK(mutex, CALLBACK); @@ -670,11 +670,11 @@ void runCameraCalibrationPath( ftl::Configurable* root, auto idx = stream->originStream(0, i); CHECK(idx >= 0) << "negative index"; - + fs.frames[i].download(Channel::Left+Channel::Right); Mat &left = fs.frames[i].get<Mat>(Channel::Left); Mat &right = fs.frames[i].get<Mat>(Channel::Right); - + CHECK(!left.empty() && !right.empty()); cv::cvtColor(left, rgb_new[2*idx], cv::COLOR_BGRA2BGR); @@ -684,7 +684,7 @@ void runCameraCalibrationPath( ftl::Configurable* root, rgb_new[2*idx].size(), Size(fs.frames[i].getLeftCamera().width, fs.frames[i].getLeftCamera().height)); camera_parameters[2*idx+1] = ftl::calibration::scaleCameraMatrix(createCameraMatrix(fs.frames[i].getRightCamera()), rgb_new[2*idx].size(), Size(fs.frames[i].getRightCamera().width, fs.frames[i].getRightCamera().height)); - + if (res.empty()) res = rgb_new[2*idx].size();*/ } } @@ -726,7 +726,7 @@ void runCameraCalibrationPath( ftl::Configurable* root, calib.object_points_ = calibration_target; cv::FileStorage fs(path + filename, cv::FileStorage::READ); - fs["resolution"] >> params.size; + fs["resolution"] >> params.size; params.idx_cameras.resize(n_cameras); std::iota(params.idx_cameras.begin(), params.idx_cameras.end(), 0); calib.loadInput(path + filename, params.idx_cameras); @@ -798,7 +798,7 @@ void runCameraCalibrationPath( ftl::Configurable* root, int main(int argc, char **argv) { auto options = ftl::config::read_options(&argv, &argc); auto root = ftl::configure(argc, argv, "registration_default"); - + // run calibration from saved input? const bool load_input = root->value<bool>("load_input", false); // should calibration input be saved @@ -822,7 +822,7 @@ int main(int argc, char **argv) { const string registration_file = root->value<string>("registration_file", FTL_LOCAL_CONFIG_ROOT "/registration.json"); // location where extrinsic calibration files saved const string output_directory = root->value<string>("output_directory", "./"); - + const bool offline = root->value<bool>("offline", false); CalibrationParams params; @@ -833,7 +833,7 @@ int main(int argc, char **argv) { params.output_path = output_directory; params.registration_file = registration_file; params.reference_camera = ref_camera; - + LOG(INFO) << "\n" << "\n" << "\n save_input: " << (int) save_input @@ -858,4 +858,4 @@ int main(int argc, char **argv) { } return 0; -} \ No newline at end of file +} diff --git a/applications/calibration/src/lens.cpp b/applications/calibration/src/lens.cpp index 72694ebacda05370a4a32a214e400c72a6d94d0e..ef00ba17779fe3a61327e67b5b3da5e8a4ff0f5e 100644 --- a/applications/calibration/src/lens.cpp +++ b/applications/calibration/src/lens.cpp @@ -62,7 +62,7 @@ void ftl::calibration::intrinsic(map<string, string> &opt) { // assume no tangential and thin prism distortion and only estimate first // three radial distortion coefficients - + int calibrate_flags = 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; @@ -76,13 +76,13 @@ void ftl::calibration::intrinsic(map<string, string> &opt) { camera_matrix.clear(); vector<Mat> tmp; Size tmp_size; - + loadIntrinsics(filename_intrinsics, camera_matrix, tmp, tmp_size); CHECK(camera_matrix.size() == static_cast<unsigned int>(n_cameras)); if ((tmp_size != image_size) && (!tmp_size.empty())) { for (Mat &K : camera_matrix) { - K = ftl::calibration::scaleCameraMatrix(K, image_size, tmp_size); + K = ftl::calibration::scaleCameraMatrix(K, tmp_size, image_size); } } @@ -104,13 +104,13 @@ void ftl::calibration::intrinsic(map<string, string> &opt) { LOG(ERROR) << "Could not open camera device"; return; } - camera.set(cv::CAP_PROP_FRAME_WIDTH, image_size.width); + camera.set(cv::CAP_PROP_FRAME_WIDTH, image_size.width); camera.set(cv::CAP_PROP_FRAME_HEIGHT, image_size.height); } vector<vector<vector<Vec2f>>> image_points(n_cameras); vector<vector<vector<Vec3f>>> object_points(n_cameras); - + vector<Mat> img(n_cameras); vector<Mat> img_display(n_cameras); vector<int> count(n_cameras, 0); @@ -124,7 +124,7 @@ void ftl::calibration::intrinsic(map<string, string> &opt) { std::atomic<bool> ready = false; auto capture = std::thread( [n_cameras, delay, &m, &ready, &count, &calib, &img, &image_points, &object_points]() { - + vector<Mat> tmp(n_cameras); while(true) { if (!ready) { @@ -138,7 +138,7 @@ void ftl::calibration::intrinsic(map<string, string> &opt) { img[c].copyTo(tmp[c]); } m.unlock(); - + for (int c = 0; c < n_cameras; c++) { vector<Vec2f> points; if (calib.findPoints(tmp[c], points)) { @@ -168,13 +168,13 @@ void ftl::calibration::intrinsic(map<string, string> &opt) { ready = true; m.unlock(); } - + for (int c = 0; c < n_cameras; c++) { img[c].copyTo(img_display[c]); m.lock(); if (image_points[c].size() > 0) { - + for (auto &points : image_points[c]) { calib.drawCorners(img_display[c], points); } @@ -192,13 +192,13 @@ void ftl::calibration::intrinsic(map<string, string> &opt) { } cv::destroyAllWindows(); - + //bool calib_ok = true; for (int c = 0; c < n_cameras; c++) { LOG(INFO) << "Calculating intrinsic paramters for camera " << std::to_string(c); vector<Mat> rvecs, tvecs; - + double rms = cv::calibrateCamera( object_points[c], image_points[c], image_size, camera_matrix[c], dist_coeffs[c], @@ -208,9 +208,9 @@ void ftl::calibration::intrinsic(map<string, string> &opt) { LOG(INFO) << "final reprojection RMS error: " << rms; if (!ftl::calibration::validate::distortionCoefficients(dist_coeffs[c], image_size)) { - LOG(ERROR) << "Calibration failed: invalid distortion coefficients:\n" + LOG(ERROR) << "Calibration failed: invalid distortion coefficients:\n" << dist_coeffs[c]; - + LOG(WARNING) << "Estimating only intrinsic parameters for camera " << std::to_string(c); dist_coeffs[c] = Mat(Size(8, 1), CV_64FC1, cv::Scalar(0.0)); @@ -228,7 +228,7 @@ void ftl::calibration::intrinsic(map<string, string> &opt) { // TODO: check for valid aperture width/height; do not run if not valid values cv::calibrationMatrixValues(camera_matrix[c], image_size, aperture_width, aperture_height, fovx, fovy, focal_length, principal_point, aspect_ratio); - + LOG(INFO) << ""; LOG(INFO) << " fovx (deg): " << fovx; LOG(INFO) << " fovy (deg): " << fovy; @@ -243,13 +243,13 @@ void ftl::calibration::intrinsic(map<string, string> &opt) { saveIntrinsics(filename_intrinsics, camera_matrix, dist_coeffs, image_size); LOG(INFO) << "intrinsic paramaters saved to: " << filename_intrinsics; - + vector<Mat> map1(n_cameras), map2(n_cameras); for (int c = 0; c < n_cameras; c++) { cv::initUndistortRectifyMap(camera_matrix[c], dist_coeffs[c], Mat::eye(3,3, CV_64F), camera_matrix[c], image_size, CV_16SC2, map1[c], map2[c]); } - + while (cv::waitKey(25) != 27) { for (auto &camera : cameras ) { camera.grab(); } for (int c = 0; c < n_cameras; c++) { diff --git a/applications/gui2/src/modules/calibration.cpp b/applications/gui2/src/modules/calibration.cpp index 2833a9ca261283712f2df51016ff904e0e64d67a..3cfe30fb57ff016f6efbed6f577637db79b44951 100644 --- a/applications/gui2/src/modules/calibration.cpp +++ b/applications/gui2/src/modules/calibration.cpp @@ -217,16 +217,16 @@ void IntrinsicCalibration::onFrame(const ftl::data::FrameSetPtr& fs) { if (calibration_enabled_ || calib_data.enabled) { LOG(INFO) << "DISABLING"; enableCalibration(frame, false); - auto& intrinsics = (channel_ == Channel::Left) ? - calib_data.intrinsic_left : calib_data.intrinsic_right; - - cv::Mat K = intrinsics.matrix(); - cv::Mat D = cv::Mat(intrinsics.distCoeffs); - - LOG(INFO) << K; - LOG(INFO) << D; - cv::Size resolution = intrinsics.resolution; - calib_.setCalibration(K, D, resolution); + for (auto& item : calib_data.data) { + if (item.channel != channel_) continue; + + auto& intrinsics = item.intrinsic; + cv::Mat K = intrinsics.matrix(); + cv::Mat D = cv::Mat(intrinsics.distCoeffs); + cv::Size resolution = intrinsics.resolution; + calib_.setCalibration(K, D, resolution); + break; + } return; } } @@ -312,14 +312,22 @@ void IntrinsicCalibration::run() { bool IntrinsicCalibration::save() { auto& frame = current_fs_->frames[id_.source()]; - CalibrationData params = CalibrationData(frame.get<CalibrationData>(Channel::CalibrationData)); - if (channel_ == Channel::Left) { - params.intrinsic_left = calib_.calibration(); - } - else { - params.intrinsic_right = calib_.calibration(); + CalibrationData calib_data = CalibrationData(frame.get<CalibrationData>(Channel::CalibrationData)); + + for (auto& calibration : calib_data.data) { + if (calibration.channel != channel_) continue; + + // update existing values and return + calibration.intrinsic = calib_.calibration(); + return update(frame, calib_data); } - return update(frame, params); + + // not found: add to calibration structure + CalibrationData::Calibration calibration; + calibration.channel = channel_; + calibration.intrinsic = calib_.calibration(); + calib_data.data.push_back(calibration); + return update(frame, calib_data); } @@ -348,3 +356,10 @@ float IntrinsicCalibration::sensorWidth() { float IntrinsicCalibration::sensorHeight() { return value("sensor_height", 0.0f); }; + +bool IntrinsicCalibration::hasChannel(Channel c) { + if (current_fs_) { + return (*current_fs_)[id_.source()].hasChannel(c); + } + return false; +} diff --git a/applications/gui2/src/modules/calibration.hpp b/applications/gui2/src/modules/calibration.hpp index f6c3bddb5fe3f27b09a83cce823701bbbb40147d..086409910c69424ad7582df232af54e2ff2b2d32 100644 --- a/applications/gui2/src/modules/calibration.hpp +++ b/applications/gui2/src/modules/calibration.hpp @@ -28,6 +28,7 @@ public: void onFrame(const ftl::data::FrameSetPtr& fs); + bool hasChannel(ftl::codecs::Channel c); /** select channel */ void setChannel(ftl::codecs::Channel c) { channel_ = c; } ftl::codecs::Channel channel() { return channel_; } diff --git a/applications/gui2/src/modules/camera.cpp b/applications/gui2/src/modules/camera.cpp index 9797ea6f60a1c2f2c13eb4f31a34e8bf5280a4f9..352d2c67b6013a7970fd2a7d9bd3489c33ac7ea7 100644 --- a/applications/gui2/src/modules/camera.cpp +++ b/applications/gui2/src/modules/camera.cpp @@ -18,7 +18,7 @@ void Camera::init() { (ftl::create<ftl::overlay::Overlay>(this, "overlay")); } -void Camera::_initiate(ftl::data::Frame &frame) { +void Camera::initiate_(ftl::data::Frame &frame) { if (frame.has(Channel::Capabilities)) { const auto &cap = frame.get<std::unordered_set<Capability>>(Channel::Capabilities); LOG(INFO) << "Camera Capabilities:"; @@ -106,7 +106,7 @@ void Camera::activate(ftl::data::FrameID id) { // Must be done in GUI thread, hence use of cv. std::unique_lock<std::mutex> lk(m); cv.wait_for(lk, 1s, [this](){ return has_seen_frame_; }); - _initiate(std::atomic_load(¤t_fs_)->frames[frame_idx]); + initiate_(std::atomic_load(¤t_fs_)->frames[frame_idx]); } void Camera::setChannel(Channel c) { diff --git a/applications/gui2/src/modules/camera.hpp b/applications/gui2/src/modules/camera.hpp index e73fc4e3b57670e47ff181a3d539999cf50f5c91..3ecdf949b299f498adf74a8fcbd860b97285ac23 100644 --- a/applications/gui2/src/modules/camera.hpp +++ b/applications/gui2/src/modules/camera.hpp @@ -49,7 +49,7 @@ private: CameraView* view = nullptr; MediaPanel* panel = nullptr; - void _initiate(ftl::data::Frame &frame); + void initiate_(ftl::data::Frame &frame); }; } diff --git a/applications/gui2/src/views/calibration/intrinsic.cpp b/applications/gui2/src/views/calibration/intrinsic.cpp index b59f7556b00d2121de23c49de27437c9723f8692..4eccb6b4fbff14c9a10a8944e698db21c763d10e 100644 --- a/applications/gui2/src/views/calibration/intrinsic.cpp +++ b/applications/gui2/src/views/calibration/intrinsic.cpp @@ -62,6 +62,7 @@ private: IntrinsicCalibration* ctrl_; nanogui::Label* txtnframes_; + nanogui::Button* bcalibrate_; nanogui::Button* bresults_; nanogui::Button* bpause_; @@ -146,6 +147,7 @@ CaptureWindow::CaptureWindow(nanogui::Widget* parent, IntrinsicCalibrationView* ctrl->reset(); ctrl->setChannel(Channel::Right); }); + button_right->setEnabled(ctrl_->hasChannel(Channel::Right)); new nanogui::Label(this, "Capture interval"); auto* interval = new nanogui::FloatBox<float>(this, ctrl_->frequency()); @@ -238,11 +240,10 @@ ControlWindow::ControlWindow(nanogui::Widget* parent, IntrinsicCalibrationView* ctrl->setCapture(!ctrl->capturing()); }); - auto* button_continue = new nanogui::Button(buttons, "Calibrate"); - button_continue->setFixedWidth(140); - button_continue->setCallback([view = view_](){ + bcalibrate_ = new nanogui::Button(buttons, "Calibrate"); + bcalibrate_->setFixedWidth(140); + bcalibrate_->setCallback([view = view_](){ - // TODO: implement as setMode() in view view->setMode(Mode::CALIBRATION); }); } @@ -250,6 +251,7 @@ ControlWindow::ControlWindow(nanogui::Widget* parent, IntrinsicCalibrationView* void ControlWindow::draw(NVGcontext* ctx) { if (ctrl_->capturing()) { bpause_->setCaption("Pause"); } else { bpause_->setCaption("Continue"); } + bcalibrate_->setEnabled(ctrl_->calib().nFrames() > 0); bresults_->setEnabled(ctrl_->calib().calibrated()); updateCount(); nanogui::Window::draw(ctx); diff --git a/applications/gui2/src/views/camera.cpp b/applications/gui2/src/views/camera.cpp index 2308b3b2fe41f2fe6bce2c6f7d3cf1e2f04621ef..806ccc33dbc2620f3c7f5d4997b7096ac9943e9d 100644 --- a/applications/gui2/src/views/camera.cpp +++ b/applications/gui2/src/views/camera.cpp @@ -127,6 +127,7 @@ CameraView::CameraView(ftl::gui2::Screen* parent, ftl::gui2::Camera* ctrl) : //LOG(INFO) << __func__ << " (" << this << ")"; imview_ = new ftl::gui2::ImageView(this); imview_->setTheme(parent->getTheme("window")); + imview_->setSize(parent->viewSize()); } CameraView::~CameraView() { diff --git a/applications/gui2/src/views/thumbnails.cpp b/applications/gui2/src/views/thumbnails.cpp index e0ec065b15953217110c29dcce05327fcb6e00b6..62867bd3b8a1bb78dfc249673c2eaf970f596590 100644 --- a/applications/gui2/src/views/thumbnails.cpp +++ b/applications/gui2/src/views/thumbnails.cpp @@ -76,19 +76,13 @@ void ThumbView::update(ftl::rgbd::Frame &frame, Channel c) { void ThumbView::draw(NVGcontext *ctx) { fit(); - center(); - // Background color - /*nvgBeginPath(ctx); - nvgRect(ctx, absolutePosition()[0], absolutePosition()[1], width(), height()); - nvgFillColor(ctx, {0.2f, 0.2f, 0.2f, 1.0f}); - nvgFill(ctx);*/ // Image ftl::gui2::ImageView::draw(ctx); // Label nvgScissor(ctx, mPos.x(), mPos.y(), mSize.x(), mSize.y()); nvgFontSize(ctx, 14); nvgFontFace(ctx, "sans-bold"); - nvgText(ctx, mPos.x() + 10, mPos.y()+mSize.y() - 10, name_.c_str(), NULL); + nvgText(ctx, mPos.x() + 10, mPos.y()+mSize.y() - 18, name_.c_str(), NULL); nvgResetScissor(ctx); } diff --git a/components/calibration/CMakeLists.txt b/components/calibration/CMakeLists.txt index e0153a9f0fd3d52dc7404aba2301bd60a0cbc26a..5f2f4f1028a71e145e006136983cf651bea49f3d 100644 --- a/components/calibration/CMakeLists.txt +++ b/components/calibration/CMakeLists.txt @@ -11,7 +11,7 @@ endif() add_library(ftlcalibration ${CALIBSRC}) target_include_directories(ftlcalibration - PUBLIC + PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include> $<INSTALL_INTERFACE:include> PRIVATE @@ -19,7 +19,8 @@ target_include_directories(ftlcalibration ${OpenCV_INCLUDE_DIRS} ) -target_link_libraries(ftlcalibration ftlcommon Threads::Threads ${OpenCV_LIBS} Eigen3::Eigen ceres) +# ftlcodecs required for ftl::data::Channel +target_link_libraries(ftlcalibration ftlcommon ftlcodecs Threads::Threads ${OpenCV_LIBS} Eigen3::Eigen ceres) if (BUILD_TESTS) ADD_SUBDIRECTORY(test) diff --git a/components/calibration/include/ftl/calibration/parameters.hpp b/components/calibration/include/ftl/calibration/parameters.hpp index f261506570921b8c243855e6ace9ea6b2fcfdd95..f0fd0cf291f56f30dbe4c2c332cb8cac742adefc 100644 --- a/components/calibration/include/ftl/calibration/parameters.hpp +++ b/components/calibration/include/ftl/calibration/parameters.hpp @@ -80,11 +80,11 @@ bool cameraMatrix(const cv::Mat &M); * @param D distortion coefficients * @param size resolution * @note Tangential and prism distortion coefficients are not validated. - * + * * Radial distortion is always monotonic for real lenses and distortion * function has to be bijective. This is verified by evaluating the distortion * function for integer values from 0 to sqrt(width^2+height^2). - * + * * Camera model documented in * https://docs.opencv.org/master/d9/d0c/group__calib3d.html#details */ @@ -126,19 +126,20 @@ inline void inverse(cv::Mat &R, cv::Mat &t) { } /** - * @brief Inverse transform inplace + * @brief Inverse transform * @param T transformation matrix (4x4) */ -inline void inverse(cv::Mat &T) { +inline cv::Mat inverse(const cv::Mat &T) { cv::Mat rmat; cv::Mat tvec; getRotationAndTranslation(T, rmat, tvec); - T = cv::Mat::eye(4, 4, CV_64FC1); + cv::Mat T_ = cv::Mat::eye(4, 4, CV_64FC1); - T(cv::Rect(3, 0, 1, 1)) = -rmat.col(0).dot(tvec); - T(cv::Rect(3, 1, 1, 1)) = -rmat.col(1).dot(tvec); - T(cv::Rect(3, 2, 1, 1)) = -rmat.col(2).dot(tvec); - T(cv::Rect(0, 0, 3, 3)) = rmat.t(); + T_(cv::Rect(3, 0, 1, 1)) = -rmat.col(0).dot(tvec); + T_(cv::Rect(3, 1, 1, 1)) = -rmat.col(1).dot(tvec); + T_(cv::Rect(3, 2, 1, 1)) = -rmat.col(2).dot(tvec); + T_(cv::Rect(0, 0, 3, 3)) = rmat.t(); + return T_; } inline cv::Point3d apply(const cv::Point3d& point, const cv::InputArray& R, const cv::InputArray& t) { @@ -160,10 +161,10 @@ inline cv::Point3d apply(const cv::Point3d& point, const cv::InputArray& T) { /** * @brief Scale camera intrinsic matrix - * @param size_new New resolution * @param size_old Original (camera matrix) resolution + * @param size_new New resolution */ -cv::Mat scaleCameraMatrix(const cv::Mat &K, const cv::Size &size_new, const cv::Size &size_old); +cv::Mat scaleCameraMatrix(const cv::Mat &K, const cv::Size &size_old, const cv::Size &size_new); /** * @brief Calculate MSE reprojection error diff --git a/components/calibration/include/ftl/calibration/structures.hpp b/components/calibration/include/ftl/calibration/structures.hpp index 032dfb761be79f030ff5318b225bda3cfabc92bc..4b277f2350df0392085ce853d74c792bce067121 100644 --- a/components/calibration/include/ftl/calibration/structures.hpp +++ b/components/calibration/include/ftl/calibration/structures.hpp @@ -2,12 +2,14 @@ #define _FTL_CALIBRATION_STRUCTURES_HPP_ #include <ftl/utility/msgpack.hpp> +#include <ftl/codecs/channels.hpp> namespace ftl { namespace calibration { struct CalibrationData { struct Intrinsic { + Intrinsic(); cv::Mat matrix() const; static Intrinsic fromMatrix(const cv::Mat &K, cv::Size sz); static Intrinsic fromMatrix(const cv::Mat &K, const cv::Mat &D, cv::Size sz); @@ -21,28 +23,32 @@ struct CalibrationData { MSGPACK_DEFINE(resolution, fx, fy, cx, cy, distCoeffs); }; struct Extrinsic { + Extrinsic(); cv::Mat matrix() const; static Extrinsic fromMatrix(const cv::Mat &T); cv::Vec3d rvec; cv::Vec3d tvec; MSGPACK_DEFINE(rvec, tvec); + }; + struct Calibration { + ftl::codecs::Channel channel; + Intrinsic intrinsic; + Extrinsic extrinsic; + MSGPACK_DEFINE(channel, intrinsic, extrinsic); }; + CalibrationData() : enabled(false) {} bool enabled; - Intrinsic intrinsic_left; - Intrinsic intrinsic_right; - Extrinsic extrinsic_left; - Extrinsic extrinsic_right; - - MSGPACK_DEFINE(enabled, intrinsic_left, intrinsic_right, extrinsic_left, extrinsic_right); + std::vector<Calibration> data; - static bool fromFile(const std::string &path, CalibrationData &out); - bool toFile(const std::string &path) const; + static CalibrationData readFile(const std::string &path); + void writeFile(const std::string &path) const; + MSGPACK_DEFINE(enabled, data); }; } } -#endif \ No newline at end of file +#endif diff --git a/components/calibration/src/parameters.cpp b/components/calibration/src/parameters.cpp index 049e655a262a6f6a14b6b382403fdc9c62b05f88..eca5105d8c6f4578775e3d0f7ac7b5ef728b1ddd 100644 --- a/components/calibration/src/parameters.cpp +++ b/components/calibration/src/parameters.cpp @@ -278,7 +278,7 @@ bool ftl::calibration::validate::distortionCoefficients(const Mat &D, Size size) return true; } -Mat ftl::calibration::scaleCameraMatrix(const Mat &K, const Size &size_new, const Size &size_old) { +Mat ftl::calibration::scaleCameraMatrix(const Mat &K, const Size &size_old, const Size &size_new) { Mat S(cv::Size(3, 3), CV_64F, 0.0); double scale_x = ((double) size_new.width) / ((double) size_old.width); double scale_y = ((double) size_new.height) / ((double) size_old.height); diff --git a/components/calibration/src/structures.cpp b/components/calibration/src/structures.cpp index dabf71d2c234a4daaafe3feadb6c065ec3e9b075..bbc820f3a926f93231de3a4959f1e5de62287da7 100644 --- a/components/calibration/src/structures.cpp +++ b/components/calibration/src/structures.cpp @@ -7,6 +7,9 @@ using ftl::calibration::CalibrationData; +CalibrationData::Intrinsic::Intrinsic() : + resolution(0, 0), fx(0.0), fy(0.0), cx(0.0), cy(0.0), distCoeffs(4, 0.0) {} + CalibrationData::Intrinsic CalibrationData::Intrinsic::fromMatrix(const cv::Mat &K, cv::Size size) { CalibrationData::Intrinsic params; params.fx = K.at<double>(0, 0); @@ -35,6 +38,10 @@ cv::Mat CalibrationData::Intrinsic::matrix() const { return K; } +//////////////////////////////////////////////////////////////////////////////// + +CalibrationData::Extrinsic::Extrinsic() : rvec(0.0, 0.0, 0.0), tvec(0.0, 0.0, 0.0) {} + CalibrationData::Extrinsic CalibrationData::Extrinsic::fromMatrix(const cv::Mat &T) { CalibrationData::Extrinsic params; cv::Rodrigues(T(cv::Rect(0, 0, 3, 3)), params.rvec); @@ -55,60 +62,58 @@ cv::Mat CalibrationData::Extrinsic::matrix() const { return T; } -bool CalibrationData::fromFile(const std::string &path, CalibrationData &out) { +//////////////////////////////////////////////////////////////////////////////// - cv::Size resolution; - std::vector<cv::Mat> intrinsic; - std::vector<cv::Mat> distCoeffs; - std::vector<cv::Vec3d> rvec; - std::vector<cv::Vec3d> tvec; +CalibrationData CalibrationData::readFile(const std::string &path) { - try { - cv::FileStorage fs; - - fs.open((path).c_str(), cv::FileStorage::READ); - if (!fs.isOpened()) { - return false; - } - - fs["resolution"] >> resolution; - fs["K"] >> intrinsic; - fs["D"] >> distCoeffs; - fs["rvec"] >> rvec; - fs["tvec"] >> tvec; - fs.release(); + cv::FileStorage fs; + fs.open(path.c_str(), cv::FileStorage::READ); + if (!fs.isOpened()) { + throw std::exception(); + } + CalibrationData data; + fs["enabled"] >> data.enabled; + + for (auto it = fs["calibration"].begin(); it != fs["calibration"].end(); it++) { + Calibration calib; + (*it)["channel"] >> calib.channel; + (*it)["resolution"] >> calib.intrinsic.resolution; + (*it)["fx"] >> calib.intrinsic.fx; + (*it)["fy"] >> calib.intrinsic.fy; + (*it)["cx"] >> calib.intrinsic.cx; + (*it)["cy"] >> calib.intrinsic.cy; + (*it)["distCoeffs"] >> calib.intrinsic.distCoeffs; + (*it)["rvec"] >> calib.extrinsic.rvec; + (*it)["tvec"] >> calib.extrinsic.tvec; + data.data.push_back(calib); } - catch (cv::Exception &e) { - return false; - } - out.intrinsic_left = CalibrationData::Intrinsic::fromMatrix(intrinsic[0], distCoeffs[0], resolution); - out.intrinsic_right = CalibrationData::Intrinsic::fromMatrix(intrinsic[1], distCoeffs[1], resolution); - out.extrinsic_left.rvec = rvec[0]; - out.extrinsic_left.tvec = tvec[0]; - out.extrinsic_right.rvec = rvec[1]; - out.extrinsic_right.tvec = tvec[1]; - return true; + + return data; } -bool CalibrationData::toFile(const std::string &path) const { +void CalibrationData::writeFile(const std::string &path) const { cv::FileStorage fs(path, cv::FileStorage::WRITE); - if (!fs.isOpened()) { return false; } - - fs << "resolution" << intrinsic_left.resolution - << "K" << std::vector<cv::Mat> - {intrinsic_left.matrix(), intrinsic_right.matrix()} - - << "D" << std::vector<std::vector<double>> - {intrinsic_left.distCoeffs, intrinsic_right.distCoeffs} - - << "rvec" << std::vector<cv::Vec3d> - {extrinsic_left.rvec, extrinsic_right.rvec} - - << "tvec" << std::vector<cv::Vec3d> - {extrinsic_left.rvec, extrinsic_right.rvec} - ; - + if (!fs.isOpened()) { + throw std::exception(); + } + + fs << "enabled" << enabled; + fs << "calibration" << "["; + for (auto &item : data) { + fs << "{:" + << "channel" << int(item.channel) + << "resolution" << item.intrinsic.resolution + << "fx" << item.intrinsic.fx + << "fy" << item.intrinsic.fy + << "cx" << item.intrinsic.cx + << "cy" << item.intrinsic.cy + << "distCoeffs" << item.intrinsic.distCoeffs + << "rvec" << item.extrinsic.rvec + << "tvec" << item.extrinsic.rvec + << "}"; + } + fs << "]"; + fs.release(); - return true; } diff --git a/components/calibration/test/CMakeLists.txt b/components/calibration/test/CMakeLists.txt index 0bd1f1f198347c38c74973b14a5253b4bcd93d09..59acdd613fa71a4863863c2bf04714d299e8d4a3 100644 --- a/components/calibration/test/CMakeLists.txt +++ b/components/calibration/test/CMakeLists.txt @@ -6,5 +6,5 @@ add_executable(calibration_parameters_unit ) target_include_directories(calibration_parameters_unit PUBLIC "${CMAKE_CURRENT_SOURCE_DIR}/../include") -target_link_libraries(calibration_parameters_unit ftlcommon Threads::Threads ${OS_LIBS} ${OpenCV_LIBS}) -add_test(CalibrationValidateTest calibration_parameters_unit) \ No newline at end of file +target_link_libraries(calibration_parameters_unit ftlcalibration ftlcommon ftlcodecs Threads::Threads ${OS_LIBS} ${OpenCV_LIBS}) +add_test(CalibrationValidateTest calibration_parameters_unit) diff --git a/components/calibration/test/test_parameters.cpp b/components/calibration/test/test_parameters.cpp index 7d19b9d757587ff7e73cde565c3643f8b8d5937e..be5722f79a9d0e764756da80afb9b540e8353f87 100644 --- a/components/calibration/test/test_parameters.cpp +++ b/components/calibration/test/test_parameters.cpp @@ -1,5 +1,6 @@ #include "catch.hpp" -#include "ftl/calibration/parameters.hpp" +#include <ftl/calibration/parameters.hpp> +#include <ftl/calibration/structures.hpp> using cv::Size; using cv::Mat; @@ -17,7 +18,7 @@ TEST_CASE("Calibration values", "") { D.at<double>(0) = 1.0; D.at<double>(1) = 1.0; REQUIRE(ftl::calibration::validate::distortionCoefficients(D, Size(1920, 1080))); - + D.at<double>(0) = 0.01512461889185869; D.at<double>(1) = -0.1207895096066378; D.at<double>(4) = 0.1582571415357494; @@ -38,3 +39,32 @@ TEST_CASE("Calibration values", "") { REQUIRE(!ftl::calibration::validate::distortionCoefficients(D, Size(1920, 1080))); } } + +TEST_CASE("Test reading/writing file") { + using ftl::calibration::CalibrationData; + using ftl::codecs::Channel; + + CalibrationData::Calibration calib; + CalibrationData calib_read; + CalibrationData data; + + calib.channel = Channel::Left; + calib.intrinsic.resolution = {1, 1}; + calib.intrinsic.fx = 1.0; + calib.intrinsic.fy = 1.0; + calib.intrinsic.cx = 0.5; + calib.intrinsic.cy = 0.5; + data.data.push_back(calib); + + data.writeFile("/tmp/calib.yml"); + calib_read = CalibrationData::readFile("/tmp/calib.yml"); + REQUIRE(calib_read.data[0].channel == calib.channel); + + data.writeFile("/tmp/calib.json"); + calib_read = CalibrationData::readFile("/tmp/calib.json"); + REQUIRE(calib_read.data[0].channel == calib.channel); + + data.writeFile("/tmp/calib.xml"); + calib_read = CalibrationData::readFile("/tmp/calib.xml"); + REQUIRE(calib_read.data[0].channel == calib.channel); +} diff --git a/components/rgbd-sources/CMakeLists.txt b/components/rgbd-sources/CMakeLists.txt index 345d065bc231ae3c1fb9473b21f77cae50ee78f4..8fafef6448d82ebdbb34efc3ae1c04c172baf2ef 100644 --- a/components/rgbd-sources/CMakeLists.txt +++ b/components/rgbd-sources/CMakeLists.txt @@ -1,5 +1,5 @@ set(RGBDSRC - src/sources/stereovideo/calibrate.cpp + src/sources/stereovideo/rectification.cpp src/sources/stereovideo/opencv.cpp src/source.cpp src/frame.cpp diff --git a/components/rgbd-sources/src/sources/stereovideo/calibrate.cpp b/components/rgbd-sources/src/sources/stereovideo/calibrate.cpp deleted file mode 100644 index 546060127d979f6a2a4402314b9a22a521133b63..0000000000000000000000000000000000000000 --- a/components/rgbd-sources/src/sources/stereovideo/calibrate.cpp +++ /dev/null @@ -1,346 +0,0 @@ -/* - * Copyright 2019 Nicolas Pope - */ - -#include <loguru.hpp> -#include <ftl/config.h> -#include <ftl/configuration.hpp> -#include <ftl/threads.hpp> -#include <ftl/calibration/parameters.hpp> - -#include "calibrate.hpp" -#include "ftl/exception.hpp" - -#include <opencv2/core.hpp> -#include <opencv2/core/utility.hpp> -#include <opencv2/imgproc.hpp> -#include <opencv2/calib3d.hpp> -#include <opencv2/cudawarping.hpp> - -using ftl::rgbd::detail::Calibrate; - -using cv::FileStorage; - -using cv::INTER_LINEAR; - -using cv::FileNode; -using cv::FileNodeIterator; - -using cv::Mat; -using cv::cuda::GpuMat; -using cv::cuda::Stream; - -using cv::Size; - -using cv::Point2f; -using cv::Point3f; -using cv::Matx33d; -using cv::Scalar; - -using std::string; -using std::vector; - -Calibrate::Calibrate(nlohmann::json &config, Size image_size, cv::cuda::Stream &stream) : - ftl::Configurable(config) { - - img_size_ = image_size; - calib_size_ = image_size; - - K_ = vector<Mat>(2); - K_[0] = Mat::eye(Size(3, 3), CV_64FC1); - K_[1] = Mat::eye(Size(3, 3), CV_64FC1); - D_ = vector<Mat>(2); - D_[0] = Mat::zeros(Size(5, 1), CV_64FC1); - D_[1] = Mat::zeros(Size(5, 1), CV_64FC1); - pose_ = Mat::eye(Size(4, 4), CV_64FC1); - pose_adjustment_ = Mat::eye(Size(4, 4), CV_64FC1); - Q_ = Mat::eye(Size(4, 4), CV_64FC1); - Q_.at<double>(3, 2) = -1; - Q_.at<double>(2, 3) = 1; - - setRectify(true); -} - -Mat Calibrate::_getK(size_t idx, Size size) { - CHECK(idx < K_.size()); - CHECK(!size.empty()); - return ftl::calibration::scaleCameraMatrix(K_[idx], size, calib_size_); -} - -Mat Calibrate::_getK(size_t idx) { - return _getK(idx, img_size_); -} - -double Calibrate::getBaseline() const { - if (t_.empty()) { return 0.0; } - return cv::norm(t_); -} - -double Calibrate::getDoff() const { - return -(Q_.at<double>(3,3) * getBaseline()); -} - -double Calibrate::getDoff(const Size& size) const { - return getDoff() * ((double) size.width / (double) img_size_.width); -} - -Mat Calibrate::getCameraMatrixLeft(const cv::Size res) { - if (rectify_) { - return ftl::calibration::scaleCameraMatrix(Mat(P1_, cv::Rect(0, 0, 3, 3)), res, img_size_); - } else { - return ftl::calibration::scaleCameraMatrix(K_[0], res, calib_size_); - } -} - -Mat Calibrate::getCameraMatrixRight(const cv::Size res) { - if (rectify_) { - return ftl::calibration::scaleCameraMatrix(Mat(P2_, cv::Rect(0, 0, 3, 3)), res, img_size_); - } else { - return ftl::calibration::scaleCameraMatrix(K_[1], res, calib_size_); - } -} - -Mat Calibrate::getCameraDistortionLeft() { - if (rectify_) { return Mat::zeros(Size(5, 1), CV_64FC1); } - else { return D_[0]; } -} - -Mat Calibrate::getCameraDistortionRight() { - if (rectify_) { return Mat::zeros(Size(5, 1), CV_64FC1); } - else { return D_[1]; } -} - -Mat Calibrate::getPose() const { - Mat T; - if (rectify_) { - Mat R1 = Mat::eye(4, 4, CV_64FC1); - R1_.copyTo(R1(cv::Rect(0, 0, 3, 3))); - T = pose_ * R1.inv(); - } - else { - pose_.copyTo(T); - } - return pose_adjustment_ * T; -} - -bool Calibrate::setRectify(bool enabled) { - if (t_.empty() || R_.empty()) { enabled = false; } - if (enabled) { - rectify_ = calculateRectificationParameters(); - } - else { - rectify_ = false; - } - return rectify_; -} - -bool Calibrate::setDistortion(const vector<Mat> &D) { - if (D.size() != 2) { return false; } - for (const auto d : D) { if (d.size() != Size(5, 1)) { return false; }} - D[0].copyTo(D_[0]); - D[1].copyTo(D_[1]); - return true; -} - -bool Calibrate::setIntrinsics(const Size &size, const vector<Mat> &K) { - if (K.size() != 2) { return false; } - if (size.empty() || size.width <= 0 || size.height <= 0) { return false; } - for (const auto k : K) { - if (!ftl::calibration::validate::cameraMatrix(k)) { - return false; - } - } - - calib_size_ = Size(size); - K[0].copyTo(K_[0]); - K[1].copyTo(K_[1]); - return true; -} - -bool Calibrate::setExtrinsics(const Mat &R, const Mat &t) { - if (!ftl::calibration::validate::rotationMatrix(R) || - !ftl::calibration::validate::translationStereo(t)) { return false; } - - R.copyTo(R_); - t.copyTo(t_); - return true; -} - -bool Calibrate::setPose(const Mat &P) { - if (!ftl::calibration::validate::pose(P)) { return false; } - P.copyTo(pose_); - return true; -} - -bool Calibrate::setPoseAdjustment(const Mat &T) { - if (!ftl::calibration::validate::pose(T)) { return false; } - pose_adjustment_ = T * pose_adjustment_; - return true; -} - -bool Calibrate::loadCalibration(const string &fname) { - ftl::calibration::CalibrationData params; - if (ftl::calibration::CalibrationData::fromFile(fname, params)) { - setCalibration(params); - LOG(INFO) << "calibration loaded from: " << fname; - return true; - } - return false; - - /*FileStorage fs; - - fs.open((fname).c_str(), FileStorage::READ); - if (!fs.isOpened()) { - LOG(WARNING) << "Could not open calibration file"; - return false; - } - - Size calib_size; - vector<Mat> K; - vector<Mat> D; - Mat R; - Mat t; - Mat pose; - Mat pose_adjustment; - - fs["resolution"] >> calib_size; - fs["K"] >> K; - fs["D"] >> D; - fs["R"] >> R; - fs["t"] >> t; - fs["P"] >> pose; - fs["adjustment"] >> pose_adjustment; - fs.release(); - - bool retval = true; - if (calib_size.empty()) { - LOG(ERROR) << "calibration resolution missing in calibration file"; - retval = false; - } - if (!setIntrinsics(calib_size, K)) { - LOG(ERROR) << "invalid intrinsics in calibration file"; - retval = false; - } - if (!setDistortion(D)) { - LOG(ERROR) << "invalid distortion parameters in calibration file"; - retval = false; - } - if (!setExtrinsics(R, t)) { - LOG(ERROR) << "invalid extrinsics in calibration file"; - retval = false; - } - if (!setPose(pose)) { - LOG(ERROR) << "invalid pose in calibration file"; - retval = false; - } - if (!setPoseAdjustment(pose_adjustment)) { - LOG(WARNING) << "invalid pose adjustment in calibration file (using identity)"; - }*/ -} - -bool Calibrate::writeCalibration( const string &fname, const Size &size, - const vector<Mat> &K, const vector<Mat> &D, - const Mat &R, const Mat &t, const Mat &pose, - const Mat &pose_adjustment) { - - cv::FileStorage fs(fname, cv::FileStorage::WRITE); - if (!fs.isOpened()) { return false; } - - fs << "resolution" << size - << "K" << K - << "D" << D - << "R" << R - << "t" << t - << "P" << pose - << "adjustment" << pose_adjustment; - ; - - fs.release(); - return true; -} - -bool Calibrate::saveCalibration(const string &fname) { - // note: never write rectified parameters! - - // TODO: make a backup of old file - //if (std::filesystem::is_regular_file(fname)) { - // // copy to fname + ".bak" - //} - - return writeCalibration(fname, calib_size_, K_, D_, R_, t_, pose_, pose_adjustment_); -} - -bool Calibrate::calculateRectificationParameters() { - - Mat K1 = _getK(0, img_size_); - Mat D1 = D_[0]; - Mat K2 = _getK(1, img_size_); - Mat D2 = D_[1]; - double alpha = value("alpha", 0.0); - - try { - cv::stereoRectify( K1, D1, K2, D2, - img_size_, R_, t_, - R1_, R2_, P1_, P2_, Q_, 0, alpha); - - initUndistortRectifyMap(K1, D1, R1_, P1_, img_size_, CV_32FC1, map1_.first, map2_.first); - initUndistortRectifyMap(K2, D2, R2_, P2_, img_size_, CV_32FC1, map1_.second, map2_.second); - - // CHECK Is this thread safe!!!! - map1_gpu_.first.upload(map1_.first); - map1_gpu_.second.upload(map1_.second); - map2_gpu_.first.upload(map2_.first); - map2_gpu_.second.upload(map2_.second); - - Mat map0 = map1_.first.clone(); - Mat map1 = map2_.first.clone(); - cv::convertMaps(map0, map1, map1_.first, map2_.first, CV_16SC2); - - map0 = map1_.second.clone(); - map1 = map2_.second.clone(); - cv::convertMaps(map0, map1, map1_.second, map2_.second, CV_16SC2); - } - catch (cv::Exception &ex) { - LOG(ERROR) << ex.what(); - return false; - } - - return true; -} - -void Calibrate::rectifyStereo(GpuMat &l, GpuMat &r, Stream &stream) { - if (!rectify_) { return; } - // cv::cuda::remap() can not use same Mat for input and output - // TODO: create tmp buffers only once - GpuMat l_tmp(l.size(), l.type()); - GpuMat r_tmp(r.size(), r.type()); - cv::cuda::remap(l, l_tmp, map1_gpu_.first, map2_gpu_.first, cv::INTER_LINEAR, 0, cv::Scalar(), stream); - cv::cuda::remap(r, r_tmp, map1_gpu_.second, map2_gpu_.second, cv::INTER_LINEAR, 0, cv::Scalar(), stream); - stream.waitForCompletion(); - l = l_tmp; - r = r_tmp; -} - -void Calibrate::rectifyStereo(cv::Mat &l, cv::Mat &r) { - if (!rectify_) { return; } - // cv::cuda::remap() can not use same Mat for input and output - cv::remap(l, l, map1_.first, map2_.first, cv::INTER_LINEAR, 0, cv::Scalar()); - cv::remap(r, r, map1_.second, map2_.second, cv::INTER_LINEAR, 0, cv::Scalar()); -} - -void Calibrate::rectifyLeft(cv::Mat &l) { - if (!rectify_) { return; } - cv::remap(l, l, map1_.first, map2_.first, cv::INTER_LINEAR, 0, cv::Scalar()); -} - -void Calibrate::rectifyRight(cv::Mat &r) { - if (!rectify_) { return; } - cv::remap(r, r, map1_.second, map2_.second, cv::INTER_LINEAR, 0, cv::Scalar()); -} - -void Calibrate::setCalibration(ftl::calibration::CalibrationData data) { - setIntrinsics(data.intrinsic_left.resolution, { - data.intrinsic_left.matrix(), - data.intrinsic_right.matrix(), - }); -} \ No newline at end of file diff --git a/components/rgbd-sources/src/sources/stereovideo/calibrate.hpp b/components/rgbd-sources/src/sources/stereovideo/calibrate.hpp deleted file mode 100644 index b972830b1100947b187475ec654e0ff4ca84c5cc..0000000000000000000000000000000000000000 --- a/components/rgbd-sources/src/sources/stereovideo/calibrate.hpp +++ /dev/null @@ -1,247 +0,0 @@ -/* - * Copyright 2019 Nicolas Pope - */ - -#ifndef _FTL_CALIBRATION_HPP_ -#define _FTL_CALIBRATION_HPP_ - -#include <opencv2/core.hpp> -#include <opencv2/core/cuda.hpp> -#include <string> -#include <vector> -#include <ftl/rgbd/camera.hpp> -#include <ftl/calibration/structures.hpp> - -namespace cv { -class FileStorage; -class FileNode; -}; - -namespace ftl { -namespace rgbd { -namespace detail { - -/** - * Manage local calibration details: undistortion, rectification and camera - * parameters. - */ -class Calibrate : public ftl::Configurable { - public: - Calibrate(nlohmann::json &config, cv::Size image_size, cv::cuda::Stream &stream); - - /** - * @brief Rectify and undistort stereo pair images (GPU) - */ - void rectifyStereo(cv::cuda::GpuMat &l, cv::cuda::GpuMat &r, cv::cuda::Stream &stream); - - /** - * @brief Rectify and undistort stereo pair images (CPU) - */ - void rectifyStereo(cv::Mat &l, cv::Mat &r); - - /** - * @brief Rectify and undistort left image (CPU) - */ - void rectifyLeft(cv::Mat &l); - - /** - * @brief Rectify and undistort right image (CPU) - */ - void rectifyRight(cv::Mat &r); - - void updateCalibration(const ftl::rgbd::Camera &p); - - /** - * @brief Get disparity to depth matrix - * @note Disparity offset is in image_size (scale) - * - * 2020/01/15: StereoVideoSource creates a Camera object which is used to - * calculate depth from disparity (disp2depth.cu). Seems to be - * used only in StereoVideoSource to get doff and baseline - * parameter values in updateParameters() - */ - [[deprecated]] - const cv::Mat &getQ() const { return Q_; } - - /** - * @brief Get camera pair baseline - */ - double getBaseline() const; - - /** - * @brief Get camera pair disparity offset - * @param size (optional) scale to given resolution. - * - * Returns disparity offset for image_size resolution if size not provided. - */ - double getDoff() const; - double getDoff(const cv::Size& size) const; - - /** - * @brief Get intrinsic paramters. If rectification is enabled, returns - * rectified intrinsic parameters, otherwise returns values from - * calibration. Parameters are scaled for given resolution. - * @param res camera resolution - */ - cv::Mat getCameraMatrixLeft(const cv::Size res); - /** @brief Same as getCameraMatrixLeft() for right camera */ - cv::Mat getCameraMatrixRight(const cv::Size res); - - /** @brief Get camera distortion parameters. If rectification is enabled, - * returns zeros. Otherwise returns calibrated distortion - * parameters values. - */ - cv::Mat getCameraDistortionLeft(); - /** @brief Same as getCameraDistortionLeft() for right camera */ - cv::Mat getCameraDistortionRight(); - - /** - * @brief Get camera pose from calibration. Returns pose to rectified - * camera if rectification is enabled. - */ - cv::Mat getPose() const; - - /** - * @brief Enable/disable recitification. If disabled, instance returns - * original camera intrinsic parameters (getCameraMatrixLeft() and - * getCameraMatrixRight() methods). When enabled (default), those - * methods return camera parameters for rectified images. Does not - * enable rectification, if valid parameters are missing. - * @param Rectification on/off - * @returns Status after call - */ - bool setRectify(bool enabled); - - /** - * @brief Set intrinsic parameters for both cameras. - * - * @param size calibration size - * @param K 2 camera matricies (3x3) - * @returns true if valid parameters - */ - bool setIntrinsics(const cv::Size &size, const std::vector<cv::Mat> &K); - - /** - * @brief Set lens distortion parameters - * @param D 2 distortion parameters (5x1) - */ - bool setDistortion(const std::vector<cv::Mat> &D); - - /** - * @brief Set extrinsic parameters. - * - * @param R Rotation matrix (3x3) from left to right camera - * @param t Translation vector (1x3) from left to right camera - * @returns true if valid parameters - */ - bool setExtrinsics(const cv::Mat &R, const cv::Mat &t); - - /** - * @brief Set pose - * @param pose Pose for left camera - * @returns true if valid pose - */ - bool setPose(const cv::Mat &P); - - /** - * @brief Set adjustment, which is applied to pose: T_update*T_pose - */ - bool setPoseAdjustment(const cv::Mat &T); - - /** - * @breif Set calibration - */ - void setCalibration(ftl::calibration::CalibrationData data); - - /** - * @brief Calculate rectification parameters and maps. Can fail if - * calibration parameters are invalid. - * @returns true if successful - */ - bool calculateRectificationParameters(); - - /** - * @brief Load calibration from file - * @param fname File name - */ - bool loadCalibration(const std::string &fname); - - /** - * @brief Write calibration parameters to file - * - * Assumes two cameras and intrinsic calibration parameters have the same - * resolution. - * - * @todo Validate loaded values - * - * @param fname file name - * @param size calibration resolution (intrinsic parameters) - * @param K intrinsic matrices - * @param D distortion coefficients - * @param R rotation from first camera to second - * @param t translation from first camera to second - * @param pose first camera's pose - */ - static bool writeCalibration(const std::string &fname, - const cv::Size &size, - const std::vector<cv::Mat> &K, - const std::vector<cv::Mat> &D, - const cv::Mat &R, const cv::Mat &t, - const cv::Mat &pose, - const cv::Mat &pose_adjustment); - - /* @brief Save current calibration to file - * @param File name - */ - bool saveCalibration(const std::string &fname); - -private: - // rectification enabled/disabled - volatile bool rectify_; - - /** - * @brief Get intrinsic matrix saved in calibration. - * @param Camera index (0 left, 1 right) - * @param Resolution - */ - cv::Mat _getK(size_t idx, cv::Size size); - cv::Mat _getK(size_t idx); - - // calibration resolution (loaded from file by loadCalibration) - cv::Size calib_size_; - // camera resolution - cv::Size img_size_; - - // rectification maps - std::pair<cv::Mat, cv::Mat> map1_; - std::pair<cv::Mat, cv::Mat> map2_; - std::pair<cv::cuda::GpuMat, cv::cuda::GpuMat> map1_gpu_; - std::pair<cv::cuda::GpuMat, cv::cuda::GpuMat> map2_gpu_; - - // parameters for rectification, see cv::stereoRectify() documentation - cv::Mat R1_; - cv::Mat P1_; - cv::Mat R2_; - cv::Mat P2_; - - // disparity to depth matrix - cv::Mat Q_; - - // intrinsic parameters and distortion coefficients - std::vector<cv::Mat> K_; - std::vector<cv::Mat> D_; - - // transformation from left to right camera: R_ and T_ - cv::Mat R_; - cv::Mat t_; - // pose for left camera - cv::Mat pose_; - cv::Mat pose_adjustment_; -}; - -} -} -} - -#endif // _FTL_CALIBRATION_HPP_ - diff --git a/components/rgbd-sources/src/sources/stereovideo/device.hpp b/components/rgbd-sources/src/sources/stereovideo/device.hpp index 7a7fcb45b6cd95e6588b93deee5e0efda4d26c9b..baae8a30821f6232470ab0fb66a7aa12966ef58b 100644 --- a/components/rgbd-sources/src/sources/stereovideo/device.hpp +++ b/components/rgbd-sources/src/sources/stereovideo/device.hpp @@ -9,7 +9,7 @@ namespace ftl { namespace rgbd { namespace detail { -class Calibrate; +class StereoRectification; struct DeviceDetails { std::string name; @@ -21,7 +21,7 @@ struct DeviceDetails { /** * Abstract base class for camera or stereo camera sources. Just wraps the * basic grab and retrieve functionality with rectification. - * + * * @see OpenCVDevice * @see PylonDevice */ @@ -33,7 +33,7 @@ class Device : public Configurable { //virtual const std::vector<DeviceDetails> &listDevices()=0; virtual bool grab()=0; - virtual bool get(cv::cuda::GpuMat &l, cv::cuda::GpuMat &r, cv::cuda::GpuMat &h_l, cv::Mat &h_r, Calibrate *c, cv::cuda::Stream &stream)=0; + virtual bool get(cv::cuda::GpuMat &l, cv::cuda::GpuMat &r, cv::cuda::GpuMat &h_l, cv::Mat &h_r, StereoRectification *c, cv::cuda::Stream &stream)=0; virtual unsigned int width() const =0; virtual unsigned int height() const =0; @@ -42,9 +42,9 @@ class Device : public Configurable { virtual unsigned int fullHeight() const =0; inline bool hasHigherRes() const { return fullWidth() != width(); } - + virtual double getTimestamp() const =0; - + virtual bool isStereo() const =0; }; @@ -52,4 +52,4 @@ class Device : public Configurable { } } -#endif \ No newline at end of file +#endif diff --git a/components/rgbd-sources/src/sources/stereovideo/opencv.cpp b/components/rgbd-sources/src/sources/stereovideo/opencv.cpp index 23c5165a8d332ea3b72c31c09df9e1be0c1efbe7..5efd1ba3e6fd2b9f6f8545fef94e36f29b9bb7a6 100644 --- a/components/rgbd-sources/src/sources/stereovideo/opencv.cpp +++ b/components/rgbd-sources/src/sources/stereovideo/opencv.cpp @@ -10,7 +10,7 @@ #include <ftl/profiler.hpp> #include "opencv.hpp" -#include "calibrate.hpp" +#include "rectification.hpp" #include <opencv2/core.hpp> #include <opencv2/opencv.hpp> #include <opencv2/xphoto.hpp> @@ -31,7 +31,8 @@ #endif using ftl::rgbd::detail::OpenCVDevice; -using ftl::rgbd::detail::Calibrate; +using ftl::rgbd::detail::StereoRectification; +using ftl::codecs::Channel; using cv::Mat; using cv::VideoCapture; using cv::Rect; @@ -125,7 +126,7 @@ OpenCVDevice::OpenCVDevice(nlohmann::json &config) camera_a_->set(cv::CAP_PROP_FRAME_HEIGHT, value("height", 720)); camera_a_->set(cv::CAP_PROP_FPS, 1000 / ftl::timer::getInterval()); //camera_a_->set(cv::CAP_PROP_BUFFERSIZE, 0); // Has no effect - + Mat frame; if (!camera_a_->grab()) LOG(ERROR) << "Could not grab a video frame"; camera_a_->retrieve(frame); @@ -207,7 +208,7 @@ std::vector<ftl::rgbd::detail::DeviceDetails> OpenCVDevice::_selectDevices() { } else { - + } } @@ -219,7 +220,7 @@ std::vector<ftl::rgbd::detail::DeviceDetails> OpenCVDevice::_selectDevices() { #else int fd; - v4l2_capability video_cap; + v4l2_capability video_cap; v4l2_frmsizeenum video_fsize; LOG(INFO) << "Video Devices:"; @@ -312,8 +313,8 @@ bool OpenCVDevice::grab() { } bool OpenCVDevice::get(cv::cuda::GpuMat &l_out, cv::cuda::GpuMat &r_out, - cv::cuda::GpuMat &l_hres_out, cv::Mat &r_hres_out, Calibrate *c, cv::cuda::Stream &stream) { - + cv::cuda::GpuMat &l_hres_out, cv::Mat &r_hres_out, StereoRectification *c, cv::cuda::Stream &stream) { + Mat l, r ,hres; // Use page locked memory @@ -337,7 +338,7 @@ bool OpenCVDevice::get(cv::cuda::GpuMat &l_out, cv::cuda::GpuMat &r_out, cv::cvtColor(frame_r_, rfull, cv::COLOR_BGR2BGRA); if (stereo_) { - c->rectifyRight(rfull); + c->rectify(rfull, Channel::Right); if (hasHigherRes()) { // TODO: Use threads? @@ -378,8 +379,8 @@ bool OpenCVDevice::get(cv::cuda::GpuMat &l_out, cv::cuda::GpuMat &r_out, if (stereo_) { //FTL_Profile("Rectification", 0.01); //c->rectifyStereo(lfull, rfull); - c->rectifyLeft(lfull); - + c->rectify(lfull, Channel::Left); + // Need to resize //if (hasHigherRes()) { // TODO: Use threads? diff --git a/components/rgbd-sources/src/sources/stereovideo/opencv.hpp b/components/rgbd-sources/src/sources/stereovideo/opencv.hpp index 1db067b8d5e08078e1d1136eccfc0938bfbe3c32..9915cf701bb60402f150f39eecf65cab5f848913 100644 --- a/components/rgbd-sources/src/sources/stereovideo/opencv.hpp +++ b/components/rgbd-sources/src/sources/stereovideo/opencv.hpp @@ -18,20 +18,20 @@ class OpenCVDevice : public ftl::rgbd::detail::Device { ~OpenCVDevice(); static std::vector<DeviceDetails> listDevices(); - + bool grab() override; - bool get(cv::cuda::GpuMat &l, cv::cuda::GpuMat &r, cv::cuda::GpuMat &h_l, cv::Mat &h_r, Calibrate *c, cv::cuda::Stream &stream) override; + bool get(cv::cuda::GpuMat &l, cv::cuda::GpuMat &r, cv::cuda::GpuMat &h_l, cv::Mat &h_r, StereoRectification *c, cv::cuda::Stream &stream) override; unsigned int width() const override { return dwidth_; } unsigned int height() const override { return dheight_; } unsigned int fullWidth() const override { return width_; } unsigned int fullHeight() const override { return height_; } - + double getTimestamp() const override; - + bool isStereo() const override; - + private: double timestamp_; //double tps_; diff --git a/components/rgbd-sources/src/sources/stereovideo/pylon.cpp b/components/rgbd-sources/src/sources/stereovideo/pylon.cpp index 3156294ea042cadc0fff5ad6299582e196f99a6a..33acac1148e78eca9d1cf352a7630bd109885522 100644 --- a/components/rgbd-sources/src/sources/stereovideo/pylon.cpp +++ b/components/rgbd-sources/src/sources/stereovideo/pylon.cpp @@ -1,6 +1,7 @@ #include "pylon.hpp" -#include "calibrate.hpp" +#include "rectification.hpp" + #include <loguru.hpp> #include <ftl/threads.hpp> #include <ftl/rgbd/source.hpp> @@ -13,6 +14,7 @@ #include <nlohmann/json.hpp> +using ftl::rgbd::detail::StereoRectification; using ftl::rgbd::detail::PylonDevice; using std::string; using ftl::codecs::Channel; @@ -21,7 +23,7 @@ using cv::Mat; using namespace Pylon; PylonDevice::PylonDevice(nlohmann::json &config) - : ftl::rgbd::detail::Device(config), ready_(false), lcam_(nullptr), rcam_(nullptr) { + : ftl::rgbd::detail::Device(config), ready_(false), lcam_(nullptr), rcam_(nullptr) { auto &inst = CTlFactory::GetInstance(); @@ -60,7 +62,7 @@ PylonDevice::PylonDevice(nlohmann::json &config) if (dev_left_num == -1) dev_left_num = 0; try { - lcam_ = new CBaslerUniversalInstantCamera( CTlFactory::GetInstance().CreateDevice(devices[dev_left_num])); + lcam_ = new CBaslerUniversalInstantCamera( CTlFactory::GetInstance().CreateDevice(devices[dev_left_num])); lcam_->RegisterConfiguration( new Pylon::CSoftwareTriggerConfiguration, Pylon::RegistrationMode_ReplaceAll, Pylon::Cleanup_Delete); lcam_->Open(); @@ -80,7 +82,7 @@ PylonDevice::PylonDevice(nlohmann::json &config) ready_ = true; } catch (const Pylon::GenericException &e) { // Error handling. - LOG(ERROR) << "Pylon: An exception occurred - " << e.GetDescription(); + LOG(ERROR) << "Pylon: An exception occurred - " << e.GetDescription(); } // Choose a good default depth res @@ -171,7 +173,7 @@ bool PylonDevice::grab() { return true; } -bool PylonDevice::get(cv::cuda::GpuMat &l_out, cv::cuda::GpuMat &r_out, cv::cuda::GpuMat &h_l, cv::Mat &h_r, Calibrate *c, cv::cuda::Stream &stream) { +bool PylonDevice::get(cv::cuda::GpuMat &l_out, cv::cuda::GpuMat &r_out, cv::cuda::GpuMat &h_l, cv::Mat &h_r, StereoRectification *c, cv::cuda::Stream &stream) { if (!isReady()) return false; Mat l, r ,hres; @@ -213,7 +215,7 @@ bool PylonDevice::get(cv::cuda::GpuMat &l_out, cv::cuda::GpuMat &r_out, cv::cuda cv::cvtColor(wrap_right, rfull, cv::COLOR_BayerRG2BGRA); if (isStereo()) { - c->rectifyRight(rfull); + c->rectify(rfull, Channel::Right); if (hasHigherRes()) { cv::resize(rfull, r, r.size(), 0.0, 0.0, cv::INTER_CUBIC); @@ -249,7 +251,7 @@ bool PylonDevice::get(cv::cuda::GpuMat &l_out, cv::cuda::GpuMat &r_out, cv::cuda cv::cvtColor(wrap_left, lfull, cv::COLOR_BayerRG2BGRA); if (isStereo()) { - c->rectifyLeft(lfull); + c->rectify(lfull, Channel::Left); } if (hasHigherRes()) { @@ -273,6 +275,6 @@ bool PylonDevice::get(cv::cuda::GpuMat &l_out, cv::cuda::GpuMat &r_out, cv::cuda } bool PylonDevice::isReady() const { - return lcam_ && lcam_->IsOpen(); + return lcam_ && lcam_->IsOpen(); } diff --git a/components/rgbd-sources/src/sources/stereovideo/pylon.hpp b/components/rgbd-sources/src/sources/stereovideo/pylon.hpp index 84b0742fb3b141f0785f87de38bcb1b349b69675..7af8ad2bf15aca2905feb8a05f178cd2cb152e58 100644 --- a/components/rgbd-sources/src/sources/stereovideo/pylon.hpp +++ b/components/rgbd-sources/src/sources/stereovideo/pylon.hpp @@ -21,16 +21,16 @@ class PylonDevice : public ftl::rgbd::detail::Device { static std::vector<DeviceDetails> listDevices(); bool grab() override; - bool get(cv::cuda::GpuMat &l, cv::cuda::GpuMat &r, cv::cuda::GpuMat &h_l, cv::Mat &h_r, Calibrate *c, cv::cuda::Stream &stream) override; + bool get(cv::cuda::GpuMat &l, cv::cuda::GpuMat &r, cv::cuda::GpuMat &h_l, cv::Mat &h_r, StereoRectification *c, cv::cuda::Stream &stream) override; unsigned int width() const override { return width_; } unsigned int height() const override { return height_; }; unsigned int fullWidth() const override { return fullwidth_; } unsigned int fullHeight() const override { return fullheight_; } - + double getTimestamp() const override { return 0.0; } - + bool isStereo() const override { return lcam_ && rcam_; } bool isReady() const; diff --git a/components/rgbd-sources/src/sources/stereovideo/rectification.cpp b/components/rgbd-sources/src/sources/stereovideo/rectification.cpp new file mode 100644 index 0000000000000000000000000000000000000000..78c935eccef49bda930e445739dc5abdedae8478 --- /dev/null +++ b/components/rgbd-sources/src/sources/stereovideo/rectification.cpp @@ -0,0 +1,216 @@ +/* + * Copyright 2019 Nicolas Pope + */ + +#include <loguru.hpp> +#include <ftl/config.h> +#include <ftl/configuration.hpp> +#include <ftl/calibration/parameters.hpp> + +#include "rectification.hpp" +#include "ftl/exception.hpp" + +#include <opencv2/core.hpp> +#include <opencv2/core/utility.hpp> +#include <opencv2/imgproc.hpp> +#include <opencv2/calib3d.hpp> + +using ftl::rgbd::detail::StereoRectification; +using ftl::calibration::CalibrationData; +using ftl::codecs::Channel; + +StereoRectification::StereoRectification(nlohmann::json &config, cv::Size image_size) : + ftl::Configurable(config), image_resolution_(image_size), + enabled_(false), valid_(false), interpolation_(cv::INTER_LINEAR), + baseline_(0.0) { + +} + +void StereoRectification::setSize(cv::Size size) { + image_resolution_ = size; + calculateParameters(); +} + +void StereoRectification::setInterpolation(int interpolation) { + interpolation_ = interpolation; +} + +void StereoRectification::setEnabled(bool value) { + enabled_ = value; +} + +bool StereoRectification::enabled() { + return enabled_; +} + +bool StereoRectification::calibrated() { + return valid_; +} + +void StereoRectification::setCalibration(const CalibrationData &calib) { + + for (const auto &data : calib.data) { + if (data.channel == Channel::Left) { + calib_left_ = data; + } + else if (data.channel == Channel::Right) { + calib_right_ = data; + } + } + + calculateParameters(); +} + +void StereoRectification::calculateParameters() { + using namespace ftl::calibration; + // TODO: lock + valid_ = false; + bool valid = true; + valid_ &= !calib_left_.intrinsic.resolution.empty(); + valid_ &= !calib_right_.intrinsic.resolution.empty(); + valid_ &= validate::cameraMatrix(calib_left_.intrinsic.matrix()); + valid_ &= validate::cameraMatrix(calib_right_.intrinsic.matrix()); + if (!valid) { return; } + + // create temporary buffers + if (tmp_l_.size() != image_resolution_) { + //using cv::cuda::HostMem; + //tmp_l_ = HostMem(image_resolution_, CV_8UC4, HostMem::AllocType::PAGE_LOCKED); + tmp_l_ = cv::Mat(image_resolution_, CV_8UC4); + } + if (tmp_l_.size() != image_resolution_) { + //using cv::cuda::HostMem; + //tmp_r_ = HostMem(image_resolution_, CV_8UC4, HostMem::AllocType::PAGE_LOCKED); + tmp_r_ = cv::Mat(image_resolution_, CV_8UC4); + } + + cv::Mat K_l = calib_left_.intrinsic.matrix(); + cv::Mat K_r = calib_right_.intrinsic.matrix(); + cv::Mat dc_l = cv::Mat(calib_left_.intrinsic.distCoeffs); + cv::Mat dc_r = cv::Mat(calib_right_.intrinsic.distCoeffs); + + // scale if necessary + if (calib_left_.intrinsic.resolution != image_resolution_) { + K_l = scaleCameraMatrix(K_l, calib_left_.intrinsic.resolution, image_resolution_); + } + if (calib_right_.intrinsic.resolution != image_resolution_) { + K_r = scaleCameraMatrix(K_r, calib_right_.intrinsic.resolution, image_resolution_); + } + + // calculate rotation and translation from left to right using calibration + cv::Mat T_l = calib_left_.extrinsic.matrix(); + cv::Mat T_r = calib_left_.extrinsic.matrix(); + cv::Mat T = transform::inverse(T_r) * T_l; + cv::Mat R; + cv::Mat t; + transform::getRotationAndTranslation(T, R, t); + + // calculate rectification parameters + cv::stereoRectify( K_l, dc_l, K_r, dc_r, image_resolution_, + R, t, R_l_, R_r_, P_l_, P_r_, Q_, 0, 0); + + baseline_ = cv::norm(t); + + // for CPU remap, CV_16SC2 should give best performance + // https://docs.opencv.org/master/da/d54/group__imgproc__transform.html + cv::initUndistortRectifyMap(K_l, dc_l, R_l_, P_l_, image_resolution_, + CV_16SC2, map_l_.first, map_l_.second); + cv::initUndistortRectifyMap(K_r, dc_r, R_r_, P_r_, image_resolution_, + CV_16SC2, map_r_.first, map_r_.second); + + valid_ = true; +} + +void StereoRectification::rectify(cv::InputOutputArray im, Channel c) { + + if (!enabled_ || !valid_) { return; } + + if (im.size() != image_resolution_) { + auto sz = im.size(); + throw ftl::exception("Input has wrong size"); + } + if (im.isMat()) { + cv::Mat &in = im.getMatRef(); + if (c == Channel::Left) { + cv::remap(in, tmp_l_, map_l_.first, map_l_.second, interpolation_); + cv::swap(in, tmp_l_); + } + else if (c == Channel::Right) { + cv::remap(in, tmp_r_, map_r_.first, map_r_.second, interpolation_); + cv::swap(in, tmp_r_); + } + else { + throw ftl::exception("Bad channel for rectification"); + } + } + else if (im.isGpuMat()) { + throw ftl::exception("GPU rectification not implemented"); + } + else { + throw ftl::exception("Input not Mat/GpuMat"); + } +} + +cv::Mat StereoRectification::getPose(Channel c) { + using ftl::calibration::transform::inverse; + + if (enabled_ && valid_) { + cv::Mat T = cv::Mat::eye(4, 4, CV_64FC1); + if (c == Channel::Left) { + R_l_.copyTo(T(cv::Rect(0, 0, 3, 3))); + T = calib_left_.extrinsic.matrix() * inverse(T); + } + else if (c == Channel::Right) { + R_r_.copyTo(T(cv::Rect(0, 0, 3, 3))); + T = calib_right_.extrinsic.matrix() * inverse(T); + } + } + else { + if (c == Channel::Left) { + return calib_left_.extrinsic.matrix(); + } + else if (c == Channel::Right) { + return calib_right_.extrinsic.matrix(); + } + } + throw ftl::exception("Invalid channel, expected Left or Right"); +} + +double StereoRectification::baseline() { + return baseline_; +} + +double StereoRectification::doff() { + return -(Q_.at<double>(3,3) * baseline_); +} + +double StereoRectification::doff(cv::Size size) { + return doff() * double(size.width)/double(image_resolution_.width); +} + +cv::Mat StereoRectification::cameraMatrix(Channel c) { + if (enabled_ && valid_) { + if (c == Channel::Left) { + // P_l_: Left camera is origin in rectified system, there extrinsic + // is no rotation and intrinsic matrix can be directly extracted. + return cv::Mat(P_l_, cv::Rect(0, 0, 3, 3)).clone(); + } + else if (c == Channel::Right) { + // Extrinsics are included in P_r_, can't do same as above + throw ftl::exception("Not implemented"); + } + } + else { + if (c == Channel::Left) { + return calib_left_.intrinsic.matrix(); + } + else if (c == Channel::Right) { + return calib_right_.intrinsic.matrix(); + } + } + throw ftl::exception("Invalid channel, expected Left or Right"); +} + +cv::Mat StereoRectification::cameraMatrix(cv::Size size, Channel c) { + return ftl::calibration::scaleCameraMatrix(cameraMatrix(c), image_resolution_, size); +} diff --git a/components/rgbd-sources/src/sources/stereovideo/rectification.hpp b/components/rgbd-sources/src/sources/stereovideo/rectification.hpp new file mode 100644 index 0000000000000000000000000000000000000000..948f12c49f4fd066c1b5e818259626512118c309 --- /dev/null +++ b/components/rgbd-sources/src/sources/stereovideo/rectification.hpp @@ -0,0 +1,100 @@ +/* + * Copyright 2019 Nicolas Pope + */ + +#ifndef _FTL_CALIBRATION_HPP_ +#define _FTL_CALIBRATION_HPP_ + +#include <opencv2/core.hpp> +#include <opencv2/core/cuda.hpp> + +#include <string> +#include <vector> + +#include <ftl/codecs/channels.hpp> +#include <ftl/rgbd/camera.hpp> +#include <ftl/calibration/structures.hpp> + +namespace ftl { +namespace rgbd { +namespace detail { + +/** + * Stereo rectification. Performs rectification for left and right channels. + * Rectified image is same size as input image. + */ +class StereoRectification : public ftl::Configurable { +public: + StereoRectification(nlohmann::json &config, cv::Size image_size); + + void setInterpolation(int interpolation); + + void setSize(cv::Size); + /** + * Calculate rectification parameters from given calibration. + */ + void setCalibration(const ftl::calibration::CalibrationData &calib); + bool calibrated(); + + void rectify(cv::InputOutputArray im, ftl::codecs::Channel c); + + /** + * Enable/disable rectification. + */ + void setEnabled(bool enabled); + bool enabled(); + + /** + * Get camera pose (rectified if enabled and valid) + */ + cv::Mat getPose(ftl::codecs::Channel c = ftl::codecs::Channel::Left); + + /** + * Get intrinsic matrix. + */ + cv::Mat cameraMatrix(ftl::codecs::Channel c = ftl::codecs::Channel::Left); + cv::Mat cameraMatrix(cv::Size size, ftl::codecs::Channel c = ftl::codecs::Channel::Left); + + /** Stereo baseline */ + double baseline(); + /** Disparity offset */ + double doff(); + double doff(cv::Size); + +protected: + void calculateParameters(); + +private: + cv::Size calib_resolution_; + ftl::calibration::CalibrationData::Calibration calib_left_; + ftl::calibration::CalibrationData::Calibration calib_right_; + + cv::Size image_resolution_; + + // rectification parameters and maps + bool enabled_; + bool valid_; + int interpolation_; + double baseline_; + cv::Mat Q_; + cv::Mat R_l_; + cv::Mat R_r_; + cv::Mat P_l_; + cv::Mat P_r_; + + std::pair<cv::Mat,cv::Mat> map_l_; + std::pair<cv::Mat,cv::Mat> map_r_; + + // temporary buffers + // cv::cuda::HostMem tmp_l_; + // cv::cuda::HostMem tmp_r_; + cv::Mat tmp_l_; + cv::Mat tmp_r_; +}; + +} +} +} + +#endif // _FTL_CALIBRATION_HPP_ + diff --git a/components/rgbd-sources/src/sources/stereovideo/stereovideo.cpp b/components/rgbd-sources/src/sources/stereovideo/stereovideo.cpp index eadfefb7f3f4d3cc8e9d7597b99f3360d7ccab9b..815e426577533abf3ee83ca4c96e62ba72c77005 100644 --- a/components/rgbd-sources/src/sources/stereovideo/stereovideo.cpp +++ b/components/rgbd-sources/src/sources/stereovideo/stereovideo.cpp @@ -1,41 +1,40 @@ #include <loguru.hpp> +#include <unordered_set> + #include <Eigen/Eigen> #include <opencv2/core/eigen.hpp> -#include "stereovideo.hpp" - #include <ftl/configuration.hpp> #include <ftl/profiler.hpp> #include <nlohmann/json.hpp> #ifdef HAVE_OPTFLOW -#include "ftl/operators/opticalflow.hpp" +#include <ftl/operators/opticalflow.hpp> #endif -#include "ftl/operators/smoothing.hpp" -#include "ftl/operators/colours.hpp" -#include "ftl/operators/normals.hpp" -#include "ftl/operators/filling.hpp" -#include "ftl/operators/segmentation.hpp" -#include "ftl/operators/disparity.hpp" -#include "ftl/operators/mask.hpp" +#include <ftl/operators/smoothing.hpp> +#include <ftl/operators/colours.hpp> +#include <ftl/operators/normals.hpp> +#include <ftl/operators/filling.hpp> +#include <ftl/operators/segmentation.hpp> +#include <ftl/operators/disparity.hpp> +#include <ftl/operators/mask.hpp> #include <ftl/rgbd/capabilities.hpp> +#include <ftl/calibration/structures.hpp> +#include "stereovideo.hpp" #include "ftl/threads.hpp" -#include "calibrate.hpp" -#include "opencv.hpp" -#include <unordered_set> +#include "rectification.hpp" -#include <ftl/calibration/structures.hpp> +#include "opencv.hpp" #ifdef HAVE_PYLON #include "pylon.hpp" #endif -using ftl::rgbd::detail::Calibrate; using ftl::rgbd::detail::StereoVideoSource; using ftl::codecs::Channel; using std::string; @@ -67,7 +66,6 @@ StereoVideoSource::StereoVideoSource(ftl::rgbd::Source *host, const string &file } StereoVideoSource::~StereoVideoSource() { - delete calib_; delete lsrc_; } @@ -118,140 +116,40 @@ void StereoVideoSource::init(const string &file) { #endif pipeline_input_->append<ftl::operators::ColourChannels>("colour"); - calib_ = ftl::create<Calibrate>(host_, "calibration", cv::Size(lsrc_->fullWidth(), lsrc_->fullHeight()), stream_); + cv::Size size_full = cv::Size(lsrc_->fullWidth(), lsrc_->fullHeight()); + rectification_ = std::unique_ptr<StereoRectification> + (ftl::create<StereoRectification>(host_, "rectification", size_full)); string fname_default = "calibration.yml"; - auto fname_config = calib_->get<string>("calibration"); + auto fname_config = host_->get<string>("calibration"); string fname = fname_config ? *fname_config : fname_default; auto calibf = ftl::locateFile(fname); if (calibf) { - fname = *calibf; - if (calib_->loadCalibration(fname)) { - calib_->calculateRectificationParameters(); - calib_->setRectify(true); - } + fname_calib_ = *calibf; + calibration_ = ftl::calibration::CalibrationData::readFile(fname_calib_); } else { - fname = fname_config ? *fname_config : - string(FTL_LOCAL_CONFIG_ROOT) + "/" - + std::string("calibration.yml"); - - LOG(ERROR) << "No calibration, default path set to " + fname; + fname_calib_ = fname_config ? *fname_config : + string(FTL_LOCAL_CONFIG_ROOT) + "/" + + std::string("calibration.yml"); - // set use config file/set (some) default values - - cv::Mat K = cv::Mat::eye(cv::Size(3, 3), CV_64FC1); - K.at<double>(0,0) = host_->value("focal", 800.0); - K.at<double>(1,1) = host_->value("focal", 800.0); - K.at<double>(0,2) = host_->value("centre_x", color_size_.width/2.0f); - K.at<double>(1,2) = host_->value("centre_y", color_size_.height/2.0f); - - calib_->setIntrinsics(color_size_, {K, K}); + LOG(ERROR) << "No calibration file found, calibration will be saved to " + fname; } - fname_calib_ = fname; - - //////////////////////////////////////////////////////////////////////////// - // RPC callbacks to update calibration - // Should only be used by calibration app (interface may change) - // Tries to follow interface of ftl::Calibrate - - host_->getNet()->bind("set_pose", - [this](cv::Mat pose){ - if (!calib_->setPose(pose)) { - LOG(ERROR) << "invalid pose received (bad value)"; - return false; - } - do_update_params_ = true; - LOG(INFO) << "new pose"; - return true; - }); - - host_->getNet()->bind("set_pose_adjustment", - [this](cv::Mat T){ - if (!calib_->setPoseAdjustment(T)) { - LOG(ERROR) << "invalid pose received (bad value)"; - return false; - } - do_update_params_ = true; - LOG(INFO) << "new pose adjustment"; - return true; - }); - - - host_->getNet()->bind("set_intrinsics", - [this](cv::Size size, cv::Mat K_l, cv::Mat D_l, cv::Mat K_r, cv::Mat D_r) { - if (!calib_->setIntrinsics(size, {K_l, K_r})) { - LOG(ERROR) << "bad intrinsic parameters (bad values)"; - return false; - } - - if (!D_l.empty() && !D_r.empty()) { - if (!calib_->setDistortion({D_l, D_r})) { - LOG(ERROR) << "bad distortion parameters (bad values)"; - return false; - } - } - do_update_params_ = true; - LOG(INFO) << "new intrinsic parameters"; - return true; - }); - - host_->getNet()->bind("set_extrinsics", - [this](cv::Mat R, cv::Mat t){ - if (!calib_->setExtrinsics(R, t)) { - LOG(ERROR) << "invalid extrinsic parameters (bad values)"; - return false; - } - do_update_params_ = true; - LOG(INFO) << "new extrinsic (stereo) parameters"; - return true; - }); - - host_->getNet()->bind("save_calibration", - [this, fname](){ - LOG(INFO) << "saving calibration to " << fname; - return calib_->saveCalibration(fname); - }); - - host_->getNet()->bind("set_rectify", - [this](bool enable){ - bool retval = calib_->setRectify(enable); - do_update_params_ = true; - LOG(INFO) << "rectification " << (retval ? "on" : "off"); - return retval; - }); - - host_->getNet()->bind("get_distortion", [this]() { - return std::vector<cv::Mat>{ - cv::Mat(calib_->getCameraDistortionLeft()), - cv::Mat(calib_->getCameraDistortionRight()) }; - }); - - //////////////////////////////////////////////////////////////////////////// - - // Generate camera parameters from camera matrix - //updateParameters(); + // Generate camera parameters for next frame do_update_params_ = true; LOG(INFO) << "StereoVideo source ready..."; ready_ = true; - - //state_.set("name", host_->value("name", host_->getID())); } ftl::rgbd::Camera StereoVideoSource::parameters(Channel chan) { - /*if (chan == Channel::Right) { - return state_.getRight(); - } else { - return state_.getLeft(); - }*/ return params_; } void StereoVideoSource::updateParameters(ftl::rgbd::Frame &frame) { Eigen::Matrix4d pose; - cv::cv2eigen(calib_->getPose(), pose); + cv::cv2eigen(rectification_->getPose(Channel::Left), pose); frame.setPose() = pose; @@ -269,33 +167,40 @@ void StereoVideoSource::updateParameters(ftl::rgbd::Frame &frame) { } } - frame.create<ftl::calibration::CalibrationData>(Channel::CalibrationData) = {true}; // Seb: Add calib_->getData()... + frame.create<ftl::calibration::CalibrationData>(Channel::CalibrationData) = calibration_; calibration_change_ = frame.onChange(Channel::CalibrationData, [this] (ftl::data::Frame& frame, ftl::codecs::Channel) { - + auto &change = frame.get<ftl::calibration::CalibrationData>(Channel::CalibrationData); - change.toFile(fname_calib_); - calib_->setRectify(change.enabled); - calib_->setCalibration(change); + try { + change.writeFile(fname_calib_); + } + catch (...) { + LOG(ERROR) << "Saving calibration to file failed"; + } + + calibration_ = ftl::calibration::CalibrationData(change); + rectification_->setCalibration(calibration_); + rectification_->setEnabled(change.enabled); return true; }); cv::Mat K; // same for left and right - float baseline = static_cast<float>(calib_->getBaseline()); - float doff = calib_->getDoff(color_size_); + float baseline = static_cast<float>(rectification_->baseline()); + float doff = rectification_->doff(color_size_); double d_resolution = this->host_->getConfig().value<double>("depth_resolution", 0.0); float min_depth = this->host_->getConfig().value<double>("min_depth", 0.45); float max_depth = this->host_->getConfig().value<double>("max_depth", 12.0); // left - - K = calib_->getCameraMatrixLeft(color_size_); + + K = rectification_->cameraMatrix(color_size_); float fx = static_cast<float>(K.at<double>(0,0)); - + if (d_resolution > 0.0) { // Learning OpenCV p. 442 // TODO: remove, should not be used here @@ -315,16 +220,17 @@ void StereoVideoSource::updateParameters(ftl::rgbd::Frame &frame) { baseline, doff }; - + host_->getConfig()["focal"] = params_.fx; host_->getConfig()["centre_x"] = params_.cx; host_->getConfig()["centre_y"] = params_.cy; host_->getConfig()["baseline"] = params_.baseline; host_->getConfig()["doffs"] = params_.doffs; - + // right + // Not used anymore? - K = calib_->getCameraMatrixRight(color_size_); + K = rectification_->cameraMatrix(color_size_, Channel::Right); frame.setRight() = { static_cast<float>(K.at<double>(0,0)), // Fx static_cast<float>(K.at<double>(1,1)), // Fy @@ -346,7 +252,7 @@ bool StereoVideoSource::capture(int64_t ts) { bool StereoVideoSource::retrieve(ftl::rgbd::Frame &frame) { FTL_Profile("Stereo Retrieve", 0.03); - + if (do_update_params_) { updateParameters(frame); do_update_params_ = false; @@ -360,7 +266,7 @@ bool StereoVideoSource::retrieve(ftl::rgbd::Frame &frame) { if (lsrc_->isStereo()) { cv::cuda::GpuMat &left = frame.create<cv::cuda::GpuMat>(Channel::Left); cv::cuda::GpuMat &right = frame.create<cv::cuda::GpuMat>(Channel::Right); - if (!lsrc_->get(left, right, hres, hres_r, calib_, stream2_)) { + if (!lsrc_->get(left, right, hres, hres_r, rectification_.get(), stream2_)) { frame.remove(Channel::Left); frame.remove(Channel::Right); } @@ -368,7 +274,7 @@ bool StereoVideoSource::retrieve(ftl::rgbd::Frame &frame) { else { cv::cuda::GpuMat &left = frame.create<cv::cuda::GpuMat>(Channel::Left); cv::cuda::GpuMat right; - if (!lsrc_->get(left, right, hres, hres_r, calib_, stream2_)) { + if (!lsrc_->get(left, right, hres, hres_r, rectification_.get(), stream2_)) { frame.remove(Channel::Left); } } diff --git a/components/rgbd-sources/src/sources/stereovideo/stereovideo.hpp b/components/rgbd-sources/src/sources/stereovideo/stereovideo.hpp index 24ff7ef09b37b41ee9d7041974cd7469ac635bd4..db10d8cc5c4bf3c5c0c7b586c5727b390a89e4c8 100644 --- a/components/rgbd-sources/src/sources/stereovideo/stereovideo.hpp +++ b/components/rgbd-sources/src/sources/stereovideo/stereovideo.hpp @@ -4,7 +4,9 @@ #include "../../basesource.hpp" #include <ftl/operators/operator.hpp> +#include <ftl/calibration/structures.hpp> #include <string> +#include <memory> namespace ftl { @@ -12,15 +14,15 @@ namespace rgbd { namespace detail { class Device; -class Calibrate; +class StereoRectification; class Disparity; /** * RGBD source from either a stereo video file with left + right images, or - * direct from two camera devices. + * direct from two camera devices. */ class StereoVideoSource : public BaseSourceImpl { - public: +public: explicit StereoVideoSource(ftl::rgbd::Source*); StereoVideoSource(ftl::rgbd::Source*, const std::string &); ~StereoVideoSource(); @@ -31,11 +33,13 @@ class StereoVideoSource : public BaseSourceImpl { Camera parameters(ftl::codecs::Channel chan) override; - private: +private: void updateParameters(ftl::rgbd::Frame &); Device *lsrc_; - Calibrate *calib_; + std::unique_ptr<StereoRectification> rectification_; + ftl::calibration::CalibrationData calibration_; + int64_t capts_; cv::Size color_size_; @@ -49,12 +53,12 @@ class StereoVideoSource : public BaseSourceImpl { bool ready_; bool do_update_params_ = false; - + cv::cuda::Stream stream_; cv::cuda::Stream stream2_; cv::Mat mask_l_; - + ftl::Handle calibration_change_; std::string fname_calib_;