DEMO-lidar原理介绍及数据集运行测试

文章目录

DEMO-lidar介绍
- 论文标识指代：
- 运动估计公式推导：
- 运动估计算法流程
- 深度特征点关联
- BA(Bundle Adjustment)后端优化
- demo_lidar的坐标系
- - 旋转矩阵
  - 世界坐标系与相机坐标系转换
  - 相邻两帧相机坐标系之间的变换
- 代码文件
- 数据集测试

DEMO-lidar介绍

论文：《Real-time Depth Enhanced Monocular Odometry》
Real-time Depth Enhanced Monocular Odometry概述
DEMO-lidar源码解析
demo-rgbd解读
http://wiki.ros.org/demo_lidar

DEMO-lidar：Depth Enhanced Monocular Odometry (Demo)，是一个组合深度信息的单目视觉里程计。主要由三个主线程：

“feature tracking” 特征跟踪：提取并跟踪匹配角点(Harris corners)特征，使用Kanade Lucas Tomasi (KLT) 完成特征提取。
“visual odometry” 视觉里程计，基于帧与帧之间的特征点匹配关系，计算帧与帧之间的相机运动。其中附加上深度信息的特征点(通过深度图或通过估计相机运动的三角估计方法得到深度信息)解决6自由度运动估计，没有深度信息的特征点有助于解决方向估计。
“bundle adjustment” 光束法平差，BA优化，就是通过特征点投影在图像上的像素，来优化运动估计。

如果有IMU数据，转向测量值可由角速率和加速度积分得到，可用于优化滚转以及俯仰角，VO来优化偏航角和平移。

硬件：单目+（imu）+ lidar/rgbd
以下为算法框架图：

论文标识指代：

k指第k个关键帧。
C为系统坐标系，采用右手坐标系，x朝左，y朝上，z朝前。其实，使用z朝前，可以正好对应相机模型的深度信息。

大写I为一帧的特征点集合，i指代其中的特征点iii，i∈Ii \in Ii∈I。
设第k帧图像中，坐标系为CkC^kCk，特征点i具有深度信息，则设其坐标为Xik=[xik,yik,zik]X_i^k=[x_i^k, y_i^k, z_i^k]Xik=[xik,yik,zik].
如果特征点没有深度信息，则使用归一化坐标X‾ik\overline{X}_i^kXik来表示特征点i坐标，即X‾ik=[x‾ik,y‾ik,1]\overline X_i^k=[\overline x_i^k, \overline y_i^k, 1]Xik=[xik,yik,1]。

运动估计公式推导：

设两帧k-1与k帧之间的一对匹配点Xik−1X_i^{k-1}Xik−1
与XikX_i^kXik。则有Xik=RXik−1+TX_i^k=RX_i^{k-1}+TXik=RXik−1+T
1）假设Xik−1X_i^{k-1}Xik−1深度信息已知，但由于第k帧的运动估计尚未进行，因此，没有得到XikX_i^kXik的深度信息。因此，将Xik−1X_i^{k-1}Xik−1按归一化坐标表示，
即，X‾ik=[x‾ik,y‾ik,1]\overline X_i^k=[\overline x_i^k, \overline y_i^k, 1]Xik=[xik,yik,1]。
令zikz_i^kzik为XikX_i^kXik深度信息。
则：
zikX‾ik=RXik−1+Tz_i^k\overline X_i^k=RX_i^{k-1}+TzikXik=RXik−1+T
因为zikz_i^kzik未知，使用初等行变换消去zikz_i^kzik。

将等式两侧，第一行减去第三行乘以x‾ik\overline x_i^kxik，得到论文公式(3):

将等式两侧，第二行减去第三行乘以y‾ik\overline y_i^kyik，得到论文公式(4):

其中Rh及ThR_h及T_hRh及Th的下表h表示RRR矩阵的第h行。

