Least Square (triangulation +...

LeastSquare(triangulation+PnP)

paintingbyLEANNESHAPTONSlidespreparedbyHyunSoo Park

Camera3DRegistrationPerspective-n-PointAlgorithm

Singular Value Decomposition

A U= D VT

m x n m x n n x n n x n

U

Column orthogonal matrix

UTu= Im m VVT

u= In n

Singular value matrix

D , , ,= n"ı ı ı1 2diag{ } t t t tn"ı ı ı1 2 0where

U(:,1:4) D(1:4,1:4) V(:,1:4)’

Nullspace

A U= D VT

m x n m x n n x n n x n

null(A) = Vr :n Vr :nA = 0

2

7

The SVD and Spectral Decomposition

Let A = UVT be an SVD

with = diag(�1, . . . , �c) and V = (v1 · · · vc)

We have that

the �i are the square root of the eigenvalues of ATA

the vi are the eigenvectors of ATA

Proof

ATA = VTUTUVT = V2VT 8

Kernel, Rank and the SVD

SVD0

A VU

T

The rank is 2

The left kernel has dimension 5� 2 = 3

The right kernel has dimension 3� 2 = 1

Basis for the right kernel

Partial basis for the left kernel

F Vector x is in the right kernel of A i& Ax = 0

F Vector y is in the left kernel of A i& yTA = 0T

F The rank k of matrix A is the number of nonzero singular values(0 � k � min(r, c))

F The right kernel has dimension c� k

F The left kernel has dimension r � k

9

Linear Least Squares (LLS) Optimization: Problem Statement

minx;Rn

kAx� bk2 s.t. C(x) = 0

minx;Rn

mX

j=1

¡aTj x� bj

¢2s.t. C(x) = 0

A(m×n) =

C

hhH

.

.

.

aTj...

I

nnO and b(m×1) =

C

hhH

.

.

.

bj...

I

nnO

m ‘equations’ and n ‘unknowns’

‘constraints’ means both ‘equations’ and ‘unknowns’ and is thus ambiguous

10

A Bit of History…

� 1795 – discovery of Least Squares by Gauss� 1801 – used by Gauss in astronomical calculations

(prediction of the orbit of the asteroid Ceres)� 1805 – publication of an algebraic procedure by

Legendre (dispute with Gauss)� The method quickly became the standard procedure

(for analysis of astronomical and geodetic data)� 1809, 1821, 1823 – statistical justification by Gauss� 1912 – rediscovery by Markov

11

Why Least Squares?

� Assume the noise on b is Independently and Identically Distributed (i.i.d.)

� Least Squares gives the Best Linear Unbiased Estimator (BLUE) – best = minimum variance

� Assume further that the noise on the data follows a gaussian distribution

� The Maximum Likelihood Estimator (MLE) is obtained by Least Squares

12

Two Cases of Interest

� (i) Unconstrained affine Linear Least Squares

� (ii) Norm-constrained homogeneous Linear Least Squares

� Notes� Both problems frequently appear in Computer Vision

(and in scientific computing in general)� (ii) handles (i) while (i) does not always handle (ii)

minx;Rn

kAx� bk2 with kbk 6= 0

minx;Rn

kAxk2 s.t. kxk = 1

TwotypesofLeastSquareProblem:

=

Linear Inhomogeneous Equations

A x b

m x n n x 1 m x 1

rank(A) = r < n : infinite number of solutions

rank(A) = n : exact solution

�n m : no exact solution in general (needs least squares)

�x = VD U b+ V + + Vr r n n"Ȝ Ȝ1 T+1 +1

Particular solution Homogeneous solution

where and . A = UDV T > @V = V Vn"1

�x = A b1

� �x = A A A b-1T T

xmin Ax -b 2

Î

or in MATLAB. x = A \ b

1)

2)

3)

2

7

The SVD and Spectral Decomposition

Let A = UVT be an SVD

with = diag(�1, . . . , �c) and V = (v1 · · · vc)

We have that

the �i are the square root of the eigenvalues of ATA

the vi are the eigenvectors of ATA

Proof

ATA = VTUTUVT = V2VT 8

Kernel, Rank and the SVD

SVD0

A VU

T

The rank is 2

The left kernel has dimension 5� 2 = 3

