精华内容
下载资源
问答
  • 摄像机标定学习笔记(7)关于旋转矩阵和旋转向量的关系
    万次阅读
    2017-08-15 15:28:29

    摄像机标定中的外部参数矩阵,是由旋转矩阵和平移矩阵构成的,旋转矩阵是一个3×3的正交矩阵,有3个自由度。处理旋转矩阵的问题时,通常采用旋转矩阵的方式来描述,也可以用旋转向量来表示,两者之间可以通过罗德里格斯(Rodrigues)变换来进行转换。


    其中,旋转向量的长度(模)表示绕轴逆时针旋转的角度(弧度)。

    norm为求向量的模。反变换也可以很容易的通过如下公式实现:


    在opencv中的函数为:

    int cvRodrigues2( const CvMat* src, CvMat* dst, CvMat* jacobian=0 );

    例子如下:

    更多相关内容
  • 修改:标定不用修改……matlab内置的已经很强大了,而且没有必要用python和c++再做一遍,因此考虑的方法为——matlab标定后,参数导入到opencv中使用,这也是应用到python的一个原因。 一、matlab标定工具箱 matlab...

    这一章由来:二郎之前研究matlab的双目立体视觉,已经得到了需要的信息,可是,二郎想要对代码进行修改使其更适宜自己的应用目的。
    修改:标定不用修改……matlab内置的已经很强大了,而且没有必要用python和c++再做一遍,因此考虑的方法为——matlab标定后,参数导入到opencv中使用,这也是应用到python的一个原因。

    一、matlab标定工具箱

    matlab提供了两种方法:1.用自带的app进行标定(在双目立体视觉的文章中有详细讲解);2.用matlab标定工具箱,像下面这样的东西
    在这里插入图片描述
    与其说它是一个工具箱,更确切地说它就是没有被matlab集成到内部,可以看到的m文件。
    用法非常简单,下载在网上找一下,很多,或者找二郎要……吐槽一下,有的博主把这些东西挂到CSDN卖积分……感觉好不地道。
    http://www.vision.caltech.edu/bouguetj/calib_doc/htmls/example.html(一篇英文教程,不想看英文的也可以接着文章往下看,二郎给大家做一遍)
    1.下载的压缩包解压……到哪都行
    在这里插入图片描述
    这里需要注意……点进来之后,要把这个文件夹添加到执行目录,要不后续无法进行。
    2.运行文件
    在这里插入图片描述
    3.选择进入程序
    在这里插入图片描述
    4.将图片目录添加到执行目录,且进入图片目录
    在这里插入图片描述
    5.以此运行,不用设置参数
    在这里插入图片描述
    在这里插入图片描述
    在这里插入图片描述
    在这里插入图片描述
    在这里插入图片描述
    点完后,去看matlab命令行窗口
    在这里插入图片描述
    上面二郎之前做过所以又记录,其实是需要输入棋盘格的尺寸的,也就是那个30,30
    在这里插入图片描述
    …………然后就一直持续标四个点,直至标完所有,二郎认为大家肯定会出现在标定中小手一抖,标跑偏的现象,没关系,后面能修改,先把四个点都标上再说。
    表完之后点计算标定
    在这里插入图片描述
    在这里插入图片描述
    你会发现我们想要求得值就出来了。然后用分析一下错误,可以看到我们哪张图错误率较大

    在这里插入图片描述
    我们可以用一下重投影,将我们求得内外参数代入公式,算出各点,带回到图像,计算误差(当然,这一步是计算机做的),重投影主要是为了recop.corners计算提高精度。再次确定角点时,以重投影的角点为起始角点,开始寻找角点。
    在这里插入图片描述
    做完上面一些步骤后我们的精度已经得到明显的提高
    之后就可以针对单张图来进行操作了,比如再进行一下步骤3,这次不选所有图,而是单独找我们之前标错的图,进行重新的角点标注。在重新计算角点时,可以人为地设置窗口大小,之前默认为5*5,可以适当调大或者缩小。
    多次进行角点检测的方法
    第二次:Recomp.corners→[9,9]
    第三次:Recomp.corners→[8,8]
    第四次:Recomp.corners→[7,7]
    逐渐缩小角点检测的窗口,避免产生窗口过小,造成的局部误差。

    双目标定
    当我们左右相机的单目标定完成后,将标定结果进行保存,建议保存名字为
    (Calib_Result_left.mat和Calib_Result_right.mat)
    运行工具箱里的stereo_gui.m
    在这里插入图片描述
    导入数据(Calib_Result_left.mat和Calib_Result_right.mat),进行双目标定。

    二、旋转矩阵和旋转向量

    从上面的介绍中可以看到matlab已经集成的标定程序和标定工具箱各有各自的优点,弄哪一个都是可以的。在matlab已经集成的标定程序使用时,我们的2相机相对于1相机的旋转关系为旋转矩阵(9个参数),然而,在opencv和python中,多数程序使用的数据为旋转向量(3个参数)形式,这里涉及了一个转换定理——罗德里格斯(Rodrigues)旋转向量与矩阵的变换
    在这里插入图片描述
    图片截取于:https://wenku.baidu.com/view/b471efda6037ee06eff9aef8941ea76e58fa4acb.html?from=search
    在这里插入图片描述
    而具体如何实现旋转矩阵和旋转向量的转换呢?
    下面列出了程序
    python程序——程序不是二郎写的,来自github,方便大家看,因此黏贴过来。
    https://github.com/blzq/tf_rodrigues/commit/338758887a18f0db4e4a38c949be120d7f5169e3#diff-c5ff043af7299aff712f4dc2fb35bba3

    #!/usr/bin/python3
    # -*- encoding: utf-8 -*-
    
    import tensorflow as tf
    
    def rodrigues_batch(rvecs):
        """ 
        Convert a batch of axis-angle rotations in rotation vector form shaped
        (batch, 3) to a batch of rotation matrices shaped (batch, 3, 3).
        See
        https://en.wikipedia.org/wiki/Rodrigues%27_rotation_formula#Matrix_notation
        https://en.wikipedia.org/wiki/Rotation_matrix#Rotation_matrix_from_axis_and_angle
        --- second article is wrong?
        """
        batch_size = tf.shape(rvecs)[0]
    
        thetas = tf.norm(rvecs, axis=1, keepdims=True)
        u = rvecs / thetas
    
        # Each K is the cross product matrix of unit axis vectors
        # pyformat: disable
        zero = tf.zeros([batch_size])  # for broadcasting
        Ks_1 = tf.stack([  zero   , -u[:, 2],  u[:, 1] ], axis=1)  # row 1
        Ks_2 = tf.stack([  u[:, 2],  zero   , -u[:, 0] ], axis=1)  # row 2
        Ks_3 = tf.stack([ -u[:, 1],  u[:, 0],  zero    ], axis=1)  # row 3
        # pyformat: enable
        Ks = tf.stack([Ks_1, Ks_2, Ks_3], axis=2)                  # stack rows
    
        Rs = tf.eye(3, batch_shape=[batch_size]) + \
             tf.sin(thetas)[..., tf.newaxis] * Ks + \
             (1 - tf.cos(thetas)[..., tf.newaxis]) * tf.matmul(Ks, Ks)
    
        print(Rs.shape)
        return Rs
    
    # For testing only
    if __name__ == '__main__':
        import numpy as np
        import cv2
        rvecs = np.random.randn(4, 3).astype(np.float32)
    
        rvecs_tf = tf.constant(rvecs)
        Rs = rodrigues_batch(rvecs_tf)
        with tf.Session() as sess:
            print("TensorFlow: ")
            print(sess.run(Rs))
    
        print("\nOpenCV: ")
        for rvec in rvecs:
            mat, _ = cv2.Rodrigues(rvec)
            print(mat.T)
            print()
    

    opencv的程序——来自网上,具体出处不明(输入旋转向量则转化为旋转矩阵,输入旋转矩阵,则转化为旋转向量)

    #include <stdio.h>
    #include <cv.h>
    
    void main()
    {
        int i;
        double r_vec[3]={-2.100418,-2.167796,0.273330};
        double R_matrix[9];
        CvMat pr_vec;
        CvMat pR_matrix;
    
        cvInitMatHeader(&pr_vec,1,3,CV_64FC1,r_vec,CV_AUTOSTEP);
        cvInitMatHeader(&pR_matrix,3,3,CV_64FC1,R_matrix,CV_AUTOSTEP);
        cvRodrigues2(&pr_vec, &pR_matrix,0);
    
        for(i=0; i<9; i++)
        {
            printf("%f\n",R_matrix[i]);
        }
    }
    

    同样,可以去opencv官网:https://docs.opencv.org/3.4.0/d9/d0c/group__calib3d.html#ga61585db663d9da06b68e70cfbf6a1eac

    Mat rvec1, tvec1;
    solvePnP(objectPoints, corners1, cameraMatrix, distCoeffs, rvec1, tvec1);#求旋转矩阵
    Mat rvec2, tvec2;
    solvePnP(objectPoints, corners2, cameraMatrix, distCoeffs, rvec2, tvec2);
    Mat R1, R2;
    Rodrigues(rvec1, R1);#实现旋转矩阵向旋转向量的转换
    Rodrigues(rvec2, R2);
    

    三、opencv所需标定结果格式(matlab)

    在这里插入图片描述
    在这里插入图片描述
    Radial:径向畸变,光学透镜的特性使得成像存在着径向畸变(k1,k2,k3),k3可以有,也可以为0。
    Tangen:切向畸变,装配方面的误差,传感器与光学镜头不在同一水平线,可由两个参数P1,P2确定。
    这里提一下,畸变参数的排列(K1,K2,P1,P2,K3)(用在opencv中)
    在这里插入图片描述
    在这里插入图片描述
    在这里插入图片描述
    在这里插入图片描述

    Mat cameraMatrixL = (Mat_<double>(3, 3) << 904.0589, 0, 656.6037,
        0, 869.2423, 308.9319,
        0, 0, 1);
    //对应matlab里的左相机标定矩阵
    
    Mat distCoeffL = (Mat_<double>(5, 1) << 0.3370, -0.1561, -0.0245, 4.2596e-4, 0.0000);
    //对应Matlab所得左i相机畸变参数
            
    Mat cameraMatrixR = (Mat_<double>(3, 3) << 903.1359, 0, 659.2396,
        0, 870.4047, 321.2040,
        0, 0, 1);
    //对应matlab里的右相机标定矩阵
    
    Mat distCoeffR = (Mat_<double>(5, 1) << 0.3338, -0.0631, -0.0105, -0.0022, 0.00000);
    //对应Matlab所得右相机畸变参数
    
    Mat T = (Mat_<double>(3, 1) << -118.9937, 2.5308, -1.8361);//T平移向量
                                                        //对应Matlab所得T参数
    Mat rec = (Mat_<double>(3, 1) << XXXXX, XXXXXX,XXXXX);//rec旋转向量,对应matlab om参数,这里需要把旋转矩阵变为旋转向量
    Mat R;//R 旋转矩阵
    

    下面opencv的代码可以参照博文:https://blog.csdn.net/xiao__run/article/details/78900652

    展开全文
  • 旋转向量求解旋转矩阵当刚体在三维空间中运动时,如果已知旋转向量,根据罗德里格斯公式是比较容易求得旋转矩阵的.罗德里格斯公式如图所示 其中,I 是单位矩阵,n 是旋转向量的单位向量, theta是旋转向量的模长.2. 旋转...

    公式

    1. 旋转向量求解旋转矩阵

    当刚体在三维空间中运动时,如果已知旋转向量,根据罗德里格斯公式是比较容易求得旋转矩阵的.

    罗德里格斯公式如图所示

    9a848865e9ac03d99ed65bbe26ef657e.png

    其中,I 是单位矩阵,n 是旋转向量的单位向量, theta是旋转向量的模长.

    2. 旋转矩阵求解旋转向量

    如果已知旋转矩阵,求解旋转向量时,theta是比较容易求解的.根据上图,对等式两端取迹便可以得到旋转向量的模长

    1c275218537a9a36c688711c71bddc23.png

    4e0a4d3f1b9078bc1f5530fd132cb1e2.png

    记旋转向量的单位向量为 r(rx, ry, rz) ,通过下图公式便可求解得出 r 向量的反对成矩阵,即可得出 r 向量

    0e415fceba9b9fab7f466afb61adda44.png

    代码

    这里用代码简单的求解一下

    #include #include using namespace std;

    using namespace Eigen;

    int main()

    {

    double angle = M_PI / 6.; //X轴向的转角

    AngleAxisd angleAxisd(angle, Vector3d(1, 0, 0));

    {

    cout << "根据旋转向量求解旋转矩阵..." << endl;

    double theta = angleAxisd.angle();

    Vector3d n = angleAxisd.axis();

    cout<< "theta is " << theta << endl;

    cout<< "n is " << n << endl;

    Matrix3d n_hat;

    n_hat << 0, -n[2], n[1],

    n[2], 0, -n[0],

    -n[1], n[0], 0;

    Matrix3d R_solved = cos(theta) * Matrix3d::Identity() + (1 - cos(theta)) * n * n.transpose() + sin(theta) * n_hat;

    cout << "R_solved is " << R_solved << endl;

    }

    用OpenCV Documention上的方法求解旋转向量

    点击此处搜索Rodrigues

    {

    cout<< "根据旋转矩阵求解旋转向量..." << endl;

    Matrix3d R = angleAxisd.toRotationMatrix();

    double theta = acos((R.trace() - 1) * 0.5);//待求的旋转向量的模长

    Matrix3d Right = (R - R.transpose()) * 0.5 / sin(theta);

    Vector3d r;//待求的旋转向量的单位向量

    r[0] = (Right(2,1) - Right(1,2))*0.5;

    r[1] = (Right(0,2) - Right(2,0))*0.5;

    r[2] = (Right(1,0) - Right(0,1))*0.5;

    cout<

    用求特征值为1对应的特征向量方法求解旋转向量

    {

    cout<< "根据旋转矩阵求解旋转向量..." << endl;

    //此处RR和n是相互对应的,用所求的旋转单位向量同n对比验证

    Matrix3d RR;

    RR << -0.036255, 0.978364, -0.203692,

    0.998304,0.026169, -0.051994,

    -0.045539,-0.205232,-0.977653;

    Vector3d n(-2.100418,-2.167796,0.273330);

    double theta = acos((RR.trace() - 1) * 0.5);

    EigenSolversolver(RR);

    Matrix3cd vectors = solver.eigenvectors();

    Vector3cd values = solver.eigenvalues();

    cout

    展开全文
  • 三维重建学习(1):基础知识:旋转矩阵与旋转向量

    万次阅读 多人点赞 2017-12-29 10:41:37
    前言由于摄像机标定中会使用到旋转矩阵以及旋转向量的知识,所以就整理了一下有关与这一部分基础知识的笔记,并进行详细的数学推导。旋转矩阵假设坐标系分别绕着xx轴旋转ϕ\phi角,绕yy轴旋转θ\theta角,绕zz轴旋转...

    前言

    由于摄像机标定中会使用到旋转矩阵以及旋转向量的知识,所以就整理了一下有关与这一部分基础知识的笔记,并进行详细的数学推导。

    旋转矩阵

    这里写图片描述

    假设坐标系分别绕着 x x x轴旋转 ϕ \phi ϕ角,绕 y y y轴旋转 θ \theta θ角,绕 z z z轴旋转 ψ \psi ψ角,这里旋转的角度就是我们常说的pitch, roll, yaw。设任意某点在旋转前的坐标系中的坐标是 ( x , y , z ) (x,y,z) (x,y,z),旋转后的坐标是 ( x ′ , y ′ , z ′ ) (x^{&#x27;}, y^{&#x27;}, z^{&#x27;}) (x,y,z)

    我们可以很容易地推导出这三种情况下的旋转矩阵:

    • 绕X轴旋转φ角(PITCH):

    R x = [ 1 0 0 0 cos ⁡ ϕ − sin ⁡ ϕ 0 sin ⁡ ϕ cos ⁡ ϕ ] R_x=\begin{bmatrix}1 &amp; 0 &amp; 0 \\ 0 &amp; \cos{\phi} &amp; -\sin{\phi} \\ 0 &amp; \sin{\phi} &amp; \cos{\phi} \end{bmatrix} Rx=1000cosϕsinϕ0sinϕcosϕ

    • 绕Y轴旋转θ角(ROLL):

    R y = [ cos ⁡ θ 0 sin ⁡ θ 0 1 0 − sin ⁡ θ 0 cos ⁡ θ ] R_y=\begin{bmatrix} \cos{\theta} &amp; 0 &amp; \sin{\theta} \\ 0 &amp; 1 &amp; 0 \\ - \sin{\theta} &amp; 0 &amp; \cos{\theta} \end{bmatrix} Ry=cosθ0sinθ010sinθ0cosθ

    • 绕Z轴旋转ψ角(YAW):

    R z = [ cos ⁡ ψ − sin ⁡ ψ 0 sin ⁡ ψ cos ⁡ ψ 0 0 0 1 ] R_z=\begin{bmatrix} \cos{\psi} &amp; -\sin{\psi} &amp; 0 \\ \sin{\psi} &amp; \cos{\psi} &amp; 0 \\ 0 &amp; 0 &amp; 1 \end{bmatrix} Rz=cosψsinψ0sinψcosψ0001

    详细步骤我就不列出来了,我以前的博客中有详细的推导步骤,虽然使用场景不同,但是概念都是一样的。(四旋翼姿态解算——基础理论及推导)

    如果已经上面的三个角度 ϕ \phi ϕ θ \theta θ ψ \psi ψ,,我们可以通过将每一步的旋转矩阵相乘得到整个旋转矩阵。

    R = R x ∗ R y ∗ R z R=R_x * R_y *R_z R=RxRyRz

    旋转向量

    相机标定中表示旋转的方式,除了使用前面介绍的旋转矩阵之外,还可以使用旋转向量来表示。

    我在网上搜索相关资料时,发现大多数都是直接给出了Rodrigue旋转向量的公式,而没有推导过程。如果只是拿来用,那肯定是没问题的,但是不把它的公式推导一遍,心里总是有点不踏实。(好吧,我知道我有点废话)下面我会给出我自己的证明步骤,在最后会给出公式。


    补充概念:向量的拉格朗日公式

    对于任意向量 a ⃗ ( a 1 , a 2 , a 3 ) \vec{a}(a_1,a_2,a_3) a (a1,a2,a3), b ⃗ ( b 1 , b 2 , b 3 ) \vec{b}(b_1,b_2,b_3) b (b1,b2,b3) c ⃗ ( c 1 , c 2 , c 3 ) \vec{c}(c_1,c_2,c_3) c (c1,c2,c3),有如下公式成立:

    a ⃗ × ( b ⃗ × c ⃗ ) = ( a ⃗ ⋅ c ⃗ ) b ⃗ − ( a ⃗ ⋅ b ⃗ ) c ⃗ \vec{a} \times (\vec{b} \times \vec{c}) = (\vec{a} \cdot \vec{c})\vec{b} - (\vec{a} \cdot \vec{b}) \vec{c} a ×(b ×c )=(a c )b (a b )c

    证明如下:

    d ⃗ = b ⃗ × c ⃗ \vec{d} = \vec{b} \times \vec{c} d =b ×c ,且可表示为 d ⃗ ( d 1 , d 2 , d 3 ) \vec{d}(d_1,d_2,d_3) d (d1,d2,d3)

    则有: d ⃗ = b ⃗ × c ⃗ = [ i ⃗ j ⃗ k ⃗ b 1 b 2 b 3 c 1 c 2 c 3 ] = i ⃗ ( b 2 c 3 − b 3 c 2 ) − j ⃗ ( b 1 c 3 − b 3 c 1 ) + k ⃗ ( b 1 c 2 − b 2 c 1 ) \vec{d} = \vec{b} \times \vec{c}=\begin{bmatrix} \vec{i} &amp; \vec{j} &amp; \vec{k} \\ b_1 &amp; b_2 &amp; b_3 \\ c_1 &amp; c_2 &amp; c_3 \end{bmatrix}\\=\vec{i}(b_2c_3-b_3c_2)-\vec{j}(b_1c_3-b_3c_1)+\vec{k}(b_1c_2-b_2c_1) d =b ×c =i b1c1j b2c2k b3c3=i (b2c3b3c2)j (b1c3b3c1)+k (b1c2b2c1)

    对应地就有: d 1 = b 2 c 3 − b 3 c 2 d_1=b_2c_3-b_3c_2 d1=b2c3b3c2 d 2 = b 3 c 1 − b 1 c 3 d_2=b_3c_1-b_1c_3 d2=b3c1b1c3 d 3 = b 1 c 2 − b 2 c 1 d_3=b_1c_2-b_2c_1 d3=b1c2b2c1

    再设 e ⃗ = a ⃗ × d ⃗ = a ⃗ × ( b ⃗ × c ⃗ ) \vec{e} = \vec{a} \times \vec{d} = \vec{a} \times (\vec{b} \times \vec{c}) e =a ×d =a ×(b ×c ),且可表示为 e ⃗ ( e 1 , e 2 , e 3 ) \vec{e}(e_1,e_2,e_3) e (e1,e2,e3)

    那么: e ⃗ = a ⃗ × d ⃗ = [ i ⃗ j ⃗ k ⃗ a 1 a 2 a 3 d 1 d 2 d 3 ] = i ⃗ ( a 2 d 3 − a 3 d 2 ) − j ⃗ ( a 1 d 3 − a 3 d 1 ) + k ⃗ ( a 1 d 2 − a 2 d 1 ) \vec{e} = \vec{a} \times \vec{d}=\begin{bmatrix} \vec{i} &amp; \vec{j} &amp; \vec{k} \\ a_1 &amp; a_2 &amp; a_3 \\ d_1 &amp; d_2 &amp; d_3 \end{bmatrix}\\=\vec{i}(a_2d_3-a_3d_2)-\vec{j}(a_1d_3-a_3d_1)+\vec{k}(a_1d_2-a_2d_1) e =a ×d =i a1d1j a2d2k a3d3=i (a2d3a3d2)j (a1d3a3d1)+k (a1d2a2d1)

    对应地得到得到: e 1 = a 2 d 3 − a 3 d 2 e_1=a_2d_3-a_3d_2 e1=a2d3a3d2 e 2 = a 1 d 3 − a 3 d 1 e_2=a_1d_3-a_3d_1 e2=a1d3a3d1 e 3 = a 1 d 2 − a 2 d 1 e_3=a_1d_2-a_2d_1 e3=a1d2a2d1

    对于 e 1 e_1 e1,代入前面推导出来的 d 2 d_2 d2 d 3 d_3 d3

    e 1 = a 2 d 3 − a 3 d 2 = a 2 ( b 1 c 2 − b 2 c 1 ) − a 3 ( b 3 c 1 − b 1 c 3 ) = b 1 ( a 2 c 2 + a 3 c 3 ) − c 1 ( a 3 b 3 + a 2 b 2 ) = b 1 ( a ⃗ ⋅ c ⃗ − a 1 c 1 ) − c 1 ( a ⃗ ⋅ b ⃗ − a 1 b 1 ) = b 1 ( a ⃗ ⋅ c ⃗ ) − c 1 ( a ⃗ ⋅ b ⃗ ) e_1=a_2d_3-a_3d_2=a_2(b_1c_2-b_2c_1)-a_3(b_3c_1-b_1c_3) \\ =b_1 (a_2c_2 + a_3c_3) - c_1 (a_3b_3 + a_2b_2) \\ = b_1 (\vec{a} \cdot \vec{c} - a_1c_1) - c_1 (\vec{a} \cdot \vec{b} - a_1b_1) \\ = b_1 (\vec{a} \cdot \vec{c}) - c_1 (\vec{a} \cdot \vec{b}) e1=a2d3a3d2=a2(b1c2b2c1)a3(b3c1b1c3)=b1(a2c2+a3c3)c1(a3b3+a2b2)=b1(a c a1c1)c1(a b a1b1)=b1(a c )c1(a b )

    同理可得:

    e 2 = b 2 ( a ⃗ ⋅ c ⃗ ) − c 2 ( a ⃗ ⋅ b ⃗ ) e_2 = b_2 (\vec{a} \cdot \vec{c}) - c_2 (\vec{a} \cdot \vec{b}) e2=b2(a c )c2(a b )

    e 3 = b 3 ( a ⃗ ⋅ c ⃗ ) − c 3 ( a ⃗ ⋅ b ⃗ ) e_3 = b_3 (\vec{a} \cdot \vec{c}) - c_3 (\vec{a} \cdot \vec{b}) e3=b3(a c )c3(a b )

    把这三个式子合并,表示成向量形式:

    e ⃗ = b ⃗ ( a ⃗ ⋅ c ⃗ ) − c ⃗ ( a ⃗ ⋅ b ⃗ ) \vec{e} = \vec{b} (\vec{a} \cdot \vec{c}) - \vec{c} (\vec{a} \cdot \vec{b}) e =b (a c )c (a b )

    证明完毕,后面我们还会用到这个公式。


    这里写图片描述

    上图摘自维基百科。假设我们在任意空间中有任意一个向量 v ⃗ \vec{v} v k ⃗ \vec{k} k 是某个单位向量。现在,我们将向量 v ⃗ \vec{v} v 以单位向量 k ⃗ \vec{k} k 为轴,旋转任意角度 θ \theta θ,得到旋转后的向量 v r o t ⃗ \vec{v_{rot}} vrot 如图中所示。在图中,我们将 v ⃗ \vec{v} v 关于 k ⃗ \vec{k} k 做正交分解(高中物理),会得到两个新的向量: v ⊥ ⃗ \vec{v_{\bot}} v v ∥ ⃗ \vec{v_{\parallel}} v

    很明显: v ⊥ ⃗ \vec{v_{\bot}} v k ⃗ \vec{k} k 相互垂直, v ∥ ⃗ \vec{v_{\parallel}} v k ⃗ \vec{k} k 共线(平行),且还有 v ⃗ = v ⊥ ⃗ + v ∥ ⃗ \vec{v}=\vec{v_{\bot}}+\vec{v_{\parallel}} v =v +v

    因为 v ∥ ⃗ \vec{v_{\parallel}} v k ⃗ \vec{k} k 共线,且 k ⃗ \vec{k} k 是一个单位向量,所以:

    v ∥ ⃗ = ∥ v ∥ ⃗ ∥ ⋅ k ⃗ = v ⃗ ⋅ k ⃗ ∥ k ⃗ ∥ ⋅ k ⃗ = ( v ⃗ ⋅ k ⃗ ) ⋅ k ⃗ \vec{v_{\parallel}}=\|\vec{v_{\parallel}}\| \cdot \vec{k}= \frac{\vec{v} \cdot \vec{k}}{\| \vec{k} \|} \cdot \vec{k} = (\vec{v} \cdot \vec{k}) \cdot \vec{k} v =v k =k v k k =(v k )k

    接下来求 v ⊥ ⃗ \vec{v_{\bot}} v ,由我们前面推导的向量的拉格朗日定理: a ⃗ × ( b ⃗ × c ⃗ ) = ( a ⃗ ⋅ c ⃗ ) b ⃗ − ( a ⃗ ⋅ b ⃗ ) c ⃗ \vec{a} \times (\vec{b} \times \vec{c}) = (\vec{a} \cdot \vec{c})\vec{b} - (\vec{a} \cdot \vec{b}) \vec{c} a ×(b ×c )=(a c )b (a b )c ,有:

    k ⃗ × ( k ⃗ × v ⃗ ) = ( k ⃗ ⋅ v ⃗ ) ⋅ k ⃗ − ( k ⃗ ⋅ k ⃗ ) ⋅ v ⃗ = ( k ⃗ ⋅ v ⃗ ) ⋅ k ⃗ − v ⃗ \vec{k} \times (\vec{k} \times \vec{v}) = (\vec{k} \cdot \vec{v}) \cdot \vec{k} - (\vec{k} \cdot \vec{k}) \cdot \vec{v}=(\vec{k} \cdot \vec{v}) \cdot \vec{k} - \vec{v} k ×(k ×v )=(k v )k (k k )v =(k v )k v

    然后,我们就可以推出:

    v ⊥ ⃗ = v ⃗ − v ∥ ⃗ = − k ⃗ × ( k ⃗ × v ⃗ ) \vec{v_{\bot}}=\vec{v} - \vec{v_{\parallel}} = -\vec{k} \times (\vec{k} \times \vec{v}) v =v v =k ×(k ×v )

    我们可以根据前面给出的示意图来直观地理解这些式子: k ⃗ × v ⃗ \vec{k} \times \vec{v} k ×v 可以看作是 v ⊥ ⃗ \vec{v_{\bot}} v 绕着 k ⃗ \vec{k} k 逆时针旋转90度得到的,即图中的 w ⃗ = k ⃗ × v ⃗ \vec{w}=\vec{k} \times \vec{v} w =k ×v ;对于 k ⃗ × ( k ⃗ × v ⃗ ) = k ⃗ × w ⃗ \vec{k} \times (\vec{k} \times \vec{v})=\vec{k} \times\vec{w} k ×(k ×v )=k ×w ,在图中观察发现,这个式子的结果其实就是 v ⊥ ⃗ \vec{v_{\bot}} v 绕着 k ⃗ \vec{k} k 逆时针旋转180度得到的结果,共线且夹角为180度,正好对应推导出的结果: v ⊥ ⃗ = − k ⃗ × ( k ⃗ × v ⃗ ) \vec{v_{\bot}} = -\vec{k} \times (\vec{k} \times \vec{v}) v =k ×(k ×v )

    还是根据这幅图来看,旋转过程中 v ∥ ⃗ \vec{v_{\parallel}} v 的大小始终不变,因为它是 v ⃗ \vec{v} v k ⃗ \vec{k} k 方向上的投影,就在旋转轴上,自然不会变。所以有: v r o t , ∥ ⃗ = v ∥ ⃗ \vec{v_{rot, \parallel}} = \vec{v_{\parallel}} vrot, =v

    接下来看 v r o t , ⊥ ⃗ \vec{v_{rot, \bot}} vrot, 的变化:

    首先,可以肯定的是旋转过程中矢量的模长不变,只会改变方向,所以: ∥ v r o t , ⊥ ⃗ ∥ = ∥ v ⊥ ⃗ ∥ \| \vec{v_{rot, \bot}} \| = \| \vec{v_{\bot}} \| vrot, =v

    我们可以由 v ⊥ ⃗ \vec{v_{\bot}} v k ⃗ \vec{k} k 方向的投影,组合得到 v r o t , ⊥ ⃗ \vec{v_{rot, \bot}} vrot,

    v r o t , ⊥ ⃗ = v ⊥ ⃗ cos ⁡ θ + ∥ v ⊥ ⃗ ∥ sin ⁡ θ w ⃗ ∥ w ⃗ ∥ \vec{v_{rot, \bot}}=\vec{v_{\bot}}\cos\theta + \| \vec{v_{\bot}} \| \sin\theta \frac{\vec{w}}{\|\vec{w}\|} vrot, =v cosθ+v sinθw w

    前面有: w ⃗ = k ⃗ × v ⃗ \vec{w}=\vec{k} \times \vec{v} w =k ×v 可以看作是 v ⊥ ⃗ \vec{v_{\bot}} v 绕着 k ⃗ \vec{k} k 逆时针旋转90度得到的。那么有: ∥ v ⊥ ⃗ ∥ = ∥ w ⃗ ∥ \| \vec{v_{\bot}} \| = \|\vec{w}\| v =w

    上式可以化简为:

    v r o t , ⊥ ⃗ = v ⊥ ⃗ cos ⁡ θ + sin ⁡ θ w ⃗ = v ⊥ ⃗ cos ⁡ θ + sin ⁡ θ ( k ⃗ × v ⃗ ) \vec{v_{rot, \bot}} = \vec{v_{\bot}}\cos\theta + \sin\theta \vec{w}= \vec{v_{\bot}}\cos\theta + \sin\theta (\vec{k} \times \vec{v}) vrot, =v cosθ+sinθw =v cosθ+sinθ(k ×v )

    v r o t , ⊥ ⃗ \vec{v_{rot, \bot}} vrot, v r o t , ∥ ⃗ \vec{v_{rot, \parallel}} vrot, 相加,即可得到 v r o t ⃗ \vec{v_{rot}} vrot

    v r o t ⃗ = v r o t , ⊥ ⃗ + v r o t , ∥ ⃗ = v ∥ ⃗ + v ⊥ ⃗ cos ⁡ θ + sin ⁡ θ ( k ⃗ × v ⃗ ) = v ∥ ⃗ + ( v ⃗ − v ∥ ⃗ ) cos ⁡ θ + sin ⁡ θ ( k ⃗ × v ⃗ ) = v ⃗ cos ⁡ θ + ( 1 − cos ⁡ θ ) v ∥ ⃗ + sin ⁡ θ ( k ⃗ × v ⃗ ) \begin{aligned} \vec{v_{rot}} &amp;= \vec{v_{rot, \bot}} + \vec{v_{rot, \parallel}} \\ &amp;= \vec{v_{\parallel}} + \vec{v_{\bot}}\cos\theta + \sin\theta (\vec{k} \times \vec{v}) \\ &amp;= \vec{v_{\parallel}} + (\vec{v} - \vec{v_{\parallel}}) \cos\theta +\sin\theta (\vec{k} \times \vec{v}) \\ &amp;= \vec{v}\cos\theta + (1-\cos\theta) \vec{v_{\parallel}} + \sin\theta (\vec{k} \times \vec{v}) \end{aligned} vrot =vrot, +vrot, =v +v cosθ+sinθ(k ×v )=v +(v v )cosθ+sinθ(k ×v )=v cosθ+(1cosθ)v +sinθ(k ×v )

    前面推出来了: v ∥ ⃗ = ∥ v ∥ ⃗ ∥ ⋅ k ⃗ = v ⃗ ⋅ k ⃗ ∥ k ⃗ ∥ ⋅ k ⃗ = ( v ⃗ ⋅ k ⃗ ) ⋅ k ⃗ \vec{v_{\parallel}}=\|\vec{v_{\parallel}}\| \cdot \vec{k}= \frac{\vec{v} \cdot \vec{k}}{\| \vec{k} \|} \cdot \vec{k} = (\vec{v} \cdot \vec{k}) \cdot \vec{k} v =v k =k v k k =(v k )k ,直接把那个结论代入这里。

    v r o t ⃗ = v ⃗ cos ⁡ θ + ( 1 − cos ⁡ θ ) ( v ⃗ ⋅ k ⃗ ) ⋅ k ⃗ + sin ⁡ θ ( k ⃗ × v ⃗ ) \begin{aligned} \vec{v_{rot}} &amp;= \vec{v}\cos\theta + (1-\cos\theta) (\vec{v} \cdot \vec{k}) \cdot \vec{k} + \sin\theta (\vec{k} \times \vec{v}) \end{aligned} vrot =v cosθ+(1cosθ)(v k )k +sinθ(k ×v )

    现在我们已经推导出了旋转向量公示了,我们可以套用这个公式来表示任意旋转。

    接下来要进一步整理成矩阵形式:

    对于 k ⃗ × v ⃗ \vec{k} \times \vec{v} k ×v ,用矩阵形式来表示:(无非就是向量叉乘,拆成三个轴上的分量来表示)

    [ ( k ⃗ × v ⃗ ) x ( k ⃗ × v ⃗ ) y ( k ⃗ × v ⃗ ) z ] = [ k y v z − k z v y k z v x − k x v z k x v y − k y v x ] = [ 0 − k z k y k z 0 − k x − k y k x 0 ] ⋅ [ v x v y v z ] \begin{bmatrix} (\vec{k} \times \vec{v})_x \\ (\vec{k} \times \vec{v})_y \\ (\vec{k} \times \vec{v})_z \end{bmatrix} = \begin{bmatrix}k_yv_z-k_zv_y \\ k_zv_x-k_xv_z \\ k_xv_y - k_yv_x \end{bmatrix} = \begin{bmatrix} 0 &amp; -k_z &amp; k_y \\ k_z &amp; 0 &amp; -k_x \\ -k_y &amp; k_x &amp;0 \end{bmatrix} \cdot \begin{bmatrix} v_x \\ v_y \\ v_z\end{bmatrix} (k ×v )x(k ×v )y(k ×v )z=kyvzkzvykzvxkxvzkxvykyvx=0kzkykz0kxkykx0vxvyvz

    使用一个大写的 K K K来表示:

    K = [ 0 − k z k y k z 0 − k x − k y k x 0 ] K=\begin{bmatrix} 0 &amp; -k_z &amp; k_y \\ k_z &amp; 0 &amp; -k_x \\ -k_y &amp; k_x &amp;0 \end{bmatrix} K=0kzkykz0kxkykx0

    那么有: K v ⃗ = k ⃗ × v ⃗ K\vec{v}=\vec{k} \times \vec{v} Kv =k ×v

    还有: K ( K v ⃗ ) = K 2 v ⃗ = K ( k ⃗ × v ⃗ ) = k ⃗ × ( K v ⃗ ) = k ⃗ × ( k ⃗ × v ⃗ ) K(K\vec{v})=K^2\vec{v}=K(\vec{k} \times \vec{v}) = \vec{k} \times (K\vec{v}) = \vec{k} \times (\vec{k} \times \vec{v}) K(Kv )=K2v =K(k ×v )=k ×(Kv )=k ×(k ×v )

    公式可以进一步化简为:

    v r o t ⃗ = v ⃗ cos ⁡ θ + ( 1 − cos ⁡ θ ) ( v ⃗ ⋅ k ⃗ ) ⋅ k ⃗ + sin ⁡ θ ( k ⃗ × v ⃗ ) = v ⃗ cos ⁡ θ + ( 1 − cos ⁡ θ ) ( v ⃗ + k ⃗ × ( k ⃗ × v ⃗ ) ) + sin ⁡ θ ( k ⃗ × v ⃗ ) = v ⃗ + ( 1 − cos ⁡ θ ) K 2 v ⃗ + s i n θ K v ⃗ \begin{aligned} \vec{v_{rot}} &amp;= \vec{v}\cos\theta + (1-\cos\theta) (\vec{v} \cdot \vec{k}) \cdot \vec{k} + \sin\theta (\vec{k} \times \vec{v}) \\ &amp;= \vec{v}\cos\theta + (1-\cos\theta) (\vec{v} + \vec{k} \times (\vec{k} \times \vec{v})) + \sin\theta (\vec{k} \times \vec{v}) \\ &amp;= \vec{v} + (1-\cos\theta)K^2\vec{v} + sin\theta K \vec{v} \end{aligned} vrot =v cosθ+(1cosθ)(v k )k +sinθ(k ×v )=v cosθ+(1cosθ)(v +k ×(k ×v ))+sinθ(k ×v )=v +(1cosθ)K2v +sinθKv

    给出旋转矩阵 R R R有: v r o t ⃗ = R v ⃗ \vec{v_{rot}} = R \vec{v} vrot =Rv

    那么消去 v ⃗ \vec{v} v :$R = I + (1-\cos\theta)K^2 + sin\theta K $

    Rodrigue旋转向量公式

    已知单位向量 k ⃗ = ( k x , k y , k z ) \vec{k}=(k_x,k_y,k_z) k =(kx,ky,kz),向量 v ⃗ \vec{v} v 绕着它旋转 θ \theta θ角。旋转后的向量 v r o t ⃗ \vec{v_{rot}} vrot 可以通过如下公式求得:

    v r o t ⃗ = v ⃗ cos ⁡ θ + ( 1 − cos ⁡ θ ) ( v ⃗ ⋅ k ⃗ ) ⋅ k ⃗ + sin ⁡ θ ( k ⃗ × v ⃗ ) \begin{aligned} \vec{v_{rot}} &amp;= \vec{v}\cos\theta + (1-\cos\theta) (\vec{v} \cdot \vec{k}) \cdot \vec{k} + \sin\theta (\vec{k} \times \vec{v}) \end{aligned} vrot =v cosθ+(1cosθ)(v k )k +sinθ(k ×v )

    我们可以把 v ⃗ \vec{v} v 提取出来,就得到了:

    v r o t ⃗ = R v ⃗ = ( I ⋅ cos ⁡ θ + ( 1 − cos ⁡ θ ) k ⃗ ⋅ k ⃗ + sin ⁡ θ K ) v ⃗ \begin{aligned} \vec{v_{rot}} &amp;= R \vec{v} \\ &amp;= (I \cdot \cos\theta + (1-\cos\theta) \vec{k} \cdot \vec{k} + \sin\theta K)\vec{v} \end{aligned} vrot =Rv =(Icosθ+(1cosθ)k k +sinθK)v

    其中:

    K = [ 0 − k z k y k z 0 − k x − k y k x 0 ] K=\begin{bmatrix} 0 &amp; -k_z &amp; k_y \\ k_z &amp; 0 &amp; -k_x \\ -k_y &amp; k_x &amp;0 \end{bmatrix} K=0kzkykz0kxkykx0

    根据上面这个式子,我们可以给出Rodrigue旋转公式:

    对于旋转向量 r ⃗ = ( r x , r y , r z ) \vec{r} = (r_x, r_y, r_z) r =(rx,ry,rz)
    这里写图片描述

    θ \theta θ为向量 r ⃗ \vec{r} r 的模长,而且要保证向量 r ⃗ \vec{r} r 是一个单位向量。

    反变换也可以很容易求出:
    这里写图片描述

    注:在opencv中有相关的实现(Rodrigues2函数

    当然还有一种写法:

    v r o t ⃗ = R v ⃗ = I ⋅ v ⃗ + ( 1 − cos ⁡ θ ) K 2 ⋅ v ⃗ + s i n θ K ⋅ v ⃗ \vec{v_{rot}} = R\vec{v} = I \cdot \vec{v} + (1-\cos\theta)K^2 \cdot \vec{v} + sin\theta K \cdot \vec{v} vrot =Rv =Iv +(1cosθ)K2v +sinθKv

    对应的旋转矩阵:(我们可以用这个公式来求旋转矩阵)

    R = I + ( 1 − cos ⁡ θ ) K 2 + s i n θ K R = I + (1-\cos\theta)K^2 + sin\theta K R=I+(1cosθ)K2+sinθK

    其中:

    K = [ 0 − k z k y k z 0 − k x − k y k x 0 ] K=\begin{bmatrix} 0 &amp; -k_z &amp; k_y \\ k_z &amp; 0 &amp; -k_x \\ -k_y &amp; k_x &amp;0 \end{bmatrix} K=0kzkykz0kxkykx0

    参考链接:

    1. http://blog.sina.com.cn/s/blog_5fb3f125010100hp.html

    2. https://www.cnblogs.com/xpvincent/archive/2013/02/15/2912836.html

    3. Rodrigues旋转向量维基百科

    4. http://blog.csdn.net/tl_tj/article/details/47006007

    5. http://wiki.opencv.org.cn/index.php/Cv照相机定标和三维重建#Rodrigues2

    展开全文
  • 三维坐标变换——旋转矩阵与旋转向量

    万次阅读 多人点赞 2018-02-24 23:25:17
    用 opencv 进行过双目相机标定的同学都知道,单目标定 calibrateCamera() 函数能够对每一张标定图像计算出一对 rvec 和 tvec,即旋转平移向量,代表世界坐标系到相机坐标系的转换关系。而 stereoCalibrate() 函数则...
  • 除了采用旋转矩阵描述外,还可以用旋转向量来描述旋转,旋转向量的长度(模)表示绕轴逆时针旋转的角度(弧度)。旋转向量与旋转矩阵可以通过罗德里格斯 openCV实现Rodrigues变换的函数为 int cvRodrigues2( const ...
  • 旋转矩阵和旋转向量

    千次阅读 2016-05-10 16:29:09
    旋转矩阵和旋转向量的转化
  • 机器视觉2D点旋转中心标定旋转后的坐标计算一、旋转中心标定二、点旋转后的坐标计算 我们在做机器视觉项目时,尤其是与机械臂的视觉应用中,会经常遇到旋转中心,旋转坐标计算等问题。现将本人在机械臂的相关视觉...
  • 除了采用旋转矩阵描述外,还可以用旋转向量来描述旋转,旋转向量的长度(模)表示绕轴逆时针旋转的角度(弧度)。旋转向量与旋转矩阵可以通过罗德里格斯(Rodrigues)变换进行转换。 算法过程如下: 式中...
  • openCV---旋转向量转旋转矩阵

    千次阅读 2017-09-23 15:39:49
    Mat rvecs; // 旋转向量 Mat rotation_matrix = Mat(3, 3, CV_64FC1, Scalar::all(0)); // 图像的旋转矩阵 // 将旋转向量转换为相对应的旋转矩阵 */ Rodrigues(rvecs, rotation_matrix);
  • 欧拉角是用来表示三维坐标系中方向和方向变换的,我们平时说的欧拉角其实还可以细分为欧拉角(Eular-angles)和泰特布莱恩角(Tait-Bryan-angles),这两种方法都利用了笛卡尔坐标系的三轴作为旋转轴,主要区别在于选取...
  • 旋转向量-旋转矩阵

    千次阅读 2015-07-22 17:32:24
    今天接触到摄像机标定,其中用到旋转矩阵的知识,就具体恶补学习了一下,顺便做个笔记。物体在空间中的旋转物体在三维空间中的旋转,可以被分为解为在直接坐标系下,分别先后围绕x,y,z坐标轴旋转得到。旋转的角度也...
  • 在做双目立体视觉深度图像生成的时候,遇到旋转向量(1x3)与旋转矩阵(3x3)的概念,得知二者可以通过罗德里格斯相互转化。 1.旋转的表示 处理三维旋转问题时,通常采用旋转矩阵的方式来描述旋转变换。旋转矩阵有...
  • 标定结果表明,主点坐标误差不超过1.8 pixel,Z分量相对平移向量的最大偏差值小于0.15 mm,姿态数为10或10以上时标定结果趋于稳定。借助于精密位移台对标定结果进行了精度验证,结果表明采用所提方法标定后位移测量...
  • 本文基于VINS-Fusion解释VIO系统的初始化,包括在线标定IMU-Camera的外参旋转,IMU角速度偏置,重力方向,单目尺度。单目初始化相比于双目,多一个构建SFM问题优化位姿、3D点的过程。 详细标定内容参考知乎卢涛的...
  • 经典手眼标定算法之Tsai-Lenz的OpenCV实现

    万次阅读 多人点赞 2016-06-09 17:17:58
    本文主要是讲解经典手眼标定问题中的TSAI-LENZ 文献方法,参考文献为“A New Technique for Fully Autonomous and Efficient 3D Robotics Hand/Eye Calibration”,并且实现了基于OpenCV的C++代码程序,code可去CSDN...
  • 最近学习了一些关于三维空间旋转相关的知识,借此...表示三维空间的旋转有多种互相等价的方式,常见的有旋转矩阵、DCM、旋转向量、四元数、欧拉角等。本篇文章主要梳理一下这些表示方式及相互转换的方法。1. 欧拉...
  • 现有双目相机标定的方法是通过矩阵变换求得各个相机间的旋转平移关系,再通过优化求得最终参数。非线性优化步骤多,相机内外参数与镜头畸变存在耦合,时间花费较大。提出一种畸变矫正与平面单应性矩阵结合的双目相机...
  • 一、相机到机械臂末端位姿自动标定 机械臂和realsense初始化: import jkrc import math import time import pyrealsense2 as rs import numpy as np import cv2 # 提示没有aruco的看问题汇总 import cv2.aruco as ...
  • 误差约为0.03像素点,代码修改中解释,文件包含标定图和标定结果文件,可自行修改。可获取相机内外参数,取其中一幅图的旋转向量和平移向量,即为外参
  • matlab标定工具箱中的旋转矩阵说明

    千次阅读 2019-08-03 12:59:31
    1、利用matlab标定工具箱进行双目视觉标定获得的om即为两相机之间的旋转向量。T为平移向量。 2、而后续计算需要的是旋转矩阵。因此有必要将mo旋转向量转为旋转矩阵。 3、于是利用罗德里格斯(Rodrigues)旋转方程...
  • 我第一次动手标定一个相机就发现标定结果中旋转矩阵尺寸是3乘1,我很疑惑因为课上一直讲是3*3的旋转矩阵,于是找了很多博客都没有个解答,最后还是请教了师兄才知道其中的原理,因为得到的根本就不是旋转矩阵,而是...
  • 说明:MATLAB的相机标定工具箱camera calibration toolbox是由加州理工大学开发的,网上有相应的使用帮助文档。...Calib中相机外参数的确定平移(3*1平移向量)旋转(3*3旋转矩阵)算法核心函数go_calib_optim.m标定,...
  • 算法首先假定主点位于像面中心附近的某个位置,再求出焦距、旋转矩阵、平移向量和比例因子,最后通过评价函数将相机标定转换成寻找两相机最佳主点对的非线性最小化问题。在传统进化策略中引进个体的自我改进系数、个体...

空空如也

空空如也

1 2 3 4 5 ... 20
收藏数 5,385
精华内容 2,154
关键字:

旋转向量 标定