Colorado School of Mines
Professor William Hoff
Dept of Electrical Engineering &Computer Science
Colorado School of Mines
1

Alignment using Nonlinear Least Squares
2
Colorado School of Mines Computer Vision

Recall 2D Rigid Transformation
• A 2D rigid transform (rotation + translation) is x
B

 y
B
1


 

 c s
 c
0 0 s t x t
1 t



 x
A y
A
1



, where c
 cos

, s
 sin

• Or x
B y
B

 cx
A sx
A
 sy
A
 cy
A
 t x
 t y
• We put into the form Ax = b , to get a system of linear equations
A






( 1 ) x
A y

( 1 )
A y
A
( N )
 ( 1 ) y
A x
( 1 )
A x
( N )
A
1
0
0
0
1
1




,
 x





 t c s t y x




,
 b





 x y

( 1 )
B
( 1 )
B y
B
( N )





Note: c and s are not really independent variables; however we treat them as independent so that we get a system of linear equations
3
Colorado School of Mines Computer Vision

Linear Least Squares
• Want to minimize
E

Ax
 b
2
• Expanding we get
E
 x
T

A
T
A
 x

2 x
T
 
 b
2
• To find the minimum, take derivative wrt x and set to zero, getting

A
T
A
 x

A
T b
Called the “normal equations”
• To solve, can do x


A
T
A


1
A
T b
• In Matlab can do
– x = pinv(A)*b;
– or x = A\b;
“pseudo inverse”
A
 
 

1
A T
Colorado School of Mines Computer Vision
4

Example
% Using imtool, we find corresponding
% points (x;y), which are the four
% corners of the book pA = [
213 398 401 223;
29 20 293 297]; pB = [
207 391 339 164;
7 34 302 270];
N = size(pA,2);
A = zeros(2*N,4); for i=1:N
A( 2*(i-1)+1, :) = [ pA(1,i) -pA(2,i) 1 0];
A( 2*(i-1)+2, :) = [ pA(2,i) pA(1,i) 0 1]; end b = reshape(pB, [], 1); x = A\b; theta = acos(x(1)); tx = x(3); ty = x(4);
Colorado School of Mines Computer Vision
“A”
“B”
5

System of Nonlinear Equations
• Since we treated c = cos

and s = sin

as independent variables, we got a system of linear equations x
B y
B

 cx
A sx
A
 sy
A
 cy
A
 t x
 t y
• But c,s are not independent – we should really just solve for 3 variables ( t x
,t y
,

), not 4 variables
• But this gives us a system of non linear equations x
B
 cos
 x
A
 sin
 y
A
 t x y
B
 sin
 x
A
 cos
 y
A
 t y
• We can still solve for the unknowns using least squares, but it requires an iterative algorithm
6
Colorado School of Mines Computer Vision

Newton’s Method
• Example for a scalar function
– Given
• A known function y = f ( x )
• A value of y , call it y
1
– Find x
1
such that y
1
= f ( x
1
)
• We need a starting guess for x , call it x
0
Colorado School of Mines Computer Vision
7

Newton’s Method
• Example for a scalar function
– Given
• A known function y = f ( x )
• A value of y , call it y
1
– Find x
1
such that y
1
= f ( x
1
)
• We need a starting guess for x , call it x
0
Colorado School of Mines Computer Vision
8

Nonlinear Least Squares
• We have a nonlinear function y = f ( x )
– x is a vector of our unknowns
– y is a vector of our observations
• We start with a guess for x , call it x0 y




 y y


 y
N
1
2





 , x
• We linearize (take the Taylor series expansion) about that point




 x x


 x
M
1
2





 dy = [∂ f / ∂ x ] x0 dx
• The matrix of partial derivatives of f with respect to x is called the
Jacobian matrix
J
 
 
  f
 x i j

 







 f f
1
2


 f
N

 x
1
 x
1
 x
1
 f
1
 f
2

 x
2
 x
2
 f
N
 x
2




 f f
 f
N
1
2

 x x
M
M

 x
M






Colorado School of Mines Computer Vision
9

Nonlinear Iterative Least Squares Algorithm
We have
• y0 = observations or measurements
• x0 = a guess for x
• y = f(x) is a non linear function
1.
Initialize x to x0
2.
Compute y = f(x) . Residual error is dy = y y0
3.
Calculate Jacobian of f , evaluate it at x . We now have dy = J dx
4.
Solve for dx using pseudo inverse dx = (J T J) 1 J T dy
5.
Set x <= x + dx
6.
Repeat steps 2-5 until convergence (no more change in x )
Colorado School of Mines Computer Vision
10

