文章目录

• 1.Visual Odometry
• 课程视频中讲述的比较快，引用《slam14讲》中的一部分内容。
• • 两两帧的视觉里程计
  • 以传统的匹配特征点——求 PnP 的方法为例
  • PNP问题
  • Opencv::pnp
  • code
  • 根据相机旋转矩阵求解三个轴的旋转角/欧拉角/姿态角
  • 根据《子坐标系C在父坐标系W中的旋转》求出相机在世界坐标系中的位置。
  • • XYZ空间内某点绕X、Y、Z轴旋转一次
  • 空间点绕任意轴旋转,见附录
  • 测量空间点，对P5点重投影，投影公式为
  • 验证上述逻辑。code见附录——PnP
• 讨论
• • 用 RANSAC 的解作为初值，再用非线性优化求一个最 优值
  • 地图与帧匹配
  • 过程
  • 讨论
• Bundle Adjustment
• • BA的作用
  • • 投影
    • 特征匹配
    • 重投影
    • 重投影误差
    • 最小化重投影误差
  • BundleAdjustment的工作过程
  • 明天尝试BA代码
  • 一晃一年过去了
• 附录
• • 空间点绕任意轴旋转
  • 验证PnP
  • 一个SVO的项目结构

1.Visual Odometry

• VSLAM有直接法和特征点法两大类。

  • 直接法：提取梯度纹理特征明显的像素，帧间VO是靠图像对齐，即通过
    • 最小化像素灰度差函数来优化帧间位姿。
  • 特征点法：提取特征点（通常是角点），帧间VO靠PNP，即缩小在后帧图像上，
    • 重投影点与特征匹配点距离误差，来优化帧间位姿。

• VO：特征提取和匹配

  • 提取稀疏特征点（类似特征点法），帧间VO用图像对齐（类似于直接法），
  • SVO结合了直接法和特征点法，因此，称它为半直接法。
  • SVO算法分成两部分: 位姿估计，深度估计（最基本的深度估计就是三角化）

通过跟踪视觉特征逐步估计您相对于初始参考系的位置和方向的过程。

和多视图/BA有何不同？

• BA可以具有较大的基准线，不同的摄像机，稀疏的视点
•VO基于视频，并且需要递增
•视频允许运动模型

与vSLAM有什么区别？

•可互换使用，但vSLAM还会生成特征图，而VO的主要任务是相机轨迹。

视觉里程表工具汇总：

•窗口上的BA调整——为了最大程度地减少漂移
•关键帧选择
•5点的RANSAC或3点减少的最小问题。
•视觉闭合，在重新访问场所时产生独特的轨迹——分两步：•使用特征检索（词汇树）搜索最近的访问图像•具有极线约束的几何一致性

可参考的书籍：

课程视频中讲述的比较快，引用《slam14讲》中的一部分内容。

两两帧的视觉里程计

先来考虑特征点法。 VO 任务是，根据输入的图像，计算相 机运动和特征点位置。（220页）

只关心两个帧之间的运动估计，并且不优化特征点的位置。然 而把估得的位姿“串”起来，也能得到一条运动轨迹。这种方式可以看成两两帧间的（Pairwise），无结构（Structureless）的 VO，实现起来最为简单。

在这种 VO 里，我们定义了参考帧（Reference）和当前帧（Current） 这两个概念。以参考帧为坐标系，我们把当前帧与它进行特 征匹配，并估计运动关系。

这可以通过特征点匹 配、光流或直接法 得到，但这里我们只关心运动，不关心结构。换句话说，只要通过特征 点成功求出了运动，我们就不再需要这帧的特征点了。这种做法当然会有缺陷，但是忽略 掉数量庞大的特征点可以节省许多的计算量。

以传统的匹配特征点——求 PnP 的方法为例

由于各种原因，我们设计的上述 VO 算法，每一步都有可能失败。例 如图片中不易提特征、特征点缺少深度值、误匹配、运动估计出错等等。

• OpenCV::solvePnP
• 参考：https://www.cnblogs.com/singlex/p/pose_estimation_1.html

PNP问题