2）假设Xik−1X_i^{k-1}Xik−1深度信息未知，也使用归一化坐标Xik−1X_i^{k-1}Xik−1表示，
即，X‾ik−1=[x‾ik−1,y‾ik−1,1]\overline X_i^{k-1}=[\overline x_i^{k-1}, \overline y_i^{k-1}, 1]Xik−1=[xik−1,yik−1,1]。
得到匹配特征点之间的归一化坐标关系如下：
zikX‾ik=zik−1RX‾ik−1+Tz_i^k\overline X_i^k=z_i^{k-1} R\overline X_i^{k-1}+TzikXik=zik−1RXik−1+T
仍利用初等行变换，消去zik与zik−1z_i^k与z_i^{k-1}zik与zik−1。
第一行减去第三行乘以x‾ik\overline x_i^kxik，
第二行减去第三行乘以y‾ik\overline y_i^kyik，
第一行乘以y‾ik\overline y_i^kyik 减去第二行乘以y‾ik\overline y_i^kyik，最后整理可得三个约束方程，论文公式(6)：

利用LM算法得到最优值。

运动估计算法流程

输入：X‾ik\overline X_i^kXik(当前帧无深度信息)，Xik−1X_i^{k-1}Xik−1(上一帧有深度信息的点云)或X‾ik−1\overline X_i^{k-1}Xik−1(上一帧有深度信息的点云)
输出：θ,T\theta, Tθ,T(旋转及平移变换估计)

根据上帧是否有深度信息，选择对应的约束方程，并迭代优化得到变换值。

深度特征点关联

深度点云降采样，以角度间隔均匀分布特征点(离相机越近，则点越密)。且以球坐标表示，即弦长、方位角(左右)、极角(上下)，变换到Ck−1C^{k-1}Ck−1下。
以2D KD-tree存储点云(将点云归一化到深度为10 的平面，来寻找近邻3点，找到后恢复深度为其特征点匹配深度)。设i∈Ck−1i\in C^{k-1}i∈Ck−1，在Ck−1C^{k-1}Ck−1的kdtree中找到与i相近的3个点，记为：
则Xik−1X^{k-1}_iXik−1的深度值通过解以下方程得到。

该方程与loam的点到面类似，只是没有分母项，意思是约束点i落到三个j点构成的平面上。

BA(Bundle Adjustment)后端优化

相对于前端的运动估计，BA优化采用批量优化，估计频率是前端odom的1/5。感觉优化思路类似，都是优化投影误差方程来得到运动估计，只是BA为批量优化，处理点云的点云为累积点云，而非单帧的点云。
约束方程：就是通过优化相同特征点在不同帧的图像中获得的特征点的投影坐标(可理解为像素，归一化坐标)，来估计得到更加准确的定位估计。
约束方程如下：

其中：X~ij\widetilde X_i^jXij表示特征点i在j frame坐标系下的观测值。
BA使用开源的iSAM库实现，作者说iSAM可以支持相机参数自定义，并且能够很好的处理有无深度的特征点。

demo_lidar的坐标系

demo_lidar采用的坐标系为西天北(左上前)，因此对应的RPY旋转顺序为：z轴->x轴->y轴

此坐标系下的转转矩阵：

旋转矩阵

参考链接：https://blog.csdn.net/weixin_41469272/article/details/108117485
若坐标系A绕其x轴转过角度rx后与坐标系B重合，则该矢量在两个坐标系中的投影可以通过一个初等旋转矩阵实现转换，即：

若坐标系A绕其y轴转过ry后与坐标系B重合，则有：

若坐标系A绕其z轴转过rz后与坐标系B重合，则有：

DEMO旋转变换遵循R(roll)P(pitch)Y(yaw)，对应DEMO的坐标系下的(ry,rx,rz)

世界坐标系与相机坐标系转换

transformSum对应其求解的相机位姿，transformSum={rx,ry,rz, tx,ty,tz}
0，1，2对应旋转三角(rx,ry,rz)， 3，4，5对应平移(tx,ty,tz)。

将点云从世界坐标系下变换到当前坐标系下(W→CW\rightarrow CW→C)时，使用的旋转顺序(ry,rx,rz);
将点云从当前坐标系下变换到世界坐标系下(C→WC\rightarrow WC→W)时，使用的旋转顺序(-rz,-rx,-ry);

将世界坐标系下的点piwp_i^wpiw转换到相机坐标系picp_i^cpic下：

