在这篇文章中对《视觉slam十四讲》中的ch5实践部分——stereoVision代码进行解析，做一下学习记录。
1.实现效果
先展示一下这部分最后的效果图
最后会出来两张图，上面这张是归一化后的视差图，下面那张是点云图。
1.1 出现的问题
（1）高博提供的代码中的CMake部分是不全的，可以按照我以下的代码修改：
cmake_minimum_required(VERSION 2.8)
set(CMAKE_BUILD_TYPE "Release")
# 添加c++ 11标准支持
set(CMAKE_CXX_FLAGS "-std=c++11 -O2")
find_package(Pangolin REQUIRED)
find_package(OpenCV REQUIRED)
add_executable(stereoVision stereoVision.cpp)
include_directories("/usr/local/include/eigen3") #根据自己的eigen所在位置填写
target_link_libraries(stereoVision ${OpenCV_LIBS} ${Pangolin_LIBRARIES})
（2）除此之外我还出现了以下问题（显示核心以转储）：
terminate called after throwing an instance of 'cv::Exception'
what(): OpenCV(3.4.15) /home/rxz/opencv3/modules/imgproc/src/median_blur.dispatch.cpp:283: error: (-215:Assertion failed) !_src0.empty() in function 'medianBlur'
问题原因：因为无法找到源代码中图像路径所对应的图片。
解决方法：修改图片路径（可通过查看图片属性了解图片的路径）
// 文件路径
string left_file = "/home/rxz/slambook2/ch5/stereo/left.png";
string right_file = "/home/rxz/slambook2/ch5/stereo/right.png";2.代码分析
#include <opencv2/opencv.hpp>
#include <vector>
#include <string>
#include <Eigen/Core>
#include <pangolin/pangolin.h>
#include <unistd.h>
using namespace std;
using namespace Eigen;
// 文件路径
string left_file = "/home/rxz/slambook2/ch5/stereo/left.png";
string right_file = "/home/rxz/slambook2/ch5/stereo/right.png";
// 在pangolin中画图，已写好，无需调整
void showPointCloud(
const vector<Vector4d, Eigen::aligned_allocator<Vector4d>> &pointcloud);
int main(int argc, char **argv) {
// 内参
double fx = 718.856, fy = 718.856, cx = 607.1928, cy = 185.2157;
// 基线
double b = 0.573;
// 读取图像
cv::Mat left = cv::imread(left_file, 0);
cv::Mat right = cv::imread(right_file, 0);
cv::Ptr<cv::StereoSGBM> sgbm = cv::StereoSGBM::create(
0, 96, 9, 8 * 9 * 9, 32 * 9 * 9, 1, 63, 10, 100, 32); // sgbm经典参数配置
cv::Mat disparity_sgbm, disparity;
sgbm->compute(left, right, disparity_sgbm);//输入前面两张图，第三个参数是输出
disparity_sgbm.convertTo(disparity, CV_32F, 1.0 / 16.0f); //将disparity_sgbm变成32F类型的disparity，这里的disparity才是视差图。 如果Mat类型数据的深度不满足上面的要求，则需要使用convertTo()函数来进行转换。convertTo()函数负责转换数据类型不同的Mat
// 生成点云
vector<Vector4d, Eigen::aligned_allocator<Vector4d>> pointcloud;
// 如果你的机器慢，请把后面的v++和u++改成v+=2, u+=2
//默认计算出的是左视差图
for (int v = 0; v < left.rows; v++)
for (int u = 0; u < left.cols; u++) {
//排除不在视差范围里的像素点
if (disparity.at<float>(v, u) <= 0.0 || disparity.at<float>(v, u) >= 96.0) continue; //这里的96是视差搜索范围
Vector4d point(0, 0, 0, left.at<uchar>(v, u) / 255.0); // 前三维为xyz,第四维为颜色，如果不是255，那么取整后就为0，那么对应图像的颜色就为黑色，反之为白色
// 根据双目模型计算 point 的位置 根据双目模型恢复像素点的三维位置
double x = (u - cx) / fx;
double y = (v - cy) / fy;
double depth = fx * b / (disparity.at<float>(v, u)); //计算深度 视差图转化为深度图
//把三维位置赋值给点的前三个数
point[0] = x * depth;
point[1] = y * depth;
point[2] = depth;
pointcloud.push_back(point);
}
cv::imshow("disparity", disparity / 96.0); //归一化后的视差图
cv::waitKey(0);
// 画出点云
showPointCloud(pointcloud);
// cv::waitKey(0);
// cv::destroyAllWindows();
return 0;
}
void showPointCloud(const vector<Vector4d, Eigen::aligned_allocator<Vector4d>> &pointcloud) {
if (pointcloud.empty()) {
cerr << "Point cloud is empty!" << endl;
return;
}
//创建一个Pangolin窗口
pangolin::CreateWindowAndBind("Point Cloud Viewer", 1024, 768);
glEnable(GL_DEPTH_TEST);//根据物体远近，实现遮挡效果
glEnable(GL_BLEND);//使用颜色混合模型，让物体显示半透明效果
glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
pangolin::OpenGlRenderState s_cam(
pangolin::ProjectionMatrix(1024, 768, 500, 500, 512, 389, 0.1, 1000),
pangolin::ModelViewLookAt(0, -0.1, -1.8, 0, 0, 0, 0.0, -1.0, 0.0)
);
//ProjectionMatrix()中各参数依次为图像宽度=1024、图像高度=768、fx=500、fy=500、cx=512、cy=389、最近距离=0.1和最远距离=1000
//ModelViewLookAt()中各参数为相机位置，被观察点位置和相机哪个轴朝上
//比如，ModelViewLookAt(0, -0.1, -1.8, 0, 0, 0, 0.0, -1.0, 0.0)表示相机在(0, -0.1, -1.8)位置处观看视点(0, 0, 0)，并设置相机XYZ轴正方向为（0，-1，0），即右上前
//创建交互视图，显示上一帧图像内容
pangolin::View &d_cam = pangolin::CreateDisplay()
.SetBounds(0.0, 1.0, pangolin::Attach::Pix(175), 1.0, -1024.0f / 768.0f)
.SetHandler(new pangolin::Handler3D(s_cam));
//SetBounds()内的前4个参数分别表示交互视图的大小，均为相对值，范围在0.0至1.0之间
//第1个参数表示bottom，即为视图最下面在整个窗口中的位置
//第2个参数为top，即为视图最上面在整个窗口中的位置
//第3个参数为left，即视图最左边在整个窗口中的位置
//第4个参数为right，即为视图最右边在整个窗口中的位置
//第5个参数为aspect，表示横纵比
while (pangolin::ShouldQuit() == false) {//如果pangolin窗口没有关闭，则执行
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);//清空颜色和深度缓存，使得前后帧不会互相干扰
d_cam.Activate(s_cam);//激活显示，并设置相机状态
glClearColor(1.0f, 1.0f, 1.0f, 1.0f);//设置背景颜色为白色
glPointSize(2);
glBegin(GL_POINTS);//绘制点云
for (auto &p: pointcloud) {
glColor3f(p[3], p[3], p[3]);//设置颜色信息
glVertex3d(p[0], p[1], p[2]);//设置位置信息
}
glEnd();
pangolin::FinishFrame();//按照上面的设置执行渲染
usleep(5000); // sleep 5 ms
}
return;
}参考文献
3.原理分析及重要函数分析
3.1 sgbm参数解释：
cv::Ptr<cv::StereoSGBM> sgbm = cv::StereoSGBM::create(
0, 96, 9, 8 * 9 * 9, 32 * 9 * 9, 1, 63, 10, 100, 32); // sgbm经典参数配置static Ptr cv::StereoSGBM::create (
int minDisparity = 0, //minDisparity 最小可能的差异值。通常情况下，它是零，但有时整流 算法可能会改变图像，所以这个参数需要作相应的调整
int numDisparities = 96, //numDisparities 视差搜索范围。该值总是大于零。在当前的实现中，该参数必须可以被16整除
int blockSize = 9, //BLOCKSIZE 匹配的块大小。它必须是> = 1的奇数。通常情况下，它应该在3…11的范围内
int P1 = 8 * 9 * 9, //P1 控制视差平滑度的第一个参数。
int P2 = 32 * 9 * 9, //P2 第二个参数控制视差平滑度。值越大，差异越平滑。P1是相邻像素之间的视差变化加或减1的惩罚。P2是相邻像素之间的视差变化超过1的惩罚。该算法需要P2> P1。
int disp12MaxDiff = 1, //左右视差检查中允许的最大差异（以整数像素为单位）。将其设置为非正值以禁用检查。
int preFilterCap = 63, //预滤波图像像素的截断值。
int uniquenessRatio = 10,
int speckleWindowSize = 100, //平滑视差区域的最大尺寸，以考虑其噪声斑点和无效。将其设置为0可禁用斑点过滤。否则，将其设置在50-200的范围内。
int speckleRange = 32, //每个连接组件内的最大视差变化。如果你做斑点过滤，将参数设置为正值，它将被隐式乘以16.通常，1或2就足够好了。
)
3.2 convertTo函数
用于计算距离的视差图（CV_32F）和用于肉眼看的视差图（CV_8U）使用的格式不同，并且用于计算的视差图无需进行裁剪和归一化，这些只是为了显示的可读性和美观。所以，在对sgbm进行compute之后得到视差图disparity_sgbm，除以16得到用于计算的视差图disparity（除以16是因为每个像素值由一个16bit表示，其中低位的4位存储的是视差值得小数部分，所以真实视差值应该是该值除以16）。
用法：
需要转化的原图.convertTo(dst, type, scale, shift)
dst：目的矩阵；
type：需要输出的矩阵类型，或者更明确的，是输出矩阵的深度，如果是负值（常用-1）则输出矩阵和输入矩阵类型相同。转换位深度本质上就是对原深度下的数据做线性变换，使原位深度下的最小值和最大值分别对应转换后位深度下的最小值和最大值。
scale:范围比例因子；eg. 原来的图是255的灰度图，转成float型数据并归一化到0~1,那么就是scale=1.0/255
shift：将输入数组元素按比例缩放后添加的值；
3.3 归一化处理
v::imshow("disparity", disparity / 96.0); //归一化后的视差图如果没有归一化，效果如下：
