From 173178243e99a1466b50f9023b3e9b489cd3799e Mon Sep 17 00:00:00 2001
From: "Eric.L" <xiang.li@ivglass.com>
Date: Tue, 17 Aug 2021 19:38:49 +0800
Subject: [PATCH] update

---
 Azure_Kinect_Lab/src/main.cpp | 51 +++++++++++++++++++++++++++++++++--
 1 file changed, 49 insertions(+), 2 deletions(-)

diff --git a/Azure_Kinect_Lab/src/main.cpp b/Azure_Kinect_Lab/src/main.cpp
index 9bcfb7d..12973c8 100644
--- a/Azure_Kinect_Lab/src/main.cpp
+++ b/Azure_Kinect_Lab/src/main.cpp
@@ -199,7 +199,6 @@ static bool point_cloud_color_to_depth(k4a_transformation_t transformation_handl
             float z = depth.at<unsigned short>(row, col);
             if (z > 0)
             {
-                
                 v.z = (float)(z) / 1000; // 转换为单位米
                 v.x = (col - cx_d) * v.z / fx_d;
                 v.y = (row - cy_d) * v.z / fy_d;
@@ -260,6 +259,8 @@ static bool point_cloud_color_to_depth(k4a_transformation_t transformation_handl
     depth2color.at<float>(2, 1) = calibration.color_camera_calibration.extrinsics.rotation[7];
     depth2color.at<float>(2, 2) = calibration.color_camera_calibration.extrinsics.rotation[8];
     depth2color.at<float>(2, 3) = calibration.color_camera_calibration.extrinsics.translation[2] / 1000;
+
+
     //
     for (it = pts_3d.begin(); it != pts_3d.end(); it++)
     {
@@ -421,7 +422,46 @@ int main()
     fprintf(fpWrite, "%f,%f,%f,%f,%f,%f,%f,%f,%f\n", fx_d,fy_d,cx_d,cy_d,k1,k2,k3,p1,p2);
     fprintf(fpWrite, "%f,%f,%f,%f,%f,%f,%f,%f,%f\n", fx_c, fy_c, cx_c, cy_c, k1c, k2c, k3c, p1c, p2c);
     fclose(fpWrite);
-   
+
+    //----------------------- 记录外参
+    cv::Mat depth2color(3, 4, CV_32FC1);
+    depth2color.at<float>(0, 0) = calibration.color_camera_calibration.extrinsics.rotation[0];
+    depth2color.at<float>(0, 1) = calibration.color_camera_calibration.extrinsics.rotation[1];
+    depth2color.at<float>(0, 2) = calibration.color_camera_calibration.extrinsics.rotation[2];
+    depth2color.at<float>(0, 3) = calibration.color_camera_calibration.extrinsics.translation[0] / 1000;
+    //depth2color(0, 3) = calib.color_camera_calibration.extrinsics.translation[0];
+    depth2color.at<float>(1, 0) = calibration.color_camera_calibration.extrinsics.rotation[3];
+    depth2color.at<float>(1, 1) = calibration.color_camera_calibration.extrinsics.rotation[4];
+    depth2color.at<float>(1, 2) = calibration.color_camera_calibration.extrinsics.rotation[5];
+    depth2color.at<float>(1, 3) = calibration.color_camera_calibration.extrinsics.translation[1] / 1000;
+    //depth2color(1, 3) = calib.color_camera_calibration.extrinsics.translation[1];
+    depth2color.at<float>(2, 0) = calibration.color_camera_calibration.extrinsics.rotation[6];
+    depth2color.at<float>(2, 1) = calibration.color_camera_calibration.extrinsics.rotation[7];
+    depth2color.at<float>(2, 2) = calibration.color_camera_calibration.extrinsics.rotation[8];
+    depth2color.at<float>(2, 3) = calibration.color_camera_calibration.extrinsics.translation[2] / 1000;
+
+    FILE* fpWriteRT = fopen("E:/git_quick/azure_kinect_lab/Azure_Kinect_Lab/Depth2RGB_RT.txt", "w+");
+    if (fpWrite == NULL)
+    {
+        return 0;
+    }
+    // 记录 相机标定参数
+    fprintf(fpWriteRT, "%f,%f,%f,%f\n",
+        depth2color.at<float>(0, 0),
+        depth2color.at<float>(0, 1),
+        depth2color.at<float>(0, 2),
+        depth2color.at<float>(0, 3));
+    fprintf(fpWriteRT, "%f,%f,%f,%f\n",
+        depth2color.at<float>(1, 0),
+        depth2color.at<float>(1, 1),
+        depth2color.at<float>(1, 2),
+        depth2color.at<float>(1, 3));
+    fprintf(fpWriteRT, "%f,%f,%f,%f\n",
+        depth2color.at<float>(2, 0),
+        depth2color.at<float>(2, 1),
+        depth2color.at<float>(2, 2),
+        depth2color.at<float>(2, 3));
+    fclose(fpWriteRT);
     // ----------------------
     // Camera capture and application specific code would go here
     while (true) {
@@ -502,6 +542,9 @@ int main()
         namedWindow("color_show", 0);
         imshow("color_show", color_show);
 
+        sprintf(buf, "E:/git_quick/azure_kinect_lab/Azure_Kinect_Lab/datasets/%06d-color.jpg", img_id);
+        imwrite(buf, color_show);
+
         _resolution_y = k4a_image_get_height_pixels(ir_image);
         _resolution_x = k4a_image_get_width_pixels(ir_image);
         cv::Mat ir_show(_resolution_y, _resolution_x, CV_16UC1);
@@ -513,6 +556,10 @@ int main()
         namedWindow("ir_show", 0);
         imshow("ir_show", ir_show_8u);
 
+        sprintf(buf, "E:/git_quick/azure_kinect_lab/Azure_Kinect_Lab/datasets/%06d-ir.jpg", img_id);
+        imwrite(buf, ir_show_8u);
+
+
         int ir_thr = 25; // 用作三维点云分割辅助使用 ir 阈值
 
         if (waitKey(10) == 27){
-- 
GitLab