diff --git a/Azure_Kinect_Lab/src/main.cpp b/Azure_Kinect_Lab/src/main.cpp index 2ee5e7dcf00a0d3b8823a1da3c821c0a5ed88cbe..a7625947e60d5f3a5fdee54040121433942e7b6e 100644 --- a/Azure_Kinect_Lab/src/main.cpp +++ b/Azure_Kinect_Lab/src/main.cpp @@ -1,7 +1,8 @@ #pragma comment(lib, "k4a.lib") #include -#include +#include +#include #include #include @@ -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); + vectorpts_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(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(row, col) > ir_thr) { depth_color.at(row, col)[0] = c_show.at(row, col)[0]; @@ -207,7 +228,6 @@ static bool point_cloud_color_to_depth(k4a_transformation_t transformation_handl depth_color.at(row, col)[1] = 0; depth_color.at(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::iterator it; + // + cv::Mat depth2color(3, 4, CV_32FC1); + depth2color.at(0, 0) = calibration.color_camera_calibration.extrinsics.rotation[0]; + depth2color.at(0, 1) = calibration.color_camera_calibration.extrinsics.rotation[1]; + depth2color.at(0, 2) = calibration.color_camera_calibration.extrinsics.rotation[2]; + depth2color.at(0, 3) = calibration.color_camera_calibration.extrinsics.translation[0] / 1000; + //depth2color(0, 3) = calib.color_camera_calibration.extrinsics.translation[0]; + depth2color.at(1, 0) = calibration.color_camera_calibration.extrinsics.rotation[3]; + depth2color.at(1, 1) = calibration.color_camera_calibration.extrinsics.rotation[4]; + depth2color.at(1, 2) = calibration.color_camera_calibration.extrinsics.rotation[5]; + depth2color.at(1, 3) = calibration.color_camera_calibration.extrinsics.translation[1] / 1000; + //depth2color(1, 3) = calib.color_camera_calibration.extrinsics.translation[1]; + depth2color.at(2, 0) = calibration.color_camera_calibration.extrinsics.rotation[6]; + depth2color.at(2, 1) = calibration.color_camera_calibration.extrinsics.rotation[7]; + depth2color.at(2, 2) = calibration.color_camera_calibration.extrinsics.rotation[8]; + depth2color.at(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(0, 0) = Xd; + pt_3D.at(1, 0) = Yd; + pt_3D.at(2, 0) = Zd; + pt_3D.at(3, 0) = 1.; + + cv::Mat pt3D_2_Color = depth2color * pt_3D; // Depth Camera 坐标系转为 RGB 坐标系 + // 三维点云做 RGB 相机投影 + float Xdc = pt3D_2_Color.at(0, 0); + float Ydc = pt3D_2_Color.at(1, 0); + float Zdc = pt3D_2_Color.at(2, 0); + // printf("%f, %f , %f", pt3D_2_Color.at(0, 0), pt3D_2_Color.at(1, 0), pt3D_2_Color.at(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);