# Getting Rotation Matrix Using 3-Point Calibration

I am not really sure whether this question is more suited here or in the robotics stackexchange website. It is more a mathematical formulation than it is with robotics since I am dealing with rotation matrices using 3 point calibration system.

Simple Question:

As demonstrated in the image below, how can I calculate the rotation matrix that relates the base coordinate and the rectangle coordinate, given 3 known points P1, P2, P3 on the surface of the rectangle with respect to the base coordinate?

Long Question:

I am currently working on OMRON pick-and-place robots. Basically, the overall layout of the system is similar to what is shown in the following picture:

.

The system has a rotary encoder that reflects the conveyor belt motion.

What I am trying to do is to come out with a transformation matrix T that represents the belt with respect to the robot coordinates. This transformation matrix is automatically calculated using a calibration wizard, however I am trying to understand how such matrix is calculated based on the 3-point belt calibration carried out as below:

## Simple Calibration Steps:

The calibration can simply be completed after teaching 3 points on the belt as shown in the following image:

The steps are as follows:

1. A product is placed on the belt at the upstream limit that a robot can start tracking the product from.
2. The robot tip is then placed on the product. Its location and the encoder value are recorded. Let the point be P1 and encoder value be E1.
3. The belt is then allowed to move in the product feeding direction until the it reaches to another location which the robot tip can reach.
4. Robot tip is placed on top of the product. This is the second calibration point P2. The encoder value is also recorded. Let it be E2.
5. Lastly the robot tip is moved and placed on the calibration product on a side location of the belt called downstream pick limit. This would be the the third point P3 and it is recorded.

After I followed these steps for a test robot in hand, I got the following quantities:

$$P_1 = \begin{bmatrix}-286.8 & -338.0 & 244.6\end{bmatrix} ; E_{1}=-71873$$

$$P_2 = \begin{bmatrix}140.3 & -422.2 & 246.7\end{bmatrix} ; E_{2}=-158710$$ $$P_3 = \begin{bmatrix}-18.004 & -590.452 & 248.746\end{bmatrix}$$

Let b = [bx by bz] be a belt point. Since the belt motion is having a magnitude along the x direction, then the scale factor in units of mm/pulses can be calculated by:

$$s = \frac {|P2-P1|}{E2-E1} = \frac {\sqrt{(140.3-(-286.8))^2 + (-422.2 -(-338.0))^2 + (246.7-244.6)^2}}{−158710 -(-71873)} =-0.005013$$

This is one important scale factor that we need to calculate during the calibration process (the minus sign means the encoder counts opposite to belt rotation)

Along with this scale that we get during calibration, we need to calculate all members of the transformation matrix T which relates belt coordinates with robot coordinates. This has the rotation matrix R and translation matrix D. $$p=Tb = \begin{bmatrix}& R & & D\\ 0 & 0 & 0 & 1 \end{bmatrix}b$$

$$p=\begin{bmatrix} p_x \\ p_y\\ p_z\\ 1 \end{bmatrix} = \begin{bmatrix} r_{11} & r_{12} & r_{13} & d_{x}\\ r_{21} & r_{22} & r_{23} & d_{y}\\ r_{31} & r_{32} & r_{33} & d_{z}\\ 0 & 0 & 0 & 1\\ \end{bmatrix} \begin{bmatrix} b_x \\ b_y\\ b_z\\ 1 \end{bmatrix}$$

Since b always has one component along the x axis of the belt, then the coordinate of the first calibration point with respect to belt coordinate is (Belt did not move yet): $$b_1 = \begin{bmatrix} 0 \\ 0\\ 0\\ 1 \end{bmatrix}$$

and the coordinate of the second calibration point with respect to belt coordinate is (Belt moved from P1 to P2):

$$b_2 = \begin{bmatrix} s\times(E2-E1) \\ 0\\ 0\\ 1 \end{bmatrix} = \begin{bmatrix} 435.3 \\ 0\\ 0\\ 1 \end{bmatrix}$$

Therefore, from P1 calibration, we have:

$$p_1=\begin{bmatrix} −286.8 \\ −338.0\\ 244.6\\ 1 \end{bmatrix} = \begin{bmatrix} r_{11} & r_{12} & r_{13} & d_{x}\\ r_{21} & r_{22} & r_{23} & d_{y}\\ r_{31} & r_{32} & r_{33} & d_{z}\\ 0 & 0 & 0 & 1\\ \end{bmatrix} \begin{bmatrix} 0 \\ 0\\ 0\\ 1 \end{bmatrix}$$

Solving for T matrix members, we get dx = −286.8, dy = −338.0 and dz = 244.6

