一、原理
1.双目测距原理
由双目测距可得
2.针孔相机模型
由式(3)可得
其中即由双目测距求得的深度。如此便从点P的像素坐标
求出了我们需要的点P在三维空间中的坐标
。（注：
中的
）
二、代码实现
1.使用pangolin库画点云图
slam十四讲第五章的代码。
#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 = "../left.png";
string right_file = "../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);//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); // 神奇的参数
cv::Mat disparity_sgbm, disparity;
//计算两幅图像的视差
sgbm->compute(left, right, disparity_sgbm);
//disparity才是最后的视差图
disparity_sgbm.convertTo(disparity, CV_32F, 1.0 / 16.0f);
// 生成点云
//前三维为x,y,z,表示位置信息，后一维表示颜色信息，此处为灰度
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++)
{
//设置一个check，排除视差不在（10.0,96.0）范围内的像素点
if (disparity.at<float>(v, u) <= 0.0 || disparity.at<float>(v, u) >= 96.0) continue;
Vector4d point(0, 0, 0, left.at<uchar>(v, u) / 255.0); // 前三维为xyz,第四维为颜色
// 根据双目模型计算 point 的位置
//根据双目模型恢复像素点的三维位置
double x = (u - cx) / fx;
double y = (v - cy) / fy;
double depth = fx * b / (disparity.at<float>(v, u));
//将x,y,z赋值给point的前三维
point[0] = x * depth;
point[1] = y * depth;
point[2] = depth;
pointcloud.push_back(point);
}
//disparity / 96.0表示归一化之后的视差图像
cv::imshow("disparity", disparity / 96.0);
//停止执行，等待一个按键输入
cv::waitKey(0);
// 画出点云
showPointCloud(pointcloud);
return 0;
}
void showPointCloud(const vector<Vector4d, Eigen::aligned_allocator<Vector4d>> &pointcloud) {
if (pointcloud.empty()) {
cerr << "Point cloud is empty!" << endl;
return;
}
//创建一个pangolin窗口，窗口名“Point Cloud Viewer”,窗口宽度1024,窗口高度768
pangolin::CreateWindowAndBind("Point Cloud Viewer", 1024, 768);
glEnable(GL_DEPTH_TEST);//根据物体远近，实现遮挡效果
glEnable(GL_BLEND);//使用颜色混合模型，让物体显示半透明效果
//GL_SRC_ALPHA表示使用源颜色的alpha值作为权重因子，GL_ONE_MINUS_SRC_ALPHA表示使用(1-源颜色的alpha值)作为权重因子
glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
/*
//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::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)
);
//创建交互视图，显示上一帧图像内容
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，表示横纵比
*/
//如果pangolin窗口没有关闭，则执行
while (pangolin::ShouldQuit() == false)
{
//清空颜色和深度缓存，使得前后帧不会互相干扰
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();
//停止执行5ms
usleep(5000); // sleep 5 ms
}
return;
}ERROR:如果运行时报错terminate called after throwing an instance of 'cv::Exception'
terminate called after throwing an instance of 'cv::Exception'
what(): OpenCV(4.2.0) /home/lessle6/opencv-4.2.0/modules/core/src/matrix_expressions.cpp:23: error: (-5:Bad argument) Matrix operand is an empty matrix. in function 'checkOperandsExist'
Aborted (core dumped)则说明图像读取出了问题，没有读取到，将代码中的文件路径换成绝对路径。
2.使用pcl库画点云图
#include <iostream>
#include <opencv2/opencv.hpp>
#include <pcl/point_types.h>
#include <pcl/visualization/cloud_viewer.h>
using namespace std;
int main(int argc, char **argv) {
if(argc!=3)
{
cout<<"Please input left img and right img!!!!"<<endl;
}
// 双目相机参数
double fx = 718.856, fy = 718.856, cx = 607.1928, cy = 185.2157;
double b = 0.573;
// 读取左右两目图像并计算视差
cv::Mat left = cv::imread(argv[1],0);
cv::Mat right = cv::imread(argv[2],0);
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, right, disparity_sgbm);
disparity_sgbm.convertTo(disparity, CV_32F, 1.0 / 16.0f);
// 定义点云使用的格式：这里用的是XYZRGB
pcl::PointCloud<pcl::PointXYZRGB>::Ptr road_cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
// 根据视差和相机模型计算每一个点的三维坐标, 并添加到PCL点云中
for (int v = 0; v < left.rows; v++)
for (int u = 0; u < left.cols; u++) {
if (disparity.at<float>(v, u) <= 10.0 || disparity.at<float>(v, u) >= 96.0)
continue;
double depth = fx * b / (disparity.at<float>(v, u));
pcl::PointXYZRGB p;
p.x = depth * (u - cx) / fx;
p.y = depth * (v - cy) / fy;
p.z = depth;
p.b = left.at<cv::Vec3b>(v, u)[0];
p.g = left.at<cv::Vec3b>(v, u)[1];
p.r = left.at<cv::Vec3b>(v, u)[2];
road_cloud->points.push_back(p);
}
//可视化视差图
cv::imshow("disparity", disparity / 96.0);
cv::waitKey(0);
//可视化三维点云
road_cloud->height = 1;
road_cloud->width = road_cloud->points.size();
pcl::visualization::CloudViewer viewer("Cloud Viewer");
viewer.showCloud(road_cloud);
while(!viewer.wasStopped()){}
return 0;
}对应的CMakeLists.txt
# cmake版本及工程名
cmake_minimum_required( VERSION 2.8 )
project( stereo2pcd )
# 设置opencv库
find_package( OpenCV REQUIRED )
include_directories( ${OpenCV_INCLUDE_DIRS} )
# 设置pcl库
find_package( PCL 1.10 REQUIRED)
include_directories( ${PCL_INCLUDE_DIRS} )
link_directories(${PCL_LIBRARY_DIRS})
add_definitions(${PCL_DEFINITIONS})
# 编译与链接
add_executable( stereo2pcd stereo2pcd.cpp)
target_link_libraries( stereo2pcd ${OpenCV_LIBS} ${PCL_LIBRARIES} )
install(TARGETS stereo2pcd RUNTIME DESTINATION bin)3.SGBM参数解释
SGBM（Semi-Global Block Matching）是一种用于计算双目视觉中视差（disparity）的半全局匹配算法，它是基于全局匹配算法和局部匹配算法的优缺点，提出了一种折中的方法，既能保证视差图的质量，又能降低计算复杂度。在OpenCV中集成了StereoSGBM 类可以直接输入两张图像计算得到每个像素点的视差。
/*
cv::Ptr<cv::StereoSGBM> sgbm = cv::StereoSGBM::create(
minDisparity, numDisparities, blockSize, P1, P2, disp12MaxDiff, preFilterCap, uniquenessRatio, speckleWindowSize, speckleRange);
*/
cv::Ptr<cv::StereoSGBM> sgbm = cv::StereoSGBM::create(
0, 96, 9, 8 * 9 * 9, 32 * 9 * 9, 1, 63, 10, 100, 32);cv::Ptr<cv::StereoSGBM> sgbm = cv::StereoSGBM::create(
minDisparity, numDisparities, blockSize, P1, P2, disp12MaxDiff, preFilterCap, uniquenessRatio, speckleWindowSize, speckleRange);
- minDisparity：最小视差值，默认为0。最小视差越大，物体离相机近的程度就会变小。
- 最小视差过高，视差图会被高估，因为物体不可能有大于最小视差的负的视差值。
- 最小视差过低，则可能会受到噪声的影响，产生错误的视差值。
- numDisparities：视差范围，默认为16。必须是16的整数倍。
- 视差数量越多，能够获取到更多详细的深度信息，但计算量会增加进而导致运行速度变慢和噪声增多。
- 视差范围增大会使得视差图中可以估计的深度范围增大，但如果增大的视差范围超过了场景中实际的深度范围，就无法进行有效的匹配或估计深度，在视差图上表现为黑色区域。
- blockSize：匹配块大小，默认为3。必须是奇数且大于1。
- 增大blockSize的值，可增加包含的像素，产生更稳定但粗略的视差图。
- 减小blockSize的值，可以获得一些锐利但嘈杂（不确定）的视察边缘。
- P1：控制视差平滑度的第一个参数，默认为8blockSize。P1越大，越倾向于生成连续的视差图。
- P2：控制视差平滑度的第二个参数，默认为32blockSize。P2越大，越倾向于消除小的视差变化。P2必须大于P1。
- 两种参数都是控制视差变化规则的，从而使结果更平滑，增加这些值会使抗噪声能力更强但同时会失去保留锐度的细节。
- P1、P2值过大，会导致平滑的结果，丢失更多的细节和锐度。
- P1、P2值过小，会使视差中出现许多噪声或未对齐的图像。
- disp12MaxDiff：左右一致性检查时允许的最大视差差异，默认为-1，表示不进行检查。这个参数用于限制左右视图之间的最大视差数量差异。
- 值太大，可能会导致插值和未对齐的像素点在图像中显示。
- 值太小，则会导致视差较光滑，缺少细节特征。
- preFilterCap：预处理时截断梯度值的上限，默认为63。该参数控制了像素的最大值。如果已经将值限制在负值的范围内，那么它必须和像素值相比较，过滤掉那些值过大的像素点。
- uniquenessRatio：唯一性检查时的阈值，默认为10。表示最佳视差值与次佳视差值之间的比例要大于该阈值才被认为是有效的。这个参数是用来控制像素值的唯一性，唯一性比例越高，则得到的视差图的噪声和未对齐的像素会越小。但如果唯一性比例太高，则有可能会失去细节特征。
- speckleWindowSize：消除噪声斑点时考虑的窗口大小，默认为0，表示不进行消除。这个参数被用来滤除孤立噪点或者离群值。
- 窗口太小，会过滤不到足够的噪声点。
- 窗口太大，会损失一些细节特征。
- speckleRange：消除噪声斑点时考虑的最大视差变化，默认为0，表示不进行消除。这个参数规定一个视差变化的阈值，如果发现视差变化超出了这个阈值，则这个像素应该是一些无用的孤立像素。适当调整该参数可以使其过滤掉孤立的杂点和噪声。
mode：SGBM算法选择模式，默认为StereoSGBM::MODE_SGBM。
可选值有：
- StereoSGBM::MODE_SGBM_3WAY（速度快）
SGBM_3WAY：这是一种三通道SGBM算法，它将输入图像的三个通道（BGR）分别作为独立的视差图像进行处理。然后，将三个视差图像中的像素最小化，从而得到最终的视差图。这种方法可以改善处理彩色图像时的准确性。
- StereoSGBM::MODE_SGBM（速度中等）
SGBM：这是默认的解释模式，也是最常用的模式。它代表了Semi-Global Block Matching (SGBM) 算法，该算法利用全局视差的一致性来获得更准确的视差图。
- StereoSGBM::MODE_HH（速度慢）、 StereoSGBM::MODE_HH4（速度慢）
HHSGBM：这代表了H.Hirschmüller的快速近似SGBM算法（H.Hirschmüller's Hierarchical Semi-Global Block Matching），是一种更快速的算法。它在速度上相对于标准SGBM算法有所优化，但可能在某些情况下会牺牲一些准确性。
三、实验结果
slam十四讲上的示例
输入的左目图片：
输入的右目图片：
运行结果：
视差图：
图的左边有一部分是空的，是右边的相机没看到的部分。
点云图：
放大一点，有种走进街道的感觉
自己的图片测试，记得要换成自己相机的内参！！！
输入左目图片：
输入的右目图片：
视差图：
生成的点云图：
可以看到自己拍摄图片的效果比起书上的示例差了很多，考虑给拍摄图片去畸变和立体校正。
1万+
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
