一、CMakeLists文件

cmake_minimum_required(VERSION 3.15) project(rgbd) find_package(OpenCV REQUIRED) find_package(Pangolin REQUIRED) set(CMAKE_CXX_STANDARD 14) include_directories("/usr/include/eigen3") add_executable(rgbd main.cpp) target_link_libraries(rgbd ${OpenCV_LIBS} ${Pangolin_LIBRARIES})

二、.cpp文件

#include <iostream> //c++中对文件的操作 #include <fstream> #include <opencv2/opencv.hpp> #include <boost/format.hpp> // for formating strings #include <sophus/se3.hpp> #include <pangolin/pangolin.h> using namespace std; typedef vector<Sophus::SE3d, Eigen::aligned_allocator<Sophus::SE3d>> TrajectoryType; typedef Eigen::Matrix<double, 6, 1> Vector6d; // 在pangolin中画图，已写好，无需调整 void showPointCloud( const vector<Vector6d, Eigen::aligned_allocator<Vector6d>> &pointcloud); int main(int argc, char **argv) { vector<cv::Mat> colorImgs, depthImgs; // 彩色图和深度图vector TrajectoryType poses; // 相机位姿 //检测当前文件夹内是否有位姿文件 ifstream fin("/home/zhangman/myvslam2/ch5/rgbd/pose.txt"); if (!fin) { cerr << "请在有pose.txt的目录下运行此程序" << endl; return 1; } for (int i = 0; i < 5; i++) { boost::format fmt("./%s/%d.%s"); //设置图像文件格式 //读取图像 colorImgs.push_back(cv::imread((fmt % "color" % (i + 1) % "png").str())); depthImgs.push_back(cv::imread((fmt % "depth" % (i + 1) % "pgm").str(), -1)); // 使用-1读取原始图像 //定义位姿数组 double data[7] = {0}; //读取位姿数组并存储到pose中 for (auto &d:data) fin >> d; Sophus::SE3d pose(Eigen::Quaterniond(data[6], data[3], data[4], data[5]), Eigen::Vector3d(data[0], data[1], data[2])); poses.push_back(pose); } // 计算点云并拼接 // 相机内参 double cx = 325.5; double cy = 253.5; double fx = 518.0; double fy = 519.0; double depthScale = 1000.0; //定义点云vector vector<Vector6d, Eigen::aligned_allocator<Vector6d>> pointcloud; pointcloud.reserve(1000000); //开始融合彩色图和深度图 for (int i = 0; i < 5; i++) { cout << "转换图像中: " << i + 1 << endl; //取彩色图和深度图和位姿 cv::Mat color = colorImgs[i]; cv::Mat depth = depthImgs[i]; Sophus::SE3d T = poses[i]; //遍历彩色图每一个点 for (int v = 0; v < color.rows; v++) for (int u = 0; u < color.cols; u++) { //取图像中该点的深度值 unsigned int d = depth.ptr<unsigned short>(v)[u]; // 深度值 if (d == 0) continue; // 为0表示没有测量到 Eigen::Vector3d point; //根据深度值计算图像中该点在相机坐标系中的坐标 point[2] = double(d) / depthScale; point[0] = (u - cx) * point[2] / fx; point[1] = (v - cy) * point[2] / fy; //基于相机位姿计算图像中该点在世界坐标系中的坐标，这一步实际上就是拼接 //这是最简单的拼接，不进行优化，直接硬性拼接 Eigen::Vector3d pointWorld = T * point; //将图像中该点的颜色和世界坐标存储到点云中 Vector6d p; p.head<3>() = pointWorld; p[5] = color.data[v * color.step + u * color.channels()]; // blue p[4] = color.data[v * color.step + u * color.channels() + 1]; // green p[3] = color.data[v * color.step + u * color.channels() + 2]; // red pointcloud.push_back(p); } } cout << "点云共有" << pointcloud.size() << "个点." << endl; //点云可视化 showPointCloud(pointcloud); return 0; } //展示电晕效果 void showPointCloud(const vector<Vector6d, Eigen::aligned_allocator<Vector6d>> &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) { glColor3d(p[3] / 255.0, p[4] / 255.0, p[5] / 255.0); glVertex3d(p[0], p[1], p[2]); } glEnd(); pangolin::FinishFrame(); usleep(5000); // sleep 5 ms } return; }