轉(zhuǎn)載自：https://blog.csdn.net/kfqcome/article/details/9358853

參見文章(Least-Squares Rigid Motion?Using SVD)

一 問題描述

假設(shè)P={p1,p2,...,pn}和Q={q1,q2,...,qn}是兩組Rd空間中的對應(yīng)點(diǎn)集，現(xiàn)在想要根據(jù)這個(gè)兩個(gè)點(diǎn)集的數(shù)據(jù)來計(jì)算出它們之間的剛性轉(zhuǎn)置信息，可以知道這其實(shí)是一個(gè)最小二乘求優(yōu)問題，問題可以用如下計(jì)算式描述：

其中wi>0，是點(diǎn)集中每個(gè)點(diǎn)對的權(quán)重。

要求(1)式中的最小值，即為求式中對R和t求導(dǎo)數(shù)為0的解。

二 計(jì)算位移

將(1)式中的R設(shè)為不變量對t進(jìn)行求導(dǎo)，同時(shí)令F(t)=(R,t)，對F(t)求導(dǎo)可得：

從上可以看出，問題經(jīng)過轉(zhuǎn)化后變得更加簡單，對原來的點(diǎn)集做一個(gè)減中心點(diǎn)的預(yù)處理，然后再求兩個(gè)最小二乘的旋轉(zhuǎn)量。

三 計(jì)算旋轉(zhuǎn)量

將(8)式用矩陣表示形式展開，可得：

（由于旋轉(zhuǎn)矩陣R是正交矩陣，因而有RRT = I）。同時(shí)可以知道上式中yiTRxi和xiTRTyi都是標(biāo)量，而一個(gè)標(biāo)量的轉(zhuǎn)置仍然等于標(biāo)量本身，因而有：

現(xiàn)在變成要求(11)式的最小值，而該式中只有一項(xiàng)與R有關(guān)，其他兩項(xiàng)(xiTxi和yiTyi)都是常量，所以問題轉(zhuǎn)換為求其中一項(xiàng)可變量的最小值，即

?

(14)式中的轉(zhuǎn)換是將累加轉(zhuǎn)換成矩陣相乘，其中W是n×n的對角矩陣，X和Y是3×n的矩陣，這些矩陣相乘后的跡就等于等式左邊的值。同時(shí)，對于矩陣的跡，有如下變換關(guān)系：

(18)式中最后一步的變換也用到了(15)式的性質(zhì)。由于U、R、V都是正交矩陣，那么M=VTRU也是正交矩陣。

由上述兩式可以知道，要求最大跡，就必須使得mii的值等于1，而M又是正交矩陣，那么M就必然是單位矩陣，即有

I = M = VTRU ?=> V = RU ? R = VUT (21)

四 旋轉(zhuǎn)結(jié)果校正

到上面(21)式為止，求得的R是最優(yōu)的正交矩陣，但是這個(gè)正交矩陣既可以是旋轉(zhuǎn)矩陣，也可以是反射矩陣。反射矩陣參見博文“旋轉(zhuǎn)和反射”。

根據(jù)R的行列式值可以判斷該結(jié)果是旋轉(zhuǎn)矩陣還是反射矩陣，假如是反射矩陣，那么其行列式值就為-1。如果我們嚴(yán)格限定我們求解的必須是旋轉(zhuǎn)矩陣，那么當(dāng)前求解出來的反射矩陣就不符合要求。這個(gè)時(shí)候就必須求解下一個(gè)符合要求的最優(yōu)解。

將目標(biāo)問題重新組織成如下形式：

如果我們將mii當(dāng)作變量，它的取值范圍就是[-1,1]。函數(shù)f對于mii來說是線性的，所以它在定義域的邊界上才取得極值。很顯然對于所有的mii來說，所有都取1能取得最大的極值，但是該取值必須被排除（用這個(gè)得出的R是反射矩陣），那么下一個(gè)最優(yōu)的(m11,m22,...,mdd)取值就是(1,1,...1,-1)，即除最后一個(gè)值取-1外，其他的值仍然為1。

為什么是取最后一個(gè)mdd為-1，這是進(jìn)行SVD分解之后，Σ矩陣?yán)飳蔷€上的值經(jīng)過了排序的，即σd的值最小。

五 使用范例

現(xiàn)在要對兩個(gè)kinect攝像頭進(jìn)行空間位置標(biāo)定，分別從兩個(gè)kinect攝像頭獲取棋盤格角點(diǎn)的三維坐標(biāo)，使用這兩組點(diǎn)集即可計(jì)算出兩個(gè)攝像頭之間的剛性轉(zhuǎn)置信息。