Example – 2D rigid transformation
• Recall x
B y
B
 cos

 sin
 x
A x
A
 sin

 cos
 y
A y
A
 t x
 t y
• We have N corresponding points
 x
(
B i )
, y
(
B i )
  x
(
A i )
, y
(
A i )
,

• Let y = f(x) , where i

1  N y
 






 x y x y
(

( 1 )
B
( 1 )
B
B
(
B
2 )
2 ) x
B
( N )







 y
B
( N )
2 Nx 1
 x
 

 t t y x



3 x 1
, f ( x )
 






 cos sin cos sin

 cos


 x
( 1 )
A
( 1 ) x
A x
(
A
2 )
( 2 ) x
A x
(
A
N ) sin
 x
( N )
A




 sin cos sin cos
 sin





 cos
 y
( 1 ) y
A
( 1 )
A y
A
( 2 )
( 2 ) y
A
 t x
 t y
 t x
 t y y
A y
(
(
A
N
N )
)  t x
 t y








2 Nx 1
Colorado School of Mines Computer Vision
11

Example
• Jacobian?
J




 f
 x i j






 f
1
 f
2
 f
2 N

 x
1
 x
1
 x
1
 f
1
 f
2
 x
2
 x
2
 f
2 N

 x
2

 f f
1
2
 x
3
 x
3
 f
2 N

 x
3


2 Nx 3
 x
 

 t t y x



3 x 1
, f ( x )
 






 cos sin cos sin
 cos



 x
A
( 1 ) x
A
( 1 ) x
A
( 2 ) x
A
( 2 ) x (
A
N ) sin
 x
(
A
N )
 sin
 cos

 sin
 cos


 sin



 cos
 y y
( 1 )
A y
(
A
2 ) y
( 1 )
A
( 2 )
A
 t x
 t y
 t x
 t y y (
A y
A
N
( N )
)  t x
 t y








2 Nx 1
12
Colorado School of Mines Computer Vision

Matlab Functions
• A function in Matlab is declared using function [out1, out2, ...] = myfun(in1, in2, ...)
• You need to put this in a separate file myfun.m
• You can call the function using its name; e.g., v = myfun(a,b,c);
• Example (in file stat.m) function [mean,stdev] = stat(x)
% Simple function to return mean
% and standard deviation.
% Input is x, a vector of numbers
% (These comments are printed if you
% type "help stat") n = length(x); mean = sum(x)/n; stdev = sqrt(sum((x-mean).^2/n)); return % return (or end) is optional
[mean stdev] = stat([12.7 45.4 98.9
26.6 53]) mean =
47.3200 stdev =
29.4085
13
Colorado School of Mines Computer Vision

Function to transform one point
• Write a function to transform input coordinates (xin,yin) to output coordinates (xout,yout), given a rotation angle theta and translation tx,ty
– pIn = [xin;yin;1] is the input point
– y = [xout;yout] is the output point
– x = [theta;tx;ty] is the vector of transformation parameters function y = fRigid(x, pIn)
14
Colorado School of Mines Computer Vision

Function to transform one point
• Write a function to transform input coordinates (xin,yin) to output coordinates (xout,yout), given a rotation angle theta and translation tx,ty
– pIn = [xin;yin;1] is the input point
– y = [xout;yout] is the output point
– x = [theta;tx;ty] is the vector of transformation parameters function y = fRigid(x, pIn)
Colorado School of Mines function y = fRigid(x,pIn)
% Do 2D rigid transform, on an input point
% Get params theta = x(1); tx = x(2); ty = x(3);
H = [ cos(theta) -sin(theta) tx;
sin(theta) cos(theta) ty;
0 0 1]; pOut = H*pIn; y = pOut(1:2); % 1st two rows return
Computer Vision
15

Function to transform a set of points
• Now modify the function to transform a set of points
– x = [theta;tx;ty] is the vector of transformation parameters
– pIn = is the set of input points
– y = are the output points pIn
 
 x y
1
1
1 x
2 y
2
1


 function y = fRigid(x, pIn) x
N y
N
1

 y x
1









 y x y
 x
N y
N
1
2
2









16
Colorado School of Mines Computer Vision

Function to transform a set of points
• Now modify the function to transform a set of points
– x = [theta;tx;ty] is the vector of transformation parameters
– pIn = is the set of input points
– y = are the output points pIn
 
 x y
1
1
1 x
2 y
2
1


 function y = fRigid(x, pIn) function y = fRigid(x,pIn)
