To Estimate the Transformation Between Two Images

The transformation of a 3D object from one point cloud to another point cloud is characterized by a unitary matrix and a offset. For 2D image, if multiple images are for the same scene, then we are facing with the similar task of matching between different images. Assuming that by feature point matching or tracking or other algorithms, it is found that N matching pairs of points \((x, y)\) and \((x', y')\) in two images. Our task is to identify a transformation for \((x, y) \rightarrow (x', y')\).


For 3D points under similar scenario, given two matrices with the size of \(3 \times N\) in each, we find the cross product of them, run SVD on the cross product, then the transform matrix is \(U \times V\) whereas \(U\) and \(V\) are the two unitary matrices obtained by SVD. For 2D matching, the procedure is more complicated. The first step is to add one dimension. Instead of  \((x, y)\) and \((x', y')\), now we have  \((x, y, 1)\) and \((x', y', 1)\). This added dimension can be treated as a dimension for the infinite horizon. This new triplet is insensitive to scaling, which means that \((x, y, 1)\) and \((kx, ky, k)\) represent the same pixel in the image. The cross product between these two triplets are \({\bf 0}\):
\[ \begin{bmatrix}
    i   &    j & k  \\
    x    &   y & 1  \\
    kx     &  ky & k  \\
\end{bmatrix}={\bf 0}
\]
The transformation between two images is defined as the projective transformation matrix \({\bf P}\) which is
\[

\begin{bmatrix}   kx'   \\
   ky'   \\
    k
\end{bmatrix} = {\bf P} \begin{bmatrix}
   x   \\
   y   \\
    1
\end{bmatrix}
\]
Assuming that
\[ {\bf P} = \begin{bmatrix}
    h_{1}   &    h_{2}&  h_{3}  \\
    h_{4}    &   h_{5}& h_{6}  \\
    h_{7}     &  h_{8}& h_{9}  \\
\end{bmatrix}\
\]
Then
\[

\begin{bmatrix}
   kx'   \\
   ky'   \\
    k
\end{bmatrix} = \begin{bmatrix}
   h_{1}x+h_{2}y+h_{3}   \\
  h_{4}x+h_{5}y+h_{6}   \\
    h_{7}x+h_{8}y+h_{9}
\end{bmatrix}
\]
Using the property that the cross product between \((x', y', 1)\) and \((kx', ky', k)\) is 0, we have
\[ \begin{bmatrix}
    i   &    j & k  \\
    h_{1}x+h_{2}y+h_{3}    &   h_{4}x+h_{5}y+h_{6} & h_{7}x+h_{8}y+h_{9}  \\
    x'     &  y' & 1  \\
\end{bmatrix}={\bf 0}
\]
This gives us three equations but only two of them are linear independent. The reason is that given
\[ \begin{bmatrix}
    i   &    j & k  \\
    a    &   b & c  \\
    u     &  v & w  \\
\end{bmatrix}={\bf 0}
\]
If \(aw-cu=0\) and \(bw-cv=0\), then \(av-bu=0\) for sure.

With two linearly independent equations, we have
\[ \begin{bmatrix}
    0    &   0 & 0 & x & y & 1 & -xy' & -yy' & -y'  \\
    x    &  y & 1 & 0 & 0 & 0 & -x'x & -x'y & -x' \\
\end{bmatrix}
\begin{bmatrix}
    h_{1} \\
   h_{2} \\
   h_{3} \\
   h_{4} \\
h_{5} \\
h_{6} \\
h_{7} \\
h_{8} \\
h_{9}
\end{bmatrix}
={\bf 0}
\]


With N pairs of correspondences, we can construct a matrix \({\bf M}\) which has 2N lines
\[ {\bf M}=\begin{bmatrix}
    0    &   0 & 0 & x_{1} & y_{1} & 1 & -x_{1}y_{1}' & -y_{1}y_{1}' & -y_{1}'  \\
    x_{1}    &  y_{1} & 1 & 0 & 0 & 0 & -x_{1}'x_{1} & -x_{1}'y_{1} & -x_{1}' \\
...\\
 0    &   0 & 0 & x_{N} & y_{N} & 1 & -x_{N}y_{N}' & -y_{N}y_{N}' & -y_{N}'  \\
    x_{N}    &  y_{N} & 1 & 0 & 0 & 0 & -x_{N}'x_{N} & -x_{N}'y_{N} & -x_{N}' \\
\end{bmatrix}
\]
The last step is SVD \({\bf USV = M}\). Then the last column of the matrix \({\bf V}\) corresponds to elements in matrix \({\bf P}\) since the last column of \({\bf V}\) is for the smallest Eigen value. It is the closest solution we can find for making
\[ {\bf M}
\begin{bmatrix}
    h_{1} \\
   h_{2} \\
   h_{3} \\
   h_{4} \\
h_{5} \\
h_{6} \\
h_{7} \\
h_{8} \\
h_{9}
\end{bmatrix}
={\bf 0}
\]

