《视觉slam十四讲》ch5相机与图像 学习笔记（2）——实践部分双目视觉代码讲解以及eigen路径设置_stereovision.cpp

cmake_minimum_required(VERSION 2.8)
set(CMAKE_BUILD_TYPE "Release")
# 添加c++ 11标准支持
set(CMAKE_CXX_FLAGS "-std=c++11 -O2")
find_package(Pangolin REQUIRED)
find_package(OpenCV REQUIRED)
add_executable(stereoVision stereoVision.cpp)
include_directories("/usr/local/include/eigen3")  #根据自己的eigen所在位置填写
 
target_link_libraries(stereoVision ${OpenCV_LIBS} ${Pangolin_LIBRARIES})

 
// 文件路径
string left_file = "/home/rxz/slambook2/ch5/stereo/left.png";
string right_file = "/home/rxz/slambook2/ch5/stereo/right.png";

#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 = "/home/rxz/slambook2/ch5/stereo/left.png";
string right_file = "/home/rxz/slambook2/ch5/stereo/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);
    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);    // sgbm经典参数配置
    cv::Mat disparity_sgbm, disparity;
    sgbm->compute(left, right, disparity_sgbm);//输入前面两张图，第三个参数是输出
    disparity_sgbm.convertTo(disparity, CV_32F, 1.0 / 16.0f);  //将disparity_sgbm变成32F类型的disparity，这里的disparity才是视差图。  如果Mat类型数据的深度不满足上面的要求，则需要使用convertTo()函数来进行转换。convertTo()函数负责转换数据类型不同的Mat
 
    // 生成点云
    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++) {
            //排除不在视差范围里的像素点
            if (disparity.at<float>(v, u) <= 0.0 || disparity.at<float>(v, u) >= 96.0) continue;  //这里的96是视差搜索范围
 
            Vector4d point(0, 0, 0, left.at<uchar>(v, u) / 255.0); // 前三维为xyz,第四维为颜色，如果不是255，那么取整后就为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;
 
            pointcloud.push_back(point);
        }
 
    cv::imshow("disparity", disparity / 96.0);  //归一化后的视差图
    cv::waitKey(0);
    // 画出点云
    showPointCloud(pointcloud);
    //  cv::waitKey(0);
    //    cv::destroyAllWindows();
    return 0;
}
 
void showPointCloud(const vector<Vector4d, Eigen::aligned_allocator<Vector4d>> &pointcloud) {
 
    if (pointcloud.empty()) {
        cerr << "Point cloud is empty!" << endl;
        return;
    }
//创建一个Pangolin窗口
    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)
    );
//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::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，表示横纵比
 
    while (pangolin::ShouldQuit() == false) {//如果pangolin窗口没有关闭，则执行
        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;
}

    cv::Ptr<cv::StereoSGBM> sgbm = cv::StereoSGBM::create(
        0, 96, 9, 8 * 9 * 9, 32 * 9 * 9, 1, 63, 10, 100, 32);    // sgbm经典参数配置

v::imshow("disparity", disparity / 96.0); //归一化后的视差图