一起做RGB-D SLAM (1)-(7) 代码实践

https://www.cnblogs.com/gaoxiang12/p/4754948.html
高博代码地址

0. 视觉SLAM漫谈
   用通俗易懂的方式讲解的了SLAM，目前为止只理解了前半部分，学习中…
   
   
   
   通常，我们会把fx,fy,cx,cy这四个参数定义为相机的内参矩阵C，也就是相机做好之后就不会变的参数。相机的内参可以用很多方法来标定，详细的步骤比较繁琐，我们这里就不提了。给定内参之后呢，每个点的空间位置与像素坐标就可以用简单的矩阵模型来描述了：

和t是相机的姿态,R代表旋转矩阵，t代表位移矢量

SLAM (Simultaneous Localization and Mapping) 同时定位与建图
SLAM 系统 “图像前端，优化后端，闭环检测”的三部曲

ICP（Iterative Closest Point，迭代最近点
PnP问题的模型是这样的：

R为相机的姿态，C为相机的标定矩阵。R是不断运动的，而C则是随着相机做死的。
openCV的SolvePnPRANSAC函数或者PCL的ICP算法来求解。
RANSAC（Random Sample Consensus，随机采样一致性）
路标，在我们的图像处理过程中就是指SIFT提出来的关键点
2D SLAM，那么机器人位置就是x, y加一个转角theta。
如果是3D SLAM，就是x,y,z加一个四元数姿态（或者rpy姿态）。

1. pcl_viewer 可以查看点云图
2. Matlab写个脚本plot一下可查看txt轨迹文件 或者用python的Matpyplot

实践 第二讲 从图像到点云

两种数据：RGB图(彩色图像)和深度图像。深度图就是彩色图像里每个像素距传感器的距离

RGB图和深度图可能会有些小问题：

有一些时差（约几到十几个毫秒）。这个时差的存在，会产生“RGB图已经向右转了，怎么深度图还没转”的感觉哦。
光圈中心未对齐。因为深度毕竟是靠另一个相机获取的，所以深度传感器和彩色传感器参数可能不一致。
深度图里有很多“洞”。因为RGB-D相机不是万能的，它有一个探测距离的限制啦！太远或太近的东西都是看不见的呢。关于这些“洞”，我们暂时睁一只眼闭一只眼，不去理它。以后我们也可以靠双边bayes滤波器去填这些洞。但是！这是RGB-D相机本身的局限性。软件算法顶多给它修修补补，并不能完全弥补它的缺陷。

看图的方法：

1. rosrun image_view image_view image:=/camera/rgb/image_color来显示彩色图像。
2. 在Rviz里看到图像与点云的可视化数据。

一个空间点[x,y,z]和它在图像中的像素坐标[u,v,d] (d指深度数据) 的对应关系

fx,fy指相机在x,y两个轴上的焦距，cx,cy指相机的光圈中心，s指深度图的缩放因子。
已知(u,v,d)，推导(x,y,z)的方式

fx,fy,cx,cy这四个参数定义为相机的内参矩阵C
每个点的空间位置与像素坐标的矩阵模型描述
R和t是相机的姿态。R代表旋转矩阵，t代表位移矢量。
因为我们现在做的是单幅点云，认为相机没有旋转和平移。
所以，把R设成单位矩阵I，把t设成了零。s是scaling factor，即深度图里给的数据与实际距离的比例。
由于深度图给的都是short (mm单位)，s通常为1000。
明天研读详细代码
https://www.cnblogs.com/gaoxiang12/p/4652478.html