【计算机视觉】opencv靶标相机姿态解算1之基本概念（空间旋转、旋转轴）

关键词：空间旋转、旋转轴

用途：相机位姿估计、无人机位姿估计、3D游戏、3D建模

文章类型：概念、公式总结（本文不带推倒过程，若想了解公式是如何推出来的请搜索文献），C++函数展示

@Author：VShawn(singlex@foxmail.com)

@Date:2016-11-04

@Lab: CvLab202@CSU

写在前面的一些概念

右手系

关于这个概念，搞3D的人应该都懂，而像我这样做图像处理的可能就对这个知道的比较少了。右手系这个概念其实很简单，看图就懂了。在坐标系中，右手摆成下图的样子，当拇指指向X轴食指指向Y轴时，中指指向了Z轴，满足这个条件的坐标系就是右手系。本文所有概念都在右手系下进行讨论。

右手系

 

旋转90°到底是怎么转

当我要让一个点，绕Y轴转动了90°，并且用程序计算出了旋转结果，为了验证这个点是否旋转正确，我们需要知道这个90°是怎么转的。在网上搜索了挺多文章，都没有对这个东西进行明确的定义，那么这里给出我的总结。从原点（0,0,0）往Y轴方向看，此时视野中的坐标系降维到二维坐标系XOZ，那么让点绕O点顺时针转90°，即为正确的旋转结果。

问题一：XYZ空间内某点绕X、Y、Z轴旋转一次

这个问题比较简单，网上已经有较多总结：

设旋转前坐标为，旋转后坐标为。

 1.绕Z轴旋转γ角

首先给出向量表示：

[x′,y′,z′,1]=[x,y,z,1]⎡⎣⎢⎢⎢cosγ−sinγ00sinγcosγ0000100001⎤⎦⎥⎥⎥[x′,y′,z′,1]=[x,y,z,1][cos⁡γsin⁡γ00−sin⁡γcos⁡γ0000100001]

然后是公式表示：

x′=cosγ⋅x−sinγ⋅yy′=sinγ⋅x+coγ⋅yz′=z(1)(2)(3)(1)x′=cosγ⋅x−sinγ⋅y(2)y′=sin⁡γ⋅x+coγ⋅y(3)z′=z

最后是代码表示

 

//将空间点绕Z轴旋转

//输入参数 x y为空间点原始x y坐标

//thetaz为空间点绕Z轴旋转多少度，角度制范围在-180到180

//outx outy为旋转后的结果坐标

void codeRotateByZ(double x,double y,double thetaz,double& outx, double& outy)

{

    double x1 = x;//将变量拷贝一次，保证&x == &outx这种情况下也能计算正确

    double y1 = y;

    double rz = thetaz * CV_PI / 180;

    outx = cos(rz) * x1 - sin(rz) * y1;

    outy = sin(rz) * x1 + cos(rz) * y1;

 

}

 

2.绕Y轴旋转β角

首先给出向量表示：

[x′,y′,z′,1]=[x,y,z,1]⎡⎣⎢⎢⎢cosβ0sinβ00100−sinβ0cosβ00001⎤⎦⎥⎥⎥[x′,y′,z′,1]=[x,y,z,1][cos⁡β0−sin⁡β00100sin⁡β0cos⁡β00001]

然后是公式表示：

x′=cosβ⋅x+sinβ⋅zy′=yz′=−sinβ⋅x+cosβ⋅z(4)(5)(6)(4)x′=cosβ⋅x+sinβ⋅z(5)y′=y(6)z′=−sin⁡β⋅x+cos⁡β⋅z

最后是代码表示

//将空间点绕Y轴旋转

//输入参数 x z为空间点原始x z坐标

//thetay为空间点绕Y轴旋转多少度，角度制范围在-180到180

//outx outz为旋转后的结果坐标

void codeRotateByY(double x,double z,double thetay,double& outx, double& outz)

{

    double x1 = x;

    double z1 = z;

    double ry = thetay * CV_PI / 180;

    outx = cos(ry) * x1 + sin(ry) * z1;

    outz = cos(ry) * z1 - sin(ry) * x1;

}

　　 

3.绕X轴旋转α角

首先给出向量表示：

[x′,y′,z′,1]=[x,y,z,1]⎡⎣⎢⎢⎢10000cosα−sinα00sinαcosα00001⎤⎦⎥⎥⎥[x′,y′,z′,1]=[x,y,z,1][10000cos⁡αsin⁡α00−sin⁡αcos⁡α00001]

