diff --git a/applications/calibration-multi/src/main.cpp b/applications/calibration-multi/src/main.cpp index dbea22842d4b0a70546187f48e161def746af207..e08eb7bfddf31ccf795aa7d779ba962e1a1e842c 100644 --- a/applications/calibration-multi/src/main.cpp +++ b/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"; } diff --git a/components/rgbd-sources/src/sources/stereovideo/calibrate.cpp b/components/rgbd-sources/src/sources/stereovideo/calibrate.cpp index fc99d701fa40a8b824248c775a7020ed7d449fd1..a946f0fa2167f6e9184258c8ef9f57fe68f9dcb6 100644 --- a/components/rgbd-sources/src/sources/stereovideo/calibrate.cpp +++ b/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; diff --git a/components/rgbd-sources/src/sources/stereovideo/calibrate.hpp b/components/rgbd-sources/src/sources/stereovideo/calibrate.hpp index 4561b90a79129dd5a0d46d9d54bd005147d766a7..eb04f288c9cf1da2602b5c9221422dc02f35d84e 100644 --- a/components/rgbd-sources/src/sources/stereovideo/calibrate.hpp +++ b/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_; }; } diff --git a/components/rgbd-sources/src/sources/stereovideo/stereovideo.cpp b/components/rgbd-sources/src/sources/stereovideo/stereovideo.cpp index 0b0cd15a1a584e6811b6483fe36de7bde5293ce3..c668134f1c7d807381bf7a3f394ab2a91b9db00c 100644 --- a/components/rgbd-sources/src/sources/stereovideo/stereovideo.cpp +++ b/components/rgbd-sources/src/sources/stereovideo/stereovideo.cpp @@ -1,5 +1,8 @@ #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_ = {