stereovideo and calibration: save pose

applications/calibration-multi/src/main.cpp

@@ -120,12 +120,11 @@ bool saveIntrinsics(const string &ofile, const vector<Mat> &M, const Size &size)
 	}
 }
 
-bool saveExtrinsics(const string &ofile, Mat &R, Mat &T, Mat &R1, Mat &R2, Mat &P1, Mat &P2, Mat &Q) {
+bool saveExtrinsics(const string &ofile, Mat &R, Mat &T, Mat &poseLeft, Mat &poseRight) {
 	cv::FileStorage fs;
 	fs.open(ofile, cv::FileStorage::WRITE);
 	if (fs.isOpened()) {
-		fs << "R" << R << "T" << T << "R1" << R1 << "R2" << R2 << "P1"
-			<< P1 << "P2" << P2 << "Q" << Q;
+		fs << "R" << R << "T" << T << "poseLeft" << poseLeft << "poseRight" << poseRight;
 		fs.release();
 		return true;
 	} else {
@@ -238,13 +237,16 @@ void calibrate(	MultiCameraCalibrationNew &calib, vector<string> &uri_cameras,
 		Mat R1, R2;
 		Mat R_c1c2, T_c1c2;
 		
+		Mat poseLeft = getMat4x4(R[c], t[c]);
+		Mat poseRight = getMat4x4(R[c + 1], t[c + 1]);
+
 		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);
 
 		// calculate extrinsics from rectified parameters
 		Mat _t = Mat(Size(1, 3), CV_64FC1, Scalar(0.0));
-		Rt_out[c] = getMat4x4(R[c], t[c]) * getMat4x4(R1, _t).inv();
-		Rt_out[c + 1] = getMat4x4(R[c + 1], t[c + 1]) * getMat4x4(R2, _t).inv();
+		Rt_out[c] = poseLeft * getMat4x4(R1, _t).inv();
+		Rt_out[c + 1] = poseRight * getMat4x4(R2, _t).inv();
 
 		{
 			string node_name;
@@ -258,7 +260,7 @@ void calibrate(	MultiCameraCalibrationNew &calib, vector<string> &uri_cameras,
 				//			on vision node. Consider saving extrinsic global
 				//			for node as well?
 				saveExtrinsics(	params.output_path + node_name + "-extrinsic.yml",
-								R_c1c2, T_c1c2, R1, R2, P1, P2, Q
+								R_c1c2, T_c1c2, poseLeft, poseRight
 				);
 				LOG(INFO) << "Saved: " << params.output_path + node_name + "-extrinsic.yml";
 			}

components/rgbd-sources/src/sources/stereovideo/calibrate.cpp

@@ -114,16 +114,14 @@ bool Calibrate::_loadCalibration(cv::Size img_size, std::pair<Mat, Mat> &map1, s
 		return false;
 	}
 
+	// TODO (calibration app already should support format)
+	//fs["poseLeft"] >> poseLeft_; 
+	//fs["poseRight"] >> poseRight_; 
+	poseLeft_ = cv::Mat::eye(cv::Size(4, 4), CV_64FC1);
+	poseRight_ = cv::Mat::eye(cv::Size(4, 4), CV_64FC1);
 	fs["R"] >> R_;
 	fs["T"] >> T_;
 	
-	/* re-calculate rectification from camera parameters
-	fs["R1"] >> R1_;
-	fs["R2"] >> R2_;
-	fs["P1"] >> P1_;
-	fs["P2"] >> P2_;
-	fs["Q"] >> Q_;
-	*/
 	fs.release();
 
 	img_size_ = img_size;

components/rgbd-sources/src/sources/stereovideo/calibrate.hpp

@@ -59,6 +59,8 @@ class Calibrate : public ftl::Configurable {
 	const cv::Mat &getCameraMatrixRight() { return Kr_; }
 	const cv::Mat &getCameraMatrix() { return getCameraMatrixLeft(); }
 
+	cv::Mat getPose() { return poseLeft_; }
+
 private:
 	void _updateIntrinsics();
 	bool _loadCalibration(cv::Size img_size, std::pair<cv::Mat, cv::Mat> &map1, std::pair<cv::Mat, cv::Mat> &map2);
@@ -84,6 +86,10 @@ private:
 	cv::Mat Kr_;
 
 	cv::Size img_size_;
+
+	// camera pose
+	cv::Mat poseLeft_;
+	cv::Mat poseRight_;
 };
 
 }

components/rgbd-sources/src/sources/stereovideo/stereovideo.cpp

 #include <loguru.hpp>
 
+#include <Eigen/Eigen>
+#include <opencv2/core/eigen.hpp>
+
 #include "stereovideo.hpp"
 
 #include "ftl/configuration.hpp"
@@ -68,6 +71,10 @@ void StereoVideoSource::init(const string &file) {
 	calib_ = ftl::create<Calibrate>(host_, "calibration", size, stream_);
 	if (!calib_->isCalibrated()) LOG(WARNING) << "Cameras are not calibrated!";
 
+	Eigen::Matrix4d pose;
+	cv::cv2eigen(calib_->getPose(), pose);
+	host_->setPose(pose);
+
 	// Generate camera parameters from camera matrix
 	cv::Mat K = calib_->getCameraMatrix();
 	params_ = {