extrinsic direction and bug in intrinsic

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,15 +78,13 @@ 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) {
@@ -96,26 +94,28 @@ void IntrinsicCalibration::setChannel(Channel channel) {
 }
 
 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()

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));

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(5);
+		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);

components/streams/src/feed.cpp

@@ -730,7 +730,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 {};
 }