The right kernel has dimension 3� 2 = 1

Basis for the right kernel

Partial basis for the left kernel

F Vector x is in the right kernel of A i& Ax = 0

F Vector y is in the left kernel of A i& yTA = 0T

F The rank k of matrix A is the number of nonzero singular values(0 � k � min(r, c))

F The right kernel has dimension c� k

F The left kernel has dimension r � k

9

Linear Least Squares (LLS) Optimization: Problem Statement

minx;Rn

kAx� bk2 s.t. C(x) = 0

minx;Rn

mX

j=1

¡aTj x� bj

¢2s.t. C(x) = 0

A(m×n) =

C

hhH

.

.

.

aTj...

I

nnO and b(m×1) =

C

hhH

.

.

.

bj...

I

nnO

m ‘equations’ and n ‘unknowns’

‘constraints’ means both ‘equations’ and ‘unknowns’ and is thus ambiguous

10

A Bit of History…

� 1795 – discovery of Least Squares by Gauss� 1801 – used by Gauss in astronomical calculations

(prediction of the orbit of the asteroid Ceres)� 1805 – publication of an algebraic procedure by

Legendre (dispute with Gauss)� The method quickly became the standard procedure

(for analysis of astronomical and geodetic data)� 1809, 1821, 1823 – statistical justification by Gauss� 1912 – rediscovery by Markov

11

Why Least Squares?

� Assume the noise on b is Independently and Identically Distributed (i.i.d.)

� Least Squares gives the Best Linear Unbiased Estimator (BLUE) – best = minimum variance

� Assume further that the noise on the data follows a gaussian distribution

� The Maximum Likelihood Estimator (MLE) is obtained by Least Squares

12

Two Cases of Interest

� (i) Unconstrained affine Linear Least Squares

� (ii) Norm-constrained homogeneous Linear Least Squares

� Notes� Both problems frequently appear in Computer Vision

(and in scientific computing in general)� (ii) handles (i) while (i) does not always handle (ii)

minx;Rn

kAx� bk2 with kbk 6= 0

minx;Rn

kAxk2 s.t. kxk = 1

TwotypesofLeastSquareProblem:

Linear Homogeneous Equations

= A x 0

m x n n x 1 m x 1

Linear least square solve produces a trivial solution:

x = 0� �x = A A A bT TÎ

An additional constraint on to avoid the trivial solution: x x = 1

rank(A) = <r n -1 : infinite number of solutions

x = V + + Vr r n n"Ȝ Ȝ+1 +1

rank(A) = n -1 : one exact solution

x = Vn

¦n

ii = r

Ȝ2+1

1where

�n m : no exact solution in general (needs least squares)

xmin Ax 2

Î subject to x = 1 x = Vn

1)

2)

3)

Fundamental Matrix Estimation

x FxT2 1 0

ª º« » « »« »¬ ¼

xuv1

1 1

1

ª º« » « »« »¬ ¼

xuv2

2 2

1

Fundamental Matrix Estimation x FxT

2 1 0

� � � � � � � � u u f u v f u f v u f v v f v f u f v f f2 1 11 2 1 12 2 13 2 1 21 2 1 22 2 23 1 31 1 32 33 0

ª º ª º ª º« » « » « » « » « » « »« » « » « »¬ ¼ ¬ ¼ ¬ ¼

u f f f uv f f f v

f f f

T

2 11 12 13 1

2 21 22 23 1

31 32 33

0

1 1

ª º« »« »« »« »

ª º « »« » « » « » « »« » « »¬ ¼

« »« »« »« »¬ ¼

0

fff

u u u v u v u v v v u v ffffff

# # # # # # # # #

11

12

13

2 1 2 1 2 2 1 2 1 2 1 1 21

22

23

31

32

33

1

9

Therefore, 8 point correspondences are required to estimate a fundamental matrix.

Problem: squared error heavily penalizes outliers

Least squares: Robustness to noise

outlier!

SilvioSavarese

FittingGoal: Choose a parametric model to

fit a certain quantity from data

Techniques: •Least square methods

•RANSAC

•Hough transform

SilvioSavarese

Basic philosophy(voting scheme)

• Data elements are used to vote for one (or multiple) models

