rgb-d slam(2)笔记

最新推荐文章于 2022-01-18 22:08:02 发布

出击桃子

最新推荐文章于 2022-01-18 22:08:02 发布

阅读量424

点赞数

分类专栏： SLAM 文章标签： rgbd-slam

本文链接：https://blog.csdn.net/qq_30513593/article/details/77488322

版权

SLAM 专栏收录该内容

5 篇文章 0 订阅

订阅专栏

半闲居士slam(2)
这次笔记是拜读半闲居士的rgbd-slam(2)之后所写,上面是链接

理论部分

点云

一个世界由点云构成,X={x1,x2,…,xn},其中每个点都有六个变量,xyz,rgb分别表示位置和颜色.色彩已经可以由rgb摄像头得出,xyz则可以由图像,相机模型,和姿态一起算出.

针孔相机

对于常规的相机,slam使用针孔相机模型,针孔相机,如下图所示

照片来自 http://blog.csdn.net/tangguotupaopao/article/details/26477533 此blog还有更多介绍,可参考

计算xyz

这里先假设摄像头不动,也就是只有一张图片一个姿势,小孔的位置是[0,0,0];
一个空间点[x,y,z]和它在图像中的坐标[u,v,d]关系是非常密切的,可简单看为是相似三角形的关系,所以

u=x*fx/z+cx;
v=y*fy/z+cy;
d=z*s;
其中,fx fy分别是xy轴上的焦距, 此处我卡了一下,因为居士写的小孔太简单了,我想当然地认为了xy焦距不应该是一样的么,后来才想通,椭圆不就有俩焦距么
s是深度的缩放因子,通常是1000
这样反过来就能计算出xyz
fx fy cx cy为相机内参 一旦相机做好之后,内参不再变化, 内参需要标定

相机移动

如果相机移动就可以通过下面公式进行推导:

s ⋅ [ u v 1 ] = C ⋅ ( R ⋅ [ x y z ] + t )

其中R是旋转矩阵,t是平移矩阵,C为内参矩阵;
旋转我了解到的有三种方法可以实现:
*四元数法,最后一个数为除数,为0时,可表示无穷远
*旋转矩阵
*欧拉角
但是只是知道没有用在这上面过,只是前两天看到过四元数,就看了两眼

编程实现

本来直接复制粘贴就可以的,但是想想自己匮乏的C++,还是听居士的建议,自己打一遍吧,然后就发现了很多,如果copy就得不到的知识.

程序结构

首先include

C++标准库io,string;opencv库,core.hpp,highgui.hpp;pcl库,io,point;
opencv和pcl有区别,我之前不是特别清楚这俩关系,一直以为这俩不知道是谁包含了谁呢,include的时候发现了不一样.opencv是计算机视觉库,更侧重处理图片的方面,二维很多算法,相当实用.pcl是point cloud lib,干的就是点云,在三维上有很多应用.

定义相机内参和pointcloud

四个相机内参的定义,fx fy cx cy. 还有s.还有为了省劲,typedef了point的类型.
xyzrgba七个变量写程序时感觉不太对,不是xyzrgb六个变量么?a是个啥!查资料发现a是alpha 透明度.一般情况下,一张照片有三个通道rgb,后来有人提出应该再设置一个通道来表示这个像素是否对照片有贡献,之前只有1/0,现在已经可以由很多值来表示透明程度.

进入main

1,定义并读取图像,imread( 照片,flag)

2,创建智能指针

智能指针比较有用的就是自动销毁.
new 开辟一个储存空间,并返回地址

3,遍历深度图

*读取深度信息,Mat.ptr<>(0)是指向mat第一行的指针,Mat.ptr<>(0)[1]是指向第一行第二个元素,此处需要注意括号和{0是第一个元素}
*若深度信息为0,则跳过
*若不为0,创建点pointT p;并为其xyzrgb赋值

p.z = double(d) / camera_factor;
p.x = (n - camera_cx) * p.z / camera_fx;
p.y = (m - camera_cy) * p.z / camera_fy;
p.b = rgb.ptr<uchar>(m)[n*3];
p.g = rgb.ptr<uchar>(m)[n*3+1];
p.r = rgb.ptr<uchar>(m)[n*3+2];