关于PNP问题就是指通过世界中的N个3D特征点与图像成像中的N个2D像点，计算出其投影关系，从而获得相机或物体位姿的问题。

以下讨论中设相机位于点Oc，P1、P2、P3……为特征点。

Case1：当N=3时
一个以P1为球心的球A，一个以P2为球心的球B，一个以P3为球心的球C，相机这次位于ABC三个球面的相交处，这次应该会有4个解，其中一个就是我们需要的真解了。

Case2：当N大于3时
N=3时求出4组解，好像再加一个点就能解决这个问题了，事实上也几乎如此。N>3后，能够求出正解了，先用3个点计算出4组解获得四个旋转矩阵、平移矩阵。
将第四个点的世界坐标代入公式，获得其在图像中的四个投影（一个解对应一个投影），取出其中投影误差最小的那个解，就是我们所需要的正解。

Opencv::pnp

类的调用顺序：1.初始化PNPSolver类；2.调用SetCameraMatrix(),SetDistortionCoefficients()设置好相机内参数与镜头畸变参数；3.向Points3D，Points2D中添加一一对应的特征点对；4.调用Solve()方法运行计算；5.从属性Theta_C2W中提取旋转角，从Position_OcInW中提取出相机在世界坐标系下的坐标。

OpenCV提供了三种方法进行PNP计算

方法名	说明	paper	测试结论
CV_P3P	这个方法使用非常经典的Gao方法解P3P问题，求出4组可能的解，再通过对第四个点的重投影，返回重投影误差最小的点。	论文《Complete Solution Classification for the Perspective-Three-Point Problem》	任意4个特征点求解，不要共面，特征点数量不为4时报错
CV_ITERATIVE	该方法基于Levenberg-Marquardt optimization迭代求解PNP问题，实质是迭代求出重投影误差最小的解，这个解显然不一定是正解。		实测该方法只有用4个共面的特征点时才能求出正确的解，使用5个特征点或4点非共面的特征点都得不到正确的位姿。
CV_EPNP	该方法使用EfficientPNP方法求,对于N个特征点，只要N>3就能够求出正解。对于N个特征点，只要N>3就能够求出正解	论文《EPnP: Efficient Perspective-n-Point Camera Pose Estimation》

推荐使用CV_P3P来解决实际问题，该方法对于有四个特征点的情况限制少、运算速度快。当特征点数大于4时，可以取多组4特征点计算出结果再求平均值，或者为了简单点就直接使用CV_EPNP法。

code

通过迭代法解PNP问题，得到相机坐标系关于世界坐标系的旋转矩阵R与平移矩阵T后，根据《根据相机旋转矩阵求解三个轴的旋转角》获得相机坐标系的三轴旋转角，实现了对相机位姿的估计。知道相机在哪后，我们就可以通过两张照片，计算出照片中某个点的高度，实现对环境的测量。

opencv接口

bool solvePnP(InputArray objectPoints, InputArray imagePoints, InputArray cameraMatrix, InputArray distCoeffs, OutputArray rvec, OutputArray tvec, bool useExtrinsicGuess=false, int flags=ITERATIVE )

第一个输入objectPoints为特征点的世界坐标，坐标值需为float型，不能为double型，可以输入mat类型，也可以直接输入vector 。

第二个输入imagePoints为特征点在图像中的坐标，需要与前面的输入一一对应。同样可以输入mat类型，也可以直接输入vector 。

第三个输入cameraMatrix为相机内参数矩阵，大小为3×3，形式为：

第四个输入distCoeffs输入为相机的畸变参数，为1×5的矩阵。

第五个rvec为输出矩阵，输出解得的旋转向量。

第六个tvec为输出平移向量。

第七个设置为true后似乎会对输出进行优化。

最后的输入参数有三个可选项：

CV_ITERATIVE，默认值，它通过迭代求出重投影误差最小的解作为问题的最优解。

CV_P3P则是使用非常经典的Gao的P3P问题求解算法。

CV_EPNP使用文章《EPnP: Efficient Perspective-n-Point Camera Pose Estimation》中的方法求解。

---

3D:

2D

调用即可求出相机姿态