Sample Matlab source code for the whole procedure is shown below:
clear all; close all;
% to generate (x, y, 1)
srcPos = [-1483         853        4172        2572       -1196       -4241         308        4340         688       -4881
    3308         497       -2142        2537         678       -4460        2792       -3701        -306       -1629];


[M, N] = size(srcPos);
srcPos = [srcPos.' ones(N, 1)].';
PM = [1 2 0
    0 1 0
    -0.01 0.01 1];
dstTemp = PM * srcPos;
dstPos(1,:) = dstTemp(1,:) .* (1 ./ dstTemp(3,:));
dstPos(2,:) = dstTemp(2,:) .* (1 ./ dstTemp(3,:));
dstPos(3,:) = dstTemp(3,:) .* (1 ./ dstTemp(3,:));


% (x', y', 1)
dstPos = round(dstPos);
M = zeros(2*N, 9);
for i = 1:N
    M(2*i-1,:) = [0 0 0 srcPos(1,i) srcPos(2,i) 1 -srcPos(1,i)*dstPos(2,i) -srcPos(2,i)*dstPos(2,i) -dstPos(2,i)];
    M(2*i,:) = [srcPos(1,i) srcPos(2,i) 1 0 0 0 -srcPos(1,i)*dstPos(1,i) -srcPos(2,i)*dstPos(1,i) -dstPos(1,i)];
end


% To estmate matrix P
[u, s, v]= svd(M);
h = v(:,9)/v(9,9)

PM_hat = [h(1) h(2) h(3)
    h(4) h(5) h(6)
    h(7) h(8) h(9)]


To estimate

\[ {\bf P} =  \begin{bmatrix}
    1 & 2 & 0 \\
0 & 1 & 0 \\
-0.01 & 0.01 & 1
\end{bmatrix}\]

the result of above code is
\[ {\bf \hat{P}} =  \begin{bmatrix}
    1.0060  &  2.0137 & -56.3469\\
    0.0014  &  1.0073 & -14.1412\\
   -0.0100  &  0.0100   & 1.0000
\end{bmatrix}\]

The offsets of -56.3469 and -14.1412 are due to rounding error. If we change dstPos = round(dstPos) in the code to dstPos = (dstPos). Then \( {\bf \hat{P}}\) becomes

\[ {\bf \hat{P}} =  \begin{bmatrix}
        1.0000  &  2.0000 &   0.0000\\
   -0.0000  &  1.0000  &  0.0000\\
   -0.0100  &  0.0100  &  1.0000
\end{bmatrix}\]

Using least square estimation here does not yield good results since we don't know \(k\) in 

\[

\begin{bmatrix}   kx'   \\
   ky'   \\
    k
\end{bmatrix} = {\bf P} \begin{bmatrix}
   x   \\
   y   \\
    1
\end{bmatrix}
\]


(Added on 08/13/2016) Another way to solve this equation is this: since \({h_{1},...,h_{9}}\) parameters have ambiguity in scaling, we can force \({h_{9}=1}\). Then the equation can be rewritten as:
\[ {\bf M'}
\begin{bmatrix}
    h_{1} \\
   h_{2} \\
   h_{3} \\
   h_{4} \\
h_{5} \\
h_{6} \\
h_{7} \\
h_{8}
\end{bmatrix}
=\begin{bmatrix}
    y_{1}'  \\
    x_{1}' \\
...\\
 y_{N}'  \\
    x_{N}' \\
\end{bmatrix}
\]
where
\[ {\bf M'}=\begin{bmatrix}
    0    &   0 & 0 & x_{1} & y_{1} & 1 & -x_{1}y_{1}' & -y_{1}y_{1}'  \\
    x_{1}    &  y_{1} & 1 & 0 & 0 & 0 & -x_{1}'x_{1} & -x_{1}'y_{1} \\
...\\
 0    &   0 & 0 & x_{N} & y_{N} & 1 & -x_{N}y_{N}' & -y_{N}y_{N}' \\
    x_{N}    &  y_{N} & 1 & 0 & 0 & 0 & -x_{N}'x_{N} & -x_{N}'y_{N} \\
\end{bmatrix}
\]

Equation in this form can be solved by least square method.

A (Very) Useful Formula for Matrix Inversion

Here is a very useful formula for matrix inverse, I think the first time I saw it is from Prof. John Cioffi's book.

\[(A+BD^{-1}C)^{-1}=A^{-1}-A^{-1}B(D+CA^{-1}B)^{-1}CA^{-1}\]

let me give one example of how it can be used. Assuming that \(R_{uu}=h_{2}h_{2}^{h}+\lambda I\) where \(h_{2}\) is a vector. Then we have

\[R_{uu}^{-1}=\lambda^{-1}I-\frac{1}{\lambda^{2}+\lambda h_{2}^{h}h_{2}}h_{2}h_{2}^{h}\]

By using this formula, the end result turns out to be much simpler than where it starts from.

Implementation in Arduino Servo Libary for Rotation with Less Than 1 Degree Granularity

Arduino is popular open-source software/hardware platform for DIY. Using Arduino platform, one can control all kinds of device such as sensor, servo, LCD etc. Servo is a motor with feedback loop for rotation measurement. The feedback loop helps the servo to achieve precise control. If you tell a servo to rotate 90 degrees, it will do that. This makes servo an essential part for projects like robots. However, the smallest rotation the existing Arduino servo library can achieve is 1 degree. The reason is that the existing library only allows one to program integer number of degrees for rotation. Therefore, if one want to program servo for rotation less than 1 degree to achieve more smooth transition, the library has to be modified but the modification is rather straightforward. This post gives an example of how to modify the library.

The existing servo rotation function looks like this: an integer value is used as the input. First, the function will make sure that this integer is between 0 and 180 since that is the scope of the servo rotation. Then this integer will be mapped into a value between SERVO_MIN() and SERVO_MAX(). Here is how the map() function work: assuming that y = map(x, 0, 180, SERVO_MIN, SERVO_MAX), then (x - 0)/(180 - x) = (y - SERVO_MIN)/(SERVO_MAX - y). This y will be used to drive the servo operation. Below is the existing function in the Arduino servo library:

void Servo::write(int value)
{
  if(value < MIN_PULSE_WIDTH)
  {  // treat values less than 544 as angles in degrees (valid values in microseconds are handled as microseconds)
    if(value < 0) value = 0;
    if(value > 180) value = 180;
    value = map(value, 0, 180, SERVO_MIN(),  SERVO_MAX());
  }
  this->writeMicroseconds(value);
}


To make sub-degree rotation possible, we create a new function with floating-point number as input.

// To write angle in floating value /Nan Zhang
void Servo::writeFloat(float value)
{
  if(value < MIN_PULSE_WIDTH)
  {  // treat values less than 544 as angles in degrees (valid values in microseconds are handled as microseconds)
    if(value < 0.0) value = 0.0;
    if(value > 180.0) value = 180.0;
    value = map((int) (value*10), 0, 1800, SERVO_MIN(),  SERVO_MAX());
  }
  this->writeMicroseconds(value);
}

In this new function, we will first check whether this input is between 0.0 and 180.0. After that, the rounded value of (input*10) will be injected into the map function as map((int) (value*10), 0, 1800, SERVO_MIN(),  SERVO_MAX()). Essentially, this makes the smallest achievable rotation to be 0.1 degree. You may want to further increase the granularity by doing things like map((int) (value*100), 0, 18000, SERVO_MIN(),  SERVO_MAX()). But you will face physical limitation of how precise the servo can operate. For example, the default value for MAX and MIN are:

#define MIN_PULSE_WIDTH       544     // the shortest pulse sent to a servo
#define MAX_PULSE_WIDTH      2400     // the longest pulse sent to a servo

That means the best the device can do is (180)/(2400-544)=0.097 degree. Therefore, increasing granularity beyond 0.1 degree does not make sense for this servo device.