这段代码是如何工作的？

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

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

编辑：

$R 是一个 [3x3] 矩阵，如 Pigpen 所示，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 是缩写形式。

如果您将其放入 C++ 中，我建议您只使用 OpenCV 的 cv::Rodrigues 函数...

---

cv::Mat axis_skewed;

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

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

cv::Rodgrigues( axis_skewed, R )

---

完成...

// 这是 Rodrigues
$R = $eye3 + sin($角度)*$axis_skewed + (1-cos($角度))*$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 checkout OpenCV ，构建它，然后自己进行测试以验证 cv::Rodrigues 为您提供与其他代码相同的答案，然后将该函数移植到您的 C++ 项目。链接到 opencv 会更容易，但也许你不想这样做。