//初始化输出矩阵
//rvec为输出矩阵，输出解得的旋转向量
// tvec为输出平移向量。
cv::Mat rvec = cv::Mat::zeros(3, 1, CV_64FC1);
cv::Mat tvec = cv::Mat::zeros(3, 1, CV_64FC1);//三种方法求解//其中solvePnP的默认方法迭代法
solvePnP(Points3D, Points2D, camera_matrix, distortion_coefficients, rvec, tvec, false, CV_ITERATIVE);    //实测迭代法似乎只能用共面特征点求位置
//solvePnP(Points3D, Points2D, camera_matrix, distortion_coefficients, rvec, tvec, false, CV_P3P);        //Gao的方法可以使用任意四个特征点
//solvePnP(Points3D, Points2D, camera_matrix, distortion_coefficients, rvec, tvec, false, CV_EPNP);//旋转向量变旋转矩阵
double rm[9];
cv::Mat rotM(3, 3, CV_64FC1, rm);
Rodrigues(rvec, rotM);

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

//计算出相机坐标系的三轴旋转欧拉角，旋转后可以转出世界坐标系。
//旋转顺序为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;
//参考：https://www.cnblogs.com/singlex/p/RotateMatrix2Euler.html

根据《子坐标系C在父坐标系W中的旋转》求出相机在世界坐标系中的位置。

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

XYZ空间内某点绕X、Y、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;}//将空间点绕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;
}//将空间点绕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;
}

求出相机在世界坐标系中的位置。

空间点绕任意轴旋转,见附录

测量空间点，对P5点重投影，投影公式为

验证上述逻辑。code见附录——PnP

假设参考帧相对世界坐标的变换矩阵为 Trw，当前帧与世界坐 标系间为 Tcw，则待估计的运动与这两个帧的变换矩阵构成左乘关系：

在 t 到 t + 1 时刻，我们又以 t 时刻 为参考帧，考虑 t 到 t + 1 间的运动关系。如此往复，就得到了一条运动轨迹。

讨论

《slam14讲》书中的一个表格，这个表格很有代表性

可以看到，特征点的提取和匹配占据了绝大多数的计算时间，而看似复杂的 PnP 优 化，计算量与之相比基本可以忽略。要提高特征提取、匹配算法的速度！

用 RANSAC 的解作为初值，再用非线性优化求一个最 优值

书中，采用的方法是：
在位姿估计时，使用了 RANSAC 求出的 PnP 解。由于 RANSAC 只采用少数 几个随机点来计算 PnP，虽然能够确定 inlier，但该方法容易受噪声影响。在 3D-2D 点存在噪声的情形下，推荐 用我们要用 RANSAC 的解作为初值，再用非线性优化求一个最 优值。这种做法是优于现在的做法的。

地图与帧匹配

是否使用地图取决你对精度——效率这个矛盾的把握。我 们完全可以出于效率的考量，使用两两无结构式的 VO；也可以为了更好的精度，构建局部地图。

所以这里的地图仅是一个临时性的概念，指的是把各帧特征点缓存 到一个地方，构成了特征点的集合，我们称它为地图。 我们只保留离相机当前位置 较近的特征点，而把远的或视野外的特征点丢掉。

过程

• 1. 在提取第一帧的特征点之后，将第一帧的所有特征点全部放入地图中：
• 2. 后续的帧中，使用 OptimizeMap 函数对地图进行优化。包括删除不在视野内的点， 在匹配数量减少时添加新点等等。可以使用三角化来更 新特征点的世界坐标，或者考虑更好地动态管理地图规模的策略。
• 特征匹配代码。匹配之前，我们从地图中拿出一些候选点（出现在视野内的点），然 后将它们与当前帧的特征描述子进行匹配。

关于关键帧
关键帧是相机运动过程当中某几个特殊的帧，这里“特殊”的意义是可以由我们自己 指定的。常见的做法时，每当相机运动经过一定间隔，就取一个新的关键帧并保存起来（这在李代数上很容易实现）。 这些关键帧的位姿将被仔细优化，而位于两个关键帧之间的忽略掉。

讨论

