一、CMakeLists文件

cmake_minimum_required(VERSION 3.15) project(stereoVision) find_package(Pangolin REQUIRED) find_package(OpenCV REQUIRED) include_directories(${OpenCV_INCLUDE_DIRS}) include_directories(${Pangolin_INCLUDE_DIRS}) set(CMAKE_CXX_STANDARD 14) add_executable(stereoVision main.cpp) target_link_libraries(stereoVision ${OpenCV_LIBS} ${Pangolin_LIBRARIES})

二、.cpp文件

#include <opencv2/opencv.hpp> //容器vector的头文件，vector是可以存放任意类型的动态数组 #include <vector> #include <string> #include <Eigen/Core> //用于显示3D视觉图像 #include <pangolin/pangolin.h> //Linux系统服务头文件 #include <unistd.h> using namespace std; using namespace Eigen; // 文件路径 string left_file = "/home/zhangman/slambook2-master/ch5/stereo/left.png"; string right_file = "/home/zhangman/slambook2-master/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); //创建一个指针，指向OPenCV中的sgbm（半全局立体匹配算法），其中最小视差为0，最大视差为96 cv::Ptr<cv::StereoSGBM> sgbm = cv::StereoSGBM::create( 0, 96, 9, 8 * 9 * 9, 32 * 9 * 9, 1, 63, 10, 100, 32); // 神奇的参数 //定义CV_32F格式视差图disparity，定义sgbm算法输出是插图disparity_sgbm //CV_32F格式在下面转换的时候指定，该格式像素为0-1.0之间的任意float类型数，是一些数值计算要求的格式 cv::Mat disparity_sgbm, disparity; //调用sgbm算法计算左右目图像视差，视差图输出到disparity中 sgbm->compute(left, right, disparity_sgbm); //将disparity_sgbm转换成CV_32F类型图像，存储在disparity中，第三个参数代表尺度的变化 //CV_32F是浮点型的-像素可以有0-1.0之间的任何值，这对于数据的某些计算集很有用，但必须将其转换为8位以保存或显示，方法是将每个像素乘以255。 disparity_sgbm.convertTo(disparity, CV_32F); cout<<disparity.at<float>(100,100); //将sgbm输出的图像矩阵转化为CV_32F，并缩小像素值为1/16（这个缩小倍数应该和sgbm算法有关），缩小后的像素范围为0-96 disparity_sgbm.convertTo(disparity, CV_32F, 1.0/16.0f); // 生成点云：一起一个容器对象pointcloud，其元素是四维向量 //当vector中元素是Eigen中的类型的时候，必须调用Eigen::aligned_allocator<>分配内存空间，否则编译不会报错，运行会报错 //而内置类型则不用，如vector<int> a;定义一个名字为a的动态数组，数组元素类型为int vector<Vector4d, Eigen::aligned_allocator<Vector4d>> pointcloud; // 遍历元素 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; //创建一个四维向量用于存储一个三维点的信息，前三维为xyz // 第四维为颜色，这里采用原始图像颜色作为参考，显示效果更好，并对颜色做了归一化处理 Vector4d point(0, 0, 0, right.at<uchar>(v, u) / 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; //将计算得到的三维点从尾部添加到点云容器point中 pointcloud.push_back(point); } //显示视差图，对视差图中所有像素灰度值除以96，从而将像素值归一化到0-1，符合CV_32F格式条件，并显示 cv::imshow("disparity", disparity / 96.0); cv::waitKey(0); // 画出点云 showPointCloud(pointcloud); return 0; } //使用pangolin绘制点云图函数 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); //如果想绘制其他图像，将这里的pointcloud改成对应的容器就可以 //或者将pointcloud对应到其他图像 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; }