Cant recreate lidar to camera projection

Hey guys,

recently I did a Lidar-Camera-Calibration with matlab.

The external Matrix looked a bit weird but it seems to work with the build-in-function "projectLidarPointsOnImage".

But I tried to recreate this by Hand because i want this to transfer in C++.

ci is the internal calibration and rot_trans the external.

With this Code not a single point was on the image.

I tried different things and I managed to project the cloud on the image with a different x.

But the result is still a different one and I dont know why.

Does anybody know what im doing wrong ?

Answer 1

[Milan Bansal]

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.

https://www.mathworks.com/help/images/ref/rigidtform3d.html

Hope it helps!