然后是公式表示：

x′=xy′=cosα⋅y−sinα⋅zz′=sinα⋅y+sinα⋅z(7)(8)(9)(7)x′=x(8)y′=cos⁡α⋅y−sin⁡α⋅z(9)z′=sin⁡α⋅y+sin⁡α⋅z

最后是代码表示

//将空间点绕X轴旋转

//输入参数 y z为空间点原始y z坐标

//thetax为空间点绕X轴旋转多少度，角度制范围在-180到180

//outy outz为旋转后的结果坐标

void codeRotateByX(double y,double z,double thetax,double& outy, double& outz)

{

    double y1 = y;//将变量拷贝一次，保证&y == &y这种情况下也能计算正确

    double z1 = z;

    double rx = thetax * CV_PI / 180;

    outy = cos(rx) * y1 - sin(rx) * z1;

    outz = cos(rx) * z1 + sin(rx) * y1;

}

　　 

问题二：空间点绕任意轴旋转

首先，需要定义"任意轴"的单位向量，例如X轴可以用向量来表示。

那么假设旋转轴的单位向量为，旋转前坐标为，旋转后坐标为，旋转角为，于是有：

x′=(vx⋅vx⋅(1−cosθ)+cosθ)⋅x+(vx⋅vy⋅(1−cosθ)−vz⋅sinθ)⋅y+(vx⋅vz⋅(1−cosθ)+vy⋅sinθ)⋅zy′=(vx⋅vy⋅(1−cosθ)+vz⋅sinθ)⋅x+(vy⋅vy⋅(1−cosθ)+cosθ)⋅y+(vy⋅vz⋅(1−cosθ)−vx⋅sinθ)⋅zz′=(vx⋅vz⋅(1−cosθ)−vy⋅sinθ)⋅x+(vy⋅vz⋅(1−cosθ)+vx⋅sinθ)⋅y+(vz⋅vz⋅(1−cosθ)+cosθ)⋅z(10)(11)(12)(10)x′=(vx⋅vx⋅(1−cosθ)+cosθ)⋅x+(vx⋅vy⋅(1−cosθ)−vz⋅sinθ)⋅y+(vx⋅vz⋅(1−cosθ)+vy⋅sinθ)⋅z(11)y′=(vx⋅vy⋅(1−cosθ)+vz⋅sinθ)⋅x+(vy⋅vy⋅(1−cosθ)+cosθ)⋅y+(vy⋅vz⋅(1−cosθ)−vx⋅sinθ)⋅z(12)z′=(vx⋅vz⋅(1−cos⁡θ)−vy⋅sin⁡θ)⋅x+(vy⋅vz⋅(1−cos⁡θ)+vx⋅sin⁡θ)⋅y+(vz⋅vz⋅(1−cos⁡θ)+cos⁡θ)⋅z

计算时照着公式代入即可。

最后给出代码实现：

 

//定义返回结构体

struct Point3f

{

    Point3f(double _x,double _y,double _z)

    {

        x = _x;

        y = _y;

        z = _z;

    }

    double x;

    double y;

    double z;

};

 

//点绕任意向量旋转，右手系

//输入参数old_x，old_y，old_z为旋转前空间点的坐标

//vx，vy，vz为旋转轴向量

//theta为旋转角度角度制，范围在-180到180

//返回值为旋转后坐标点

Point3f RotateByVector(double old_x,double old_y,double old_z,double vx,double vy,double vz,double theta)

{

    double r = theta * CV_PI / 180;

    double c = cos(r);

    double s = sin(r);

    double new_x = (vx*vx*(1 - c) + c) * old_x + (vx*vy*(1 - c) - vz*s) * old_y + (vx*vz*(1 - c) + vy*s) * old_z;

    double new_y = (vy*vx*(1 - c) + vz*s) * old_x + (vy*vy*(1 - c) + c) * old_y + (vy*vz*(1 - c) - vx*s) * old_z;

    double new_z = (vx*vz*(1 - c) - vy*s) * old_x + (vy*vz*(1 - c) + vx*s) * old_y + (vz*vz*(1 - c) + c) * old_z;

    return Point3f(new_x, new_y, new_z);

}

　

 问题三：空间点绕xyz轴连续旋转

前面的问题比较基础，到这个问题就需要一点空间想象力了。

首先我假设一个点绕x、y、z轴旋转90°，最终它会落在哪里？这个答案不是唯一的，因为旋转的顺序将会影响到最终的结果。

 

以点(1,2,3)为例

