-1

我在一个我无法完全理解的论坛上找到了这段 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);

您是否将向量与矩阵相乘以使用标​​准矩阵乘法得到结果向量？
您是否将两个矩阵相乘，然后使用矩阵变换点？

4

3 回答 3

2

我很确定那是伪代码。它绝对不是 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 矩阵]

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

于 2010-10-25T02:40:58.847 回答
2

据我所知，最后一部分是标准矩阵乘法。[3x3] 乘以 [3x1] 将产生 [3x1]。我不喜欢它不容易阅读的语法......

编辑：

$R 是一个 [3x3] 矩阵，如 pigpen 所示，R= [3x3]+sin(scalar) [3x3]+(1-cos(scalar)) [3x3]*[3x3]。

第二项是 [3x3]，每个元素按 sin(angle) 缩放，第三项是 [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 左右的问题），所以如果你有几天时间花时间阅读它们。数学背后也没有太多，所以试一试吧！

于 2010-10-25T02:44:31.310 回答
1

您正在尝试做 $axis_skewed[3][3] 的矩阵指数，Rodrigues 是它的缩写形式。

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

cv::Mat axis_skewed;

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

简历::垫R；// 完成后将是 3x3

简历::罗德里格斯(axis_skewed, R)

完毕...

// 这里是罗德里格斯 $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 factorial) 并得到相同的答案。

当然，维基百科在数学上的扩展比上面更多：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 会更容易，但也许您不想这样做。

于 2010-10-25T03:18:26.380 回答