To find 3D point on a line closest to another 3D point in Multi-view Geometry?

Question

I have two stereo cameras and I captured the images of the markers both from left camera and the right cameras.

I have a perfect calibration

1. Intrinsics
2. Distortion Corrected
3. Extrinsic

Now coming to the Markers, I have a set of markers on the board and I estimated the Pose of it. And also, I computed the 3D point of each marker based on Triangulate 3D points

cv::triangulatePoints(Projection_Matrix_L[o], Projection_Matrix_R[o], pointsMat1, pointsMat2, pnts3D);

Then I know the camera center(CL_O)

CL_O = -Rotation_matrix.inv * translation

which is mostly (0,0,0)

Similarly done for the other camera view. For simplicity let us just stick with one.

I assume the below equation should calculate the points in the line based on lambda(λ)

line(λ) = CL_O + λ*t

CL_O, is a 3D point say (x0, y0, z0)
t, is a Vector (a, b, c)

With correct(λ) value, The result will be a 3D point say Q. But still its not close to Marker Point P. So I need to compute the minimum distance between Point P and Q.

Questions - How should I determine λ for each 3D marker points, such that it gives me a 3Dpoint Q close to P with minimal error. I don't know if the below equation is correct?

f(λ) = abs(Point P - line(λ))

If you think its correct, I need help with sample code/pseudo code to implement it (preferably C++ or python ).

Even though I have written above, I still feel that something is still missing. Please connect the dots and tell me how to proceed.

Answer 1

I had done a similar problem in java sometimes its algorithm can help you

Check out it at https://github.com/sreeragrnandan/JAVA-ASSIGNMENT/blob/master/Mdis.java