How to find rotation and translation of a triangle made of 3 points with 3 infrared leds

Question

I am working on a project, where i have only 1 static camera (not moving) but an object plane with 3 points on it (its a flat plane, so the difference of the points in the z direction is zero). The camera is always on the same position and the plane moving and rotating in the space. I have calibrated the camera with 25 chessboard images and the function:

calibrateCamera(object_points, image_points, image.size(), intrinsic, distCoeffs, rvecs,      tvecs);

and the undistortion works fine. Now i made opencv (c++) to write the intrinsics and distCoeffs into an xml file.

How can I use these parameters in a new program, to finds 3 points which form a triangle (made with infrared- leds), and return me the rotation and translation of this triangle?

Answer 1

2D case

This part of the answer only discusses rigid motions in the plane , so it doesn't fully match the question.

Suppose you have identified your points, which seems to work according to your comment . Now you can compute the distances between these points and order them. The shortest distance between two detected points should correspond to the shortest edge of your original triangle and so on. With this information, you can label your points as A , B and C .

Next, you move (translate) all points so that one point lies at the origin afterwards. This is assuming that your original triangle has one corner at the origin. Let us assume that A is at the origin, both in your original triangle and in the moved set of points. The negated version of this vector indicates the translation of your triangle.

Now for rotation. Assume for the moment that for your original triangle, B is located at the positive x axis, ie has y = 0 and x > 0. Now you can take the (moved) position of the point B from your camera, and using atan2 of its coordinates, you can determine the angle between the line AB and the x axis. So that's the angle of rotation for your triangle.

You can combine both of these to form an affine transformation. If your original triangle is not located with A at the origin and B on the positive part of the x axis, then you can use similar techniques to determine how your original triangle relates to one that has the described properties.

3D case

For rigid motions in space, I have no ready-to-use solution. Only an idea. Suppose your camera is located at the origin, and the image you analyze is embedded at z = 1. Then a point ( x , y ) in the image corresponds to a point ( x , y , 1) in that embedded location, and the ray passing through that point is given by multiples of that coordinate, ie ( λx , λy , λ ). So starting from the locations of points A , B and C in your image, you can compute the corresponding points in space, based on a single variable per point, eg a through c .

Now your task is to choose these three variables in such a way that the distances between the points in space match the edge lengths of your triangle. This leads to a system of three equations in three variables:

(a - b)^2 + (Ay*a - By*b)^2 + (Ax*a - Bx*b)^2 == AB^2
(a - c)^2 + (Ay*a - Cy*c)^2 + (Ax*a - Cx*c)^2 == AC^2
(b - c)^2 + (By*b - Cy*c)^2 + (Bx*b - Cx*c)^2 == BC^2

Unfortunately, these equations aren't linear. There are squares of your variables all over the place. Simply feeding them into a computer algebra system hasn't yielded an explicit solution for me yet. I guess there might be numeric methods to find an approximation using some iterative process, but you'll have to do some research on that.

Once you have the 3D coordinates, computing the transformation from these should be simple by comparison. It all depends on the format you desire for the description of a rotation.