视觉SLAM笔记(63) RGB-D 稠密建图

视觉SLAM笔记(63) RGB-D 稠密建图


1. 建立点云地图

所谓点云,就是由一组离散的点表示的地图
最基本的点包含 x, y, z 三维坐标,也可以带有 r, g, b 的彩色信息
由于 RGB-D 相机提供了彩色图和深度图,很容易根据相机内参来计算 RGB-D 点云

如果通过某种手段,得到了相机的位姿
那么只要直接把点云进行加和,就可以获得全局的点云
视觉SLAM笔记(25) 拼接点云 给出了一个通过相机内外参拼接点云的例子
不过,在实际建图当中,还会对点云加一些滤波处理,获得更好的视觉效果

在本程序中,主要使用两种滤波器:外点去除滤波器以及降采样滤波器
由于部分代码与之前的相同,主要看改变的部分:

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;
}

思路没有太大变化,主要不同之处在于:

  1. 在生成每帧点云时,去掉深度值太大或无效的点
    这主要是考虑到 Kinect 的有效量程,超过量程之后的深度值会有较大误差
  2. 利用统计滤波器方法去除孤立点
    该滤波器统计每个点与它最近 N 个点的距离值的分布,去除距离均值过大的点
    这样,保留了那些“粘在一起”的点,去掉了孤立的噪声点
  3. 最后,利用体素滤波器(Voxel Filter)进行降采样
    由于多个视角存在视野重叠,在重叠区域会存在大量的位置十分相近的点,这会无益地占用许多内存空间
    体素滤波保证在某个一定大小的立方体(或称体素)内仅有一个点
    相当于对三维空间进行了降采样,从而节省了很多存储空间

运行程序

$ ./pointcloud_mapping

在这里插入图片描述
最后,把生成的点云以 pcd 格式存储在 map.pcd

用 PCL 提供的可视化程序打开这个文件:

$ pcl_viewer map.pcd 

在这里插入图片描述

可能需要放大才能看清,或者自己尝试一下
在这里插入图片描述
左侧是 视觉SLAM笔记(25) 拼接点云 生成的点云地图
而右侧是经过滤波后的点云地图

观察白色框中部分,可以看到在滤波前存在着由噪声产生的许多孤立的点
经过统计外点去除之后,消去了这些噪声,使得整个地图变得更干净
另一方面,在体素滤波器中,把分辨率调至 0.01,表示每立方厘米有一个点
这是一个比较高的分辨率,所以在截图中我们感觉不出地图的差异
然而程序输出中可以看到点数明显减少了许多(从 90 万个点减到了 44 万个点,去除了一半左右)

点云地图为提供了比较基本的可视化地图,能够大致了解环境的样子
它以三维方式存储,使得能够快速地浏览场景的各个角落,乃至在场景中进行漫游
点云的一大优势是可以直接由 RGB-D 图像高效地生成,不需要额外的处理
它的滤波操作也非常直观,且处理效率尚能接受


2. 点云地图

不过,使用点云表达地图仍然是十分初级的
不妨按照之前提的对地图的需求,看看点云地图是否能满足:

  1. 定位需求:取决于前端视觉里程计的处理方式
    如果是基于特征点的视觉里程计,由于点云中没有存储特征点信息,所以无法用于基于特征点的定位方法
    如果前端是点云的 ICP,那么可以考虑将局部点云对全局点云进行 ICP 以估计位姿
    然而,这要求全局点云具有较好的精度
    在这种处理点云的方式中,并没有对点云本身进行优化,所以是不够的
  2. 导航与避障的需求:无法直接用于导航和避障
    纯粹的点云无法表示“是否有障碍物”的信息,也无法在点云中做“任意空间点是否被占据”这样的查询
    而这是导航和避障的基本需要
    不过,可以在点云基础上进行加工,得到更适合导航与避障的地图形式
  3. 可视化和交互:具有基本的可视化与交互能力
    能够看到场景的外观,也能在场景里漫游
    从可视化角度来说,由于点云只含有离散的点,而没有物体表面信息(例如法线)
    所以不太符合人们对可视化习惯
    例如,点云地图的物体从正面看和背面看是一样的,而且还能透过物体看到它背后的东西
    这些都不符合日常的经验,因为没有物体表面的信息

综上所述,点云地图是“基础”的或“初级的”,是指它更接近于传感器读取的原始数据
它具有一些基本的功能,但通常用于调试和基本的显示,不便直接用于应用程序


3. 其他重建方法

如果希望地图有更高级的功能,点云地图是一个不错的出发点
例如,针对导航功能,可以从点云出发,构建占据网格地图(Occupancy Grid),以供导航算法查询某点是否可以通过
再如, SfM 中常用的泊松重建方法,就能通过基本的点云重建物体网格地图,得到物体的表面信息
除泊松重建之外, Surfel 亦是一种表达物体表面的方式,以面元作为地图的基本单位,能够建立漂亮的可视化地图
在这里插入图片描述
显示了泊松重建和 surfel 的一个样例
可以看成它们的视觉效果明显优于纯粹点云建图,而它们都可以通过点云进行构建
大部分由点云转换得到的地图形式都在 PCL库中提供


参考:

《视觉SLAM十四讲》


相关推荐:

视觉SLAM笔记(62) 单目稠密重建
视觉SLAM笔记(61) 单目稠密建图
视觉SLAM笔记(60) 建图
视觉SLAM笔记(59) 相似度计算
视觉SLAM笔记(58) 字典


谢谢!

发布了233 篇原创文章 · 获赞 322 · 访问量 296万+

没有更多推荐了,返回首页

©️2019 CSDN 皮肤主题: Age of Ai 设计师: meimeiellie

分享到微信朋友圈

×

扫一扫,手机浏览