matlab代碼如下:

clear;

clc;

PL0=[73.5716 -224.493 1781 25.9572 -191.805 1728 19.558 -189.427 1736 19.4678 -191.805 1728 19.558 -189.427 1736 19.558 -192.692 1736 16.1387 -168.168 1719 0 -232.847 1719 -13.0387 -231.884 1736 0 -177.042 1711;

-42.3757 -248.214 1736 -95.8748 -214.535 1702 -102.807 -212.45 1711 -102.807 -212.45 1711 -102.807 -212.45 1711 -103.288 -216.678 1719 -107.062 -182.989 1677 -126.631 -250.581 1686 -134.935 -251.078 1711 -124.754 -190.617 1661;

-158.289 -269.612 1686 -220.422 -236.623 1677 -223.571 -233.468 1677 -224.771 -237.893 1686 -227.936 -237.893 1686 -226.72 -236.623 1677 -226.579 -202.139 1653 -249.507 -265.614 1661 -253.843 -270.034 1669 -250.193 -207.35 1645;

-279.344 -292.324 1653 -347.628 -261.225 1653 -353.836 -258.116 1653 -353.836 -261.225 1653 -353.836 -258.116 1653 -353.836 -258.116 1653 -355.034 -220.792 1630 -383.011 -287.814 1645 -381.381 -292.753 1638 -377.656 -225.811 1622;

56.5635 -100.011 1772 6.48932 -65.0185 1728 6.48932 -65.0185 1728 6.51936 -65.3195 1736 3.2446 -65.0185 1728 3.21268 -64.3789 1711 6.39168 -41.6261 1702 -12.7833 -105.666 1702 -26.0773 -104.511 1736 -12.7232 -47.8044 1694;

-57.8291 -119.101 1711 -112.819 -84.778 1669 -117.134 -85.6415 1686 -117.134 -85.6415 1686 -117.69 -86.0479 1694 -119.658 -85.1843 1677 -110.724 -55.4689 1638 -137.89 -122.457 1669 -144.849 -123.044 1677 -135.907 -61.8955 1645;

-176.338 -141.974 1677 -235.89 -108.844 1653 -242.098 -105.734 1653 -240.926 -108.317 1645 -242.098 -105.734 1653 -242.098 -108.844 1653 -238.729 -73.5973 1630 -265.637 -139.265 1645 -266.929 -143.052 1653 -263.215 -79.7304 1630;

-296.525 -164.023 1645 -364.215 -131.862 1630 -367.276 -128.795 1630 -365.473 -131.215 1622 -365.473 -131.215 1622 -367.276 -131.862 1630 -358.522 -86.093 1578 -389.251 -154.188 1607 -392.268 -160.234 1607 -388.607 -92.8471 1592;

39.1161 29.3938 1736 -6.36164 60.5522 1694 -12.7833 64.0402 1702 -12.8509 61.1599 1711 -12.8509 61.1599 1711 -12.7833 60.8382 1702 0 84.3717 1661 -25.0709 21.9795 1669 -38.35 22.4141 1702 -25.0709 81.6381 1669;

-76.6998 6.40401 1702 -129.73 40.2321 1645 -134.11 40.6234 1661 -134.756 40.8191 1669 -134.756 40.8191 1669 -134.11 37.4985 1661 -123.026 70.877 1638 -147.631 6.16321 1638 -155.942 3.12487 1661 -147.631 67.7953 1638;

-192.437 -12.4393 1653 -251.695 21.2683 1615 -258.355 21.5712 1638 -258.355 21.5712 1638 -255.831 21.3605 1622 -257.093 21.4659 1630 -242.132 56.9062 1592 -275.955 -9.11504 1615 -282.96 -15.4081 1638 -271.57 51.3958 1607;

-315.377 -33.4217 1615 -376.649 0 1592 -384.551 0 1600 -384.551 0 1600 -386.233 0 1607 -384.551 0 1600 -369.788 41.1671 1563 -403.553 -26.9556 1592 -406.542 -32.9457 1592 -401.18 35.4667 1571];