double x1 = cos(ry) * p_wi.x - sin(ry) * p_wi.z;
double y1 = p_wi.y;
double z1 = sin(ry) * p_wi.x + cos(ry) * p_wi.z;double x2 = x1;
double y2 = cos(rx) * y1 + sin(rx) * z1;
double z2 = -sin(rx) * y1 + cos(rx) * z1;double x3 = cos(rz) * x2 + sin(rz) * y2;
double y3 = -sin(rz) * x2 + cos(rz) * y2;
double z3 = z2;p_ci.x = x3 - tx;
p_ci.y = y3 - ty;
p_ci.z = z3 - tz;

将相机坐标系下的点picp_i^cpic转换到世界坐标系piwp_i^wpiw下：

double x1 = cos(rz) * p_ci.x - sin(rz) * p_ci.y;
double y1 = sin(rz) * p_ci.x + cos(rz) * p_ci.y;
double z1 = p_ci.z;double x2 = x1;
double y2 = cos(rx) * y1 - sin(rx) * z1;
double z2 = sin(rx) * y1 + cos(rx) * z1;double x3 = cos(ry) * x2 + sin(ry) * z2;
double y3 = y2;
double z3 = -sin(ry) * x2 + cos(ry) * z2;p_wi.x = x3 + tx;
p_wi.y = y3 + ty;
p_wi.z = z3 + tz;

相邻两帧相机坐标系之间的变换

transform存储两帧之间的运动变换的负值。
设从l−1l-1l−1帧到l帧相机的运动为：{drx,dry,drz , dtx,dty,dtz }
但代码中transform存储的是运动的负值，即transform={-drx,-dry,-drz, -dtx,-dty,-dtz}。
0，1，2对应旋转三角负值(-drx,-dry,-drz)， 3，4，5对应平移(-dtx,-dty,-dtz)。

同理遵循RPY的旋转顺序。
将点云从世界坐标系下变换到当前坐标系下(l−1→ll-1 \rightarrow ll−1→l)时，使用的旋转顺序(dry,drx,drz);
将点云从当前坐标系下变换到世界坐标系下(l→l−1l\rightarrow l-1l→l−1)时，使用的旋转顺序(-drz,-drx,-dry);

设两帧为Il−1I_{l-1}Il−1，IlI_lIl，两个环境三维点pil−1∈Il−1p_i^{l-1}\in I_{l-1}pil−1∈Il−1，pil∈Ilp_i^l\in I_lpil∈Il。
将pil−1p_i^{l-1}pil−1从l−1l-1l−1帧相机坐标系下转化到l帧相机坐标系下，得到pilp_i^lpil

double x1 = cos(dry) * p_l_1i.x - sin(dry) * p_l_1i.z;
double y1 = p_l_1i.y;
double z1 = sin(dry) * p_l_1i.x + cos(dry) * p_l_1i.z;double x2 = x1;
double y2 = cos(drx) * y1 + sin(drx) * z1;
double z2 = -sin(drx) * y1 + cos(drx) * z1;double x3 = cos(drz) * x2 + sin(drz) * y2;
double y3 = -sin(drz) * x2 + cos(drz) * y2;
double z3 = z2;p_li.x = x3 - dtx;
p_li.y = y3 - dty;
p_li.z = z3 - dtz;

将pilp_i^lpil从lll帧相机坐标系下转化到l−1l-1l−1帧相机坐标系下，得到pil−1p_i^{l-1}pil−1

double x1 = cos(drz) * p_li.x - sin(drz) * p_li.y;
double y1 = sin(drz) * p_li.x + cos(drz) * p_li.y;
double z1 = p_li.z;double x2 = x1;
double y2 = cos(drx) * y1 - sin(drx) * z1;
double z2 = sin(drx) * y1 + cos(drx) * z1;double x3 = cos(dry) * x2 + sin(dry) * z2;
double y3 = y2;
double z3 = -sin(dry) * x2 + cos(dry) * z2;p_l_1i.x = x3 + dtx;
p_l_1i.y = y3 + dty;
p_l_1i.z = z3 + dtz;

代码文件

featureTracking.cpp
订阅：/image/raw 原始图像
输出：
/image_points_last 特征点：发送上一帧last与当前帧curr匹配上的特征点，发布的是last帧的特征点。
/image/show 加入特征点的图像。

