From d0c10bb216403cf121e3e61c904ee0bdb71f7d19 Mon Sep 17 00:00:00 2001
From: Sebastian Hahta <joseha@utu.fi>
Date: Tue, 8 Oct 2019 10:10:42 +0300
Subject: [PATCH] additional (debug) output
---
applications/calibration-multi/src/main.cpp | 47 +++++++++++++++------
1 file changed, 33 insertions(+), 14 deletions(-)
diff --git a/applications/calibration-multi/src/main.cpp b/applications/calibration-multi/src/main.cpp
index 8549bd735..bee370d4c 100644
--- a/applications/calibration-multi/src/main.cpp
+++ b/applications/calibration-multi/src/main.cpp
@@ -39,7 +39,7 @@ using ftl::net::Universe;
using ftl::rgbd::Source;
using ftl::rgbd::Channel;
-Mat getCameraMatrix(const ftl::rgbd::Camera ¶meters) {
+Mat createCameraMatrix(const ftl::rgbd::Camera ¶meters) {
Mat m = (cv::Mat_<double>(3,3) << parameters.fx, 0.0, -parameters.cx, 0.0, parameters.fy, -parameters.cy, 0.0, 0.0, 1.0);
return m;
}
@@ -173,7 +173,7 @@ void visualizeCalibration( MultiCameraCalibrationNew &calib, Mat &out,
Scalar(64, 255, 64),
Scalar(64, 255, 64),
};
-
+
vector<int> markers = {cv::MARKER_SQUARE, cv::MARKER_DIAMOND};
for (size_t c = 0; c < rgb.size(); c++) {
@@ -183,8 +183,12 @@ void visualizeCalibration( MultiCameraCalibrationNew &calib, Mat &out,
for (int r = 50; r < rgb[c].rows; r = r+50) {
cv::line(rgb[c], cv::Point(0, r), cv::Point(rgb[c].cols-1, r), cv::Scalar(0,0,255), 1);
}
- }
+ cv::putText(rgb[c],
+ "Camera " + std::to_string(c),
+ Point2i(roi[c].x + 10, roi[c].y + 30),
+ cv::FONT_HERSHEY_COMPLEX_SMALL, 1.0, Scalar(64, 64, 255), 1);
+ }
stack(rgb, out);
}
@@ -233,10 +237,19 @@ void calibrate( MultiCameraCalibrationNew &calib, vector<string> &uri_cameras,
Mat P1, P2, Q;
Mat R1, R2;
Mat R_c1c2, T_c1c2;
-
+ LOG(INFO) << K1;
+ LOG(INFO) << K2;
+
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);
+ Mat rvec;
+ cv::Rodrigues(R_c1c2, rvec);
+ LOG(INFO) << "From camera " << c << " to " << c + 1;
+ LOG(INFO) << "rotation: " << rvec.t();
+ LOG(INFO) << "translation: " << T_c1c2.t();
+
+ // 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();
@@ -247,19 +260,23 @@ void calibrate( MultiCameraCalibrationNew &calib, vector<string> &uri_cameras,
size_t pos2 = uri_cameras[c/2].find("#", pos1);
node_name = uri_cameras[c/2].substr(pos1, pos2 - pos1);
- if (params.save_extrinsic) {
- // TODO: only R and T required, rectification performed on vision node,
- // consider saving global extrinsic calibration?
- saveExtrinsics(params.output_path + node_name + "-extrinsic.yml", R_c1c2, T_c1c2, R1, R2, P1, P2, Q);
+ if (params.save_extrinsic)
+ {
+ // TODO: only R and T required when rectification performed
+ // 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
+ );
LOG(INFO) << "Saved: " << params.output_path + node_name + "-extrinsic.yml";
}
- if (params.save_intrinsic) {
+
+ if (params.save_intrinsic)
+ {
saveIntrinsics(
params.output_path + node_name + "-intrinsic.yml",
- {calib.getCameraMat(c),
- calib.getCameraMat(c + 1)},
+ { calib.getCameraMat(c), calib.getCameraMat(c + 1) },
params.size
-
);
LOG(INFO) << "Saved: " << params.output_path + node_name + "-intrinsic.yml";
}
@@ -268,6 +285,8 @@ void calibrate( MultiCameraCalibrationNew &calib, vector<string> &uri_cameras,
// for visualization
Size new_size;
cv::stereoRectify(K1, D1, K2, D2, params.size, R_c1c2, T_c1c2, R1, R2, P1, P2, Q, 0, 1.0, new_size, &roi[c], &roi[c + 1]);
+ //roi[c] = cv::Rect(0, 0, params.size.width, params.size.height);
+ //roi[c+1] = cv::Rect(0, 0, params.size.width, params.size.height);
cv::initUndistortRectifyMap(K1, D1, R1, P1, params.size, CV_16SC2, map1[c], map2[c]);
cv::initUndistortRectifyMap(K2, D2, R2, P2, params.size, CV_16SC2, map1[c + 1], map2[c + 1]);
}
@@ -393,11 +412,11 @@ void runCameraCalibration( ftl::Configurable* root,
CHECK(params.size == Size(camera_l.width, camera_l.height));
Mat K;
- K = getCameraMatrix(camera_r);
+ K = createCameraMatrix(camera_r);
LOG(INFO) << "K[" << 2 * i + 1 << "] = \n" << K;
calib.setCameraParameters(2 * i + 1, K);
- K = getCameraMatrix(camera_l);
+ K = createCameraMatrix(camera_l);
LOG(INFO) << "K[" << 2 * i << "] = \n" << K;
calib.setCameraParameters(2 * i, K);
}
--
GitLab