这个程序能够将双目相机采集的2D图像转换成3D点云。先看一下输入数据是两张图片(双目相机有两个摄像头)。左摄像头采集的数据右摄像头采集的数据程序运行后先生成视差图在这个视差图里因为左相机看到了右相机看不到的内容这一部分不参与视差计算所以左边会有黑条。视差大的区域 :颜色亮/数值大,距离近视差小的区域:颜色暗/数值小, 距离远然后根据视差图生成3D点云up截了不同视角的2张图片输入双目相机采集的两张图片(一组数据)输出没有保存输出结果运行双击直接运行视频演示https://www.bilibili.com/video/BV1Zy9eBZEeS/代码#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 ./left.png; string right_file ./right.png; // 在pangolin中画图已写好无需调整 void showPointCloud( const vectorVector4d, Eigen::aligned_allocatorVector4d 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::Ptrcv::StereoSGBM sgbm cv::StereoSGBM::create( 0, 96, 9, 8 * 9 * 9, 32 * 9 * 9, 1, 63, 10, 100, 32); // 神奇的参数 cv::Mat disparity_sgbm, disparity; sgbm-compute(left, right, disparity_sgbm); disparity_sgbm.convertTo(disparity, CV_32F, 1.0 / 16.0f); // 生成点云 vectorVector4d, Eigen::aligned_allocatorVector4d pointcloud; // 如果你的机器慢请把后面的v和u改成v2, u2 for (int v 0; v left.rows; v) for (int u 0; u left.cols; u) { if (disparity.atfloat(v, u) 0.0 || disparity.atfloat(v, u) 96.0) continue; Vector4d point(0, 0, 0, left.atuchar(v, u) / 255.0); // 前三维为xyz,第四维为颜色 // 根据双目模型计算 point 的位置 double x (u - cx) / fx; double y (v - cy) / fy; double depth fx * b / (disparity.atfloat(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 vectorVector4d, Eigen::aligned_allocatorVector4d 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; }参考高翔《视觉SLAM十四讲》P113附近
