Opencv 3D point to 2D image point projection - c++

Question

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.