point结构：
point.u 像素位置x：(像素坐标-减去中心)/x焦距
point.v 像素位置y：(像素坐标-减去中心)/y焦距
point.ind 匹配点的index，只有与当前帧匹配上的特征点，才会发布。两帧匹配特征点的index相同，以此，来对应last和curr的特征点。

将先前图像存到last，然后通过(cvGoodFeaturesToTrack)来提取角点，通过LK(cvCalcOpticalFlowPyrLK)来匹配两帧特征点。
特征点均有固定的排号，用于在odom中寻找匹配点，并用于估算位姿变换。

visualOdometry.cpp
订阅：
/image_points_last
/depth_cloud
/imu/data
/image/show
发布：
/cam_to_init
/depth_points_last
/image/show_2

接收/image_points_last，/image_points_last，找到对应的匹配特征点，并判断last特征点是否有深度信息，并对应根据文中的公式添加约束。
若没有深度信息，则使用公式(6)计算R，T

//ipr2各分量是y2分别在x,y,z,theta_1,theta_2,theta_3的偏导数
ipr2.x = v0*(crz*srx*(tx - tz*u1) - crx*(ty*u1 - tx*v1) + srz*srx*(ty - tz*v1)) - u0*(sry*srx*(ty*u1 - tx*v1) + crz*sry*crx*(tx - tz*u1) + sry*srz*crx*(ty - tz*v1))+ cry*srx*(ty*u1 - tx*v1) + cry*crz*crx*(tx - tz*u1) + cry*srz*crx*(ty - tz*v1);
ipr2.y = u0*((tx - tz*u1)*(srz*sry - crz*srx*cry) - (ty - tz*v1)*(crz*sry + srx*srz*cry) + crx*cry*(ty*u1 - tx*v1)) - (tx - tz*u1)*(srz*cry + crz*srx*sry)+ (ty - tz*v1)*(crz*cry - srx*srz*sry) + crx*sry*(ty*u1 - tx*v1);
ipr2.z = -u0*((tx - tz*u1)*(cry*crz - srx*sry*srz) + (ty - tz*v1)*(cry*srz + srx*sry*crz))- (tx - tz*u1)*(sry*crz + cry*srx*srz) - (ty - tz*v1)*(sry*srz - cry*srx*crz) - v0*(crx*crz*(ty - tz*v1) - crx*srz*(tx - tz*u1));
ipr2.h = cry*crz*srx - v0*(crx*crz - srx*v1) - u0*(cry*srz + crz*srx*sry + crx*sry*v1)- sry*srz + crx*cry*v1;
ipr2.s = crz*sry - v0*(crx*srz + srx*u1) + u0*(cry*crz + crx*sry*u1 - srx*sry*srz)- crx*cry*u1 + cry*srx*srz;
ipr2.v = u1*(sry*srz - cry*crz*srx) - v1*(crz*sry + cry*srx*srz) + u0*(u1*(cry*srz + crz*srx*sry)-v1*(cry*crz - srx*sry*srz)) + v0*(crx*crz*u1 + crx*srz*v1);
//从k-1旋转到k的顺序是：z轴->x轴->y轴
//y2是公式6[-v1*tz+ty, u1*tz-tx, -u1*ty+v1*tx]*R*[u0, v0, 1]^T
//R=[crz -srz 0;srz crz 0;0 0 1]*[1 0 0;0 crx -srx;0 srx crx]*[cry 0 sry;0 1 0;-sry 0 cry];
double y2 = (ty - tz*v1)*(crz*sry + cry*srx*srz) - (tx - tz*u1)*(sry*srz - cry*crz*srx)                                              - v0*(srx*(ty*u1 - tx*v1) + crx*crz*(tx - tz*u1) + crx*srz*(ty - tz*v1))                                                   + u0*((ty - tz*v1)*(cry*crz - srx*sry*srz) - (tx - tz*u1)*(cry*srz + crz*srx*sry)                                          + crx*sry*(ty*u1 - tx*v1)) - crx*cry*(ty*u1 - tx*v1);

