How to find rotation and translation (transformation matrix) between corresponding points in two different coorinate systens

Question

I am working on a camera-lidar calibration and I am stuck for some time on the following problem:

I am using a usb camera and a 2D lidar. I have the coordinates of the corresponding points in both lidar frame and camera frame (lets say that I have 3 points and their coordinates in lidar frame and the coordinate of the same 3 points in camera frame).

Example for one point:
lidar_pt1(xl, yl)
camera_pt1(xc, yc, zc)
...
are known.

If I hardcode the transformation matrix I get an expected result. Now I am trying not to hardcode it, but to automatically calculate it using the known values of the coordinates. What I have is 3 points in 2D coordinates in the lidar frame and exact 3 points in as 3D coordinates in the camera frame. It is here where I am struggling with the math to somehow calculate the rotation based on the coordinates value that I have. Is there a way to get that rotation?

camera_pt1 = TransformMat * lidarpt1
TransformMat = ?

I saw some examples using SVD (http://nghiaho.com/?page_id=671) but I think they require bigger sets of data and the minimum of 3 points would not give the best result.

r3mainer r3mainer · Accepted Answer · 2017-11-03T14:39:39

If you only take 3 pairs of coordinates from each system, then the maths is quite straightforward. Here's a simple example:

    |
  4 |                      (R)
    |                       : ',
    |                       :   ',
    |                       :     ',
  3 |                       :      (P)
    |                       :     ,'
    |                       :   ,'
    |                       : ,'
  2 |      (A).....(B)     (Q)
    |       :     ,'
    |       :   ,'
    |       : ,'
  1 |      (C)
    |
    |
    |
  0 +-------------------------------------
    0       1       2       3       4

Suppose you have a triangle ABC that maps to another triangle PQR. You can represent their vertices in homogeneous coordinates as follows:

      .-       -.          .-       -.
      | 1  2  1 |          | 4  3  1 |
ABC = | 2  2  1 |    PQR = | 3  2  1 |
      | 1  1  1 |          | 3  4  1 |
      '-       -'          '-       -'

You need to find a matrix M that maps ABC onto PQR (i.e., ABC × M = PQR). To do this, just multiply PQR by the inverse of ABC:

if    ABC × M  =  PQR,
then  ABC⁻¹ × ABC × M  =  ABC⁻¹ × PQR
so    M  =  ABC⁻¹ × PQR

There are plenty of references available on how to invert a 3×3 matrix. This should give you the following result:

    .-          -.
    | -1  -1   0 |
M = |  1  -1   0 |
    |  3   6   1 |
    '-          -'

How to find rotation and translation (transformation matrix) between corresponding points in two different coorinate systens

1 Answers