将车体坐标系下的点转换成图像坐标系下的点

1、将车体坐标系下的点转换到图像像素坐标系下

要将车体坐标系下的点转换到图像像素坐标系下，需要现将车体坐标系下的点通过相机的外参矩阵，转换到相机坐标系下的点；然后利用相机的内参矩阵，将相机坐标系下的点转换到图像像素坐标系下（中间包含畸变矫正、相机坐标系转到图像坐标系再转到像素坐标系）。代码实现如下：

    void VehiclePoints2UVPoints(const std::vector<geometry_msgs::msg::Point>& veh_points,  const CameraParams &camera_params, std::vector<cv::Point2f>& uv_points){
        if(veh_points.empty()) {
            return;
        }
        uint32_t point_size = veh_points.size();
        std::vector<cv::Point3f> points_cam;
        points_cam.clear();
        for (uint32_t i = 0; i < point_size; ++i) {
            Eigen::Vector4d xyz1_in_vehicle(veh_points[i].x, veh_points[i].y, veh_points[i].z, 1.0);
            Eigen::Vector4d xyz1_in_camera = camera_params.extrinsic_matrix * xyz1_in_vehicle;
            points_cam.emplace_back(cv::Point3f(xyz1_in_camera(0), xyz1_in_camera(1), xyz1_in_camera(2)));
        }

        cv::Mat rvec = cv::Mat::zeros(1, 3, CV_64F);
        cv::Mat tvec = cv::Mat::zeros(1, 3, CV_64F);

        if (camera_params.dist_model == DistortionModel::PINHOLE) {
            cv::projectPoints(points_cam, rvec, tvec, camera_params.intrinsic_matrix_cv,
                              camera_params.dist_coeffs, uv_points);
        }
        if (camera_params.dist_model == DistortionModel::FISHEYE) {
            cv::fisheye::projectPoints(points_cam, uv_points, rvec, tvec, camera_params.intrinsic_matrix_cv,
                                       camera_params.dist_coeffs);
        }

    }

上述代码中veh_points为输入的车体坐标系下的点， camera_params为相机标定参数，包括相机的内外参矩阵、畸变系数、相机的图像原始分辨率等。uv_points为计算出的原始像素坐标系下的点。