A 我让它首先绕x轴转90°，再绕y轴转90°，再绕z轴转90°。

double x = 1, y = 2, z = 3;

codeRotateByX(y, z, 90, y, z);

codeRotateByY(x, z, -90, x, z);

codeRotateByZ(x, y, -90, x, y);

cout << endl << "   (1,2,3) -> (" << x << ',' << y << ',' << z << ")" << endl << endl;

旋转结果是：

 

B 这一次我让它首先绕z轴转90°，再绕y轴转90°，最后绕z轴转90°。

double x = 1, y = 2, z = 3;

codeRotateByZ(x, y, -90, x, y);

codeRotateByY(x, z, -90, x, z);

codeRotateByX(y, z, 90, y, z);

cout << endl << "   (1,2,3) -> (" << x << ',' << y << ',' << z << ")" << endl << endl;

这次的结果是：

 

显然，不同的旋转顺序导致了结果的不同，因此在处理空间内绕轴旋转的问题时，我们需要严格定义每次旋转的顺序，否则会导致错误的答案。

2 求解三个轴的旋转角/欧拉角/姿态角旋转公式&程序

根据相机旋转矩阵求解三个轴的旋转角/欧拉角/姿态角

关键词：旋转矩阵 旋转角 欧拉角 姿态角

用途：相机位姿估计、无人机位姿估计、各种位姿估计

文章类型：概念、公式总结（本文不带推倒过程，若想了解公式是如何推出来的请自习搜索文献），C++函数展示

@Author:VShawn(singlex@foxmail.com)

@Date:2016-11-04

@Lab: CvLab202@CSU

     

前言

相机标定后得出了旋转矩阵，而后应该怎么通过旋转矩阵表示相机姿态这一问题估计困扰过每一位研究这一问题的新同学，那么本文就给出我的一种做法，帮助大家少走歪路。当然有什么好的想法，或者更牛的办法也欢迎留言交流斧正。当然，公式的推倒就不要找我了，参考文献比我厉害多了

公式

有旋转矩阵

R=⎛⎝⎜r11r21r31r12r22r32r13r23r33⎞⎠⎟R=(r11r12r13r21r22r23r31r32r33)

那么可求出各轴旋转角：

Z轴：

    

θz=atan2(r21,r11)θz=atan⁡2(r21,r11)

Y轴

θy=atan2(−r31,r312+r332−−−−−−−−√)θy=atan⁡2(−r31,r312+r332)

X轴

θx=atan2(r32,r33)θx=atan⁡2(r32,r33)

注：atan2为C++中函数，atan2(y,x)的做法：当 x 的绝对值比 y 的绝对值大时使用 atan(y/x)；反之使用 atan(x/y)。这样就保证了数值稳定性。

上面公式的意思是，相机坐标系想要转到与世界坐标系完全平行（即xc平行于x_w，y_c平行于y_w，z_c平行于z_w，且他们的方向都是相同的），需要旋转3次，设原始相机坐标系为C₀。

1、C₀绕其z轴旋转，得到新的坐标系C₁；

2、C₁绕其y轴旋转，得到新的坐标系C₂（注意旋转轴为C₁的y轴，而非C₀的y轴）；

3、C₂绕其x轴旋转，得到新的坐标系C₃。此时C₃与世界坐标系W完全平行。

特别注意：旋转顺序为z y x，切记不能调换。

C++程序

代码就比较简单了

r11-r33从旋转矩阵中提出，为double型变量。

 

//计算出相机坐标系的三轴旋转欧拉角，旋转后可以转出世界坐标系。

//旋转顺序为z、y、x

const double PI = 3.141592653;

double thetaz = atan2(r21, r11) / PI * 180;

double thetay = atan2(-1 * r31, sqrt(r32*r32 + r33*r33)) / PI * 180;

double thetax = atan2(r32, r33) / PI * 180;

 

参考文献

Computing Euler angles from a rotation matrix. Gregory G. Slabaugh

http://www.staff.city.ac.uk/~sbbh653/publications/euler.pdf

3 子坐标系C在父坐标系W中的旋转问题

子坐标系C在父坐标系W中的旋转问题

关键词：空间旋转、旋转轴、刚体旋转

用途：相机位姿估计、无人机位姿估计

文章类型：概念、公式总结（本文不带推倒过程，若想了解公式是如何推出来的请自习搜索文献），C++函数展示

@Author：VShawn(singlex@foxmail.com)

@Date:2016-11-04

@Lab: CvLab202@CSU

