slam14讲ch4作业----双目视差图恢复点云图

仅用于个人学习记录。
参考了这篇答案
一、工程描述（深蓝学院slam14–ch4课后作业）
利用kitti数据集中的双目相机图像部分，给的left.png right.png以及视差图diapaity.png来恢复出三位点云（主要计算深度z)并用Pangolin显示。

left.png
right.png
disparity.png
二、思路分析

//1.拿到left.png right.png disparity.png 三张图，但是right.png实际上是没有用到的。
//2.以left中像素点进行遍历，与left每个像素点对应，在disparity中有每个点的视差信息，用0-255表示。
//因此对于每个像素点（v,u）,其空间点的深度 z = fb/d
//3.在使用pangolin画图时，每个点具有4个值，x y z w ，z已经算出，x y 利用像素到空间的缩放平移公式 u = fxx+cx/z 来计算，w就用left对应点上点颜色即可
//4.在pangolin中画图，已写好，无需调整*

三、代码部分（主要都是课程中写好的）
需要提前安装库： opencv pangolin Eigen3等
disparity.cpp

//
// Created by 高翔 on 2017/12/15.
//

#include <opencv2/opencv.hpp>
#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";
string disparity_file = "./disparity.png";
//笔记：
//拿到left.png  right.png disparity.png 三张图，但是right.png实际上是没有用到的。
//以left中像素点进行遍历，与left每个像素点对应，在disparity中有每个点的视差信息，用0-255表示。
//因此对于每个像素点（v,u）,其空间点的深度 z = fb/d
//在使用pangolin画图时，每个点具有4个值，x y z  w ，z已经算出，x y 利用像素到空间的缩放平移公式  u = fx*x+cx/z 来计算，w就用left对应点上点颜色即可
// 在pangolin中画图，已写好，无需调整
void showPointCloud(const vector<Vector4d, Eigen::aligned_allocator<Vector4d>> &pointcloud);

int main(int argc, char **argv) {

    // 内参
    //是左相机点缩放系数fx fy,和平移系数cx cy
    double fx = 718.856, fy = 718.856, cx = 607.1928, cy = 185.2157;
    // 间距
    double b = 0.573;
    //焦距
    double f = 0.04;

    // 读取图像
    cv::Mat left = cv::imread(left_file, 0);
//    cv::Mat right = cv::imread(right_file, 0);
    cv::Mat disparity = cv::imread(disparity_file, 0); // disparty 为CV_8U,单位为像素

    // 生成点云
    vector<Vector4d, Eigen::aligned_allocator<Vector4d>> pointcloud;

    // TODO 根据双目模型计算点云
    // 如果你的机器慢，请把后面的v++和u++改成v+=2, u+=2
    for (int v = 0; v < left.rows; v+=2)
        for (int u = 0; u < left.cols; u+=2) {

            Vector4d point(0, 0, 0, left.at<uchar>(v, u) / 255.0); // 前三维为xyz,第四维为颜色

            // start your code here (~6 lines)
            // start your code here (~6 lines)
            unsigned int d=disparity.at<uchar>(v,u);
            //视差图disparty中(v,u)点，带有视差信息d，其深度d用像素值来表达0-255之间
            if(d==0)
                continue;  //视差d=0表示该点没有深度不用估计
            cout<<d<<endl;
            //基线长度b=0.573  Z=fb/d  f是内参--焦距，b是基线长度，d是视差
            double z=(f*b*1000)/d;

            point[2]=z;
            point[0]=(u-cx)*z/fx;
            point[1]=(v-cy)*z/fy;
            pointcloud.push_back(point);

            // 根据双目模型计算 point 的位置
            // end your code her
            // end your code here
        }

    // 画出点云
    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::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;
}

CMakeLists.txt

cmake_minimum_required(VERSION 2.8.3)
project(disparity)

set(CMAKE_CXX_STANDARD 11)

find_package(OpenCV)
find_package(Eigen3)
find_package(Pangolin)
include_directories(${Pangolin_INCLUDE_DIRS})
#include_directories( "/usr/include/eigen3" )
include_directories(${EIGEN3_INCLUDE_DIR})

add_executable(disparity disparity.cpp)
target_link_libraries(disparity ${OpenCV_LIBS} ${Pangolin_LIBRARIES})

结果：