PR0=[ 325.884 -278.142 1607 264.968 -216.657 1622 273.392 -219.738 1600 274.588 -220.7 1607 266.857 -215.722 1615 264.968 -216.657 1622 260.792 -191.415 1615 226.487 -251.458 1630 240.151 -246.865 1661 233.5 -188.377 1615;

209.969 -248.054 1669 162.221 -194.405 1694 163.849 -193.137 1711 166.183 -188.919 1702 163.849 -193.137 1711 160.636 -189.918 1711 147.292 -160.136 1669 120.052 -217.812 1728 134.339 -219.955 1745 131.029 -153.697 1702;

101.05 -225.352 1736 52.9658 -162.522 1763 60.1949 -167.532 1781 63.2181 -166.685 1772 59.8908 -166.685 1772 56.8508 -167.532 1781 39.5216 -125.394 1754 20.279 -186.251 1800 30.5707 -193.989 1809 26.8885 -127.967 1790;

-16.8992 -193.024 1800 -48.5796 -139.067 1848 -42.3156 -137.792 1878 -41.865 -139.82 1858 -41.865 -136.324 1858 -42.0903 -137.058 1868 -64.8948 -92.3973 1819 -81.5368 -156.285 1888 -81.1049 -158.991 1878 -75.5132 -89.4155 1828;

343.707 -139.53 1578 281.484 -86.093 1578 281.484 -83.1243 1578 281.484 -86.093 1578 281.484 -83.1243 1578 281.484 -86.093 1578 278.809 -58.8102 1563 244.413 -114.885 1607 257.76 -115.457 1615 258.924 -53.6742 1585;

230.794 -115.62 1661 173.814 -59.0867 1653 174.118 -53.9225 1686 176.338 -56.7896 1677 173.189 -53.6346 1677 177.284 -60.2663 1686 166.795 -24.7582 1645 138.147 -83.6925 1711 153.4 -86.4542 1702 150.425 -21.9795 1669;

116.807 -87.7749 1728 68.453 -32.6597 1736 72.4563 -32.9984 1754 72.8281 -33.1677 1763 72.4563 -32.9984 1754 69.1628 -32.9984 1754 61.3273 6.46798 1719 39.7244 -53.0684 1763 43.4741 -56.9608 1781 42.1804 9.75276 1728;

0 -60.3115 1781 -30.5706 -10.21 1809 -34.6997 -3.47673 1848 -34.6997 -3.47673 1848 -34.6997 -3.47673 1848 -34.3242 -6.87811 1828 -46.8183 36.857 1781 -63.1354 -24.6002 1868 -66.2862 -31.4595 1858 -57.7446 40.8397 1809;

362.289 -5.85467 1556 296.671 49.5408 1549 298.012 52.6921 1556 298.012 49.7647 1556 298.012 49.7647 1556 298.012 49.7647 1556 294.181 75.1325 1536 269.632 17.8123 1578 286.657 15.1164 1607 277.56 79.0381 1556;

247.911 18.3993 1630 187.615 77.0402 1638 191.506 77.3694 1645 192.437 74.6358 1653 192.437 77.7457 1653 188.417 74.2746 1645 184.064 105.815 1607 162.18 49.998 1661 173.189 47.3247 1677 172.237 110.938 1638;

134.225 44.8281 1702 83.5308 99.7872 1711 83.9214 103.488 1719 84.3608 100.779 1728 83.9214 103.488 1719 84.3608 100.779 1728 75.5733 135.664 1677 58.674 78.3834 1736 58.9782 75.5069 1745 64.8929 146.292 1728;

23.1726 69.6522 1763 -23.7772 125.923 1809 -20.3805 129.326 1809 -20.3805 125.923 1809 -20.3805 129.326 1809 -23.5275 124.6 1790 -32.9347 168.292 1754 -40.7609 112.309 1809 -47.8172 106.086 1819 -37.1783 176.092 1800];

if ~all(size(PL0) == size(PR0))

print 'size incorrect'

return;

end

s0 = size(PL0);

numPoints = s0(1);

for i = 1:numPoints

PL(i,:) = PL0(i,1:3);

PR(i,:) = PR0(i,1:3);

end

%使用SVD求R

avgPL0 = sum(PL,1)/numPoints;

avgPR0 = sum(PR,1)/numPoints;

PLAvg = repmat(avgPL0,numPoints,1);

PRAvg = repmat(avgPR0,numPoints,1);

PL = PL - PLAvg;

PR = PR - PRAvg;

PL = PL'; %3*n

PR = PR'; %3*n

S0 = PL*PR';

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

M = eye(3);

M(3,3) = det(V*U');

R = V*M*U';

T = avgPR0' - R*avgPL0';

PL = PL';

PR = PR';

%計(jì)算誤差

sumErrors = 0;

for i = 1:numPoints

pl = PL(i,:);

pr = PR(i,:)';

prr = R*pl' + T;

diff = pr - prr;

sumErrors = sumErrors + norm(diff);

end

sumErrors

總結(jié)

以上是生活随笔為你收集整理的使用SVD求最小二乘刚性转置的全部內(nèi)容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網(wǎng)站內(nèi)容還不錯(cuò)，歡迎將生活随笔推薦給好友。