Realsense D435i 深度相机获取距离角度坐标，C2D，D2C，相机标定，坐标系转换，ROS2

思路

C2D得到大概的目标点在深度像素坐标系下的坐标，然后对这一圈的深度进行处理

深度，角度，坐标，我都放到参数指针Ball_par里面了，这个函数会返回深度图像上取值的那一圈上色后的图片

效果

源码

懒的写注释和解析了，可读性和可移植性也不算很高，有什么不理解的地方可以和我交流，看见我会回复的

class Ball
{
public:
    double dep, probability, angle;
    Rect img_Rect;
    Eigen::Vector3d cam_pos;
};
     Mat Gain_Ball_par(cv_bridge::CvImage last_bgr,
                      cv_bridge::CvImage Depth_img,
                      target_bbox_msgs::msg::BoundingBoxes boxes,
                      Matrix3d dep_camera_matrix,
                      Matrix3d bgr_camera_matrix,
                      Matrix4d c2d_outer_matrix,
                      Matrix4d d2c_outer_matrix,
                      vector<Ball> &Ball_par)
    {
        // BGR ：( 0 , 0 )   ( 1280 , 720 )
        // Dep :( 50, 93 )   ( 584 , 393 )
        Eigen::Vector3d Depth_i, BGR_i;
        MatrixXd Depth_c_tran = MatrixXd::Constant(4, 1, 1.0);
        vector<double> dep_vec;
        vector<Point2i> target_pos_vec;
        Mat Depth_BGR(480, 640, CV_8UC3, cv::Scalar(0, 0, 0));

        MatrixXd bgr_camera_matrix_inverse = MatrixXd::Constant(4, 3, 0.0);
        bgr_camera_matrix_inverse.block<3, 3>(0, 0) = bgr_camera_matrix.inverse();
        MatrixXd bgr_camera_matrix_uninverse = MatrixXd::Constant(3, 4, 0.0);
        bgr_camera_matrix_uninverse.block<3, 3>(0, 0) = bgr_camera_matrix;
        MatrixXd dep_camera_matrix_uninverse = MatrixXd::Constant(3, 4, 1.0);
        dep_camera_matrix_uninverse.block<3, 3>(0, 0) = dep_camera_matrix;

        for (const auto &ball : Ball_par)
        {
            Depth_i = dep_camera_matrix_uninverse * c2d_outer_matrix * bgr_camera_matrix_inverse * 1 * Eigen::Vector3d(ball.img_Rect.x + ball.img_Rect.width / 2, ball.img_Rect.y + ball.img_Rect.height / 2, 1);
            // 由于bgr_camera_matrix_inverse的误差，相对于旧方法会导致x轴多6，所以这里减6
            target_pos_vec.push_back(Point2i((Depth_i / Depth_i(2, 0))(0, 0) - 6, (Depth_i / Depth_i(2, 0))(1, 0)));
        }
        for (int k = 0; k < boxes.bounding_boxes.size(); k++)
        {
            double dep_sum = 0;
            //给框限幅，不然出去了
            for (int i = ((target_pos_vec[k].x - dep_box_size) < c2d_x_min ? c2d_x_min : min(target_pos_vec[k].x - dep_box_size, c2d_x_max));
                 i < ((target_pos_vec[k].x + dep_box_size) < c2d_x_min ? c2d_x_min : min(target_pos_vec[k].x + dep_box_size, c2d_x_max)); i++)
            {
                for (int j = ((target_pos_vec[k].y - dep_box_size) < c2d_y_min ? c2d_y_min : min(target_pos_vec[k].y - dep_box_size, c2d_y_max));
                     j < ((target_pos_vec[k].y + dep_box_size) < c2d_y_min ? c2d_y_min : min(target_pos_vec[k].y + dep_box_size, c2d_y_max)); j++)
                {
                    float depth = Depth_img.image.at<uint16_t>(j, i) * pow(10.0, -1);
                    if (depth != 0)
                    {
                        c2d_real.dep_vector.push_back(depth);
                        dep_sum += depth;
                        // 如果按照上面c2d的那个式子套用dep_camera_matrix_inverse，误差非常大，所以还是老老实实算Depth_c_tran
                        Depth_c_tran.block<3, 1>(0, 0) = depth * dep_camera_matrix.inverse() * Eigen::Vector3d(i, j, 1);
                        BGR_i = bgr_camera_matrix_uninverse * d2c_outer_matrix * Depth_c_tran;
                        if (!last_bgr.image.empty())
                        {
                            //给深度图上色，知道测的是哪块区域
                            cv::Vec3b pixel = last_bgr.image.at<cv::Vec3b>(round((BGR_i / BGR_i(2, 0))(1, 0)), round((BGR_i / BGR_i(2, 0))(0, 0)));
                            Depth_BGR.at<cv::Vec3b>(j, i) = pixel;
                        }
                    }
                }
            }
            Ball_par[k].cam_pos = ((dep_sum / c2d_real.dep_vector.size()) * dep_camera_matrix.inverse() * Eigen::Vector3d(target_pos_vec[k].x, target_pos_vec[k].y, 1.0));
            Ball_par[k].dep = sqrt((pow(Ball_par[k].cam_pos(0, 0), 2.0) + pow(Ball_par[k].cam_pos(2, 0), 2.0)));
            Ball_par[k].angle = atan2(Ball_par[k].cam_pos(2, 0), Ball_par[k].cam_pos(0, 0)) * 180.0 / M_PI;
            //深度图误差有点大，很多数据都采集不了，结果就是nan，我把nan的赋值成999
            if (isnan(Ball_par[k].dep))
            {
                Ball_par[k].cam_pos << 999, 999, 999;
                Ball_par[k].dep = 999;
                Ball_par[k].angle = 999;
            }
            c2d_real.dep_vector.clear();
        }
        imshow("new_BGR", Depth_BGR);
        return Depth_BGR;
    }