From P2 calibration, we have: $$p_2=\begin{bmatrix} 140.3 \\ −422.2\\ 246.7\\ 1 \end{bmatrix} = \begin{bmatrix} r_{11} & r_{12} & r_{13} & −286.8\\ r_{21} & r_{22} & r_{23} & −338.0\\ r_{31} & r_{32} & r_{33} & 244.6\\ 0 & 0 & 0 & 1\\ \end{bmatrix} \begin{bmatrix} 435.3 \\ 0\\ 0\\ 1 \end{bmatrix}$$

Solving for the Rotation matrix, we get: $$R = \begin{bmatrix} 0.981112257 & r_{12} & r_{13}\\ -0.193378015 & r_{22} & r_{23}\\ 0.004866365 & r_{32} & r_{33} \end{bmatrix}$$

Based on OMRON calibration wizard, the following matrix is obtained after finishing teaching the 3 points: $$T = \begin{bmatrix} 0.981112257 & -0.193427089 & 0.00216786 & −286.8\\ -0.193378015 & -0.981022085 & -0.01416372 & −338.0\\ 0.004866365 & 0.013476983 & -0.999897339 & 244.6\\ 0 & 0 & 0 & 1\\ \end{bmatrix}$$

As seen, the first column and 4th conform with the results that we got. However, I am not really sure what the mathematical way to follow using the third calibration point to calculate the rest of the transformation matrix that I have. How would I be able to calculate r12, r22, r32, r13, r23 and r33?

Why I Need to Know This in The First Place!

I am an experienced software engineer with robotics and control engineering qualification. I am planning to program 3D models and create a simulation platform for the robotics world. Clear understanding of this field in mathematics is required.

• For reference, this question is cross-posted here. Commented Jun 10, 2020 at 18:18
• Yes I mentioned this in the accepted answer. Thank you for posting a comment here too Commented Jun 11, 2020 at 2:40

This method is explained in more detail in section 2.3 in the linked pdf. It works for $$n\geq 3$$ calibration points. I don't know whether there is an easier way to solve the problem in the specific $$n=3$$ case. https://www.cse.usf.edu/~r1k/MachineVisionBook/MachineVision.files/MachineVision_Chapter12.pdf

First convert the points to ray coordinates. e.g. if the centroid of the points $$p_i$$ is $$p_c$$ then set $$\tilde{p}_i=p_i-p_c$$, and similarly set $$\tilde{b}_i=b_i-b_c$$.

Given a quaternion $$q=(q_0,q_1,q_2,q_3)$$ representing a rotation, the corresponding rotation matrix is $$R(q)=\begin{pmatrix}q_0^2+q_1^2-q_2^2-q_3^2 & 2(q_1q_2-q_0q_3)&2(q1q_3+q_0q_2)\\ 2(q_1q_2+q_0q_3)&q_0^2+q_2^2-q_1^2-q_3^2&2(q_2q_3-q_0q_1)\\ 2(q_1q_3-q_0q_2)&2(q_2q_3+q_0q_1)&q_0^2+q_3^2-q_1^2-q_2^2 \end{pmatrix}$$

We want to find a rotation $$q$$ so that the rays $$\tilde{p}_i$$ and $$R(q)\tilde{b}_i$$ are aligned in the same direction i.e. $$\tilde{p}_i\cdot R(q)\tilde{b}_i=1$$.

This problem essentially amounts to that of maximizing $$\sum_{i=1}^n\tilde{p}_i\cdot R(q)\tilde{b}_i$$. Using quaternion notation, the sum can be rewritten as $$\sum_{i=1}^n\tilde{p}_i\cdot q\tilde{b}_iq^*=\sum_{i=1}^n({q}\tilde{p})\cdot ({q}\tilde{b})$$.

If we now think of $$q$$ as a column vector then it is possible to turn this sum into a quadratic form of the form $$q^TNq$$ for some matrix $$N$$. This quadratic form will attain its maximum when $$q$$ is the eigenvector corresponding to the largest positive eigenvalue of $$N$$.

I recently understood that cross-posting is not allowed in StackExchange websites. I also cannot delete the question because it has bounty that is yet to expire. I have posted the same question in the Robotics network and I received an awesome clear answer from @50k4. Following is the link for the question that is a answered:

https://robotics.stackexchange.com/questions/20745/conveyor-belt-calibration-for-a-robot-rotation-matrix#20767

I hope that someone who has the privilege can take an action to correct the behavior on this cross-posting matter. I personally believe that a feature to cross-post a question must be implemented. For my question, it matches both Mathematics and Robotics networks and is greatly useful to both communities.