Skip to content
Snippets Groups Projects
Select Git revision
  • feature/SKR
  • master default protected
  • censuseval
  • exp/labcolours
  • feature/disconflow
  • exp/multires-sgm
  • chromatest
  • bug/feedrecord
  • feature/375/fullres-fstream
  • feature/poses
  • feature/stereocalib
  • feature/gui2-pch
  • feature/gui2-nanogui-mitsuba
  • feature/use-new-frame
  • feature/depth-touch
  • feature/use10bit
  • bug/optflow-disparity
  • feature/websocket-pose
  • exp/candidatemask
  • feature/sgm-experimental
  • v0.0.6
  • v0.0.5
  • v0.0.4
  • v0.0.3
  • v0.0.2
  • v0.0.1
26 results

extrinsic.cpp

Blame
  • 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;
    	});
    }