此代碼如何工作？

Question

我在一個我不太了解的論壇上找到了這段C ++代碼。 由於我沒有他們的庫來執行矩陣/矢量數學運算，因此需要手動找出並復制功能。

計算兩個向量之間的歐拉旋轉角..我們使用羅德里格斯公式

    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

我很確定那是偽代碼。 絕對不是C ++。 所有功能都很容易解釋。

acos（）---自我解釋

$ V1 * $ V2-點積（ 注： ，通常將其解釋為規則矩陣乘法，但是在“ float $ angle = acos（$ V1 * $ V2）;”的上下文中，它不會除了點積以外，沒有其他意義）

cross（）-叉積

normalizeVector（）---自我解釋

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

得到它？

編輯

$ R = $ eye3 + sin（$ angle）* $ axis_skewed +（1-cos（$ angle））* $ axis_skewed * $ axis_skewed;

$ eye3-是一個3x3矩陣

sin（$ angle）* $ axis_skewed ---這是一個標量乘法，產生另一個3x3矩陣

（1-cos（$ angle））* $ axis_skewed ---這是一個標量乘法，產生另一個3x3矩陣

（上一個）* $ axis_skewed ---這是一個規則的矩陣乘法，導致另一個3x3矩陣

這給我們留下了：

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

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

Answer 2

從我可以看出的最后一部分是標准矩陣乘法。 [3x3]乘以[3x1]會得到[3x1]。 我不喜歡它的語法不容易閱讀...

編輯：

$ R是Pigpen所示的[3x3]矩陣，R = [3x3] + sin（標量） [3x3] +（1-cos（標量）） [3x3] * [3x3]。

第二項是[3x3]，每個元素都按sin（angle）縮放，第三項是[3x3] * [3x3]的矩陣相乘，得到另一個[3x3]。

第三元素也按因子（1-cos（angle））縮放。

所得的R在元素上執行（即，如果我有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的cv :: Rodrigues函數放入C ++中...

---

cv :: Mat axis_skewed;

..... //將值放入axis_skewed

cv :: Mat R; //完成后將為3x3

cv :: Rodgrigues（axis_skewed，R）

---

完成...

//這是Rodrigues $ R = $ eye3 + sin（$ angle）* $ 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因數），並獲得相同的答案。

然后，當然Wikipedia在數學上的擴展要比上述更多： 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甚至會更容易，但是也許您不想這樣做。