diff --git a/applications/gui2/src/modules/calibration/extrinsic.cpp b/applications/gui2/src/modules/calibration/extrinsic.cpp index 2a2e20565207f2853810ae0a77958423e4fb1ca0..a14e7323864ef93646f79afdcb37fe38981eca30 100644 --- a/applications/gui2/src/modules/calibration/extrinsic.cpp +++ b/applications/gui2/src/modules/calibration/extrinsic.cpp @@ -52,6 +52,7 @@ ExtrinsicCalibration::~ExtrinsicCalibration() { void ExtrinsicCalibration::start(unsigned int fsid, std::vector<FrameID> sources) { setCalibrationMode(false); + reset(); cameras_.clear(); cameras_.reserve(sources.size()*2); diff --git a/applications/gui2/src/modules/calibration/intrinsic.cpp b/applications/gui2/src/modules/calibration/intrinsic.cpp index 2a39b8ceced33841b6d78d16c520df9b853b59ac..44b37bef1708fbd6e39beef6d974d9f0acc9a166 100644 --- a/applications/gui2/src/modules/calibration/intrinsic.cpp +++ b/applications/gui2/src/modules/calibration/intrinsic.cpp @@ -55,16 +55,16 @@ void IntrinsicCalibration::reset() { void IntrinsicCalibration::start(ftl::data::FrameID id) { reset(); setCalibrationMode(false); + state_->id = id; - auto* view = new ftl::gui2::IntrinsicCalibrationView(screen, this); auto* filter = io->feed()->filter (std::unordered_set<uint32_t>{id.frameset()}, {Channel::Left, Channel::Right}); - filter->on([this](const FrameSetPtr& fs){ return onFrame_(fs); }); + auto* view = new ftl::gui2::IntrinsicCalibrationView(screen, this); view->onClose([filter, this](){ // if calib_ caches images, also reset() here! filter->remove(); @@ -78,44 +78,44 @@ void IntrinsicCalibration::start(ftl::data::FrameID id) { screen->setView(view); - for (auto fs : filter->getLatestFrameSets()) { - if (!(fs->frameset() == state_->id.frameset()) || - !(fs->hasFrame(state_->id.source()))) { continue; } - - // read calibration channel and set state_->channel_alt to high res if available - setChannel_(fs); - break; + while(fs_current_ == nullptr) { + auto fss = filter->getLatestFrameSets(); + if (fss.size() == 1) { fs_current_ = fss.front(); } } + auto fs = std::atomic_load(&fs_current_); + setChannel_(fs); } void IntrinsicCalibration::setChannel(Channel channel) { - state_->channel= channel; + state_->channel = channel; auto fs = std::atomic_load(&fs_current_); setChannel_(fs); } void IntrinsicCalibration::setChannel_(FrameSetPtr fs) { + // reset points, find if high res available and find correct resolution + + state_->calib = CalibrationData::Intrinsic(); + state_->points.clear(); + state_->points_object.clear(); + state_->count = 0; + state_->channel_alt = state_->channel; - if (fs == nullptr) { return; } + if (fs == nullptr) { + LOG(ERROR) << "No frame, calibration not loaded"; + } auto& frame = (*fs)[state_->id.source()].cast<ftl::rgbd::Frame>(); + if ((state_->channel== Channel::Left) && frame.has(Channel::LeftHighRes)) { state_->channel_alt = Channel::LeftHighRes; } if ((state_->channel== Channel::Right) && frame.has(Channel::RightHighRes)) { state_->channel_alt = Channel::RightHighRes; } - cv::Size size = getGpuMat(frame, state_->channel_alt).size(); - if (size == cv::Size{0, 0}) { - // FIXME: - if (state_->channel_alt == Channel::RightHighRes) { - size = getGpuMat(frame, Channel::LeftHighRes).size(); - } - else { - size = getGpuMat(frame, Channel::Left).size(); - } - } + + auto size = frame.get<cv::Mat>(state_->channel_alt).size(); try { auto calib = frame.get<CalibrationData>(Channel::CalibrationData); @@ -124,6 +124,9 @@ void IntrinsicCalibration::setChannel_(FrameSetPtr fs) { state_->calib = CalibrationData::Intrinsic(intrinsic, size); state_->calibrated = true; } + else { + state_->calib.resolution = size; + } } catch (std::exception& ex) { LOG(ERROR) << "Could not read calibration: " << ex.what() diff --git a/applications/gui2/src/views/calibration/intrinsicview.cpp b/applications/gui2/src/views/calibration/intrinsicview.cpp index b7bb033e819b305de88fa9fb3e23bdead6eadc14..d14ff335f908f9cc00b5ff6754d732c65a47eb21 100644 --- a/applications/gui2/src/views/calibration/intrinsicview.cpp +++ b/applications/gui2/src/views/calibration/intrinsicview.cpp @@ -208,20 +208,20 @@ IntrinsicCalibrationView::CaptureWindow::CaptureWindow(nanogui::Widget* parent, auto* button_left = new nanogui::Button(channels_, "Left"); button_left->setPushed(ctrl_->channel() == Channel::Left); button_left->setFlags(nanogui::Button::RadioButton); - button_left->setCallback([ctrl = ctrl_](){ + button_left->setCallback([ctrl = ctrl_, view=view_](){ if (ctrl->channel() != Channel::Left) { - ctrl->reset(); ctrl->setChannel(Channel::Left); + view->setUndistort(false); } }); auto* button_right = new nanogui::Button(channels_, "Right"); button_right->setFlags(nanogui::Button::RadioButton); button_right->setPushed(ctrl_->channel() == Channel::Right); - button_right->setCallback([ctrl = ctrl_](){ + button_right->setCallback([ctrl = ctrl_, view=view_](){ if (ctrl->channel() != Channel::Right) { - ctrl->reset(); ctrl->setChannel(Channel::Right); + view->setUndistort(false); } }); button_right->setEnabled(ctrl_->hasChannel(Channel::Right)); diff --git a/components/calibration/src/extrinsic.cpp b/components/calibration/src/extrinsic.cpp index ef0319c8b23ea57b67960e538e1ce19729d3472c..4ab4452e157a2426393ef0bdfb5e484ab98bc77d 100644 --- a/components/calibration/src/extrinsic.cpp +++ b/components/calibration/src/extrinsic.cpp @@ -491,7 +491,13 @@ void ExtrinsicCalibration::optimize() { unsigned int ncameras = calib_.size(); for (const auto& c : calib_) { - cameras.push_back(Camera(c)); + cv::Mat K = c.intrinsic.matrix(); + cv::Mat distCoeffs = c.intrinsic.distCoeffs.Mat(3); + cv::Mat rmat = c.extrinsic.rmat(); + cv::Mat tvec(c.extrinsic.tvec); + transform::inverse(rmat, tvec); // Ceres direction + cv::Size size = c.intrinsic.resolution; + cameras.push_back(Camera(K, distCoeffs, rmat, tvec, size)); } for (auto &c : cameras ) { ba.addCamera(c); @@ -585,7 +591,7 @@ void ExtrinsicCalibration::run() { updateStatus_("Starting"); calculatePairPoses(); calculateInitialPoses(); - optimize(); + //optimize(); } diff --git a/components/streams/src/feed.cpp b/components/streams/src/feed.cpp index 7e43eab80a6b2a12038657ac5cef4ad861c103be..e2215e94603af31c9f16624821778cd62b975bc1 100644 --- a/components/streams/src/feed.cpp +++ b/components/streams/src/feed.cpp @@ -732,7 +732,7 @@ const std::unordered_set<Channel> Feed::availableChannels(ftl::data::FrameID id) ftl::data::FrameSetPtr fs; std::atomic_store(&fs, latest_[id.frameset()]); if (fs && fs->hasFrame(id.source())) { - return (*fs.get())[id.source()].available(); + return (*fs.get())[id.source()].allChannels(); } return {}; }