我们看到，视觉里程计 能够估算局部时间内的相机运动以及特征点的位置，但是这种局部的方式有明显的缺点：

1. 容易丢失。一旦丢失，我们要么“等待相机转回来”（保存参考帧并与新的帧比较）， 要么重置整个 VO 以跟踪新的图像数据。
2. 2. 轨迹漂移。主要原因是每次估计的误差会累计至下一次估计，导致长时间轨迹不准 确。大一点儿的局部地图可以缓解这种现象，但它始终是存在的。

值得一提的是，如果只关心短时间内的运动，或者 VO 的精度已经满足应用需求，那 么有时候你可能需要的仅仅就是一个视觉里程计，而不用完全的 SLAM。比如某些无人机 控制或 AR 游戏的应用中，用户并不需要一个全局一致的地图，那么轻便的 VO 可能是更 好的选择。

【缺少代码实践】

---

Bundle Adjustment

Bundle Adjustment译为光束法平差，或者束调整、捆集调整。

Bundle Adjustment的名字由来于每个3D特征和相机光学中心“发射”出的光束，而这些光束可以根据结构和视角参数进行最佳调节。

3D-2D的PnP问题（有了初始化好的 3D坐标就可以解PnP问题求解位姿），线性解法(DLT)与BA解法；

BA的作用

让我们先来看看Bundle Adjustment的作用，BA不仅可以优化位姿（R和t），还可以优化特征点的空间位置。而我们又可以把BA看成是最小化重投影误差（Reprojection error）问题，同时这也是一个非线性最小二乘问题。说到这里，BA就是一个优化模型，其本质就是最小化重投影误差。

投影

首先了解一下，投影是什么意思。这里所说的投影是指空间中的某个点映射到图像中的某个像素的过程。

特征匹配

学过SLAM的童鞋应该知道，SLAM的关键问题就是求解出机器人的运动估计，也就是位姿（可用旋转矩阵+位移，或者是变换矩阵，或者是李代数的方式表示）。在视觉SLAM中，我们要从两张图中恢复机器人的运动。在恢复运动位姿之前，我们需要对某一时刻的连续两帧进行提取特征点，特征点匹配。 特征点匹配的意思就是说 ：a图像中的ua像素对应b图像中的ub像素，他们都表示空间点中的P点。

注意，这里的图像b是由机器人所携带的摄像头传感器所得到的真实数据，是观测出来的。表示空间点P在b图像的实际投影。

我们这里把ub的坐标设置为（ubx，uby）。

重投影

以单目摄像头为例子，结合多个特征匹配点，使用对极几何（E或F）或者是单应性矩阵H可以求得机器人的运动位姿。我们知道针孔相机模型如下：

利用针孔相机模型（观测模型），可以通过空间点P的坐标，获得空间点P投影在图像b中的像素坐标，我们称为ub`。

这个ub`像素点坐标是先通过位姿估计，再通过观测模型计算出来的，相当于这个像素坐标是估算出来的。

这个过程也称为重投影。

重投影误差

