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

我很确定这是伪造的。这绝对不是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矩阵

（previous）* $ axis_skewed ---这是一个常规矩阵乘法，产生另一个3x3矩阵

离开了我们：

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

这只是常规的逐步矩阵加法。

Answer 2

从我能说的最后一部分是stanadard矩阵乘法。 a [3x3]乘以[3x1]将产生[3x1]。我不喜欢它不容易阅读的语法...

修改：

$ R是猪圈所示的[3x3]矩阵，R = [3x3] + sin（标量） [3x3] +（1-cos（标量）） [3x3] * [ 3×3]

第二项是[3x3]，每个元素按sin（角度）缩放，第三项是[3x3] * [3x3]的矩阵乘法，导致另一个[3x3]。

第三个元素也按因子（1-cos（角度））缩放。

结果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]的矩阵指数，其中罗德里格斯是一个缩短的形式。

我建议你只使用OpenCV的cv :: Rodrigues函数，如果你把它放在C ++中......

cv :: Mat axis_skewed;

..... //将值放入axis_skewed

cv :: Mat R; //完成后将是3x3

cv :: Rodgrigues（axis_skewed，R）

...完成

//这是罗德里格斯 $ R = $ eye3 + sin（$ angle）* $ axis_skewed +（1-cos（$ angle））* $ axis_skewed * $ axis_skewed;

这只是一个快捷方式：R = exponential_of_matrix（axis_skewed）

e.g。在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 checkout OpenCV，构建它，然后为自己做一个测试来验证cv :: Rodrigues给你与其他代码相同的答案，然后将函数移植到你的C ++项目。只是链接到opencv会更容易，但也许你不想这样做。

这段代码是如何工作的？

3 个答案: