SLAM学习之路---双目相机照片生成点云（附C++代码）

一、概述

本文记录如何根据双目相机得到的两帧图片数据来计算出三维点云。这里主要涉及到双目视觉的成像原理，在《视觉SLAM十四讲》第2版的第五讲中有详细介绍。本文的代码根据书上给出的代码进行编写，数据来自https://github.com/gaoxiang12/slambook2/tree/master/ch5/stereo。

二、双目相机模型

众所周知，单目相机是没法确定照片中的某个像素在三维空间中的具体位置的，因为它损失了深度信息。而双目相机就像人的双眼一样，可以根据左右眼看到物体的视差来确定深度，当人用一只眼睛看的时候，深度信息就变得不可靠了。

双目相机一般由左眼相机和右眼相机两个水平放置的相机组成（当然也可以上下放置，通常为水平放置）。对于双目相机，可以把两个相机分别看成针孔相机。这两个相机的光心分别记为$O_L$和$O_R$，$O_L$和$O_R$的连线叫做【基线b】。

如下图所示，为双目视觉测距示意图：

已知相机内参，焦距$f$，像素坐标系原点到成像平面坐标系原点的偏移，分别为$x$轴和$y$轴的偏移量记为$c_x{c}_y$以及双目相机的基线长度$b$。成像坐标系的$x$轴和$y$轴与相机坐标系的$x$轴和$y$轴同向，通常$x$轴指向右边（在相机后面看，而不是镜头前），$y$轴指向下边。而相机坐标系比成像坐标系多一个维度$z$轴，$z$轴为镜头朝外方向。

对于空间中一点$P$，其在相机坐标系下的坐标为$(x,y,z)$，在像素坐标系下的坐标为$(u,v)$。

根据小孔成像模型可知：
$$\{\begin{array}{l}x=(u-c_x)/f\cdot z\\ y=(v-c_y)/f\cdot z\end{array}$$

再根据双目相机模型的相似三角形可知：
$$\frac{z-f}{z}=\frac{b-U_L+U_R}{b}\to z=\frac{fb}{U_L-U_R}$$
其中$b$为基线的长度（即左右相机光心的距离），记点$P$投影到成像平面上的点为$P^{\prime }$，则$U_L$和$U_R$分别为$P^{\prime }$在成像平面坐标系下的$x$轴坐标（因为双目相机水平放置），而在右边相机的成像平面上，$P^{\prime }$位于成像平面坐标系原点的左边，所以$U_R$为负值，计算的时候要加个负号，$U_L-U_R$即为视差。

根据上述小孔成像模型和双目相机测深度模型就可以根据双目相机拍到的两帧图像把图像中的像素转到三维空间的点了，也就是转成点云。在OpenCV中集成了 $StereoSGBM$ 这个类可以直接输入两张图像计算得到每个像素点的视差。所以现在，$P$在三维空间中的坐标$(x,y,z)$为需要求的值，根据上面提到的两个公式直接就可以计算出来了。

三、实验结果

1. 输入的两帧图像：
   left.png:
   
   right.png:
   
2. 计算左右两帧图像的视差图如下：
   
   注意到此图的左边有一部分是空的，是因为右边的相机没看到的部分。
3. 最终通过两帧图像恢复的点云结果如下：
   
   拉近点看，甚至还有点身临其境的感觉：
   

四、代码

这里我把书上的代码稍微改了一点点。

#include <iostream>
#include <opencv2/opencv.hpp>
#include <Eigen/Eigen>
#include <pcl/point_cloud.h>
#include <pcl/visualization/pcl_visualizer.h>

void showPointCloud(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr pointcloud)
{
	pcl::visualization::PCLVisualizer visualizer("showcloud");
	visualizer.addPointCloud(pointcloud);
	visualizer.spin();
}

void showPointCloud(const std::vector<cv::Vec3f> &cloudpos, const std::vector<cv::Vec3b> &cloudcol)
{
	
	cv::viz::Viz3d window("showcloud");
	cv::viz::WCloud cloud_widget(cloudpos, cloudcol);
	window.showWidget("pointcloud", cloud_widget);
	window.spin();
}

int main()
{
	double f = 718.856, cx = 607.1928, cy = 185.2157; //相机内参
	double b = 0.573; //基线长度

	cv::Mat left_img = cv::imread("left.png");
	cv::Mat right_img = cv::imread("right.png");
	
	//下面是书上说的来自网络上的神奇参数
	cv::Ptr<cv::StereoSGBM> sgbm = cv::StereoSGBM::create(0, 96, 9, 8 * 9 * 9, 32 * 9 * 9, 1, 63, 10, 100, 32); 
	cv::Mat disparity_sgbm, disparity;
	sgbm->compute(left_img, right_img, disparity_sgbm); //计算两帧图像的视差
	disparity_sgbm.convertTo(disparity, CV_32F, 1.0 / 16.0);

	/*std::vector<cv::Vec3f> pointcloud;
	std::vector<cv::Vec3b> pointcolor;
	for (int v = 0; v < left_img.rows; v++)
	{
		for (int u = 0; u < left_img.cols; u++)
		{
			if (disparity.at<float>(v, u) <= 10 || disparity.at<float>(v, u) >= 96) continue;
			cv::Vec3f pos;
			cv::Vec3b col;

			double x = (u - cx) / f;
			double y = (v - cy) / f;
			double depth = f * b / (disparity.at<float>(v, u));
			pos[0] = x * depth;
			pos[1] = y * depth;
			pos[2] = depth;
			col = left_img.at<cv::Vec3b>(v, u) / 255;
			pointcloud.emplace_back(pos);
			pointcolor.emplace_back(col);
		}
	}
	showPointCloud(pointcloud, pointcolor);*/

	pcl::PointCloud<pcl::PointXYZRGB>::Ptr pointcloud(new pcl::PointCloud<pcl::PointXYZRGB>);
	for (int v = 0; v < left_img.rows; v++)
	{
		for (int u = 0; u < right_img.cols; u++)
		{
			if (disparity.at<float>(v, u) <= 10 || disparity.at<float>(v, u) >= 96) continue;
			pcl::PointXYZRGB point;
			double x = (u - cx) / f;
			double y = (v - cy) / f;
			double depth = f * b / (disparity.at<float>(v, u));
			point.x = x * depth;
			point.y = y * depth;
			point.z = depth;
			point.b = left_img.at<cv::Vec3b>(v, u)[0];
			point.g = left_img.at<cv::Vec3b>(v, u)[1];
			point.r = left_img.at<cv::Vec3b>(v, u)[2];
			pointcloud->push_back(point);
		}
	}
	showPointCloud(pointcloud);
	
	cv::imshow("disparity", disparity / 96);
	cv::waitKey(0);
}