Nghiên cứu và gải bài toán khôi phục cấu trúc xạ ản từ ba ảnh trong thị giác máy tính 9

Luận văn thạc sĩ chuyên ngành hình học và tôpô đề tài Nghiên cứu và gải bài toán khôi phục cấu trúc xạ ản từ ba ảnh trong thị giác máy tính

PHt) Lt)C A

A.t Tich hai vectd

Cho vector x =(XUX2,X3)T, Y =(YI'Y2'Y3f trang khong gian rp2 Khi

d6:

Tich vo huang hai vectd xy =(XIYI + X2Y2 + X3Y3)

Tich hUll huang (tich cheo):

]

T X2 Y2 X3 Y3 Xl YI

X X Y =("'" y,' X, y.' X, y,

= (X2Y3 - X3Y2' X3YI - XIY3' XIY2 - X2YI)T

vie't ke't qua d<;lngma tr~n

X2 IIYI

-XI II

Y21 =[X]xY

trang d6 [x]x la ma tr~n phan d6i xung.

Ne'u vectd z thoa zy =0 va zx =0 thl n6 c6 d<;lngz =k(x x y) vai

k la h~ng so"khac O.

A.2 Phan tich ghi trj ky elfcua ma tr~n - SVD [3],[4] ,[8]

Ba't ky ma tr~n (thljc) hlnh chu nha't d€u c6 th€ phan tich thanh tich ba ma tr~n d~c bi<%t.

Trang d6:

A la ma tr~n ca'p m Xn, ta xet truong hQpm < n.

X2Y3 - X3Y21 I 0

-X3

X X Y = IX3YI - XIY3 = X3 0

XIY2 - X2YII I-X2 Xl

U va Y la hai ma tr~n tnfc giao cffp m x m va n X n (nghla la

uuT = 1m' yyT = In), cac c(}tcua U va Y la cac vecto d~c tning cua ma

tr~n AAT va AT A tudng ung.

E la ma tr~n cffp m x n va duQcbi€u di€n E = (DI 0) D la ma tr~n

dtiong cheo cffp m x m, cac ph§n tti' tren dtiong cheo dtidng giam d§n, co gia tri~

b~ng din b~c hai cua cac gia tri d~c tru'ng cua AT A , con duQc gQila gia tfi ky di

cua A 0 la ma tr~n cffp m X(n - m) co tfftca cac ph§n tii'la o.

- H~ng cua ma tr~n A: rank(A) =m b~ng s6 ph§n tii' khac 0 tren

duong cheo cua ma tr~n D.

A.3 Phu'dng phap binh phu'dng nho nha't - x =LeastSquares(A).

Day la vffn d€ thtiong g~p trong cac bai loan kh6i phl;1c,dung d€ tidc ltiQng

phep d6ng anh, ma tr~n cd sa GQi ma tr~n A cffp m x n, x la vectd thu§n nha't

n chi€u vdi m < n nghla la h~ Ax =0 co s6 phuongtrlnh it hon s6 ffn.Trong

ph§n nay chI xet hai trtiong hQp h~ co m(}t b~c tv do va h~ co hai b~c tv do

Gia sii' A duQc phan rich thanh A =UEyT nhutrong (A.2)

- H~ co m(}tb~c tv do, rank (A) = n-l, m = n-l, thi c(}tcu6i cung

- H~ co hai b~c hI do, rank (A) =n - 2, m =n - 2, thi hai c(}t cu6i

0 0 O"m 0 0

Chung minh.

Ta thfty ma tr~n dang xet A du'<jchlnh thanh tu t~p tQa dQ anh nen co kha nang xay ra sai s6 Do do ngoai nghi~m tfim thu'ong x =0 co th€ khong t6n t~i

Wi giai khac thoa Ax =o N€u loi giai khong t6n t~i thl co th€ tlm mQt vectd x gfin nhftt thoa Ax ~ o.

Do x Ia vectd thufin nhftt va d€ tranh tru'ong h<jp x =0, khong mftt tinh t6ng quat ta them rang buQc IIxll=1 (lIxll=xT x). Tu bai toan ban dfiu ta du'a v~

bai toan tlm x d€ ctlc ti€u hoa ham m1,1Ctieu IIAxl1voi rang buQc IIxll= 1.

Ta co: IIAxl1= IIUEyTxl1 =xTYET UTUEyTx (do UUT =1m)

=xTYETEyTx =IIEyTxl1

{

IIAXII =IIEyl1

va =}

Ilyll =IlyT xii=fTyyT x =xT X =IIxll =1

Xet h~ co m(}t b~c tt! do

Thi d1,1ma tr~n A trong phep d6ng anh co cftp 8x 9 co rank(A) =8 khi

do theo (A.2.2) thl co tftt ca cac phfin tii' trong cQt cu6i cling cua ma tr~n E co

gia tri O Nhu' v~y, ta chQn y =(0,0, , 0, 1) thl thoa Ilyll=1 va liE yll nho nhftt

Nhu' v~y theo (A.3.4) thl nghi~m x =yy =Vn chinh Ia CQtcu6i cling cua V

thoa (A.3.3) va ding dam baa rang buQc IIxll= Ilvnll = v~vn = 1 do Y Ia ma tr~n

trtlc giao V~y k€t qua (A.3.1) du'<jcchung minh

Xet h~ co hai b~c hi do.

Thi d\l ma tr~n A trong tinh ma tr~n cd Sa cho tru'ong h<Jp7 di€m ho~c 3 di€m c6 ca'p 7x9 c6 Tank(A) = 7 ho~c 3 x5 co Tank(A) =3 khi d6 theo

(A.2.2) thl c6 ta't ca cac phffn ta trong hai c9t cu6i cling cua ma tr~n S c6 gia tri

O Nhu' v~y, ta chQn YI= (0,0, , 1,0), Y2 =(0,0, , 0, 1) d€u thoa Ilyll= 1 va

IlL yll nho nha't Nhu' v~y theo (A.3.4) thl nghi~m Xl =VYI = Vn-l va

X2= VY2 = Vnchinh Ia hai c9t cu6i cling cua V thoa (A.3.3)va cling dam baa

rang bU9C IlxI11= Ilvn-111= V~-IVn-1 = 1 va IIX211= IIVnl1= V~Vn = 1 do V Ia

ma tr~n tn!C giao V~y k€t qua (A.3.2) du'<Jcchung minh

K€t qua nay con du'<Jcbi€t d€n voi ten gQi "phu{fflgphdp binh phu{fflg nhd

nhdt" hay sa d\lng L2 d€ t6i u'uh6a.

A.4 TIm a trong bli8c 4 cua thu~t tmln 4.2

Tli F = aFI + (1- a)F 2 va det(F) = 0 ta suy fa:

0

trong d6:

Cs =Tlqlul + T2q2uI - T2Q2u2- TIQ2uI - TIQlu2 - ~PIS2 + ~PISI - t2PISI

- T2qlul + Tlq2u2 + tlP2S2- tlP2S1 + T2Qlu2 - t2P2S2 + t2P2S1+ t2PIS2

C2 =-2t2P2s1 - 2T2q2uI + TIQlu2 + 3T2Q2u2+ ~P2S1 + TIQ2uI + t2PISI

- 2~P2S2- 2TIQ2u2 + T2QluI+ 3t2P2S2+ ~PIS2- 2~PIS2- 2T2Qlu2

CI =-3T2Q2u2 + T2Q2~ + ~PIS2 + T2%U2+ TIQ2u2- 3~P2S2 + i1.P2s2

+ t2P2S1

Co=t2P2S2 + T2Q2u2

PH{) L{)C B

THU~ T TOAN TiNH PHEP DONG ANH TRONG p2

r:r Phat bi~u bili toaD Cho n > 4 c~p di~m 2D {mi ++ m;} tu'dng ling

lIen hai anh Xac dinh phep d6ng anh H thoa m; =Hmi, i = 1,n

r:r PhaD tich

Dc1u"=" trong dAng thlic m; = Hmi co nghla hai vectd thu§n nhc1t

m;, Hmi cling phu'dng va chung sai khac nhau mQt h~ng s6 d I~ a 7: 0, nghla

la m; = aHmi'

Vi hai vectd m;, Hmi cling phu'dng nen tich cheo cua chung b~ng 0,

0 G'??

ng la a mi X mi = la sutQa Q t uann atcua mi = Xi'Yi,Wi va Y

hi~u hJ Ia hang thli j cua ma tr~n H, ta co:

Hmi=

hfmi hfmi h[mi

h2T

Y ~ 3 m - W.~ ~ m.~

:::;> m~ X Hm.~ ~ = I w~hT m - x~hT m.1~"1. ~ ~"'3 ~ =0 (B.!)

' h2T ,

M~t khac, hJmi =m[hj voi j =1,3, i =1,n nen (B.l) du'<;5Cviet l~i d~ng tich cua mQtma tr~n ~ cc1p 3 x9 voi vectd h 9-chi€u nhu'sau:

-wimi Yimi

h =I w;m; OT

-ximi

OT -Yimi ximi

ChI co 2 dong cua ~ la dQc l~p tuye'n tinh, nen m6i c~p di~m tu'ong ung cho hai phu'ong trinh, nghla la

OT

y;mJ 1I~

-x' im.T~ II II~ I =0

h3

(B.3 )

OT

.

, wimi

I T

-wimi

c:r Thu~ 1 1m!n.

Btioc 1 Dung m6i c~p di~m tu'ong ung mi ++ m; tinh ma tr~n ~ theo c6ng thuc 4.3

Btioc 2 Thie't l~p ma tr~n A co ldch thu'oc 2n X 9 b~ng cach xe'p ch6ng

n ma tr~n ~ len nhau

Btioc 3 Giiii Ah =0

Btioc 4 TImma tr~n H tu cac thanh ph~n tu'ongung cua vectd h.

PHV LVC C

CHUNG MINH THU~ T TOAN 3.2

D~ chung minh thu~t toan 3.2 tru'oc he"tcffn chung minh mQt s6 ke"tqua d~ dam baa thu~t toan co th~ thl;tchi~n du'qc

Ke't qua C.I T~p di~m anh {m; : i = 1,n,j = 1,k} co th~ thl;tchi~n rut

gQn du'qc khi va crn khi no co th~ du'qc b6 sung nhung di~m tu'dng ung

-mk+h =eh' h =1,4 ma:

- Ta't ca nhung di~m anh la co thi th1!Chi~n du(1c,nghla la m; =pi Mj

- Nhung di~m du'qc khoi phl;lC M k+h tu'dng ung voi nhung di~m anh them

vao thl khong d6ng ph~ng

Chung minh

- Gia sa t~p cho phep thl;tchi~n rut gQnva d~t pi la t~p ma tr~n Camera

dC;!ng rut gQn.D~t nhung di~m Mk+h = Eh, h = 1,4 du'Qcxc;!anh vao n anh.

XC;!anh nay la m~+h =piMk+h =piEh =eh' 'r;fi.

- Ngu'QcIC;!i,gia sa t~p di~m dff du'Qcthem vao la thl;tchi~n du'QCva nhung

di~m Mk+h khong d6ng ph~ng Trong du'ong hQp nay, co th~ chQn cd sd xC;!anh

sao choMk+h =Eh' Thl m6i m~t ph~ng anh d~u co eh = piEh' 'r;fh.Tlt di~u

nay suy ra dng m6i pj d~u co dC;!ngrut gQnnhu' (3.4)

Ke't qua C.2 N€u t~p di~m anh {m~ : i = 1,n,j = 1, k} cho phep thlfc

hi~n rUt gQn thl cho phep chuy~n vi thanh t~p {fn1 : i = 1,n,j = 1,k} trongdo

j i \-I'

mi = mj' v~, J

Day Ia tinh d6i ngfiu cd ban, thljc t€ dff du(/c chung minh d buGC6 cua thu?~t loan Bay giOco th~ chung minh du(/c tinh dung d~n cua thu?t loan

Ke't qua C.3 Cho m~ va m~+h nhu trong hlnh 3.2.a Ia t~p di~m anh tudng

ung co th~ thljc hi~n du(/c, va gia su

VGi m6i i, trong b6n di€m m~+h kh6ng co ba di~m nao th~ng hang.

B6n di~m M k+h trong k€t qua kh6i ph\lC X<;lanh kh6ng d6ng ph~ng.

Thl thu? t loan A * (n, k + 4) du(/c chung minh.

Chung minh

Bdi VI di~u ki~n d~u, t6n t<;liphep d6ng anh Hi VGi m6i i, dam bao bi€n d6i dfi' li~u nhu hlnh 3.2.b Dfi' li~u du(/c bi€n d6i nay cling co th~ thljc hi~n

du(/c do chung bao loan qua phep d6ng anh Hi.

Theo k€t qua c.! ap d\lng ten hlnh 3.2.b nhfi'ng di~m tudng ung mji co

th~ rut gQn du(/c Theo k€t qua c.2 dfi'li~u dff chuy~n vi nhu hlnh 3.3.a cling co th€ rut gQn du(/c Ap d\lng k€t qua c.! mQt I~n nfi'a chI ra dng dfi'li~u dff du<;1c

md rQng nhu hlnh 3.3.b Ia co th~ thljc hi~n du<;1c.Hdn th€ nfi'a, nhfi'ng di€m

1\1k+h thl kh6ng d6ng ph~ng, va VIv~y buGC5 Ia h<;1pI~ BuGC6,7 co th€ thljc hi~n du(/c do tinh d6i ngfiu va tinh chfft cua hlnh hQc X<;lanh, hlnh hQc Camera Nhu v~y thu~t loan du(/c chung minh

PHT) LT)C D

function [M] =Reconstruction(m)

% Xem thu~n roan 4.4, trang 76

% m: ma tr~n 9xn trang do m6i d)t chlia bi) 3 diim dnh tliO'ngling i.e m6i ci)t

% co d9ng (xl,yl,wl,x2,y2,w2,x3,y3,w3)' n>= 6.

% index: mdng srI nguyen co chdu dai t(5'ithiiu fa 6 (tham so'tity chQn)

% M: mdng co 1 ho(ic 3 phdn ta tity thui)c VaGso' ma tr~n C(Jsa rut gQn tim dlir;fC

% a bliac 3 M6i phdn ta M{i} fa ma tr~n 4xn chlia tQa di) cua t~p diim X9 dnh

% khoi ph1;tc dlir;fC.

% A: ma tr~n 6x4

[u,n] =size(m);

% Bztdc O ChQn ngJu nhien 6 diim dnh trong t~p n diim.

tmp =randperm(n);

index =tmp(l :6);

for i =1:3

mi(:,:,i) =m((i-I)*3+ 1:i*3,:);

end

% Bztdc 1 ChQnngJu nhien 4 diim dnh trang 6 diim a bliac 0 di bien d6i vaG

% el, e2, e3, e4

tmp2=randperm(6);

idx_to_map =index(tmp2(l:4));

other_idx =index(tmp2(5:1en));

% Kiim tra trang 4 diim tren khong co 3 diim naG thdng hang

% Rude 2 TImphep d6ng anh rut gQn

for i=I:3

H(:,:,i) = RedueedHomography(mi(:,idx_to_map,i)) ;

mhat(:,:,i) = H(:,:,i)*mi(:,:,i);

end

% Rude 3 TImma tr~n ca sa rut gQn

mhaC = mhat(:,other_idx,:);

FhaC = RedueedFundamentaIMatrix(squeeze(mhac(:,I,:)),

squeeze(mhaC(:,2,:)));

% Co thi nh~n dur;c 1 hoc:tc3 (co 2 ktt qua sai sf! rat hYn)ktt qua kh6i phljC

for solution= 1:length(FhaC)

% Rude 4 TIm cac tham s6'a, b, c, d

Fhat =FhaC {solution};

abe =LeastSquares([O Fhat(2,3) Fhat(3,2);

Fhat(1,3) 0 Fhat(3,1);

Fhat(1,2) Fhat(2,1) 0]);

dabe = LeastSquares(Fhat');

A =[SkewSymmetrie(abe) zeros(3,1);

SkewSymmetrie(dabe) [dabe(2)-dabe(3); dabe(3)-dabe(1); dabe(l )-dabe(2)] ] ;

abed =LeastSquares(A);

% Rude 5 Xac dtnh 6 diim trong kh6i phljc rut gQn

% 5 diim ddu lien

M5 =[1 000; 0 1 00; 00 1 0; 000 1; 1 1 1 1]';

% Diim tha 6 fa vect(f abcd=[a b c dj'

Mi = [M5 abed];

% Haile 6 Tinh 3 ma tr(in Xc;L anh tit 6 diim Xc;L anh va diim anh tuang zIng

p =[];

for view=I:3

P(:,:,view) = ProjeetiveMatrix(Mi, mi(:,[idx_to_map othecidx],view));

end

% Bztile 7 Khoi phl;le ea'u true Xc;L anh eha t(ip n diim

M {solution} = Triangulation(P,m);

end

return

-function [H] =ReducedHomography(m)

% Xem thu(it taan 4.1, trang 69

% m: ma tr(in 3x4, luu tQadt?4 diim anh

% H: ma tr(in d6ng anh bie'n 4 diim anh VaG{e1,e2,e3,e4}

% A: ma tr(in 3x3

hI = cross(m(:,2), m(:,3));

h2 = cross(m(:,3), m(:,I));

h3 = cross(m(:,I), m(:,2));

% L(ip h~ thudn nha't Aa =0

A =[

0 dot(h2,m(:,4)) -dot(h3,m(:,4));

dot(h3,m(:,4));

-dot(hl,m(:,4)) 0

dot(hl,m(:,4)) -dot(h2,m(:,4)) 0];

alphas = LeastSquares(A);

H = [alphas(1)*hl '; alphas(2)*h2'; alphas(3)*h3'];

return

function [F] =ReducedFundamentaIMatrix(ml, m2)

% Xem thu(it loan 4.2, trang 71

% ml, m2.oma trt7n 3x3, cQt thli i cua ma tr(in se chlia ciJ-pdiim tLtang ling thli i

% F.omdng co 1 hoiJ-c3 phdn tii, mlJiphdn tiila mQtma tr(in co sa 3x3

% A: ma tr(in 3x5

A=[];

% L(ip h~ thudn nh{{t Af =0

for i=1:3

A(i,:) = [m2(1,i)*ml(2,i)- m2(3,i)*ml(2,i)

m2(1,i)*ml(3,i) -m2(3,i)*ml(2,i)

m2(2,i)*ml(1,i) - m2(3,i)*ml(2,i)

m2(2,i)*ml(3,i) - m2(3,i)*ml(2,i)

m2(3,i)*ml(1,i) - m2(3,i)*ml(2,i)];

end

[fl,f2] =LeastSquares2(A);

fl(6) =-sum(fl(1:5));

f2(6) =-sum(f2(1 :5));

Fl =[0 fl(1) fl(2);

fl(3) 0 fl(4);

fl(5) fl(6) 0];

F2 =[0 f2(1) f2(2);

f2(3) 0 f2(4);

f2(5) f2(6) 0];

% B.4.oF =alpha*Fl + (J-alpha)*F2, det(F) =0

% Cac h~ sa cua phuang trlnh b(ic 3

coeff3 =

Fl(2,1)*Fl(1,3)*Fl(3,2)+F2(2,1)*F2(1,3)*Fl(3,2)-F2(2, 1)*F2(1,3)*F2(3,2)-FI(2, 1)*F2(1,3)*FI (3,2)-FI(2,

1)*FI(1,3)*F2(3,2)-FI (3, l)*1)*FI(1,3)*F2(3,2)-FI (1,2)*F2(2,3)+1)*FI(1,3)*F2(3,2)-FI (3, 1)*1)*FI(1,3)*F2(3,2)-FI(1,2)*1)*FI(1,3)*F2(3,2)-FI F2(3, I )*1)*FI(1,3)*F2(3,2)-FI (1,2)*1)*FI(1,3)*F2(3,2)-FI (2,3)-F2(2, I )*FI (1,3 )*FI (3,2)+FI (2, I )*F2( 1,3)*F2(3,2)+FI (3, I )*F2( I ,2)*(2,3)-F2(2,3)-

,2)*F2(2,3)-FI (3, I )*F2( I ,2)*,2)*F2(2,3)-FI (2,3 )+F2(2, I )*,2)*F2(2,3)-FI (1,3 )*F2(3,2)-F2(3, I )*F2( I ,2)*F2(2,3)+ F2(3, I )*F2( I ,2)*FI (2,3)+F2(3, I )*FI(1 ,2)*F2(2,3);

coeff2 =-2*F2(3,1)*F2(1,2)*FI(2,3)-2*F2(2,1)*F2(1,3)*FI(3,2)+

FI (2, I )*FI (1,3 )*F2(3,2)+ 3*F2(2, I )*F2(1,3)*F2(3,2)+FI (3, 1)*F2(1 ,2)*FI(2,3) +

FI (2, I )*F2( I ,3)*FI (3,2)+F2(3, I )*FI (1,2)*FI

(2,3)-2*FI (3, I )*F2(1 ,2)*F2(2,3)(2,3)-2*FI (2, I )*F2(1,3)*F2(3,2)+F2(2, 1)*FI(I,3)*FI(3,2) + 3*F2(3, I )*F2(1 ,2)*F2(2,3)+FI (3, I )*FI (1,2)*F2(2,3)-2 *F2(3, I )*FI (I ,2)*F2(2,3) -2 *F2(2, I )*FI (I ,3)*F2(3,2);

coeffl =-3*F2(2, 1)*F2(1,3)*F2(3,2)+F2(2, 1)*F2(1,3)*FI(3,2)+

F2(3, I)*FI (1,2)*F2(2,3)+F2(2, I)*FI(I ,3)*F2(3,2)+FI(2, 1)*F2(1 ,3)*F2(3,2)-3*F2(3, 1)*F2(1,2)*F2(2,3)+FI(3, 1)*F2(1,2)*F2(2,3) + F2(3, 1)*F2(1,2)*Fl(2,3);

coeffO = F2(3,1)*F2(1,2)*F2(2,3)+F2(2,1)*F2(1,3)*F2(3,2);

% Gidi phuang trinh b(ic 3 c3ci + c2a? + cIa? + Co=0

alphas =roots([coeff3 coeff2 coeffl coeffO]);

% Nh(in 1 nghi~m thljC(lof:li2 nghi~m phac) ho(ic 3 nghi~m thljC

solutions =alphas(find(imag(alphas) == 0));

for i = 1:length(solutions)

F{i} =solutions(i)*Fl + (1 -solutions(i))*F2;

end

return

function [P] =ProjectiveMatrix(M, m)

% Xem thuqt roan 3.5, trang 64

% M: ma trqn 4xn, miJid)t chaa tQad(Jthu6n nh(ft cua m(Jtdiim X(;ldnh, n>=6

% m: ma reJ-n3xn trong do miJi c(Jt chaa tQa d(J thu6n nh(ft cua m(Jt diim dnh

% P: ma treJ-nCamera X{Jdnh co kich thu:cYc3x4

% A: ma trqn 18x12

n=size(M,2);

% Chudn hoa dilli~u bang cach bitn dtJivi trong tam cua teJ-pdiim

[Mi,T_world]=NormalizeData(M, sqrt(3));

[mi,T_image] = NormalizeData(m, sqrt(2));

% LeJ-ph~ thu6n nh[{t A7r =0

A=[];

A(1:n,:) =[zeros(n,4) -(mi(3,:)'*ones(1,4» *Mi' (mi(2,:)'*ones(1,4» *Mi'];

=[(mi(3,:)'*ones(1,4».*Mi'zeros(n,4) -(mi(1,:)'*ones(l,4».*Mi']; A(n+l:2*n,:)

A(2*n+ 1:3*n,:) =[-(mi(2,:)'*ones(1,4» *Mi' (mi(1,:)'*ones(l,4» *Mi' zeros(n,4 )];

PI =LeastSquares(A);

% sap xtp cac thanh ph6n cua vecta PI (7r ) thanh ma treJ-nP

P = reshape(PI,4,3)';

% Tjnh titn vi miin dilli~u ban d6u

P=inv(T_image)*P*T_world;

return

function [M] = Triangulation(P,m)

% Xem thu(it roan 3.4, trang 62

% P: ma tr(in 3x4xk trong d6 P(:,:,i) la ma tr(in Camera Xt;lanh tha i va k>=2

% m: ma tr(in 3kxn trang d6 m6i d)t Iu:utQa dQ diim tu:ang ang tren k anh.

% i.e m6i cQt c6 dt;lng (xl,yl,wl,x2,y2,w2, xk,yk,wk)' biiu diln tu:ang ang

% (xl,yl,wl) < > (x2,y2,w2) < > < > (xk,yk,wk).

% M: ma tr(in 4xn trang d6 m6i cQtse Iu:utQadQ thutln nh[{tcua diim

xt;lanh-% A: ma tr(in 6x4

[d,n]=size(m);

k =d/3;

M =[];

% L(ip n hf thutln nh{{t AMi =0

for i =l:n

A = [diag(m(3:3:d,i»*squeeze(P(1 ,:,:»'-diag(m(l :3:d,i»*squeeze(P(3,:,:»'; diag(m(3:3 :d,i»*squeeze(P(2,:,: »' -diag(m(2:3 :d,i»*squeeze(P(3,:,:) )']; M(:,i) =LeastSquares(A);

end

return

-function [x]= LeastSquares(A)

% Xem ph{ll{lc A.3, trang 85-87, tru:angh(Jphf thutln nhtit Ax=O c6 1 b(ic t1ldo

% A: ma tr(in mxn

% x: nghifm cua hf la cQtcu{fi cung cua ma tr(in V

[U,S,V] =svd(A);

x =V(:,end);

return

function [xl,x2] = LeastSquares2(A)

% Xem phl;lIl;lCA3, trang 85-87, truiJng h(Jp h~ thudn nhdt co 2 b(ic tif do

% Gidi h~ Ax=O, A3xs,chl ap dl;lng trang qua trinh tim ma tr(in co sa rut g(}n

% xl,x2: nghi~m cua h~ la hai cf)t cudi cung cua ma tr(in V

[U,S,V]=svd(A);

xl =V(:,end-l);

x2 =V(:,end);

return

-function [A] =SkewSymmetric(m)

% Xem phl;lIl;lCAi, trang 84

% m: vecto 3-chdu

% A: ma tr(in phdn ddi xung co kick thuGC3x3

A= [

0 m(3) -m(2);

-m(3) 0 m(1);

m(2) -m(l) 0

];

return

-function [m_normal,T] = NormalizeData(m, rills)

% Chudnh6a di1li~u,xem thu(itroantrang [10J

% m: ma tr(in dxn trang do m6i cf)t chua t(}adf) thudn nhdt cua mf)t diim thong

% thuang d=3 (diim link) va d=4 (diim Xf;llink)

% rms: root-mean-square.

% m_normal: ma tr(in ILtut(}adf) sau khi du(Jcchudn hoa