本文接上一篇《空间点绕轴旋转公式&程序(C++)》，继续讨论空间内的旋转问题，可能会用到上一篇中定义的函数。

问题四：空间内的坐标系旋转（相机坐标系在世界坐标系中的旋转）

好了，现在问题越来越复杂了，这次的问题涉及两个坐标系：世界坐标系W与相机坐标系C。

做过相机标定或者研究过相机模型的人对这两个坐标系应该是相当熟悉的了，对于不了解这方面的同学可以这样理解：

1. 世界坐标系就是前文一直在用着的坐标系，它规定了所有东西的绝对坐标。
2. 相机坐标系是人眼或者是相机看物体的一个坐标系，它能够表示我观测的这个物体距离我有多远，方向角是多少。（其z轴为视线正中线）

 

下图中的O即为相机坐标系原点，也就是人眼的位置，OZ_c表示的是当前视线的方向，

O_w则是世界坐标系原点。

 

于是我们的问题就是，人站在某点拍摄世界坐标系的原点O_w（下标w表示该坐标是在世界坐标系中的），拍摄出的原点正好落在图像中心，现在我通过某种方法（解PNP问题）计算出O_w在相机坐标系下的坐标为（下标c表示该坐标定义在相机坐标系内），求相机或者说是人位于世界坐标系的哪里。

 

这个问题，当相机坐标系跟世界坐标系完全平行（指两对坐标系的三个轴都相互对应平行）的时候很简单，无需考虑旋转，相机的位置就是。实际上，只要相机坐标系的三根轴与世界的三轴平行（这种平行不需要保证一一对应），这个问题都很好解决。比如下面几幅图的情况（黄点为世界坐标系原点O_w），只要知道世界坐标系的原点O_w在相机坐标系中的坐标，就可以很容易计算出相机的位置。

 

 

 

然而，一旦出现了下面的情况，那么问题就不那么好解决了。

比如说相机的状态是这样：

你就很难通过相机坐标系下的O_w坐标，计算出相机在世界坐标系下的位置。因为其中涉及到了相机坐标系在世界坐标系中的旋转问题。

那么如何求解这个问题呢，事实上，在上文中通过某种方法（解PNP问题）求出原点O_w在相机坐标系下的坐标的同时，我们也获得了相机坐标系的旋转矩阵（没错，就是求相机的外参数矩阵：旋转矩阵r&平移矩阵t），这个"某种方法"我将在其他文章中给大家展示。平移矩阵t可以告诉世界坐标系原点Ow在相机坐标系中的坐标，我们暂且假设为点P=(x₀,y₀,z₀)，向量O_cP为O_c指向O_w的向量。而旋转矩阵r，可以求出三个欧拉角【具体如何计算参考《根据相机旋转矩阵求解三个轴的旋转角/欧拉角》】。当相机坐标系C按照z轴、y轴、x轴的顺序旋转以上角度后变成坐标系C₃，C₃将与世界坐标系W完全平行。此时如果知道O_w在C₃系中的坐标，自然就知道了O_c在世界坐标系W中的坐标。

显然，相机坐标系经历了三次旋转后，原点O_c位置会保持不变，而点P跟随坐标系进行了三次旋转，那么向量O_cP不再指向O_w。

为了抵消旋转的作用，每次对坐标轴旋转后，我们需要对点P进行反向旋转，使得坐标系C₃中的向量O_cP依然能指向O_w。

 

于是得到解决问题的步骤：

第一次旋转：

原始相机坐标系C绕z轴旋转了变为C₁系，此时P=P₀=(x₀,y₀,z₀)，那么单独将P点绕z轴旋转，得到P₁=(x₁,y₁,z₁)，为C₁系中O_w的坐标。

第二次旋转：

C₁绕y轴旋转了变为C₂系，此时P₁=(x₁,y₁,z₁)，那么将P₁点绕y轴旋转，得到P₂=(x₂,y₂,z₂)，为C₂系中O_w的坐标。

第三次旋转：

C₂绕x轴旋转了变为C₃系，此时P₂=(x₂,y₂,z₂)，那么将P₂点绕x轴旋转，得到P₃=(x₃,y₃,z₃)，为C₃系中O_w的坐标。

 

于是世界坐标系中，相机的位置坐标为(-x₃,-y₃,-z₃)。

 

至于程序就很简单了，只需要调用几次《空间点绕轴旋转公式&程序(C++)》中的函数即可。

 

最后，本文的成果用在了《相机位姿估计1：根据四个特征点估计相机姿态》中。