Opencv 3D point to 2D image point projection

Viewed 798

I'm converting LiDAR points to a camera image.

The LiDAR points and also the camera image are from a simulator. For simplicity I placed them at the exact same location, facing the same direction without any roll, pitch, yaw. (So camera coordinate system is the same as the lidar coordinate system)

If I understood correctly I can then just use an empty t_vec/r_vec/d_vec (as there is also no distortion in the camera image).

The Image is 785x785.

// added for debugging purpose 
std::vector<cv::Point3d> points_lidar {
    cv::Point3d {0,0,4.5},
    cv::Point3d {1.88,-0.42,4.50},
    cv::Point3d {1.85,-0.42,4.49},
    cv::Point3d {1.84,-0.42,4.49},
    cv::Point3d {1.83,-0.42,4.51},
    cv::Point3d {1.82,-0.42,4.52},
    cv::Point3d {1.81,-0.41,4.52},
};

cv::Mat d_vec = cv::Mat::zeros(4, 1, cv::DataType<double>::type);
cv::Mat r_vec = cv::Mat::zeros(3, 1, cv::DataType<double>::type);
cv::Mat t_vec = cv::Mat::zeros(3, 1, cv::DataType<double>::type);
double camera_mat[3][3] = {
        {785,0,0},
        {0,785,0},
        {0,0,1}
};
cv::Mat camera (3, 3, cv::DataType<double>::type, camera_mat);

std::vector<cv::Point2i> points_camera {};
cv::projectPoints(points_lidar, r_vec, t_vec, camera, d_vec, points_camera);

for (const auto& p : points_camera) {
    cv::Point2i pp;
    pp.x = (int)(p.x + 785/2);
    pp.y = (int)((1 - p.y) + 785/2);

    cv::circle(image->image, pp, 5, cv::Scalar(0, 0, 255), -1);
}

Unfortunately the projected points don't nearly match the 3d points and are too far to the right / bottom of the image.

Does anyone see an issue?

Edit:

I tested a simple solution without opencv

after the comment of @ema I tested a really simple solution without opencv. Which actually results in the correct image pixels but is far slower (~5ms compared to ~1ms with opencv).

for (const auto& p_lidar: points_lidar) {
    cv::Point2d p {p_lidar.x / p_lidar.z, p_lidar.y / p_lidar.z};
    p.x = (p.x + cam.canvas_width / 2) / 2;
    p.y = (p.y + cam.canvas_height / 2) / 2;

    cv::Point2i p_raster;
    p_raster.x = std::floor(p.x * cam.image_width);
    p_raster.y = std::floor((1 - p.y) * cam.image_height);

    cv::circle(image->image, p_raster, 3, cv::Scalar(0, 0, 255), -1);
}

As the pipeline has about 100ms until the next iteration of processing begins I would still prefer to use opencv for optimized calculations.

0 Answers
Related