這段代碼是如何工作的？

Question

我在一個我無法完全理解的論壇上發現了這段C++代碼。由於我沒有執行矩陣/向量數學的庫，我需要手動計算出來並複製這些功能。

計算歐拉旋轉角2之間的載體。我們用羅德里格斯公式

vector $V1 = << my first vector >>; 
    vector $V2 = << my second vector >>; 


    vector $axis; 
    float $angle; 

    $angle = acos($V1*$V2); 
    $axis = normalizeVector((cross($V1,$V2))); 


    matrix $axis_skewed[3][3] = << 
    0, (-$axis.z), ($axis.y) ; 
    ($axis.z), 0, (-$axis.x) ; 
    (-$axis.y), ($axis.x), 0 >>; 

    matrix $eye3[3][3] = << 
    1, 0, 0; 
    0, 1, 0; 
    0, 0, 1 >>; 

從這裏開始事情就變得棘手：

// here's Rodrigues 
    $R = $eye3 + sin($angle)*$axis_skewed + (1-cos($angle))*$axis_skewed*$axis_skewed; 

你添加eye3矩陣的所有屬性？
你會乘以axis_skewed矩陣的所有屬性嗎？
什麼是R？一個向量還是矩陣？或號碼？

這很簡單。

matrix $vectorMatr[3][1]; 
    $vectorMatr[0][0] = ($V1.x); 
    $vectorMatr[1][0] = ($V1.y); 
    $vectorMatr[2][0] = ($V1.z); 

再次，這是非常棘手：

// $result is the resulting vector 

    $result = ($R * $vectorMatr); 

你乘以向量與矩陣使用標準矩陣相乘得到的合成矢量？
你是否乘以這兩個矩陣，然後使用矩陣變換點？

Answer 1

我很確定這是psuedocode。這絕對不是C++。所有的功能都是非常明顯的。

ACOS（）---自我解釋

$ V1 * $ V2 ---積 （注：，通常會被解釋爲一個普通矩陣乘法，但卻在上下文「浮$角= ACOS（$ V1 * $ V2）;」，它沒有任何意義，因爲任何比積等）

交叉（）---叉積

normalizeVector（） - - 自我解釋

sin（$ angle）* $ axis_skewed ---這是一個標量乘法

得到它了嗎？

EDIT

$ R = $ eye3 + SIN（$角）* $ axis_skewed +（1-COS（$角））* $ axis_skewed * $ axis_skewed;

$ eye3 - 是一個3×3矩陣

罪（$角）* $ axis_skewed ---這是標量乘法，從而產生另一個3x3矩陣

（1-COS（$角） ）* $ axis_skewed ---這是一個標量乘法，從而導致另一3x3矩陣

（先前）* $ axis_skewed ---這是一個普通的矩陣乘法，從而導致另一3x3矩陣

這使得我們與：

$ R = [3x3矩陣] + [3x3矩陣] + [3x3矩陣]

這只是常規的入口矩陣加法。

Answer 2

從我可以告訴最後一部分是一個標準的矩陣乘法。 [3x3]次a [3x1]將產生[3x1]。我不喜歡的語法其不容易閱讀...

編輯：

$ R是[3×3]作爲基質豬圈已經示出，R = [3×3] + SIN（標量） [3x3] +（1-cos（標量）） [3x3] * [3x3]。

第二項是每個元素由sin（角度）縮放的[3x3]，第三項是[3x3] * [3x3]的矩陣乘法，導致另一個[3x3]。

該第三個元素也由係數（1-cos（角度））縮放。如果我有一個R [3x3] = S [3x3] + T [3x3]，R [1,1] = S [1,1] + T [1， 1]然後R [1,2] = S [1,2] + T [1,2] ....等等

---

如果你正在尋找類似於這個例子， Matlab的 - 你發佈的語法很混亂，不容易閱讀

在側面說明四元數需要較歐拉角度更少的操作來執行3D旋轉（並且不會遇到pi/2周圍的問題），所以如果你有幾天花時間閱讀它們，數學背後也沒有太多，所以給它一個鏡頭！

Answer 3

您正在嘗試執行$ axis_skewed [3] [3]的矩陣指數，其中Rodrigues是一個縮寫形式。

我建議你只使用OpenCV中的簡歷::羅德里格斯功能，如果你把這個在C++ ...

---

CV ::墊axis_skewed;

.....//將值寫入axis_skewed

cv :: Mat R;當完成

CV :: Rodgrigues（axis_skewed，R）

---

完成//將是3×3 ......

//這裏的羅德里格斯 $ R = $ eye3 +罪（$角）* $ axis_skewed +（1-cos（$ angle））* $ axis_skewed * $ axis_skewed;

這只是的快捷方式：R = exponential_of_matrix（axis_skewed）

例如在matlab中，你會使用expm（axis_skewed）。只有一個分析公式來寫下答案;或者你可以對一組術語進行R = I + axis_skewed + axis_skewed/2 + ... + axis_skewed^N /（N階乘），並得到相同的答案。

那當然維基百科擴展了數學比上面多一點：http://en.wikipedia.org/wiki/Rodrigues%27_rotation_formula

代碼的OpenCV的版本上面，在C++/C，從https://code.ros.org/svn/opencv/trunk/opencv/modules/calib3d/src/calibration.cpp

const double I[] = { 1, 0, 0, 0, 1, 0, 0, 0, 1 }; 

     double c = cos(theta); 
     double s = sin(theta); 
     double c1 = 1. - c; 
     double itheta = theta ? 1./theta : 0.; 

     rx *= itheta; ry *= itheta; rz *= itheta; 

     double rrt[] = { rx*rx, rx*ry, rx*rz, rx*ry, ry*ry, ry*rz, rx*rz, ry*rz, rz*rz }; 
     double _r_x_[] = { 0, -rz, ry, rz, 0, -rx, -ry, rx, 0 }; 
     double R[9]; 
     CvMat matR = cvMat(3, 3, CV_64F, R); 

     // R = cos(theta)*I + (1 - cos(theta))*r*rT + sin(theta)*[r_x] 
     // where [r_x] is [0 -rz ry; rz 0 -rx; -ry rx 0] 
     for(k = 0; k < 9; k++) 
      R[k] = c*I[k] + c1*rrt[k] + s*_r_x_[k]; 

我建議你使用svn結帳OpenCV，構建它，然後爲自己做一個測試來驗證cv :: Rodrigues爲您提供與其他代碼相同的答案，然後將函數移植到您的C++項目中。鏈接到opencv會更容易，但也許你不想那樣做。