视觉SLAM笔记（63） RGB-D 稠密建图

    int main(int argc, char** argv)
    {
    vector<cv::Mat> colorImgs, depthImgs;    // 彩色图和深度图
    vector<Eigen::Isometry3d> poses;         // 相机位姿
    
    ifstream fin("../data/pose.txt");
    if (!fin)
    {
        cerr << "cannot find pose file" << endl;
        return 1;
    }
    
    for (int i = 0; i < 5; i++)
    {
        boost::format fmt("../data/%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 };
        for (int i = 0; i < 7; i++)
        {
            fin >> data[i];
        }
        Eigen::Quaterniond q(data[6], data[3], data[4], data[5]);
        Eigen::Isometry3d T(q);
        T.pretranslate(Eigen::Vector3d(data[0], data[1], data[2]));
        poses.push_back(T);
    }
    
    // 计算点云并拼接
    // 相机内参 
    double cx = 325.5;
    double cy = 253.5;
    double fx = 518.0;
    double fy = 519.0;
    double depthScale = 1000.0;
    
    cout << "正在将图像转换为点云..." << endl;
    
    // 定义点云使用的格式：这里用的是XYZRGB
    typedef pcl::PointXYZRGB PointT;
    typedef pcl::PointCloud<PointT> PointCloud;
    
    // 新建一个点云
    PointCloud::Ptr pointCloud(new PointCloud);
    for (int i = 0; i < 5; i++)
    {
        PointCloud::Ptr current(new PointCloud);
        cout << "转换图像中: " << i + 1 << endl;
        cv::Mat color = colorImgs[i];
        cv::Mat depth = depthImgs[i];
        Eigen::Isometry3d 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表示没有测量到
                if (d >= 7000) continue; // 深度太大时不稳定，去掉
                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;
    
                PointT p;
                p.x = pointWorld[0];
                p.y = pointWorld[1];
                p.z = pointWorld[2];
                p.b = color.data[v*color.step + u * color.channels()];
                p.g = color.data[v*color.step + u * color.channels() + 1];
                p.r = color.data[v*color.step + u * color.channels() + 2];
                current->points.push_back(p);
            }
        // 利用统计滤波器方法去除孤立点
        PointCloud::Ptr tmp(new PointCloud);
        pcl::StatisticalOutlierRemoval<PointT> statistical_filter;
        statistical_filter.setMeanK(50);
        statistical_filter.setStddevMulThresh(1.0);
        statistical_filter.setInputCloud(current);
        statistical_filter.filter(*tmp);
        (*pointCloud) += *tmp;
    }
    
    pointCloud->is_dense = false;
    cout << "点云共有" << pointCloud->size() << "个点." << endl;
    
    // 体素滤波器
    pcl::VoxelGrid<PointT> voxel_filter;
    voxel_filter.setLeafSize(0.01, 0.01, 0.01);       // resolution 
    PointCloud::Ptr tmp(new PointCloud);
    voxel_filter.setInputCloud(pointCloud);
    voxel_filter.filter(*tmp);
    tmp->swap(*pointCloud);
    
    cout << "滤波之后，点云共有" << pointCloud->size() << "个点." << endl;
    
    pcl::io::savePCDFileBinary("map.pcd", *pointCloud);
    return 0;
    }

    $ ./pointcloud_mapping

    $ pcl_viewer map.pcd