slam学习（2）

http://www.cnblogs.com/gaoxiang12/p/4652478.html

从上面学习转载

18 // 相机内参
19 const double camera_factor = 1000;
20 const double camera_cx = 325.5;
21 const double camera_cy = 253.5;
22 const double camera_fx = 518.0;
23 const double camera_fy = 519.0;

从2D到3D（数学部分），其实就是从图像信息和深度信息的2d信息转化为点云（3d信息）

　　上面两个图像给出了机器人外部世界的一个局部的信息。假设这个世界由一个点云来描述： X={x1,…,xn} X={x1,…,xn}. 其中每一个点呢，有  r,g,b,x,y,z r,g,b,x,y,z一共6个分量，表示它们的颜色与空间位置。颜色方面，主要由彩色图像记录； 而空间位置，可以由图像和相机模型、姿态一起计算出来。

　　对于常规相机，SLAM里使用针孔相机模型（图来自http://www.comp.nus.edu.sg/~cs4243/lecture/camera.pdf ）：

　　简而言之，一个空间点 [x,y,z] [x,y,z]和它在图像中的像素坐标 [u,v,d] [u,v,d] ( d d指深度数据) 的对应关系是这样的：

u=x⋅fxz+cx u=x⋅fxz+cx

v=y⋅fyz+cy v=y⋅fyz+cy

d=z⋅s d=z⋅s

　　其中， fx,fy fx,fy指相机在 x,y x,y两个轴上的焦距， cx,cy cx,cy指相机的光圈中心， s s指深度图的缩放因子。

　　小萝卜：好晕啊！突然冒出这么多个变量！

　　师兄：别急啊，这已经是很简单的模型了，等你熟悉了就不觉得复杂了。 

　　这个公式是从 (x,y,z) (x,y,z)推到 (u,v,d) (u,v,d)的。反之，我们也可以把它写成已知 (u,v,d) (u,v,d)，推导 (x,y,z) (x,y,z)的方式。请读者自己推导一下。

　　不，还是我们来推导吧……公式是这样的：

 

z=d/s z=d/s

x=(u−cx)⋅z/fx x=(u−cx)⋅z/fx

y=(v−cy)⋅z/fy y=(v−cy)⋅z/fy

　　怎么样，是不是很简单呢？事实上根据这个公式就可以构建点云啦。

　　通常，我们会把 fx,fy,cx,cy fx,fy,cx,cy这四个参数定义为相机的内参矩阵 C C，也就是相机做好之后就不会变的参数。相机的内参可以用很多方法来标定，详细的步骤比较繁琐，我们这里就不提了。给定内参之后呢，每个点的空间位置与像素坐标就可以用简单的矩阵模型来描述了：

s⋅⎡⎣⎢⎢uv1⎤⎦⎥⎥=C⋅⎛⎝⎜⎜R⋅⎡⎣⎢⎢xyz⎤⎦⎥⎥+t⎞⎠⎟⎟ s⋅[uv1]=C⋅(R⋅[xyz]+t)

　　其中， R R和 t t是相机的姿态。 R R代表旋转矩阵， t t代表位移矢量。因为我们现在做的是单幅点云，认为相机没有旋转和平移。所以，把 R R设成单位矩阵 I I，把 t t设成了零。 s s是scaling factor，即深度图里给的数据与实际距离的比例。由于深度图给的都是short (mm单位)， s s通常为1000。