视觉SLAM学习【7】—–基于ubuntu16.04的深度及彩色图像立体匹配，并生成深度和彩色3D点云目录
在进行嵌入式视觉学习的时候，很多时候我们会用到立体匹配并生成3D点云，本次博客，林君学长主要带大家学习灰度及彩色图片的立体匹配，并生成灰度和彩色的3D点云，一起来看吧！
一、数据准备和库的安装及配置
1、数据准备
1）、本次需要的数据比较简单，就是图片的准备，两张对比彩色图、两张对比深度图 （1）、深度图
（2）、彩色图
对于以上图片呢，小伙伴可以通过截图获取就OK,并且，将以上图片放在创建的test1项目里的build文件夹下，后面会有说到！
2、Pangolin库的下载安装及配置
本次需要的库有opencv3.4.1(其他版本也可)，Pangolin、PCL库，接下来，林君学长就介绍安装后面两个库，opencv我相信很多小伙伴都已经安装 1）、Pangolin的安装及配置 Pangolin库的安装及配置林君学长在另一篇博客已经写好了，小伙伴可以通过如下链接进行Pangolin库的下载、安装、编译： https://blog.csdn.net/qq_42451251/article/details/105567084
我们这里再后面通过代码进行测试，这里就不进行测试了，只要编译完成百分百中途没有错误，我们默认安装成功，后面代码的时候进行测试！
二、创建项目
1、创建立体匹配文件夹
1）、在安装的opencv中创建文件夹，名字自取，例如test1
cd ~/lenovo/opencv-3.4.1/
mkdir test1
cd test1
2）、创建灰度立体匹配main.cpp文件，并生成灰度3D点云
touch main.cpp
gedit main.cpp
3）、编写main.cpp灰度立体匹配代码
#include <iostream>
#include <chrono>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <Eigen/Core>
#include<opencv2/calib3d/calib3d.hpp>
#include <pangolin/pangolin.h>
using namespace std;
using namespace Eigen;
using namespace cv;
//高博用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;// 基线
cout << "OpenCV version : " << CV_VERSION << endl;
Mat leftImg=imread("left.png",0);
Mat rightImg=imread("right.png",0);
imshow ( "leftImg", leftImg);
imshow ( "rightImg", rightImg);
waitKey ( 0 );
//OpenCV实现的SGBM立体匹配算法
Ptr<StereoSGBM> sgbm = StereoSGBM::create(
0,//minDisparity 最小视差
96, //numDisparities 视差搜索范围，值必需为16的整数倍。最大搜索边界=最小视差+视差搜索范围
9, //blockSize 块大小
//8*cn*sgbm.SADWindowSize*sgbm.SADWindowSize;
8 * 9 * 9, //P1 控制视差变化平滑性的参数。P1、P2的值越大，视差越平滑。P1是相邻像素点视差增/减 1 时的惩罚系数；P2是相邻像素点视差变化值大于1时的惩罚系数。P2必须大于P1。
//32*cn*sgbm.SADWindowSize*sgbm.SADWindowSize
32 * 9 * 9, //P2
1, //disp12MaxDiff 左右一致性检测最大容许误差阈值。
63, //preFilterCap,预处理时映射参数
10, //uniquenessRatio 唯一性检测参数，
100, //speckleWindowSize 视差连通区域像素点个数的大小。对于每一个视差点，当其连通区域的像素点个数小于speckleWindowSize时，认为该视差值无效，是噪点。
32//speckleRange 视差连通条件，在计算一个视差点的连通区域时，当下一个像素点视差变化绝对值大于speckleRange就认为下一个视差像素点和当前视差像素点是不连通的。
);
Mat disparity_sgbm, disparity;
sgbm->compute(leftImg, rightImg, disparity_sgbm); //计算视差图
disparity_sgbm.convertTo(disparity, CV_32F, 1.0 / 16.0f);//得到视差图
cv::imshow("disparity", disparity / 96.0);
cv::waitKey(0);
// 生成点云
vector<Vector4d, Eigen::aligned_allocator<Vector4d>> pointcloud;
// 如果机器慢，把后面的v++和u++改成v+=2, u+=2
for (int v = 0; v < leftImg.rows; v++)
for (int u = 0; u < leftImg.cols; u++) {
if (disparity.at<float>(v, u) <= 0.0 || disparity.at<float>(v, u) >= 96.0)
continue;
Vector4d point(0, 0, 0, leftImg.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));
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);
return 0;
}
void showPointCloud(const vector<Vector4d, Eigen::aligned_allocator<Vector4d>> &pointcloud)
{
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)
);
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));
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();
usleep(5000); // sleep 5 ms
}
return;
}
保存后，关闭！
4）、创建彩色图像立体匹配main1.cpp文件
touch main1.cpp
gedit main1.cpp
5）、编写main1.cpp彩色立体匹配代码
#include <iostream>
#include <chrono>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <Eigen/Core>
#include<opencv2/calib3d/calib3d.hpp>
#include <pangolin/pangolin.h>
#include <unistd.h>
#include <fstream>
using namespace std;
using namespace Eigen;
using namespace cv;
//高博用pangolin中显示点云
void showPointCloud(vector<Vector4d, Eigen::aligned_allocator<Vector4d>> &pointcloud, vector<Vector4d, Eigen::aligned_allocator<Vector4d>> &pointcloudRGB);
ofstream outfile("out.txt");
int main ( int argc, char** argv )
{
double fx = 718.856, fy = 718.856, cx = 607.1928, cy = 185.2157;// 内参
double b = 0.573;// 基线
cout << "OpenCV version : " << CV_VERSION << endl;
Mat leftImg=imread("left1.png",1);
Mat rightImg=imread("right1.png",1);
imshow ( "leftImg", leftImg);
imshow ( "rightImg", rightImg);
Mat leftImgRGB=imread("left1.png");
waitKey ( 0 );
//OpenCV实现的SGBM立体匹配算法
Ptr<StereoSGBM> sgbm = StereoSGBM::create(
0,//minDisparity 最小视差
96, //numDisparities 视差搜索范围，值必需为16的整数倍。最大搜索边界=最小视差+视差搜索范围
9, //blockSize 块大小
//8*cn*sgbm.SADWindowSize*sgbm.SADWindowSize;
8 * 9 * 9, //P1 控制视差变化平滑性的参数。P1、P2的值越大，视差越平滑。P1是相邻像素点视差增/减 1 时的惩罚系数；P2是相邻像素点视差变化值大于1时的惩罚系数。P2必须大于P1。
//32*cn*sgbm.SADWindowSize*sgbm.SADWindowSize
32 * 9 * 9, //P2
1, //disp12MaxDiff 左右一致性检测最大容许误差阈值。
63, //preFilterCap,预处理时映射参数
10, //uniquenessRatio 唯一性检测参数，
100, //speckleWindowSize 视差连通区域像素点个数的大小。对于每一个视差点，当其连通区域的像素点个数小于speckleWindowSize时，认为该视差值无效，是噪点。
32//speckleRange 视差连通条件，在计算一个视差点的连通区域时，当下一个像素点视差变化绝对值大于speckleRange就认为下一个视差像素点和当前视差像素点是不连通的。
);
Mat disparity_sgbm, disparity;
sgbm->compute(leftImg, rightImg, disparity_sgbm); //计算视差图
disparity_sgbm.convertTo(disparity, CV_32F, 1.0 / 16.0f);//得到视差图
cv::imshow("disparity", disparity / 96.0);
cv::waitKey(0);
// 生成点云
vector<Vector4d, Eigen::aligned_allocator<Vector4d>> pointcloud;
vector<Vector4d, Eigen::aligned_allocator<Vector4d>> pointcloudRGB;
// 如果机器慢，把后面的v++和u++改成v+=2, u+=2
for (int v = 0; v < leftImg.rows; v++)
for (int u = 0; u < leftImg.cols; u++) {
if (disparity.at<float>(v, u) <= 0.0 || disparity.at<float>(v, u) >= 96.0)
continue;
Vector4d point(0, 0, 0, 0); // 前三维为xyz,第四维为颜色
Vector4d point1(0, 0, 0, 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;
point1[0] = leftImgRGB.at<Vec3b>(v, u)[0]/255.0;
point1[1] = leftImgRGB.at<Vec3b>(v, u)[1]/255.0;
point1[2] = leftImgRGB.at<Vec3b>(v, u)[2]/255.0;
// 使用逻辑的方式,来进行颜色设计
pointcloud.push_back(point);
pointcloudRGB.push_back(point1);
}
cv::imshow("disparity", disparity / 96.0);
cv::waitKey(0);
// 画出点云
showPointCloud(pointcloud, pointcloudRGB);
return 0;
}
void showPointCloud(vector<Vector4d, Eigen::aligned_allocator<Vector4d>> &pointcloud, vector<Vector4d, Eigen::aligned_allocator<Vector4d>> &pointcloudRGB)
{
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)
);
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));
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(int i = 0;i<pointcloud.size();i++){
Vector4d p = pointcloud[i];
glVertex3d(p[0], p[1], p[2]);
outfile<<p[0]<<","<<p[1]<<","<<p[2]<<endl;
p = pointcloudRGB[i];
glColor3f(p[0], p[1], p[2]);
}
glEnd();
pangolin::FinishFrame();
usleep(5000); // sleep 5 ms
}
return;
}
6）、新建CMakeLists.txt文件
touch CMakeLists.txt
gedit CMakeLists.txt
7）、文件内容如下所示：
cmake_minimum_required(VERSION 2.6)
project(test1)
# 添加c++ 11标准支持
set( CMAKE_CXX_FLAGS "-std=c++11" )
find_package( OpenCV 3 REQUIRED )
find_package( Pangolin )
include_directories( ${OpenCV_INCLUDE_DIRS} )
include_directories("/usr/include/eigen3")
include_directories( ${Pangolin_INCLUDE_DIRS} )
add_executable(imagebinoculartest main.cpp)
target_link_libraries(imagebinoculartest ${OpenCV_LIBS})
add_executable(imagebinoculartest1 main1.cpp)
target_link_libraries(imagebinoculartest1 ${OpenCV_LIBS})
target_link_libraries( imagebinoculartest ${Pangolin_LIBRARIES})
target_link_libraries( imagebinoculartest1 ${Pangolin_LIBRARIES})
install(TARGETS imagebinoculartest RUNTIME DESTINATION bin)
2、在test1中创建编译文件夹build并进行编译
1）、在test1中创建build文件夹，存放编译文件
cd ~/lenovo/opencv-3.4.1/test1/
mkdir build
cd build
2）、编译
cmake ..
make -j8
编译成功如上所示：
三、运行结果
1、灰度立体匹配结果
./imagebinoculartest
1）、立体匹配结果
出现以上结果后，将鼠标放在图片上，然后随便选择键盘的任意键进行按下，就会得到后面的色差图 2）、色差图
出现以上结果后，将鼠标放在图片上，然后随便选择键盘的任意键进行按下，就会得到后面的3D点云图 3）、3D点云图
2、彩色立体匹配结果
./imagebinoculartest1
1）、立体匹配结果
出现以上结果后，将鼠标放在图片上，然后随便选择键盘的任意键进行按下，就会得到后面的色差图 2）、色差图
出现以上结果后，将鼠标放在图片上，然后随便选择键盘的任意键进行按下，就会得到后面的3D点云图 3）、彩色3D点云
3、将内存中的3D点的坐标（x，y，z）和颜色值，逐行写入一个文本磁盘文件中
1）、写入磁盘文件中，在上面main1.cpp函数已经写了，写入彩云的3D坐标点，运行后，该程序会在build文件夹里创建一个out.txt文件，里面全部是各个点的坐标
4、用meshlab软件以打开（.xyz）文件类型打开你程序生成的文本文件
1）、下载meshlab软件
sudo add-apt-repository ppa:zarquon42/meshlab
sudo apt-get update
sudo apt-get install meshlab
2）、新建终端，打开meshlab软件
meshlab
打开界面结果如下所示：
3）、添加刚刚的out.txt文本文件
4）、设置参数
由于我们的数据是通过逗号隔开，所以我们选择分隔符为逗号，然后点击ok 5）、模型加载如下所示：
以上3D点云的模型就加载出来了 以上就是本次博客的全部内容啦，希望小伙伴们对本次博客的阅读可以帮助大家理解彩色3D点云的形成中如何利用meshlab加载3D点云模型！ 遇到问题的小伙伴记得评论区留言，林君学长看到会为大家解答的，这个学长不太冷！ 陈一月的又一天编程岁月^ _ ^
开心洋葱 , 版权所有丨如未注明 , 均为原创丨未经授权请勿修改 , 转载请注明视觉SLAM学习【7】—–基于ubuntu16.04的深度及彩色图像立体匹配，并生成深度和彩色3D点云！
