这段代码是如何工作的？

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

我很确定这是psuedocode。这绝对不是C++。所有的功能都是非常明显的。

ACOS（）---自我解释

$ V1 * $ V2 ---积 （注：，通常会被解释为一个普通矩阵乘法，但却在上下文“浮$角= ACOS（$ V1 * $ V2）;”，它没有任何意义，因为任何比积等）

交叉（）---叉积

normalizeVector（） - - 自我解释

sin（$ angle）* $ axis_skewed ---这是一个标量乘法

得到它了吗？

EDIT

$ R = $ eye3 + SIN（$角）* $ axis_skewed +（1-COS（$角））* $ axis_skewed * $ axis_skewed;

$ eye3 - 是一个3×3矩阵

罪（$角）* $ axis_skewed ---这是标量乘法，从而产生另一个3x3矩阵

（1-COS（$角） ）* $ axis_skewed ---这是一个标量乘法，从而导致另一3x3矩阵

（先前）* $ axis_skewed ---这是一个普通的矩阵乘法，从而导致另一3x3矩阵

这使得我们与：

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

这只是常规的入口矩阵加法。

Answer 2

从我可以告诉最后一部分是一个标准的矩阵乘法。 [3x3]次a [3x1]将产生[3x1]。我不喜欢的语法其不容易阅读...

编辑：

$ R是[3×3]作为基质猪圈已经示出，R = [3×3] + SIN（标量） [3x3] +（1-cos（标量）） [3x3] * [3x3]。

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

该第三个元素也由系数（1-cos（角度））缩放。如果我有一个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中的简历::罗德里格斯功能，如果你把这个在C++ ...

---

CV ::垫axis_skewed;

.....//将值写入axis_skewed

cv :: Mat R;当完成

CV :: Rodgrigues（axis_skewed，R）

---

完成//将是3×3 ......

//这里的罗德里格斯 $ R = $ eye3 +罪（$角）* $ 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阶乘），并得到相同的答案。

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