update

Azure_Kinect_Lab/src/main.cpp

 #pragma comment(lib, "k4a.lib")
 #include <k4a/k4a.h>
 
-#include <iostream>  
+#include <iostream>
+#include <opencv2/opencv.hpp>
 #include <opencv2/core/core.hpp>  
 #include <opencv2/highgui/highgui.hpp>  
 
@@ -96,6 +97,7 @@ static bool point_cloud_color_to_depth(k4a_transformation_t transformation_handl
     const k4a_image_t depth_image,
     const k4a_image_t color_image,
     cv::Mat depth,
+    cv::Mat color_show,
     cv::Mat ir_show,
     k4a_calibration_t calibration,
     int ir_thr,
@@ -169,17 +171,36 @@ static bool point_cloud_color_to_depth(k4a_transformation_t transformation_handl
     float fy_d = calibration.depth_camera_calibration.intrinsics.parameters.param.fy;
     float cx_d = calibration.depth_camera_calibration.intrinsics.parameters.param.cx;
     float cy_d = calibration.depth_camera_calibration.intrinsics.parameters.param.cy;
+    float k1 = calibration.depth_camera_calibration.intrinsics.parameters.param.k1;
+    float k2 = calibration.depth_camera_calibration.intrinsics.parameters.param.k2;
+    float k3 = calibration.depth_camera_calibration.intrinsics.parameters.param.k3;
+    float p1 = calibration.depth_camera_calibration.intrinsics.parameters.param.p1;
+    float p2 = calibration.depth_camera_calibration.intrinsics.parameters.param.p2;
+    //
+    float fx_c = calibration.color_camera_calibration.intrinsics.parameters.param.fx;
+    float fy_c = calibration.color_camera_calibration.intrinsics.parameters.param.fy;
+    float cx_c = calibration.color_camera_calibration.intrinsics.parameters.param.cx;
+    float cy_c = calibration.color_camera_calibration.intrinsics.parameters.param.cy;
+    float k1c = calibration.color_camera_calibration.intrinsics.parameters.param.k1;
+    float k2c = calibration.color_camera_calibration.intrinsics.parameters.param.k2;
+    float k3c = calibration.color_camera_calibration.intrinsics.parameters.param.k3;
+    float p1c = calibration.color_camera_calibration.intrinsics.parameters.param.p1;
+    float p2c = calibration.color_camera_calibration.intrinsics.parameters.param.p2;
+    //
+    
     cv::Mat depth_color(_resolution_y, _resolution_x, CV_8UC3);
+    vector<CvPoint3D32f>pts_3d;
+    pts_3d.clear();
     for (int row = 0; row < depth.rows; ++row)
     {
         for (int col = 0; col < depth.cols; ++col)
         {
-
             CvPoint3D32f v;
             float z = depth.at<unsigned short>(row, col);
             if (z > 0)
             {
-                v.z = (float)(z) / 1000;
+                
+                v.z = (float)(z) / 1000; // 转换为单位米
                 v.x = (col - cx_d) * v.z / fx_d;
                 v.y = (row - cy_d) * v.z / fy_d;
                 //v.w() = 1;
@@ -187,7 +208,7 @@ static bool point_cloud_color_to_depth(k4a_transformation_t transformation_handl
                 float d_c = 0.6;
                 if ((v.z <= 0.6))
                 {
-                   
+                    pts_3d.push_back(v);
                     if (ir_show.at<unsigned char>(row, col) > ir_thr)
                     {
                         depth_color.at<cv::Vec3b>(row, col)[0] = c_show.at<cv::Vec4b>(row, col)[0];
@@ -207,7 +228,6 @@ static bool point_cloud_color_to_depth(k4a_transformation_t transformation_handl
                     depth_color.at<cv::Vec3b>(row, col)[1] = 0;
                     depth_color.at<cv::Vec3b>(row, col)[2] = 0;
                 }
-
             }
         }
     }
@@ -215,9 +235,77 @@ static bool point_cloud_color_to_depth(k4a_transformation_t transformation_handl
     namedWindow("depth_color", 0);
     imshow("depth_color", depth_color);
     char buf[256];
-    printf("img_id : %06d",id);
+    printf("img_id : %06d", id);
     sprintf(buf, "E:/git_quick/azure_kinect_lab/Azure_Kinect_Lab/datasets/%6d.jpg", id);
     imwrite(buf, depth_color);
+
+    //使用迭代器访问元素
+    printf("pts 3D size : %d\n", pts_3d.size());
+    cv::Mat color_distortion(depth_color.rows, depth_color.cols, CV_8UC3);
+    resize(color_show, color_distortion, color_distortion.size(), 0, 0, INTER_LINEAR);
+    vector<CvPoint3D32f>::iterator it;
+    //
+    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;
+    //
+    for (it = pts_3d.begin(); it != pts_3d.end(); it++)
+    {
+        CvPoint3D32f v = *it;
+        float Xd = v.x;
+        float Yd = v.y;
+        float Zd = v.z;
+
+        float x_p = Xd / Zd;
+        float y_p = Yd / Zd;
+        // 点云转为深度二维图
+        float x_depth = fx_d * x_p + cx_d;
+        float y_depth = fy_d * y_p + cy_d;
+        circle(depth_color, Point(x_depth, y_depth), 0, Scalar(88,255,88), -1);
+        //----------------------------------- 
+
+        cv::Mat pt_3D(4, 1, CV_32FC1);
+        pt_3D.at<float>(0, 0) = Xd;
+        pt_3D.at<float>(1, 0) = Yd;
+        pt_3D.at<float>(2, 0) = Zd;
+        pt_3D.at<float>(3, 0) = 1.;
+
+        cv::Mat pt3D_2_Color = depth2color * pt_3D; // Depth Camera 坐标系转为 RGB 坐标系
+        // 三维点云做 RGB 相机投影
+        float Xdc = pt3D_2_Color.at<float>(0, 0);
+        float Ydc = pt3D_2_Color.at<float>(1, 0);
+        float Zdc = pt3D_2_Color.at<float>(2, 0);
+       // printf("%f, %f , %f", pt3D_2_Color.at<float>(0, 0), pt3D_2_Color.at<float>(1, 0), pt3D_2_Color.at<float>(2, 0));
+
+        float x_pc = Xdc / Zdc;
+        float y_pc = Ydc / Zdc;
+        
+        //printf("(x,y,z) = (%.3f ,%.3f ,%.3f)\n", v.x, v.y, v.z);
+        float r2 = x_pc * x_pc + y_pc * y_pc;
+        float x_2pc = x_pc*(1. + k1c * r2 + k2c * r2 * r2 + k3c * r2 * r2 * r2) + 2. * p1c * x_pc * y_pc + p2c * (r2 + 2. * x_pc * x_pc);
+        float y_2pc = y_pc * (1. + k1c * r2 + k2c * r2 * r2 + k3c * r2 * r2 * r2) + 2. * p2c * x_pc * y_pc + p1c * (r2 + 2. * y_pc * y_pc);
+       // 
+        float x_color = fx_c * x_2pc + cx_c;
+        float y_color = fy_c * y_2pc + cy_c;
+        circle(color_show, Point(x_color, y_color), 0, Scalar(255, 88, 88), -1);
+    }
+    namedWindow("pt3d_2_depth_color", 0);
+    imshow("pt3d_2_depth_color", depth_color);
+    namedWindow("color_distortion", 0);
+    imshow("color_distortion", color_show);
+    
     //-------------------------------------------------
 
     k4a_image_release(transformed_color_image);
@@ -405,7 +493,7 @@ int main()
         }
         //-------------------------------------------
         // 彩色图映射到深度图
-        point_cloud_color_to_depth(transformation, depth_image, color_image, depth, ir_show_8u,calibration, ir_thr,img_id);
+        point_cloud_color_to_depth(transformation, depth_image, color_image, depth, color_show,ir_show_8u,calibration, ir_thr,img_id);
         img_id++;
         // releae image
         k4a_image_release(depth_image);