如果完全没有误差，那么两者的坐标是一样的，但这是不可能的，不管是实际测量出来的ub还是通过位姿估算+重投影的出来的ub`，都是有误差的，将实际值和估算值一减，就得到了重投影误差。

最小化重投影误差

由于两张图片有很多对特征匹配点，将所有特征点误差求和，取平方，再乘上1/2，这就构建了一个非线性最小二乘问题了，将里面的位姿、空间点作为优化的对象，最小化重投影误差，即可得到一个优化后的结果，这就是所谓的BA。

BundleAdjustment的工作过程

光束平差法的最终目的归结为：减少观测图像的点和参考图像（预测图像）的点之间位置投影变换（再投影）误差。

这最小化误差算法使用的是最小二乘算法，目前使用最为成功是Levenberg-Marquardt, 它具有易于实现，对大范围的初始估计能够快速收敛的优点。

光束法平差（BA）最小化一般通过Levenberg-Marquardt (LM)算法来辅助完成。然而，由于许多未知的因素作用于最小投影误差，一个通用的LM算法的实现（如MINPACK的 lmder）当应用于BA背景下的定义的最小化问题时，会带来极高的计算代价。

在基本的法方程中不同的三维点和相机参数相互之间影响较小，呈现一种稀疏的块结构（如图）。

一个基于Levenberg-Marquardt(LM)算法的通用稀疏光束法平差Sba利用这种稀疏的特性，使用LM算法的简化的稀疏变量来降低计算的复杂度。Sba是通用的，因为它保证了用户对于相机和三维结构的描述参数的定义的完全控制。因此，它事实上可以支持任何多视重建问题的显示和参数化。比如任意投影相机，部分的或完全标定的相机，由固定的三维点进行外方位元素（即姿态）的估计，精化本征参数，等等。用户要想在这类问题中使用sba，只需要提供合适的程序对这些问题和参数来计算估计的图像投影和他们的函数行列式（Jacobian）。sba包含了检查用户提供的函数行列式的一致性的程序。

链接：https://www.bbsmax.com/A/E35pbvjYzv/

明天尝试BA代码

20203月19

一晃一年过去了

2021年5月24，再过来总结下：

从每一个特征点反射出来的几束光线（bundles of light rays），在我们把相机姿态和特征点空间位置做出最优的调整 (adjustment) 之后，最后收束到相机光心的这个过程 ，简称为 BA。

思考一个相机模型，世界坐标$p$, 考虑内外参数，畸变，最后得到成像像素点坐标：
1、世界坐标转换到相机坐标，这里将用到相机外参数 (R,t)
P′=Rp+t=[x′,y′,z′]TP' = Rp+t=[x',y',z']^TP′=Rp+t=[x′,y′,z′]T

2、将 $P\prime$ 投至归一化平面，得到归一化坐标
Pc=[uc，vc,1]T=[x′/z′,y′/z′,1]TP_c=[u_c，v_c,1]^T=[x'/z',y'/z',1]^TPc​=[uc​，vc​,1]T=[x′/z′,y′/z′,1]T

3、对归一化坐标去畸变，得到去畸变后的坐标。这里暂时只考虑径向畸变
uc′=uc(1+K1rc2+k2rc4)u_c'=u_c(1+K_1r^2_c+k_2r_c^4)uc′​=uc​(1+K1​rc2​+k2​rc4​)
vc′=vc(1+K1rc2+k2rc4)v_c'=v_c(1+K_1r^2_c+k_2r_c^4)vc′​=vc​(1+K1​rc2​+k2​rc4​)

4、根据内参模型，计算像素坐标
us=fxuc′+cxu_s=f_xu_c'+c_xus​=fx​uc′​+cx​
vs=fyvc′+cyv_s=f_yv_c'+c_yvs​=fy​vc′​+cy​

抽象成一个函数：
$z=h(x,y)$

$z$代表像素坐标[ux,vs]T[u_x,v_s]^T[ux​,vs​]T
$x$ 代表相机位姿$R,t$
$y$代表世界坐标p=[x,y,z]Tp=[x,y,z]^Tp=[x,y,z]T

然后考虑误差，建立最小二乘：
$e=z-h(x,y)$

考虑到有很多三维点转化到像素点，整体的代价函数（cost function）：
0.5∗∑i=1n∣∣e∣∣20.5* \sum_{i=1}^n ||e||^20.5∗∑i=1n​∣∣e∣∣2

对这个最小二乘进行求解，相当于对位姿和路标同时作了调整，也就是所谓的 BA。

关于求解BA的过程：
利用非线性优化来实现。一般思路：从某个初始值开始，不断寻找下降方向$\Delta x$，找到最优解。

因为有很多个三维点，因此待优化量就是：很多个点的世界坐标，以及相对应的相机姿态。（这里可以时不同时间的三维点，也可以指同一时刻的不同三维点，也可以是不同时间的不同三维点）。假设有n个三维点p1,p2,…,pnp_1,p_2,…,p_np1​,p2​,...,pn​，m个相机姿态ξ1,ξ2,…ξm\xi_1,\xi_2,…\xi_mξ1​,ξ2​,...ξm​

待优化量x=[ξ1,ξ2,…ξm,p1,p2,…,pn]x=[\xi_1,\xi_2,…\xi_m,p_1,p_2,…,p_n]x=[ξ1​,ξ2​,...ξm​,p1​,p2​,...,pn​] ,$\Delta x$时对待优化量$x$的整体增量。
误差函数Cost function变成了：
0.5∗∣∣e+F∗Δξ+E∗Δx∣∣20.5*||e+F*\Delta \xi+E*\Delta x||^20.5∗∣∣e+F∗Δξ+E∗Δx∣∣2

$F$是相机姿态的偏导数，$E$是世界坐标的偏导数。这里的雅可比矩阵 $E$ 和 $F$ 必须是整体目标函数对整体变量的导数，它将是一个很大块的矩阵，而里头每个小分块，需要由每个误差项的导数 FijF_ijFi​j 和 EijE_ijEi​j “拼凑”起来。

我们使用 G-N 还是 L-M 方法，最后都将面对增量线性方程：
$H\Delta x=g$
我们知道 G-N 和 L-M 的主要差别在于，这里的 $H$ 是取 JTJJ^TJJTJ 还是 JTJ+λIJ^TJ + λIJTJ+λI 的形式。
由于我们把变量归类成了位姿和空间点两种，所以雅可比矩阵可以分块为：

$J=[FE]$

那么，以 G-N 为例，则 $H$ 矩阵为：

当然在 L-M 中我们也需要计算这个矩阵。不难发现，因为考虑了所有的优化变量，这个线性方程的维度将非常大，包含了所有的相机位姿和路标点。尤其是在视觉 SLAM 中，一个图像就会提出数百个特征点，大大增加了这个线性方程的规模。如果直接对 H 求逆来计算增量方程，由于矩阵求逆是复杂度为 O(n 3 ) 的操作 [71]，这是非常消耗计算资源的。

幸运地是，这里的 H 矩阵是有一定的特殊结构的。利用这个特殊结构，我们可以加速求解过程。21 世纪视觉 SLAM 的一个重要进展是认识到了矩阵 H 的稀疏结构，并发现该结构可以自然、显式地用图优化来表示。

附录

空间点绕任意轴旋转

//定义返回结构体
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);
}

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;

验证PnP

import cv2
import numpy as np
import math
object_3d_points = np.array(([0, 0, 0],[0, 200, 0],[150, 0, 0],[150, 200, 0]), dtype=np.double)
object_2d_point = np.array(([2985, 1688],[5081, 1690],[2997, 2797],[5544, 2757]), dtype=np.double)
camera_matrix = np.array(([6800.7, 0, 3065.8],[0, 6798.1, 1667.6],[0, 0, 1.0]), dtype=np.double)
dist_coefs = np.array([-0.189314, 0.444657, -0.00116176, 0.00164877, -2.57547], dtype=np.double)
# 求解相机位姿
found, rvec, tvec = cv2.solvePnP(object_3d_points, object_2d_point, camera_matrix, dist_coefs)
rotM = cv2.Rodrigues(rvec)[0]
camera_postion = -np.matrix(rotM).T * np.matrix(tvec)
print(camera_postion.T)
# 验证根据博客http://www.cnblogs.com/singlex/p/pose_estimation_1.html提供方法求解相机位姿
# 计算相机坐标系的三轴旋转欧拉角，旋转后可以转出世界坐标系。旋转顺序z,y,x
thetaZ = math.atan2(rotM[1, 0], rotM[0, 0])*180.0/math.pi
thetaY = math.atan2(-1.0*rotM[2, 0], math.sqrt(rotM[2, 1]**2 + rotM[2, 2]**2))*180.0/math.pi
thetaX = math.atan2(rotM[2, 1], rotM[2, 2])*180.0/math.pi
# 相机坐标系下值
x = tvec[0]
y = tvec[1]
z = tvec[2]
# 进行三次旋转
def RotateByZ(Cx, Cy, thetaZ):rz = thetaZ*math.pi/180.0outX = math.cos(rz)*Cx - math.sin(rz)*CyoutY = math.sin(rz)*Cx + math.cos(rz)*Cyreturn outX, outY
def RotateByY(Cx, Cz, thetaY):ry = thetaY*math.pi/180.0outZ = math.cos(ry)*Cz - math.sin(ry)*CxoutX = math.sin(ry)*Cz + math.cos(ry)*Cxreturn outX, outZ
def RotateByX(Cy, Cz, thetaX):rx = thetaX*math.pi/180.0outY = math.cos(rx)*Cy - math.sin(rx)*CzoutZ = math.sin(rx)*Cy + math.cos(rx)*Czreturn outY, outZ
(x, y) = RotateByZ(x, y, -1.0*thetaZ)
(x, z) = RotateByY(x, z, -1.0*thetaY)
(y, z) = RotateByX(y, z, -1.0*thetaX)
Cx = x*-1
Cy = y*-1
Cz = z*-1
# 输出相机位置
print(Cx, Cy, Cz)
# 输出相机旋转角
print(thetaX, thetaY, thetaZ)
# 对第五个点进行验证
Out_matrix = np.concatenate((rotM, tvec), axis=1)
pixel = np.dot(camera_matrix, Out_matrix)
pixel1 = np.dot(pixel, np.array([0, 100, 105, 1], dtype=np.double))
pixel2 = pixel1/pixel1[2]
print(pixel2)

求解出了相机6DOF位姿估计，通过验证第5个点[0, 100, 105]对应像素点[4159.6, 673.69]和真实像素位置[4146, 673]相差不大。

一个SVO的项目结构

rpg_svo
├── rqt_svo       为与 显示界面 有关的功能插件
├── svo           主程序文件，编译 svo_ros 时需要
│   ├── include
│   │   └── svo
│   │       ├── bundle_adjustment.h        光束法平差（图优化）
│   │       ├── config.h                   SVO的全局配置
│   │       ├── depth_filter.h             像素深度估计（基于概率） 高斯均值混合模型
│   │       ├── feature_alignment.h        特征匹配
│   │       ├── feature_detection.h        特征检测  faster角点
│   │       ├── feature.h（无对应cpp）      特征定义
│   │       ├── frame.h                    frame定义
│   │       ├── frame_handler_base.h       视觉前端基础类
│   │       ├── frame_handler_mono.h       单目视觉前端原理(较重要)==============================
│   │       ├── global.h（无对应cpp）       有关全局的一些配置
│   │       ├── initialization.h           单目初始化
│   │       ├── map.h                      地图的生成与管理
│   │       ├── matcher.h                  重投影匹配与极线搜索
│   │       ├── point.h                    3D点的定义
│   │       ├── pose_optimizer.h           图优化（光束法平差最优化重投影误差）
│   │       ├── reprojector.h              重投影
│   │       └── sparse_img_align.h         直接法优化位姿（最小化光度误差）
│   ├── src
│   │   ├── bundle_adjustment.cpp
│   │   ├── config.cpp
│   │   ├── depth_filter.cpp
│   │   ├── feature_alignment.cpp
│   │   ├── feature_detection.cpp
│   │   ├── frame.cpp
│   │   ├── frame_handler_base.cpp
│   │   ├── frame_handler_mono.cpp
│   │   ├── initialization.cpp
│   │   ├── map.cpp
│   │   ├── matcher.cpp
│   │   ├── point.cpp
│   │   ├── pose_optimizer.cpp
│   │   ├── reprojector.cpp
│   │   └── sparse_img_align.cpp
├── svo_analysis           未知
├── svo_msgs               一些配置文件，编译 svo_ros 时需要
└── svo_ros                为与ros有关的程序，包括 launch 文件├── CMakeLists.txt    定义ROS节点并指导rpg_svo的编译
├── include
│   └── svo_ros
│    └── visualizer.h                
├── launch
│   └── test_rig3.launch   ROS启动文件
├── package.xml
├── param                   摄像头等一些配置文件
├── rviz_config.rviz        Rviz配置文件（启动Rviz时调用）
└── src├── benchmark_node.cpp├── visualizer.cpp        地图可视化└── vo_node.cpp           VO主节点=======================================