SLAM荟萃（1）相机内参、外参、畸变参数及其运用：深度图与彩色图配准

这个系列主要记录一下学习高翔博士《视觉SLAM十四讲》时仔细思考了一些时间的内容和值得总结的东西，强烈推荐大家买这本书实体书，不管是学习SLAM还是工作用处都很大，并且看完一遍后并用于实践中再回顾书中的内容，会发现对SLAM基础概念的理解更加深刻。

目录

相机内参及外参

畸变参数

双目极线对齐

kinect深度图与彩色图配准

---

---

相机内参及外参

                                                          

                                  

                                                                     

      常见的相机成像模型都可以简化为针孔相机模型，空间下一个三维坐标点穿过光心投影到相机成像平面的过程可由上述公式描述。其中cx,cy,fx,fy便为相机内参，s为三维点相对于三维平面的深度

• cx,cy由相机光心垂直投影到成像平面的坐标决定
• fx,fy由相机的焦距以及水平垂直方向上单个象元的大小决定。 fx=f/dx  其中f为相机焦距，dx为相机每个像素的宽度
• 当图像通过重采样放大或缩小二倍时，其内参也同样放大或缩小二倍

       在双目系统中，相机的外参由3X3的旋转矩阵加上一个三维平移向量组成。它描述三维空间下一个坐标系向另一个坐标系的转换。

畸变参数

      相机畸变分为径向畸变和切向畸变两种。径向畸变由于镜头变形导致,畸变参数为k1,k2,k3，其公式为

                                         

      切向畸变是由于镜头不平行于成像平面导致，畸变参数为p1,p2其公式为

                                              

      两者合起来的公式为

                          

双目极线对齐

    利用matlab标定工具箱即可标定双目相机内外参数，标定完的结果用matlab  rectifyStereoImages函数或者opencv 

stereoRectify()函数即可完成极线对齐

参考资料：利用 Calibration Toolbox for Matlab 工具箱进行双目立体校正

                                              

双目对齐的原因：一般双目极线未对齐前都会用上面这张图当做参考，可以看到与这两条在基线上本应该是一条直线的线段在双目图像上却分别是倾斜度不相同的线段，这样在进行双目匹配时就要分别在不同倾斜方向上进行搜索，使得匹配过程变得麻烦，不利于优化。

对齐原理：在标定完双目系统的外参后，获取右目相对于左目的平移向量，标志着空间坐标中基线的方向向量，接下来分别旋转左右两目，使左右两目的x轴与之前的平移方向向量平行，依据新的旋转对原图像进行旋转与重采样，便获得了极线对齐可直接在水平方向上搜索视差的双目图像。

kinect深度图与彩色图配准

        深度图一般由红外摄像头或TOF摄像头获取，这里也需要事先标定好深度摄像头和彩色摄像头的内外参数。而两者配准则是要最终获取一副图像，在知道图像中每个像素点的颜色信息的同时也能知道其深度信息。

       此时根据先确定每个像素的深度值还是颜色信息以及最后保持深度图不变还是彩色图不变的配准要求，从理论上来讲有4中配准方式。

先确定深度还是先确定颜色：如果已知一个点的Z值，再结合其像素坐标（u,v）可以很轻松通过内参将该点映射回世界坐标系，之后再通过外参和彩色图内参将其投影回彩色图得到它的z值。但一个点先知道彩色，只有它的xy坐标时，其映射回世界坐标系会是一条射线，而深度图在世界坐标系下是一个充满沟壑的平面，此时求解该点深度的过程就是求世界坐标系下射线与深度图平面交点的过程，由于深度图为离散采样且不规律，很难用线性函数表示，所以上述方程虽然有唯一解但也很难直接求解。综上配准过程应当是先确定每个像素的深度值，再投影回彩色图计算其彩色值的过程。

输出保持深度图还是彩色图不变：不管保持那副图不变，另一幅图由于是空间下非平滑映射，都将会导致扭曲，实际运用场景中由于深度图本身就存在物体边缘变形的问题，更加重视保持彩色图的一致性，故输出结果应当保持彩色图不变，即最后配准图的内参与彩色图内参一致，这样深度图投影到彩色图时由于是非平滑映射会导致断裂，此时简单方法可用线性插值解决。

整体思路及代码：对于深度图上每一个点，计算其在彩色图上的位置，将其暂存在最近的图像像素中，最终再对全图做一次遍历，对深度值为0的地方进行插值，这样便最终得出了配准后的深度彩色图。

//interDepth为深度相机内参，interColor为彩色相机内参，Rotate为4X4外参矩阵,为彩色图相对于深度图的，其中深度图类型为CV_16SC1
bool Register(cv::Mat &depth, cv::Mat &color, cv::Mat &out, cv::Mat interDepth, cv::Mat interColor, cv::Mat Rotate)
{
	if (depth.rows != color.rows || depth.cols != color.cols)
		return false;
	if (interDepth.rows != 3 || interDepth.cols != 3 || interColor.rows != 3 || interColor.cols != 3 ||
		Rotate.rows != 4 || Rotate.cols != 4)
		return false;
	cv::Mat depthtemp = cv::create(depth.size, depth.type);
	for (int i = 0; i<depth.rows; i++)
	{
		unsigned short* temp = depth.ptr<unsigned short>(i);
		for (int j = 0; j<depth.cols; j++)
		{
			float z = temp[j];
			float x = (j - interDepth.data[2])*z / interDepth.data[0];
			float y = (i - interDepth.data[5])*z / interDepth.data[4];

			float px = Rotate.data[0] * x + Rotate.data[1] * y + Rotate.data[2] * z + Rotate.data[3];
			float py = Rotate.data[4] * x + Rotate.data[5] * y + Rotate.data[6] * z + Rotate.data[7];
			float pz = Rotate.data[8] * x + Rotate.data[9] * y + Rotate.data[10] * z + Rotate.data[11];
			int u = int(px*interColor.data[0] / pz + interColor.data[2]);
			int v = int(py*interColor.data[4] / pz + interColor.data[5]);

			depthtemp.data[v*depth.cols + u] = (unsigned int)pz;
		}
	}
	for (int i = 0; i<depth.rows; i++)
	{
		unsigned short* temp = depthtemp.ptr<unsigned short>(i);
		unsigned short pre = temp[0];
		int preindex = 0;
		for (int j = 1; j<depth.cols; j++)
		{
			if (temp[j] != 0)
			{
				pre = temp[j];
				preindex = j;
				continue;
			}
			int after = j + 1;
			while (after<depth.cols&&temp[after] == 0)
				after++;
			if (after >= depth.cols)
				continue;
			temp[j] = (unsigned short)(pre + (temp[after] - pre) / (after - preindex)*(j - preindex));
		}
	}
	out = depthtemp.clone();
	return true;
}