1 基于深度地图匹配点云：
在深度地图中，查找与图像角点最邻近的3个特征点，利用公式(10)(点到面的思路)估算该角点的深度值，赋值给ipr.s。
推导：

ipr的结构：
ipr.x: Last u(上一帧角点归一化像素坐标u)
ipr.y: Last v(上一帧角点归一化像素坐标v)
ipr.z: Curr u(与上一帧匹配的当前帧角点归一化像素坐标u)
ipr.h: Curr v(与上一帧匹配的当前帧角点归一化像素坐标v)
ipr.s: 深度信息
ipr.v: 深度获取情况：0.未获得深度；1.由深度图获取 2. 由三角测量获得深度

          ipr.s = (x1*y2*z3 - x1*y3*z2 - x2*y1*z3 + x2*y3*z1 + x3*y1*z2 - x3*y2*z1)/ (x1*y2 - x2*y1 - x1*y3 + x3*y1 + x2*y3 - x3*y2 + u*y1*z2 - u*y2*z1- v*x1*z2 + v*x2*z1 - u*y1*z3 + u*y3*z1 + v*x1*z3 - v*x3*z1 + u*y2*z3- u*y3*z2 - v*x2*z3 + v*x3*z2);ipr.v = 1;

2 使用三角测距获得点云深度
三角测距的方法

Demo使用
如果点的深度信息未从雷达获取到，而且该角点过了一定距离阈值，仍旧配观测到，则使用三角测距求出该点的深度值。作者先去掉两帧之间的旋转，只剩两帧之间的相对平移和角点对应的归一化坐标。

[u0,v0], [u1,v1]均为归一化平面坐标

作者将last相机位置沿O_Last->X方向移动tz到O_Last‘，从而将O_Last移动到与O_Cur z方向平行的方向，此时，tx‘=tx-tzu0，ty’=ty-tzv0

在O_Last‘作与O_Cur->X平行的辅助线，此时，[u0,v0], [u1,v1], X, O_Last, O_Cur, O_Last’都在同一平面上。
[u1,v1]->[u0,v0]平行于O_Last‘->O_Cur。

ABCOL′=OL′OCXD→ABCOL′cosθ=OL′OCXDcosθ\frac{AB}{CO_L^\prime}=\frac{O_L^\prime O_C}{XD}\rightarrow\frac{AB}{CO_L^\prime cos\theta}=\frac{O_L^\prime O_C}{XDcos\theta}COL′AB=XDOL′OC→COL′cosθAB=XDcosθOL′OC
(u1−u0)2+(v1−v0)21=(tzu0−tx)2+(tzv0−ty)2Z\frac{\sqrt{\left(u_1-u_0\right)^2+\left(v_1-v_0\right)^2}}{1}=\frac{\sqrt{\left({t_zu}_0-t_x\right)^2+\left({t_zv}_0-t_y\right)^2}}{Z}1(u1−u0)2+(v1−v0)2=Z(tzu0−tx)2+(tzv0−ty)2
Z=(tzu0−tx)2+(tzv0−ty)2(u1−u0)2+(v1−v0)2Z=\frac{\sqrt{\left({t_zu}_0-t_x\right)^2+\left({t_zv}_0-t_y\right)^2}}{\sqrt{\left(u_1-u_0\right)^2+\left(v_1-v_0\right)^2}}Z=(u1−u0)2+(v1−v0)2(tzu0−tx)2+(tzv0−ty)2
但是不知道作者为什么还要除以cos(atan2(tz * v1 - ty, tz * u1 - tx) - atan2(v0 - v1, u0 - u1))

   double delta = sqrt((v0 - v1) * (v0 - v1) + (u0 - u1) * (u0 - u1))* cos(atan2(tz * v1 - ty, tz * u1 - tx) - atan2(v0 - v1, u0 - u1));double depth = sqrt((tz * u0 - tx) * (tz * u0 - tx) + (tz * v0 - ty) * (tz * v0 - ty)) / delta;
...
//另外作者查看了特征点之前是否之前有检测到，并且有深度值，如果有的话就将先前计算得到的深度值赋给该特征点，如果没有就将三角测距的结果值赋给该点，ipr.v的值都设置为2ipr.s = depth;ipr.v = 2;