• Robust to outliers and missing data

• Assumption1: Noise features will not vote consistently for any single model (“few” outliers)

• Assumption2: there are enough features to agree on a good model (“few” missing data)

SilvioSavarese

Allbadapplesaredifferentandcomplicated

Goodapplesaresameandsimple

GE �),(Pf

OSmin^ `OPI ,: oS

such that:

� � TT PPPPf 1),( �� EE

G

Model parameters

RANSAC

Fischler & Bolles in ‘81.

(RANdom SAmple Consensus) :Learning technique to estimate parameters of a model by random sampling of observed data

SilvioSavarese

RANSAC

Algorithm:1. Select random sample of minimum required size to fit model 2. Compute a putative model from sample set3. Compute the set of inliers to this model from whole data setRepeat 1-3 until model with the most inliers over all samples is found

Sample set = set of points in 2D

SilvioSavarese

RANSAC

Algorithm:1. Select random sample of minimum required size to fit model [?]2. Compute a putative model from sample set3. Compute the set of inliers to this model from whole data setRepeat 1-3 until model with the most inliers over all samples is found


=[2]

SilvioSavarese

G

RANSAC


O = 14


=[2]

SilvioSavarese

G

RANSAC

Fischler & Bolles in ‘81.

(RANdom SAmple Consensus) :

O = 6


SilvioSavarese

How many samples?• Number of samples N

– Choose N so that, with probability p, at least one random sample is free from outliers (e.g. p=0.99) (outlier ratio: e )

• Initial number of points s– Typically minimum number needed to fit the model

• Distance threshold G– Choose G so probability for inlier is p (e.g. 0.95) – Zero-mean Gaussian noise with std. dev. ı: t2=3.84ı2

� � � �� se11log/p1logN �� proportion of outliers e

s 5% 10% 20% 25% 30% 40% 50%2 2 3 5 6 7 11 173 3 4 7 9 11 19 354 3 5 9 13 17 34 725 4 6 12 17 26 57 1466 4 7 16 24 37 97 2937 4 8 20 33 54 163 5888 5 9 26 44 78 272 1177

Source: M. Pollefeys

Pr = RPl + T R:rotationofframe‘r’ to‘l’T:vector(O2,O1)inref.‘r’

pTr Epl = 0 E = T⇥R

E = RT⇥

Pr = R(Pl � T )

pTr Epl = 0

R:rotationofframe‘r’ to‘l’T:vector(O1,O2)

RecovercameraposefromE

RelatingSVDto(t,R)

Point Triangulation

2D correspondences

Point Triangulation

2D correspondences

3D camera pose

2D projection

3D point

ª º« »¬ ¼

x =uv1

11

ª º« »« »« »¬ ¼

X =XYZ

u�P \3 41

Point Triangulation

2D correspondences

3D camera pose

2D projection

3D point

ª º ª º« » « »¬ ¼ ¬ ¼

x X= PȜ 1

11 1

ª º« »¬ ¼

x =uv1

11

ª º« »« »« »¬ ¼

X =XYZ

u�P \3 41

Point Triangulation

2D correspondences

3D camera pose

2D projection

3D point

ª º ª º« » « »¬ ¼ ¬ ¼

x X= PȜ 1

11 1

ª º« »¬ ¼

x =uv1

11

ª º« »« »« »¬ ¼

X =XYZ

u�P \3 41

u

ª º ª º « » « »

¬ ¼ ¬ ¼

x XP 01

11 1Cross product between two parallel vectors equals to zero.

Cross product as a matrix operator

v ⇥ u = v̂ · u

v̂ =

2

40 �x3 x2

x3 0 �x1

�x2 x1 0

3

5

Point Triangulation

2D correspondences

3D camera pose

2D projection

3D point

ª º« »¬ ¼

x =uv1

11

ª º« »« »« »¬ ¼

X =XYZ

ª º ª º« » « »¬ ¼ ¬ ¼

x X= PȜ 1

11 1 u

ª º ª º « » « »

¬ ¼ ¬ ¼

x XP 01

11 1

u

ª º ª º « » « »

¬ ¼ ¬ ¼

x XP 02

21 1

u�P \3 41

P2

Point Triangulation

2D correspondences

3D camera pose

2D projection

3D point

ª º« »¬ ¼

x =uv1

11

ª º« »« »« »¬ ¼

X =XYZ

ª º ª º« » « »¬ ¼ ¬ ¼

x X= PȜ 1

11 1 u

ª º ª º « » « »

¬ ¼ ¬ ¼

x XP 01

11 1

u

ª º ª º « » « »

¬ ¼ ¬ ¼

x XP 02

21 1

u�P \3 41

P2

u

u

u

ª ºª º« »« »¬ ¼« »

« »ª º« » ª º« » « »¬ ¼ « »¬ ¼« »

« »« »ª º« »« »« »¬ ¼¬ ¼

xP

xP X

0

xP

#

11

22

1

11

1F

F

Point Triangulation

2D correspondences

3D camera pose

2D projection

3D point

ª º« »¬ ¼

x =uv1

11

ª º« »« »« »¬ ¼

X =XYZ

ª º ª º« » « »¬ ¼ ¬ ¼

x X= PȜ 1

11 1 u

ª º ª º « » « »

¬ ¼ ¬ ¼

x XP 01

11 1

u

ª º ª º « » « »

¬ ¼ ¬ ¼

x XP 02

21 1

u�P \3 41

P2

u

u

u

ª ºª º« »« »¬ ¼« »

« »ª º« » ª º« » « »¬ ¼ « »¬ ¼« »

« »« »ª º« »« »« »¬ ¼¬ ¼

xP

xP X

0

xP

#

11

22

1

11

1F

F

PF

Point Triangulation

2D correspondences

3D camera pose

2D projection

3D point

ª º« »¬ ¼

x =uv1

11

ª º« »« »« »¬ ¼

X =XYZ

ª º ª º« » « »¬ ¼ ¬ ¼

x X= PȜ 1

11 1 u

ª º ª º « » « »

¬ ¼ ¬ ¼

x XP 01

11 1

u

ª º ª º « » « »

¬ ¼ ¬ ¼

x XP 02

21 1

u�P \3 41

P2

u

u

u

ª ºª º« »« »¬ ¼« »

« »ª º« » ª º« » « »¬ ¼ « »¬ ¼« »

« »« »ª º« »« »« »¬ ¼¬ ¼

xP

xP X

0

xP

#

11

22

1

11

1F

F

4

3F

PF

u

ª º« »¬ ¼

xrank( P) = 2

1

t 2FLeast squares if

Point Triangulation

> @u P K I 01 1 3 3 3 > @u P K I -C2 2 3 3

ª º« » « »« »¬ ¼

C1

0

0

(1382,986) (1144,986)

Point Triangulation

X = 0.7111 0.1743 6.8865 1.0000

% Intrinsic parameter K1 = [2329.558 0 1141.452; 0 2329.558 927.052; 0 0 1]; K2 = [2329.558 0 1241.731; 0 2329.558 927.052; 0 0 1]; % Camera matrices P1 = K1 * [eye(3) zeros(3,1)]; C = [1;0;0]; P2 = K2 * [eye(3) -C]; % Correspondences x1 = [1382;986;1]; x2 = [1144;986;1]; skew1 = Vec2Skew(x1); skew2 = Vec2Skew(x2); % Solve A = [skew1*P1; skew2*P2]; [u,d,v] = svd(A); X = v(:,end)/v(end,end); function skew = Vec2Skew(v) skew = [0 -v(3) v(2); v(3) 0 -v(1); -v(2) v(1) 0];

Point Triangulation

PointTriangulation

Alternativesolution

Alternativesolution

CSE486, Penn State

Robert Collins

Calibrated Triangulation

eerreell OOrr

pprr

PP

ppll

OOllTT

PPll RRTTPPrr

ideally, P is the point of intersection of two 3D rays:

ray through Ol with direction Pl

ray through Or with direction RTPr

CSE486, Penn State

Robert Collins

Triangulation with Noise

Unfortunately, these rays typically don’t intersect

due to noise in point locations and calibration params

P?P?

OOrrOOllTT

PPllRRTTPPrr

CSE486, Penn State

Robert Collins

Triangulation with Noise

Unfortunately, these rays typically don’t intersect

due to noise in point locations and calibration params

Solution: Choose P as the “pseudo-intersection point”.

This is point that minimizes the sum of squared distance

to both rays. (The SSD is 0 if the rays exactly intersect)

OOrr

PP

OOllTT

PPllRRTTPPrr

CSE486, Penn State

Robert Collins

Solution from T&V Book

OOrr

PP

OOllTT

PPllRRTTPPrr

Let w = Pl x RTPr (this is perpendicular to both)

ww

P is midpoint of the segment perpendicular to Pl and RTPr

Introducing three unknown scale factors a,b,c we

note we can write down the equation of a “circuit”

CSE486, Penn State

Robert Collins


OOrrOOll

Writing vector “circuit diagram” with unknowns a,b,c

a Pa Pll

a Pl

c wc w

+ c (Pl X RTPr)

- b - b RRTTPPrr

- b RTPr

TT

= T

note: this is three linear equations in three unknowns a,b,c

=> can solve for a,b,c

CSE486, Penn State

Robert Collins


OOrrOOll

After finding a,b,c , solve for midpoint of line segment

between points Ol + a Pl and Ol + T + b RT Pr

a Pa Pll

b b RRTTPPrr

TT

Camera 3D Registration Perspective-n-Point Algorithm

2D images


3D point cloud via triangulation


3D point cloud via triangulation Where?

Perspective-n-Point

3D point cloud

�X \3

Perspective-n-Point

2D image

3D point cloud

�x \2

�X \3

Perspective-n-Point

2D image

3D point cloud

lx X2D-3D correspondences

�x \2

�X \3

Perspective-n-Point

2D image

3D point cloud

u�P \3 4

�x \2

�X \3

Perspective-n-Point

2D image

3D point cloud

u�P \3 4

�x \2

�X \3

ª º ª º« » « »¬ ¼ ¬ ¼

x X= PȜ 1 1

1 1 u

ª º ª º « » « »

¬ ¼ ¬ ¼

x XP 01 1

1 1

Perspective-n-Point

2D image

3D point cloud

u�P \3 4

�x \2

�X \3

u u u

u u u

u u u u

ª º ª º �ª º�ª º ª º ª º ª º« » « » « »« » « » « » « » � �« » « » « »« » « » « » « »« » « » « »« » « » « » « »� �¬ ¼ ¬ ¼ ¬ ¼ ¬ ¼ ¬ ¼« » « »¬ ¼ ¬ ¼

X X 0 0 0 X X

X X 0 X 0

P

X 0 X

X 0 0

P PP P PP X XPP

vu u vv v u u

v u v

� � � �

� � � � �

� � �

T T T1 1 1

T11 1

T2 2 2

4 1 4 1 4 1 1

T T T1 1 1 4 1 1 4 1 1 4 1

T T1 1 4 1 4 1 1

T3 33

0 1

1 0

1 1 0u

ª º ª º« » « » « » « »« » « »¬ ¼« »¬ ¼

PP 0PX 0u �

T1T

2TT

1 31 4

u

ª º ª º « » « »

¬ ¼ ¬ ¼

x X0P1 1

1 1

u u u

u u u

u u u u

ª º ª º �ª º�ª º ª º ª º ª º« » « » « »« » « » « » « » � �« » « » « »« » « » « » « »« » « » « »« » « » « » « »� �¬ ¼ ¬ ¼ ¬ ¼ ¬ ¼ ¬ ¼« » « »¬ ¼ ¬ ¼

X X 0 0 0 X X

X X 0 X 0

P

X 0 X

X 0 0

P PP P PP X XPP

vu u vv v u u

v u v

� � � �

� � � � �

� � �

T T T1 1 1

T11 1

T2 2 2

4 1 4 1 4 1 1

T T T1 1 1 4 1 1 4 1 1 4 1

T T1 1 4 1 4 1 1

T3 33

0 1

1 0

1 1 0u

ª º ª º« » « » « » « »« » « »¬ ¼« »¬ ¼

PP 0PX 0u �

T1T

2TT

1 31 4

Perspective-n-Point

2D image

3D point cloud

u�P \3 4

�x \2

�X \3

u

ª º ª º « » « »

¬ ¼ ¬ ¼

x X0P1 1

1 1

Perspective-n-Point

2D image

3D point cloud

u�P \3 4

�x \2

�X \3

u u u

u u u

u u u u

ª º ª º �ª º�ª º ª º ª º ª º« » « » « »« » « » « » « » � �« » « » « »« » « » « » « »« » « » « »« » « » « » « »� �¬ ¼ ¬ ¼ ¬ ¼ ¬ ¼ ¬ ¼« » « »¬ ¼ ¬ ¼

X X 0 0 0 X X

X X 0 X 0

P

X 0 X

X 0 0

P PP P PP X XPP

vu u vv v u u

v u v

� � � �

� � � � �

� � �

T T T1 1 1

T11 1

T2 2 2

4 1 4 1 4 1 1

T T T1 1 1 4 1 1 4 1 1 4 1

T T1 1 4 1 4 1 1

T3 33

0 1

1 0

1 1 0u

ª º ª º« » « » « » « »« » « »¬ ¼« »¬ ¼

PP 0PX 0u �

T1T

2TT

1 31 4

u

ª º ª º « » « »

¬ ¼ ¬ ¼

x X0P1 1

1 1

Perspective-n-Point

2D image

3D point cloud

u�P \3 4

�x \2

�X \3

u

u

u

ª º� ª º« » « »� « » « »« » « »� ¬ ¼« »¬ ¼

0 X X PX 0 X P 0

PX X 0

v

u

v u

� �

� �

� �

T T T1 4 1 1 1 1

T T T1 1 4 1 1 2

TT T31 1 1 1 1 4

u

ª º ª º « » « »

¬ ¼ ¬ ¼

x X0P1 1

1 1

Perspective-n-Point

2D image

3D point cloud

u�P \3 4

�x \2

�X \3

u

u

u

ª º� ª º« » « »� « » « »« » « »� ¬ ¼« »¬ ¼

0 X X PX 0 X P 0

PX X 0

v

u

v u

� �

� �

� �

T T T1 4 1 1 1 1

T T T1 1 4 1 1 2

TT T31 1 1 1 1 4

u

u

u

ª º� ª º« » « »� « » « »« » « »� ¬ ¼« »¬ ¼

0 X X PX 0 X P 0

PX X 0

v

u

v u

� �

� �

� �

T T T1 4 2 2 2 1

T T T2 1 4 2 2 2

TT T32 2 2 2 1 4

u

ª º ª º « » « »

¬ ¼ ¬ ¼

x X0P1 1

1 1

Perspective-n-Point

2D image

3D point cloud

u�P \3 4

�x \2

�X \3

u

u

u

ª º� ª º« » « »� « » « »« » « »� ¬ ¼« »¬ ¼

0 X X PX 0 X P 0

PX X 0

v

u

v u

� �

� �

� �

T T T1 4 1 1 1 1

T T T1 1 4 1 1 2

TT T31 1 1 1 1 4

u

u

u

ª º� ª º« » « »� « » « »« » « »� ¬ ¼« »¬ ¼

0 X X PX 0 X P 0

PX X 0

v

u

v u

� �

� �

� �

T T T1 4 2 2 2 1

T T T2 1 4 2 2 2

TT T32 2 2 2 1 4

= vec(P)

u

ª º ª º « » « »

¬ ¼ ¬ ¼

x X0P1 1

1 1

Perspective-n-Point

2D image

3D point cloud

u�P \3 4

�x \2

�X \3

u

u

u

ª º� ª º« » « »� « » « »« » « »� ¬ ¼« »¬ ¼

0 X X PX 0 X P 0

PX X 0

v

u

v u

� �

� �

� �

T T T1 4 1 1 1 1

T T T1 1 4 1 1 2

TT T31 1 1 1 1 4

u

u

u

ª º� ª º« » « »� « » « »« » « »� ¬ ¼« »¬ ¼

0 X X PX 0 X P 0

PX X 0

v

u

v u

� �

� �

� �

T T T1 4 2 2 2 1

T T T2 1 4 2 2 2

TT T32 2 2 2 1 4

= vec(P)

rank( ) = 2

t 6PLeast squares if

12

3P

u

ª º ª º « » « »

¬ ¼ ¬ ¼

x X0P1 1

1 1

Least Square (triangulation +...

Documents

Transcript of Least Square (triangulation +...