Select Git revision
extrinsic.cpp
-
Sebastian Hahta authoredSebastian Hahta authored
Code owners
Assign users and groups as approvers for specific file changes. Learn more.
extrinsic.cpp 11.04 KiB
#include "calibration.hpp"
#include "../../screen.hpp"
#include "../../widgets/popupbutton.hpp"
#include "../../views/calibration/extrinsicview.hpp"
#include <opencv2/calib3d.hpp>
#include <opencv2/aruco.hpp>
#include <opencv2/cudawarping.hpp>
#include <ftl/calibration/optimize.hpp>
#include <ftl/calibration/structures.hpp>
#include <ftl/threads.hpp>
#include <nanogui/entypo.h>
using ftl::gui2::Calibration;
using ftl::calibration::CalibrationData;
using ftl::codecs::Channel;
using ftl::data::FrameID;
using ftl::data::FrameSetPtr;
using ftl::gui2::ExtrinsicCalibration;
using ftl::calibration::CalibrationObject;
using ftl::calibration::ArUCoObject;
using ftl::calibration::transform::inverse;
using ftl::calibration::transform::getRotationAndTranslation;
void ExtrinsicCalibration::init() {
reset();
}
void ExtrinsicCalibration::reset() {
if(future_.valid()) { future_.wait(); }
state_ = ExtrinsicCalibration::State();
running_ = false;
fs_current_.reset();
fs_update_.reset();
state_.calib_object = std::unique_ptr<CalibrationObject>(new ArUCoObject(cv::aruco::DICT_6X6_100));
state_.calib.points().setObject(state_.calib_object->object());
state_.min_cameras = 2;
}
ExtrinsicCalibration::~ExtrinsicCalibration() {
if(future_.valid()) {
future_.wait();
}
}
void ExtrinsicCalibration::start(unsigned int fsid, std::vector<FrameID> sources) {
setCalibrationMode(false);
reset();
state_.cameras.reserve(sources.size()*2);
auto* filter = io->feed()->filter
(std::unordered_set<uint32_t>{fsid}, {Channel::Left, Channel::Right});
filter->on([this](const FrameSetPtr& fs){ return onFrameSet_(fs);});
while(fs_current_ == nullptr) {
auto fss = filter->getLatestFrameSets();
if (fss.size() == 1) { fs_current_ = fss.front(); }
}
for (auto id : sources) {
// stereo calibration
auto cl = CameraID(id.frameset(), id.source(), Channel::Left);
auto cr = CameraID(id.frameset(), id.source(), Channel::Right);
const auto& frame = (*fs_current_)[id.source()].cast<ftl::rgbd::Frame>();
auto sz = cv::Size((int) frame.getLeftCamera().width, (int) frame.getLeftCamera().height);
state_.cameras.push_back({cl, {}});
state_.cameras.push_back({cr, {}});
state_.calib.addStereoCamera(
CalibrationData::Intrinsic(getCalibration(cl).intrinsic, sz),
CalibrationData::Intrinsic(getCalibration(cr).intrinsic, sz));
}
// initialize last points structure; can't be resized while running (without
// mutex)
unsigned int npoints = state_.calib_object->object().size();
state_.points_prev.resize(state_.cameras.size());
for (unsigned int i = 0; i < state_.cameras.size(); i++) {
state_.points_prev[i] = std::vector<cv::Point2d>(npoints);
}
auto* view = new ftl::gui2::ExtrinsicCalibrationView(screen, this);
view->onClose([this, filter](){
running_ = false;
filter->remove();
if (fs_current_ == nullptr) { return; }
for (const auto camera : state_.cameras) {
setCalibrationMode((*fs_current_)[camera.id.source()], true);
}
});
state_.capture = true;
screen->setView(view);
}
CalibrationData::Calibration ExtrinsicCalibration::calibration(int c) {
return state_.calib.calibration(c);
}
bool ExtrinsicCalibration::onFrameSet_(const FrameSetPtr& fs) {
std::atomic_store(&fs_update_, fs);
screen->redraw();
bool all_good = true;
for (const auto& [id, channel] : state_.cameras) {
std::ignore = channel;
all_good &= checkFrame((*fs)[id.source()]);
}
//if (!all_good) { return true; }
if (!state_.capture) { return true; }
if (running_.exchange(true)) { return true; }
future_ = ftl::pool.push([this, fs = fs](int thread_id) {
cv::Mat K;
cv::Mat distCoeffs;
std::vector<cv::Point2d> points;
int count = 0;
for (unsigned int i = 0; i < state_.cameras.size(); i++) {
const auto& [id, calib] = state_.cameras[i];
if (!(*fs)[id.source()].hasChannel(id.channel)) { continue; }
points.clear();
const cv::cuda::GpuMat& im = (*fs)[id.source()].get<cv::cuda::GpuMat>(id.channel);
K = calib.intrinsic.matrix();
distCoeffs = calib.intrinsic.distCoeffs.Mat();
try {
int n = state_.calib_object->detect(im, points, K, distCoeffs);
if (n > 0) {
state_.calib.points().addPoints(i, points);
state_.points_prev[i] = points;
count++;
}
}
catch (std::exception& ex) {
LOG(ERROR) << ex.what();
}
}
if (count < state_.min_cameras) {
state_.calib.points().clear();
}
else {
state_.calib.points().next();
}
running_ = false;
});
return true;
}
bool ExtrinsicCalibration::hasFrame(int camera) {
const auto id = state_.cameras[camera].id;
return (std::atomic_load(&fs_current_).get() != nullptr) &&
((*fs_current_)[id.source()].hasChannel(id.channel));
}
const cv::cuda::GpuMat ExtrinsicCalibration::getFrame(int camera) {
const auto id = state_.cameras[camera].id;
return (*fs_current_)[id.source()].cast<ftl::rgbd::Frame>().get<cv::cuda::GpuMat>(id.channel);
}
const cv::cuda::GpuMat ExtrinsicCalibration::getFrameRectified(int c) {
if (running_ || state_.maps1.size() <= int(c)) {
return getFrame(c);
}
cv::cuda::GpuMat remapped;
cv::cuda::remap(getFrame(c), remapped, state_.maps1[c], state_.maps2[c], cv::INTER_LINEAR);
return remapped;
}
int ExtrinsicCalibration::cameraCount() {
return state_.cameras.size();
}
bool ExtrinsicCalibration::next() {
if (std::atomic_load(&fs_update_).get()) {
std::atomic_store(&fs_current_, fs_update_);
std::atomic_store(&fs_update_, {});
return true;
}
return false;
}
bool ExtrinsicCalibration::capturing() {
return state_.capture;
}
void ExtrinsicCalibration::setCapture(bool v) {
state_.capture = v;
}
std::vector<std::pair<std::string, unsigned int>> ExtrinsicCalibration::listFrameSets() {
auto framesets = io->feed()->listFrameSets();
std::vector<std::pair<std::string, unsigned int>> result;
result.reserve(framesets.size());
for (auto fsid : framesets) {
auto uri = io->feed()->getURI(fsid);
result.push_back({uri, fsid});
}
return result;
}
std::vector<std::pair<std::string, ftl::data::FrameID>> ExtrinsicCalibration::listSources(unsigned int fsid, bool all) {
std::vector<std::pair<std::string, FrameID>> cameras;
for (auto id : io->feed()->listFrames()) {
if (id.frameset() != fsid) { continue; }
if (all || io->feed()->availableChannels(id).count(Channel::CalibrationData)) {
auto name = io->feed()->getURI(id.frameset()) + "#" + std::to_string(id.source());
cameras.push_back({name, id});
}
}
return cameras;
}
std::vector<ExtrinsicCalibration::CameraID> ExtrinsicCalibration::cameras() {
std::vector<ExtrinsicCalibration::CameraID> res;
res.reserve(cameraCount());
for (const auto& camera : state_.cameras) {
res.push_back(camera.id);
}
return res;
}
bool ExtrinsicCalibration::isBusy() {
return running_;
}
void ExtrinsicCalibration::updateCalibration() {
auto fs = std::atomic_load(&fs_current_);
unsigned int nsources = fs->frames.size();
std::vector<CalibrationData> data(nsources);
std::vector<bool> update(nsources, false);
for (unsigned int i = 0; i < nsources; i++) {
data[i] = (*fs)[i].get<CalibrationData>(Channel::CalibrationData);
}
for (const auto& camera : state_.cameras) {
auto srcid = camera.id.source();
data[srcid].get(camera.id.channel) = camera.calib;
update[srcid] = true;
}
for (unsigned int i = 0; i < nsources; i++) {
setCalibration((*fs)[i], data[i]);
}
}
void ExtrinsicCalibration::updateCalibration(int c) {
}
void ExtrinsicCalibration::stereoRectify(int cl, int cr,
const CalibrationData::Calibration& l, const CalibrationData::Calibration& r) {
// TODO: validROI
auto size = l.intrinsic.resolution;
cv::Mat R, t, R1, R2, P1, P2, Q, map1, map2;
getRotationAndTranslation(
inverse(l.extrinsic.matrix()) * r.extrinsic.matrix(), R, t);
LOG(INFO) << t;
cv::stereoRectify(
l.intrinsic.matrix(), l.intrinsic.distCoeffs.Mat(),
r.intrinsic.matrix(), r.intrinsic.distCoeffs.Mat(), size,
R, t, R1, R2, P1, P2, Q, 0, 1.0);
cv::initUndistortRectifyMap(l.intrinsic.matrix(), l.intrinsic.distCoeffs.Mat(),
R1, P1, size, CV_32FC1, map1, map2);
state_.maps1[cl].upload(map1);
state_.maps2[cl].upload(map2);
cv::initUndistortRectifyMap(r.intrinsic.matrix(), r.intrinsic.distCoeffs.Mat(),
R1, P1, size, CV_32FC1, map1, map2);
state_.maps1[cr].upload(map1);
state_.maps2[cr].upload(map2);
}
void ExtrinsicCalibration::run() {
if (running_.exchange(true)) { return; }
future_ = ftl::pool.push([this](int id) {
try {
LOG(INFO) << "Before:"; // DEBUG INFO
for (int c = 0; c < cameraCount(); c += 2) {
auto t1 = state_.calib.calibration(c).extrinsic.tvec;
auto t2 = state_.calib.calibration(c + 1).extrinsic.tvec;
LOG(INFO) << "baseline (" << c << ", " << c + 1 << "): "
<< cv::norm(t1, t2);
}
state_.calib.run();
LOG(INFO) << "After:"; // DEBUG INFO
for (int c = 0; c < cameraCount(); c += 2) {
auto t1 = state_.calib.calibrationOptimized(c).extrinsic.tvec;
auto t2 = state_.calib.calibrationOptimized(c + 1).extrinsic.tvec;
LOG(INFO) << "baseline (" << c << ", " << c + 1 << "): "
<< cv::norm(t1, t2);
}
// Rectification maps for visualization; stereo cameras assumed
// if non-stereo cameras added visualization/grouping (by index)
// has to be different.
state_.maps1.resize(cameraCount());
state_.maps2.resize(cameraCount());
for (int c = 0; c < cameraCount(); c += 2) {
auto l = state_.calib.calibrationOptimized(c);
auto r = state_.calib.calibrationOptimized(c + 1);
stereoRectify(c, c + 1, l, r);
}
}
catch (std::exception &ex) {
LOG(ERROR) << ex.what();
}
running_ = false;
});
}
ftl::calibration::CalibrationData::Calibration ExtrinsicCalibration::getCalibration(CameraID id) {
if (fs_current_ == nullptr) {
throw ftl::exception("No frame");
}
auto calib = (*fs_current_)[id.source()].get<CalibrationData>(Channel::CalibrationData);
if (!calib.hasCalibration(id.channel)) {
throw ftl::exception("Calibration missing for requierd channel");
}
return calib.get(id.channel);
}
const std::vector<cv::Point2d>& ExtrinsicCalibration::previousPoints(int camera) {
// not really thread safe (but points_prev_ should not resize)
return state_.points_prev[camera];
}
int ExtrinsicCalibration::getFrameCount(int camera) {
return state_.calib.points().getCount(unsigned(camera));
}
// debug method: save state to file (msgpack)
void ExtrinsicCalibration::saveInput(const std::string& filename) {
LOG(WARNING) << "DISABLED";
return;
ftl::pool.push([this, filename](int){
do {
// calib must not be modified; would be better to have mutex here
state_.capture = false;
}
while(running_);
running_ = true;
try { state_.calib.toFile(filename);}
catch (std::exception& ex) { LOG(ERROR) << "Calib save failed " << ex.what(); }
running_ = false;
});
}
// debug method: load state from file (msgpack)
void ExtrinsicCalibration::loadInput(const std::string& filename) { ftl::pool.push([this, filename](int){
do {
// calib must not be modified; would be better to have mutex here
state_.capture = false;
}
while(running_);
running_ = true;
try { state_.calib.fromFile(filename); }
catch (std::exception& ex) { LOG(ERROR) << "Calib load failed: " << ex.what(); }
running_ = false;
});
}