視覺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點雲模型！
遇到問題的小夥伴記得評論區留言，林君學長看到會爲大家解答的，這個學長不太冷！
陳一月的又一天編程歲月^ _ ^