求逆
参考SVO-https://blog.csdn.net/luoshi006/article/details/80792043
在匹配的特征点去掉相机的旋转关系之后，可以得到如下公式：
z1f1−z0f0=tz_1f_1-z_0f_0=tz1f1−z0f0=t
整理得：
[u1−u0v1−v01−1][z1z0]=[txtytz]\left[\begin{matrix}u_1&-u_0\\v_1&-v_0\\1&-1\\\end{matrix}\right]\left[\begin{matrix}z_1\\z_0\\\end{matrix}\right]=\left[\begin{matrix}t_x\\t_y\\t_z\\\end{matrix}\right]⎣⎡u1v11−u0−v0−1⎦⎤[z1z0]=⎣⎡txtytz⎦⎤
此时便得到一个超定方程，利用超定方程解法：
将Ax=bAx=bAx=b两边左乘ATA^TAT，得到ATAx=ATbA^TAx=A^TbATAx=ATb，保证ATAA^TAATA满秩，即可逆，则可根据x=(ATA)−1ATbx=(A^TA)^{-1}A^Tbx=(ATA)−1ATb解出x。
即。令A=[u1−u0v1−v01−1]A = \left[\begin{matrix}u_1&-u_0\\v_1&-v_0\\1&-1\\\end{matrix}\right]A=⎣⎡u1v11−u0−v0−1⎦⎤
x=[z1z0]x=\left[\begin{matrix}z_1\\z_0\\\end{matrix}\right]x=[z1z0]
b=[txtytz]b=\left[\begin{matrix}t_x\\t_y\\t_z\\\end{matrix}\right]b=⎣⎡txtytz⎦⎤
根据x=(ATA)−1ATbx=(A^TA)^{-1}A^Tbx=(ATA)−1ATb解出深度值。
克莱默法则
根据公式推导如下：
dref⋅pref=dcur⋅(Rrc⋅pcur)+trcd_{ref} \cdot p_{ref}=d_{cur} \cdot(R_{rc}\cdot p_{cur})+t_{rc}dref⋅pref=dcur⋅(Rrc⋅pcur)+trc
将其化为非齐次形式Ax=bAx=bAx=b
对公式两侧同时乘以prefTp_{ref}^TprefT，得到：
dref⋅prefT⋅pref−dcur⋅prefT⋅(Rrc⋅pcur)=prefT⋅trcd_{ref} \cdot p_{ref}^T\cdot p_{ref}-d_{cur} \cdot p_{ref}^T\cdot (R_{rc}\cdot p_{cur})= p_{ref}^T\cdot t_{rc}dref⋅prefT⋅pref−dcur⋅prefT⋅(Rrc⋅pcur)=prefT⋅trc
公式两侧同时乘以(Rrcpcur)T(R_{rc}p_{cur})^T(Rrcpcur)T，得到：
dref⋅(Rrcpcur)T⋅pref−dcur⋅(Rrcpcur)T⋅(Rrc⋅pcur)=(Rrcpcur)T⋅trcd_{ref} \cdot (R_{rc}p_{cur})^T\cdot p_{ref}-d_{cur} \cdot (R_{rc}p_{cur})^T\cdot (R_{rc}\cdot p_{cur})=(R_{rc}p_{cur})^T\cdot t_{rc}dref⋅(Rrcpcur)T⋅pref−dcur⋅(Rrcpcur)T⋅(Rrc⋅pcur)=(Rrcpcur)T⋅trc
化为非齐次形式，如下：
[prefT⋅pref−prefT⋅Rrc⋅pcurpcurTRrcT⋅pref−pcurT⋅pcur]⋅[drefdcur]=[prefT⋅trc(Rrcpcur)T⋅trc]\left[ \begin{matrix} p_{ref}^T\cdot p_{ref} & -p_{ref}^T\cdot R_{rc}\cdot p_{cur}\\ p_{cur}^TR_{rc}^T\cdot p_{ref} & -p_{cur}^T\cdot p_{cur} \end{matrix} \right]\cdot \left [\begin{matrix} d_{ref} \\ d_{cur}\end{matrix}\right]=\left [\begin{matrix}p_{ref}^T\cdot t_{rc} \\ (R_{rc}p_{cur})^T\cdot t_{rc}\end{matrix}\right][prefT⋅prefpcurTRrcT⋅pref−prefT⋅Rrc⋅pcur−pcurT⋅pcur]⋅[drefdcur]=[prefT⋅trc(Rrcpcur)T⋅trc]

