深度图RangeImage

        目前深度图像的获取方法有激光雷达深度成像法计算机立体视觉成像坐标测量机法莫尔条纹法结构光法等等,针对深度图像的研究重点主要集中在以下几个方面,

  • 深度图像的分割技术
  • 深度图像的边缘检测技术
  • 基于不同视点的多幅深度图像的配准技术
  • 基于深度数据的三维重建技术
  • 基于三维深度图像的三维目标识别技术
  • 深度图像的多分辨率建模和几何压缩技术等等

        在PCL中深度图像点云最主要的区别在于其近邻的检索方式的不同,并且可以互相转换。

        深度图像(Depth Images)也被称为距离影像(Range Image),是指将从图像采集器到场景中各点的距离值作为像素值的图像,它直接反应了景物可见表面的几何形状,利用它可以很方便的解决3D目标描述中的许多问题,深度图像经过点云变换可以计算为点云数据,有规则及有必要信息的点云数据可以反算为深度图像数据。(点云:当一束激光照射到物体表面时,所反射的激光会携带方位、距离等信息。若将激光束按照某种轨迹进行扫描,便会边扫描边记录到反射的激光点信息,由于扫描极为精细,则能够得到大量的激光点,因而就可形成激光点云。点云格式有*.las ;*.pcd; *.txt等。

        从数学模型上看,深度图像可以看做是标量函数j:I^{2}\rightarrow R在集合K上的离散采样,得到r_{i}=j\left ( u_{i} \right ),其中u_{i}\in {I^{2}}为二维网格(矩阵)的索引,r_{i}\in {R},i=1,...,k,以下为从不同视角获得的深度图像过程示意:

从点云创建深度图

pcl::RangeImage

        RangeImage类继承于PointCloud,主要功能是实现从一个特定视点得到一个三维场景的深度图像。其继承关系如下:

函数及参数说明

函数:通过pointCloud点云创建深度图

createFromPointCloud(pointCloud, angularResolution, maxAngleWidth, maxAngleHeight,
sensorPose, coordinate_frame, noiseLevel, minRange, borderSize)

 参数:

  • pointCloud:被检测点云
  • angularResolution=1:邻近的像素点所对应的每个光束之间相差 1°
  • maxAngleWidth=360:进行模拟的距离传感器对周围的环境拥有一个完整的360°视角,无论任何数据集都推荐使用此设置,因为最终获取的深度图像将被裁剪到有空间物体存在的区域范围
  • maxAngleHeight=180: 当传感器后面没有可以观测的点时,设置一个水平视角为180°的激光扫描仪即可,因为需要观察距离传感器前面就可以了。
  • sensorPose: 定义了模拟深度图像获取传感器的6DOF(6自由度)位置,其原始值为横滚角roll、俯仰角 pitch、偏航角 yaw 都为 0
  • coordinate_frame: 设置为CAMERA_FRAME说明系统的X轴是向右的、Y轴是向下的、Z轴是向前的,另外参数值是LASER_FRAME,其X轴向前、Y轴向左、Z轴向上
  • noiseLevel=0: 是指使用一个归一化的Z缓存区来创建深度图像,如果想让邻近点集都落在同一个像素单元,可以设置一个较高的值,例如 noiseLevel = 0.05 可以理解为深度距离值是通过查询点半径为 5cm 的圆内包含的点用来平均计算而得到的 
  • minRange=0:如果设置>0则所有模拟器所在位置半径 minRange 内的邻近点都将被忽略,即为盲区
  • borderSize=1:如果设置>0,在裁剪图像时,将在图像周围留下当前视点不可见点的边界 

代码实现 

#include <pcl/range_image/range_image.h>    //深度图像的头文件
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/visualization/range_image_visualizer.h>

int main(int argc, char** argv) 
{
    pcl::PointCloud<pcl::PointXYZ>::Ptr pointCloudPtr(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>& pointCloud = *pointCloudPtr;

    // 循环产生点云的数据
    for (float y = -0.5f; y <= 0.5f; y += 0.01f) {
        for (float z = -0.5f; z <= 0.5f; z += 0.01f) {
            pcl::PointXYZ point;
            point.x = 2.0f - y;
            point.y = y;
            point.z = z;
            pointCloud.points.push_back(point); //循环添加点数据到点云对象
        }
    }
    pointCloud.width = (uint32_t)pointCloud.points.size();
    pointCloud.height = 1; //设置点云对象的头信息
    //根据之前得到的点云图,通过1deg的分辨率生成深度图
    //angular_resolution为模拟的深度传感器的角度分辨率,即深度图像中一个像素对应的角度大小
    float angularResolution = (float)(1.0f * (M_PI / 180.0f));  // 弧度1°
    //max_angle_width为模拟的深度传感器的水平最大采样角度
    float maxAngleWidth = (float)(360.0f * (M_PI / 180.0f));  // 弧度360°
    //max_angle_height为模拟传感器的垂直方向最大采样角度,都转为弧度
    float maxAngleHeight = (float)(180.0f * (M_PI / 180.0f));  // 弧度180°
    //传感器的采集位置
    Eigen::Affine3f sensorPose = (Eigen::Affine3f)Eigen::Translation3f(0.0f, 0.0f, 0.0f);
    //深度图像遵循坐标系统
    pcl::RangeImage::CoordinateFrame coordinate_frame = pcl::RangeImage::CAMERA_FRAME;
    float noiseLevel = 0.00;    //noise_level获取深度图像深度时,近邻点对查询点距离值的影响水平
    float minRange = 0.0f;    //min_range设置最小的获取距离,小于最小获取距离的位置为传感器的盲区
    int borderSize = 1;     //border_size获得深度图像的边缘的宽度

    boost::shared_ptr<pcl::RangeImage> range_image_ptr(new pcl::RangeImage);
    pcl::RangeImage& rangeImage = *range_image_ptr;
    rangeImage.createFromPointCloud(pointCloud, angularResolution, maxAngleWidth, maxAngleHeight,sensorPose, coordinate_frame, noiseLevel, minRange, borderSize);
    std::cout << rangeImage << "\n";

    // --------------------------------------------
    // ----------------显示3D点云------------------
    // --------------------------------------------
    pcl::visualization::PCLVisualizer viewer("3D Viewer");
    viewer.setBackgroundColor(1, 1, 1);
    // 添加原始点云
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> org_image_color_handler(pointCloudPtr, 255, 100, 0);
    viewer.addPointCloud(pointCloudPtr, org_image_color_handler, "orginal image");
    viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "orginal image");
    viewer.initCameraParameters();
    viewer.addCoordinateSystem(1.0);

    // --------------------------
    // -----显示深度图-----
    // --------------------------
    //用以图像的方式可视化深度图像,图像的颜色取决于深度值
    pcl::visualization::RangeImageVisualizer range_image_widget("Range image");
    range_image_widget.showRangeImage(rangeImage);      //图像可视化方式显示深度图像
    range_image_widget.setSize(800, 600);

    while (!viewer.wasStopped())
    {
        viewer.spinOnce();
    }
    return (0);
}

运行结果:

header:
seq: 0 stamp: 0 frame_id:
points[]: 1360
width: 40
height: 34
sensor_origin_: 0 0 0
sensor_orientation_: 0 0 0 1
is_dense: 0
angular resolution: 1deg/pixel in x and 1deg/pixel in y.

 效果:

从深度图中提取边界 

        从深度图像中提取边界(从前景跨越到背景的位置定义为边界)。包括对象边界(obstacle border):这是物体的最外层和阴影边界的可见点集;阴影边界(shadow border):在背景中与遮挡物相邻的点;面纱点集(Veil points):对象边界与阴影边界之间的内插点。以下是一个典型的激光雷达获得的3D数据对应的点云图:

 代码实现

#include <iostream>
#include <boost/thread/thread.hpp>
#include <pcl/range_image/range_image.h>
#include <pcl/io/pcd_io.h>
#include <pcl/visualization/range_image_visualizer.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/features/range_image_border_extractor.h>
#include <pcl/console/parse.h>
#include <pcl/common/file_io.h>

typedef pcl::PointXYZ PointType;

// --------------------
// --------参数--------
// --------------------
float angular_resolution = 0.5f;
pcl::RangeImage::CoordinateFrame coordinate_frame = pcl::RangeImage::LASER_FRAME;
bool setUnseenToMaxRange = false;

void printUsage(const char* progName)
{
    std::cout << "\n\nUsage: " << progName << " [options] <scene.pcd>\n\n"
        << "Options:\n"
        << "-------------------------------------------\n"
        << "-r <float>   angular resolution in degrees (default " << angular_resolution << ")\n"
        << "-c <int>     coordinate frame (default " << (int)coordinate_frame << ")\n"
        << "-m           Treat all unseen points to max range\n"
        << "-h           this help\n"
        << "\n\n";
}

int main(int argc, char** argv)
{
    std::string select = "";
    std::cout << "please input '-h' or '-m':";
    std::cin >> select;
    // --------------------------------------
    // --------------命令行参数--------------
    // --------------------------------------
    if (select == "-h")
    {
        printUsage(argv[0]);
        return 0;
    }
    if (select == "-m")
    {
        setUnseenToMaxRange = true;
        cout << "Setting unseen values in range image to maximum range readings.\n";
    }
    int tmp_coordinate_frame;
    if (pcl::console::parse(argc, argv, "-c", tmp_coordinate_frame) >= 0)
    {
        coordinate_frame = pcl::RangeImage::CoordinateFrame(tmp_coordinate_frame);
        cout << "Using coordinate frame " << (int)coordinate_frame << ".\n";
    }
    if (pcl::console::parse(argc, argv, "-r", angular_resolution) >= 0)
        cout << "Setting angular resolution to " << angular_resolution << "deg.\n";
    angular_resolution = pcl::deg2rad(angular_resolution);

    // ------------------------------------------------------------------
    // -------------读取pcd文件或创建示例点云(如果未给出)--------------
    // ------------------------------------------------------------------
    pcl::PointCloud<PointType>::Ptr point_cloud_ptr(new pcl::PointCloud<PointType>);
    pcl::PointCloud<PointType>& point_cloud = *point_cloud_ptr;
    pcl::PointCloud<pcl::PointWithViewpoint> far_ranges;
    Eigen::Affine3f scene_sensor_pose(Eigen::Affine3f::Identity());
    std::vector<int> pcd_filename_indices = pcl::console::parse_file_extension_argument(argc, argv, "pcd");
    if (!pcd_filename_indices.empty())
    {
        std::string filename = argv[pcd_filename_indices[0]];
        if (pcl::io::loadPCDFile(filename, point_cloud) == -1)
        {
            cout << "Was not able to open file \"" << filename << "\".\n";
            printUsage(argv[0]);
            return 0;
        }
        scene_sensor_pose = Eigen::Affine3f(Eigen::Translation3f(point_cloud.sensor_origin_[0],
            point_cloud.sensor_origin_[1],
            point_cloud.sensor_origin_[2])) *
            Eigen::Affine3f(point_cloud.sensor_orientation_);

        std::string far_ranges_filename = pcl::getFilenameWithoutExtension(filename) + "_far_ranges.pcd";
        if (pcl::io::loadPCDFile(far_ranges_filename.c_str(), far_ranges) == -1)
            std::cout << "Far ranges file \"" << far_ranges_filename << "\" does not exists.\n";
    }
    else
    {
        //cout << "\nNo *.pcd file given => Generating example point cloud.\n\n";
        //for (float x = -0.5f; x <= 0.5f; x += 0.01f)
        //{
        //    for (float y = -0.5f; y <= 0.5f; y += 0.01f)
        //    {
        //        PointType point;  point.x = x;  point.y = y;  point.z = 2.0f - y;
        //        point_cloud.points.push_back(point);
        //    }
        //}
        //point_cloud.width = (int)point_cloud.points.size();  point_cloud.height = 1;
        std::string filename = "G:/vsdata/PCLlearn/PCDdata/table_scene_lms400_downsampled.pcd";
        if (pcl::io::loadPCDFile(filename, point_cloud) == -1)
        {
            cout << "Was not able to open file \"" << filename << "\".\n";
            printUsage(argv[0]);
            return 0;
        }
        scene_sensor_pose = Eigen::Affine3f(Eigen::Translation3f(point_cloud.sensor_origin_[0],
            point_cloud.sensor_origin_[1],
            point_cloud.sensor_origin_[2])) *
            Eigen::Affine3f(point_cloud.sensor_orientation_);

        std::string far_ranges_filename = pcl::getFilenameWithoutExtension(filename) + "_far_ranges.pcd";
        if (pcl::io::loadPCDFile(far_ranges_filename.c_str(), far_ranges) == -1)
            std::cout << "Far ranges file \"" << far_ranges_filename << "\" does not exists.\n";
    }

    // -----------------------------------------------
    // ---------------从点云创建深度图----------------
    // -----------------------------------------------
    float noise_level = 0.0;
    float min_range = 0.0f;
    int border_size = 1;
    boost::shared_ptr<pcl::RangeImage> range_image_ptr(new pcl::RangeImage);
    pcl::RangeImage& range_image = *range_image_ptr;
    range_image.createFromPointCloud(point_cloud, angular_resolution, pcl::deg2rad(360.0f), pcl::deg2rad(180.0f),
        scene_sensor_pose, coordinate_frame, noise_level, min_range, border_size);
    range_image.integrateFarRanges(far_ranges);
    if (setUnseenToMaxRange)
        range_image.setUnseenToMaxRange();

    // --------------------------------------------
    // -----------打开3D查看器并添加点云-----------
    // --------------------------------------------
    pcl::visualization::PCLVisualizer viewer("3D Viewer");
    viewer.setBackgroundColor(1, 1, 1);
    viewer.addCoordinateSystem(1.0f, "global");
    pcl::visualization::PointCloudColorHandlerCustom<PointType> point_cloud_color_handler(point_cloud_ptr, 0, 0, 0);
    viewer.addPointCloud(point_cloud_ptr, point_cloud_color_handler, "original point cloud");

    // -------------------------
    // --------提取边界---------
    // -------------------------
    pcl::RangeImageBorderExtractor border_extractor(&range_image);
    pcl::PointCloud<pcl::BorderDescription> border_descriptions;
    border_extractor.compute(border_descriptions);

    // ----------------------------------
    // --------在3D查看器中显示点--------
    // ----------------------------------
    pcl::PointCloud<pcl::PointWithRange>::Ptr border_points_ptr(new pcl::PointCloud<pcl::PointWithRange>),
        veil_points_ptr(new pcl::PointCloud<pcl::PointWithRange>),
        shadow_points_ptr(new pcl::PointCloud<pcl::PointWithRange>);
    pcl::PointCloud<pcl::PointWithRange>& border_points = *border_points_ptr,
        & veil_points = *veil_points_ptr,
        & shadow_points = *shadow_points_ptr;
    for (int y = 0; y < (int)range_image.height; ++y)
    {
        for (int x = 0; x < (int)range_image.width; ++x)
        {
            if (border_descriptions.points[y * range_image.width + x].traits[pcl::BORDER_TRAIT__OBSTACLE_BORDER])
                border_points.points.push_back(range_image.points[y * range_image.width + x]);
            if (border_descriptions.points[y * range_image.width + x].traits[pcl::BORDER_TRAIT__VEIL_POINT])
                veil_points.points.push_back(range_image.points[y * range_image.width + x]);
            if (border_descriptions.points[y * range_image.width + x].traits[pcl::BORDER_TRAIT__SHADOW_BORDER])
                shadow_points.points.push_back(range_image.points[y * range_image.width + x]);
        }
    }
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointWithRange> border_points_color_handler(border_points_ptr, 0, 255, 0);
    viewer.addPointCloud<pcl::PointWithRange>(border_points_ptr, border_points_color_handler, "border points");
    viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 7, "border points");
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointWithRange> veil_points_color_handler(veil_points_ptr, 255, 0, 0);
    viewer.addPointCloud<pcl::PointWithRange>(veil_points_ptr, veil_points_color_handler, "veil points");
    viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 7, "veil points");
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointWithRange> shadow_points_color_handler(shadow_points_ptr, 0, 255, 255);
    viewer.addPointCloud<pcl::PointWithRange>(shadow_points_ptr, shadow_points_color_handler, "shadow points");
    viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 7, "shadow points");

    //-------------------------------------
    //-----------在深度图上显示点----------
    //-------------------------------------
    pcl::visualization::RangeImageVisualizer* range_image_borders_widget = NULL;
    range_image_borders_widget =
        pcl::visualization::RangeImageVisualizer::getRangeImageBordersWidget(range_image, -std::numeric_limits<float>::infinity(), std::numeric_limits<float>::infinity(), false,
            border_descriptions, "Range image with borders");

    //--------------------
    //-------主循环-------
    //--------------------
    while (!viewer.wasStopped())
    {
        range_image_borders_widget->spinOnce();
        viewer.spinOnce();
        pcl_sleep(0.01);
    }
}

        代码解析:从磁盘中读取点云,代码实现创建深度图像并使其可视化。要提取边界信息,重要的是要区分未观察到的图像点应该观察到但超出传感器范围的点。后者通常用来标记边界,而未观察到的点通常不标记边界。因此,最好可以提供这些测量信息。如果无法提供超出这些应该观察到的传感器范围的点,则可以使用setUnseenToMaxRange函数,将那些点设置为最大深度(本例添加-m参数)。

效果:

  • 0
    点赞
  • 0
    收藏
    觉得还不错? 一键收藏
  • 打赏
    打赏
  • 0
    评论

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包

打赏作者

小镇种田家

你的鼓励将是我创作的最大动力

¥1 ¥2 ¥4 ¥6 ¥10 ¥20
扫码支付:¥1
获取中
扫码支付

您的余额不足,请更换扫码支付或充值

打赏作者

实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值