这段代码是如何工作的？

Posted 2023-02-21

技术标签:

【中文标题】这段代码是如何工作的？【英文标题】：How does this code work? 【发布时间】：2010-10-25 02:05:00 【问题描述】：

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

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

【问题讨论】：

您标记了这个 c++，并将其描述为 c++ 代码，但它看起来一点也不像 c++。试试猜猜 - gamedev.net/community/forums/topic.asp?topic_id=585682 我不懂Perl，但是Perl中不是所有的变量都以'$'开头的吗？我认为my 也是一个 Perl 关键字。 Perl 变量确实以 $ 开头，但不会用类型（向量/浮点数/矩阵）声明。 @Jenko，你想让我们猜？？它是你的程序。虽然我很乐意回答问题，但我（和大多数程序员）不会“猜测”、通过 ESP 进行调试或为其他人的项目进行研究。 【参考方案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 矩阵]

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

【讨论】：

我知道了，但正如我所说，$R = 部分是我觉得难以理解的部分。您能否进一步说明这一点？标量乘法是指标量值分别与矩阵中的每个值相乘？矩阵加法是指标量加法？或者添加所有行值（例如，将 3x3 矩阵变成 1x3）然后将这 2 个矩阵加在一起？ .. en.wikipedia.org/wiki/Matrix_addition 入口总和？ @peter - 什么意思？什么是转置？ @Jenko -- 我的意思是入口添加：en.wikipedia.org/wiki/Matrix_addition#Entrywise_sum【参考方案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(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 左右的问题），因此如果您有几天时间花时间阅读它们。数学背后也没有太多的东西，所以试一试！

【讨论】：

感谢您的帮助。 $R = 部分呢？ oops 还需要添加：$R 是一个 [3x3] 矩阵，如猪圈所示，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]....等【参考方案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)

完成...

// 这里是罗德里格斯 $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 会更容易，但也许你不想这样做。

【讨论】：

从哪里可以得到 OpenCV 的 cv::Rodrigues 函数？有链接吗？很抱歉听起来像个新手，但我真的不明白你所说的数学水平。你能用简单的代码术语解释一下我应该做什么吗？头文件：code.ros.org/svn/opencv/trunk/opencv/modules/calib3d/include/… 源：code.ros.org/svn/opencv/trunk/opencv/modules/calib3d/src/… 函数：CV_IMPL int cvRodrigues2(const CvMat* src, CvMat* dst, CvMat* jacobian) 这个函数的输入输出是什么？我需要 2 个点/向量的输入，以及欧拉角的输出。