对于上述两个公式组成的方程组，利用克莱默法则求解。克莱姆法则是一种求解线性方程组的方法，大多数线性代数教材都会提到。例如对于如下的线性方程组：

对于这样的方程，如果A的行列式不为0，方程可以通过如下方式求解：运用克莱姆法则，这个方程组的解可以如下：

其中D,Dx,DyD, D_x, D_yD,Dx,Dy，分别是如下三个行列式：

代码实现：十四讲中代码：

// 方程// d_ref * f_ref = d_cur * ( R_RC * f_cur ) + t_RC// => [ f_ref^T f_ref, -f_ref^T R f_cur ] [d_ref] = [f_ref^T t]//    [ f_cur^T R^T f_ref, -f_cur^T f_cur ] [d_cur] = [f_cur^T*R^T t]// 二阶方程用克莱默法则求解并解之Vector3d t = T_R_C.translation();Vector3d f2 = T_R_C.rotation_matrix() * f_curr; Vector2d b = Vector2d ( t.dot ( f_ref ), t.dot ( f2 ) );// 此处计算出系数矩阵Adouble A[4];A[0] = f_ref.dot ( f_ref );A[2] = f_ref.dot ( f2 );A[1] = -A[2];A[3] = - f2.dot ( f2 );// 此处计算A的行列式double d = A[0]*A[3]-A[1]*A[2];Vector2d lambdavec = Vector2d (  A[3] * b ( 0,0 ) - A[1] * b ( 1,0 ),-A[2] * b ( 0,0 ) + A[0] * b ( 1,0 )) /d;Vector3d xm = lambdavec ( 0,0 ) * f_ref;Vector3d xn = t + lambdavec ( 1,0 ) * f2;Vector3d d_esti = ( xm+xn ) / 2.0;  // 三角化算得的深度向量double depth_estimation = d_esti.norm();   // 深度值

以上参考：
三角测距方法：https://blog.csdn.net/KYJL888/article/details/107222533/
克莱默法则-https://blog.csdn.net/zbq_tt5/article/details/90043320

pointDefinition.h
点云数据定义及PCL数据注册
注册两种类型点云数据：pcl::PointCloud<ImagePoint>和pcl::PointCloud<DepthPoint>

//注册PCL点云格式
POINT_CLOUD_REGISTER_POINT_STRUCT (ImagePoint,(float, u, u)(float, v, v)(int, ind, ind))
POINT_CLOUD_REGISTER_POINT_STRUCT (DepthPoint,(float, u, u)(float, v, v)(float, depth, depth)(int, label, label)(int, ind, ind))

processDepthmap.cpp
根据视觉里程计topic("/cam_to_init")，和点云数据（"/sync_scan_cloud_filtered"），将新点云和已有点云变换到当前相机坐标系下，实现图像与点云数据的对齐。
stackDepthPoint.cpp
将视觉里程计中用于定位的特征点三维坐标分批存储，用于后面的局部BA优化。
bundleAdjust.cpp
对视觉里程计获得的odom信息进行局部BA优化，获得精度更高的odom信息
transformMaintenance.cpp
视觉里程计信息帧率较高，BA优化帧率较低，将二者结合，利用BA优化的结果修正视觉里程计，可以得到帧率和精度都较高的里程计信息
registerPointCloud.cpp
根据最终的里程计信息将三维激光的点云加入到odom坐标系中，获得点云地图

数据集测试

数据集文件：nsh_west_with_IMU.bag-需tizi

demo_lidar代码包地址，稍作修改-https://gitee.com/nie_xun/demo_lidar.git

master分支，测试运行通过，运行结果如下图：
note分支，加入一些中文注释。

参考：
DEMO_RGBD在UBUNTU16.04上运行的一些修改-https://www.freesion.com/article/7080798359/