轉載自：https://blog.csdn.net/kfqcome/article/details/9358853

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

一 問題描述

假設P={p1,p2,...,pn}和Q={q1,q2,...,qn}是兩組Rd空間中的對應點集，現在想要根據這個兩個點集的數據來計算出它們之間的剛性轉置信息，可以知道這其實是一個最小二乘求優問題，問題可以用如下計算式描述：

其中wi>0，是點集中每個點對的權重。

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

二 計算位移

將(1)式中的R設為不變量對t進行求導，同時令F(t)=(R,t)，對F(t)求導可得：

從上可以看出，問題經過轉化后變得更加簡單，對原來的點集做一個減中心點的預處理，然后再求兩個最小二乘的旋轉量。

三 計算旋轉量

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

（由于旋轉矩陣R是正交矩陣，因而有RRT = I）。同時可以知道上式中yiTRxi和xiTRTyi都是標量，而一個標量的轉置仍然等于標量本身，因而有：

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

?

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

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

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

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

四 旋轉結果校正

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

根據R的行列式值可以判斷該結果是旋轉矩陣還是反射矩陣，假如是反射矩陣，那么其行列式值就為-1。如果我們嚴格限定我們求解的必須是旋轉矩陣，那么當前求解出來的反射矩陣就不符合要求。這個時候就必須求解下一個符合要求的最優解。

將目標問題重新組織成如下形式：

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

為什么是取最后一個mdd為-1，這是進行SVD分解之后，Σ矩陣里對角線上的值經過了排序的，即σd的值最小。

五 使用范例

現在要對兩個kinect攝像頭進行空間位置標定，分別從兩個kinect攝像頭獲取棋盤格角點的三維坐標，使用這兩組點集即可計算出兩個攝像頭之間的剛性轉置信息。

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';

%計算誤差

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

總結

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

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。