视觉SLAM学习【7】—–基于ubuntu16.04的深度及彩色图像立体匹配,并生成深度和彩色3D点云

175
0
2021年1月20日 09时29分

视觉SLAM学习【7】—–基于ubuntu16.04的深度及彩色图像立体匹配,并生成深度和彩色3D点云目录

 

  • 一、数据准备和库的安装及配置
    • 1、数据准备
    • 2、Pangolin库的下载安装及配置
  • 二、创建项目
    • 1、创建立体匹配文件夹
    • 2、在test1中创建编译文件夹build并进行编译
  • 三、运行结果
    • 1、灰度立体匹配结果
    • 2、彩色立体匹配结果
    • 3、将内存中的3D点的坐标(x,y,z)和颜色值,逐行写入一个文本磁盘文件中
    • 4、用meshlab软件以打开(.xyz)文件类型打开你程序生成的文本文件

 

在进行嵌入式视觉学习的时候,很多时候我们会用到立体匹配并生成3D点云,本次博客,林君学长主要带大家学习灰度及彩色图片的立体匹配,并生成灰度和彩色的3D点云,一起来看吧!

 

一、数据准备和库的安装及配置

 

1、数据准备

 

1)、本次需要的数据比较简单,就是图片的准备,两张对比彩色图、两张对比深度图

 

(1)、深度图

 

微信图片_20210117201835

 

微信图片_20210117201843

 

(2)、彩色图

 

微信图片_20210117201914

 

微信图片_20210117201916

 

对于以上图片呢,小伙伴可以通过截图获取就OK,并且,将以上图片放在创建的test1项目里的build文件夹下,后面会有说到!

 

2、Pangolin库的下载安装及配置

 

本次需要的库有opencv3.4.1(其他版本也可),Pangolin、PCL库,接下来,林君学长就介绍安装后面两个库,opencv我相信很多小伙伴都已经安装

 

1)、Pangolin的安装及配置

 

Pangolin库的安装及配置林君学长在另一篇博客已经写好了,小伙伴可以通过如下链接进行Pangolin库的下载、安装、编译:
https://blog.csdn.net/qq_42451251/article/details/105567084

 

微信图片_20210117201936

 

我们这里再后面通过代码进行测试,这里就不进行测试了,只要编译完成百分百中途没有错误,我们默认安装成功,后面代码的时候进行测试!

 

二、创建项目

 

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;
}

 

保存后,关闭!

 

微信图片_20210117202033

 

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

 

微信图片_20210117202159

 

编译成功如上所示:

 

三、运行结果

 

1、灰度立体匹配结果

 

./imagebinoculartest

 

1)、立体匹配结果

 

微信图片_20210117202222

 

出现以上结果后,将鼠标放在图片上,然后随便选择键盘的任意键进行按下,就会得到后面的色差图

 

2)、色差图

 

微信图片_20210117202240

 

出现以上结果后,将鼠标放在图片上,然后随便选择键盘的任意键进行按下,就会得到后面的3D点云图

 

3)、3D点云图

 

微信图片_20210117202254

 

2、彩色立体匹配结果

 

./imagebinoculartest1

 

1)、立体匹配结果

 

微信图片_20210117202317

 

出现以上结果后,将鼠标放在图片上,然后随便选择键盘的任意键进行按下,就会得到后面的色差图

2)、色差图

 

微信图片_20210117202336

 

出现以上结果后,将鼠标放在图片上,然后随便选择键盘的任意键进行按下,就会得到后面的3D点云图

 

3)、彩色3D点云

 

微信图片_20210117202351

 

3、将内存中的3D点的坐标(x,y,z)和颜色值,逐行写入一个文本磁盘文件中

 

1)、写入磁盘文件中,在上面main1.cpp函数已经写了,写入彩云的3D坐标点,运行后,该程序会在build文件夹里创建一个out.txt文件,里面全部是各个点的坐标

 

微信图片_20210117202408

 

4、用meshlab软件以打开(.xyz)文件类型打开你程序生成的文本文件

 

1)、下载meshlab软件

 

sudo add-apt-repository ppa:zarquon42/meshlab
sudo apt-get update
sudo apt-get install meshlab

 

2)、新建终端,打开meshlab软件

 

meshlab

 

打开界面结果如下所示:

 

微信图片_20210117202442

 

3)、添加刚刚的out.txt文本文件

 

微信图片_20210117202500

 

微信图片_20210117202502

 

4)、设置参数

 

微信图片_20210117202529

 

由于我们的数据是通过逗号隔开,所以我们选择分隔符为逗号,然后点击ok

 

5)、模型加载如下所示:

 

微信图片_20210117202546

 

以上3D点云的模型就加载出来了
以上就是本次博客的全部内容啦,希望小伙伴们对本次博客的阅读可以帮助大家理解彩色3D点云的形成中如何利用meshlab加载3D点云模型!
遇到问题的小伙伴记得评论区留言,林君学长看到会为大家解答的,这个学长不太冷!

陈一月的又一天编程岁月^ _ ^

发表评论

后才能评论