% Do 2D rigid transform, on a set of input points
% Get params theta = x(1); tx = x(2); ty = x(3);
H = [ cos(theta) -sin(theta) tx;
sin(theta) cos(theta) ty;
0 0 1]; pOut = H*pIn; pOut = pOut(1:2, :); % 1st two rows y = reshape(pOut,[],1);
Colorado School of Mines return
Computer Vision x
N y
N
1

 y x
1









 y x y
 x
N y
N
1
2
2









17

Algorithm for 2D Rigid Transform
We have
• y0 = observations or measurements
• x0 = a guess for x
• y = f(x) is a non linear function
1.
Initialize x to x0
2.
Compute y = f(x) . Residual error is dy = y y0
3.
Calculate Jacobian of f , evaluate it at x . We now have dy = J dx
4.
Solve for dx using pseudo inverse dx = (J T J) 1 J T dy
5.
Set x <= x + dx
6.
Repeat steps 2-5 until convergence (no more change in x )
Computer Vision Colorado School of Mines clear all close all
IA = imread( 'book_A.jpg' );
IB = imread( ‘book_B.jpg' ); figure, imshow(IA,[]); figure, imshow(IB,[]);
% Using imtool, we find corresponding
% points (x;y), which are the four
% corners of the book pA = [
213 398 401 223;
29 20 293 297;
1 1 1 1]; pB = [
207 391 339 164;
7 34 302 270];
N = size(pA,2); theta = 0; tx = 0; ty = 0; x = [theta; tx; ty]; % initial guess
18

Algorithm for 2D Rigid Transform
4.
5.
6.
1.
2.
3.
Initialize x to x0
Compute y = f(x) . Residual error is dy = y y0
Calculate Jacobian of f , evaluate it at x . We now have dy = J dx
Solve for dx using pseudo inverse dx = (J T J) 1 J T dy
Set x <= x + dx
Repeat steps 2-5 until convergence (no more change in x ) while true
disp( 'Parameters (theta; tx; ty):' ), disp(x);
y = fRigid(x, pA); % Call function to compute expected measurements
dy = reshape(pB,[],1) - y; % new residual
J = zeros(2*N,3);
% Fill in values of J …
dx = pinv(J)*dy;
% Stop if parameters are no longer changing if abs( norm(dx)/norm(x) ) < 1e-6 break ; end
x = x + dx; % add correction end
Colorado School of Mines Computer Vision
19

Algorithm for 2D Rigid Transform while true
disp( 'Parameters (theta; tx; ty):' ), disp(x);
y = fRigid(x, pA); % Call function to compute expected measurements
dy = reshape(pB,[],1) - y; % new residual
J = zeros(2*N,3);
% Fill in values of J …
theta = x(1); for i=1:N
J( 2*(i-1)+1, :) = [ -sin(theta)*pA(1,i)-cos(theta)*pA(2,i) 1 0 ];
J( 2*(i-1)+2, :) = [ cos(theta)*pA(1,i)-sin(theta)*pA(2,i) 0 1 ]; end
dx = pinv(J)*dy;
% Stop if parameters are no longer changing if abs( norm(dx)/norm(x) ) < 1e-6 break ; end
x = x + dx; % add correction end
Colorado School of Mines Computer Vision
20

Apply final transform to image
% Apply transform to image theta = x(1); tx = x(2); ty = x(3);
A = [cos(theta) -sin(theta) tx;
sin(theta) cos(theta) ty;
0 0 1];
T = maketform( 'affine' , A');
I2 = imtransform(IA,T, ...
'XData' , [1 size(IA,2)], ...
'YData' , [1 size(IA,1)] ); figure, imshow(I2,[]);
Colorado School of Mines Computer Vision
21

Computing Jacobian Numerically
• An alternative way to compute the Jacobian matrix
• It’s easier, but more computation
• Recall definition of derivative
J




 x f i j

 


 f
1
 f
2

 x
1
 x
1
 f
N
 x
1
 f ( x
1
, x
2
 x i
,  x
N
)
 f ( x
1
,  , x i
 
,  x

N
)
 f ( x
1
, x
2
,  x
N
)
 f
1
 f
2

 x
2
 x
2
 f
N
 x
2
 f ( x )
 x i
 f ( x
 
 i
)
 f ( x )
• Matlab code:
J



 f
 x
1
 f
 x
2
Column vectors


 f
 x
N


% Estimate J numerically
e = 1e-6; % A tiny number
J(:,1) = (fRigid(x+[e;0;0],pA) - y)/e;
J(:,2) = (fRigid(x+[0;e;0],pA) - y)/e;
J(:,3) = (fRigid(x+[0;0;e],pA) - y)/e;



 f f
 f
N
1
2
 x
M
 x
M

 x
M


22
Colorado School of Mines Computer Vision