Hi Moritz Rumpf,
As per my understanding, you want to project lidar points on the image manually without using the built-in function "projectLidarPointsOnImage" so that the code can be transferred in C++ but the output results is not as expected.
Please refer to steps given below to project the point cloud on the image manually.
1.) Transform the LiDAR point cloud from lidar coordinates frame to the camera coordinate frame. Get the "tform" using the "rigidtform3d" function.
% get tforms from the r_rot
tform = rigidtform3d(rot_t(1:3,1:3), rot_t(1:3,4));
% transform the point cloud
transformed_pc = pctransform(pc,tform);
% get point cloud locations
pointsCamFrame = transformed_pc.Location;
2.) Get camera intrinsic parameters : Focal Lengths (fx, fy) and Principal Point (cx, cy).
3.) Project Points on the image plane.
% Perspective projection
Xc = pointsCamFrame(:, 1);
Yc = pointsCamFrame(:, 2);
Zc = pointsCamFrame(:, 3);
xProj = fx .* Xc ./ Zc + cx;
yProj = fy .* Yc ./ Zc + cy;
4.) Remove the points that are behind the camera.
validIdx = Zc > 0;
xProj = xProj(validIdx);
yProj = yProj(validIdx);
5.) Plot the LiDAR points on the image.
% Plot the image
imshow(Img);
hold on;
plot(xProj, yProj, 'r.');
hold off;
Please refer to the documentation link to learn more about "rigidtform3d" function.
Hope it helps!
