PCL点云与深度图像
PCL点云与深度图像
- 1 RangeImage概念及相关算法
-
- 1.1 深度图像简介
- 1.2 PCL中RangeImage的相关类
- 2 从一个点云创建一个深度图像
- 3 从深度图像中提取边界
- 4 点云到深度图的变换与曲面重建
目前，深度图像的获取方法有：激光雷达深度成像法、计算机立体视觉成像、坐标测量机法、莫尔条纹法、结构光法等。针对深度图像的研究重点主要集中在以下几个方面：深度图像的分割技术，深度图像的边缘检测技术，基于不同视点的多幅深度图像的配准技术，基于深度数据的三维重建技术，基于深度图像的三维目标检测技术，深度数据的多分辨率建模和几何压缩技术等。在PCL中深度图像与点云最主要的区别在于，其近邻的检索方式不同，并且可以互相转换。
本文首先对深度图像的概念以及表示方法进行简介，其次对PCL的两个RangeImage类进行简单介绍，最后通过如何从一个点云创建一个深度图像以及如何从深度图像中提取物体边界的两个应用实例，来展示如何对PCL中的RangeImage相关类进行灵活运用。
1 RangeImage概念及相关算法
1.1 深度图像简介
深度图像（Depth Images），也被称为距离影像（Range Images），是指将图像采集器到场景中各点的距离（深度）值作为像素值的图像，它直接反映了景物可见表面的几何形状，利用它可以很方便的解决3D目标描述中的许多问题。深度图像经过坐标转换可以计算为点云数据，有规则及必要信息的点云数据也可以反算为深度图像数据。
从数学模型上看，深度图像可以看作是标量函数 j : I 2 → R j:I^{2}\rightarrow R j:I2→R 在集合 K K K 上的离散采样，得到 r i = j ( u i ) r_{i}=j(u_{i}) ri=j(ui)， 其中 u i ϵ I 2 u_{i} \epsilon I^{2} uiϵI2 为二维网格（矩阵）的索引， r i ϵ R r_{i}\epsilon R riϵR， i = 1 , 2 , . . . , k i = {1,2,...,k} i=1,2,...,k，如下图所示：
1.2 PCL中RangeImage的相关类
PCL中的range_image库包含两个表达深度图像和对深度图像进行操作的类，其依赖于pcl::common模块。深度图像（或距离图像）的像素值代表从传感器到物体的距离或者深度，如下图所示。深度图像是物体三维表示形式，一般通过立体相机或者ToF相机获取。如果知道相机的内标定参数，就可以将深度图像转化为点云。
2 从一个点云创建一个深度图像
本小节将学习如何从点云和给定的传感器位置来创建深度图像。
首先创建一个工作空间range_image_creation，然后再在工作空间创建一个文件夹src用于存放源代码：
mkdir -p range_image_creation/src
接着，在range_image_creation/src路径下，创建一个文件并命名为range_image_creation.cpp，拷贝如下代码：
#include 【解释说明】
/* 生成点云数据 */
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; // 设置点云对象的头信息
这段程序首先创建一组数据作为点云的数据内容，再设置点云头的信息，整个实现生成一个呈现矩形形状的点云，如下图所示：
float angularResolution = (float) ( 1.0f * (M_PI/180.0f)); // 按弧度1°
float maxAngleWidth = (float) (360.0f * (M_PI/180.0f)); // 按弧度360°
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;
float minRange = 0.0f;
int borderSize = 1;
这部分定义了创建深度图像时需要的设置参数，将角度分辨率angularResolution定义为1°，意味着邻近的像素点所对应的每个光束之间相差1°；maxAngleWidth = 360 和 maxAngleHeight = 180 意味着，我们进行模拟的距离传感器对周围的环境拥有一个完整的360°视角，用户在任意数据集下都可以使用此设置，因为最终获取的深度图像将被裁剪到有空间物体存在的区域范围。但是，用户可以通过减小数值来节省一些计算资源，例如：当传感器后面没有可以观测的点时，一个水平视角为180°的激光扫描仪，即maxAngleWidth = 180 就足够了，这样只需要观察距离传感器前面就可以了，因为后面没有需要观察的场景。 sensorPose定义了模拟深度图像获取传感器的6自由度位置，其原始值横滚角roll、俯仰角pitch、偏航角yaw都为0。coordinate_frame = pcl::RangeImage::CAMERA_FRAME说明系统的X轴是向右的，Y轴是向下的，Z轴是向前的。另外一个选择是LASER_FRAME，其X轴向前，Y轴向左，Z轴向上。noiseLevel = 0是指使用一个归一化的Z缓冲器来创建深度图像，但是如果想让邻近点集都落在同一像素单元，用户可以设置一个较高的值，例如noiseLevel = 0.05可以理解为，深度距离值是通过查询点半径为5cm的圆内包含的点以平均计算而得到的。如果minRange > 0，则所有模拟器所在的位置半径minRange内的邻近点都将被忽略，即为盲区。在裁剪图像时，如果borderSize > 0，将在图像周围留下当前视点不可见点的边界。
pcl::RangeImage rangeImage;
rangeImage.createFromPointCloud(pointCloud,
angularResolution,
maxAngleWidth,
maxAngleHeight,
sensorPose,
coordinate_frame,
noiseLevel,
minRange,
borderSize);
std::cout << rangeImage << "\n";
最后，使用上述设定的参数，从点云创建深度图像，并且打印一些信息。
深度图像继承于PointCloud类，它的点类型具有x，y，z和range距离字段，共有三种类型的点集，有效点集是距离大于0的点集，当前视点不可见点集x = y = z = NAN且值域为负无穷大，远距离点集x = y = z = NAN且值域为无穷大。
【编译和运行程序】
在工作空间根目录range_image_creation下，编写CMakeLists.txt文件如下：
cmake_minimum_required(VERSION 2.6 FATAL_ERROR)
project(range_image_creation)
find_package(PCL 1.2 REQUIRED)
include_directories(${
PCL_INCLUDE_DIRS})
link_directories(${
PCL_LIBRARY_DIRS})
add_definitions(${
PCL_DEFINITIONS})
add_executable (${
PROJECT_NAME}_node src/range_image_creation.cpp)
target_link_libraries (${
PROJECT_NAME}_node ${
PCL_LIBRARIES})
在工作空间根目录range_image_creation下创建一个build文件夹，用于存放编译过程中产生的文件，然后执行编译：
mkdir build
cd build
cmake ..
make
此时，会在build文件夹下生成一个可执行文件range_image_creation_node，运行该可执行文件：
./range_image_creation_node
在终端中可以看到如下输出结果：
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.
3 从深度图像中提取边界
本小节将学习如何从深度图像中提取边界（从前景跨越到背景的位置定义为边界）。一般，我们只对三种类型的点集感兴趣：物体边界，这是物体最外层和阴影边界的可见点集；阴影边界：毗连于遮挡的背景上的点集；Veil点集，在被遮挡物边界和阴影边界之间的内插点，它们是由激光雷达获取的3D距离数据中的典型数据类型。这三类数据及深度图像的边界如下图所示：
首先创建一个工作空间range_image_border_extraction，然后再在工作空间创建一个文件夹src用于存放源代码：
mkdir -p range_image_border_extraction/src
接着，在range_image_border_extraction/src路径下，创建一个文件并命名为range_image_border_extraction.cpp，拷贝如下代码：
#include \n\n"
<< "Options:\n"
<< "-------------------------------------------\n"
<< "-r angular resolution in degrees (default " <<angular_resolution<<")\n"
<< "-c 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)
{
/* 解析命令行参数 */
if (pcl::console::find_argument (argc, argv, "-h") >= 0)
{
printUsage (argv[0]);
return 0;
}
if (pcl::console::find_argument (argc, argv, "-m") >= 0)
{
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文件，如果没有给出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 => Genarating 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;
}
/* 从点云创建深度图像 */
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 ();
/* 打开可视化窗口并添加点云 */
pcl::visualization::PCLVisualizer viewer ("3D Viewer");
viewer.setBackgroundColor (1, 1, 1);
viewer.addCoordinateSystem (1.0f);
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");
//PointCloudColorHandlerCustom range_image_color_handler (range_image_ptr, 150, 150, 150);
//viewer.addPointCloud (range_image_ptr, range_image_color_handler, "range image");
//viewer.setPointCloudRenderingProperties (PCL_VISUALIZER_POINT_SIZE, 2, "range image");
/* 提取边界 */
pcl::RangeImageBorderExtractor border_extractor (&range_image);
pcl::PointCloud<pcl::BorderDescription> border_descriptions;
border_extractor.compute (border_descriptions);
/* 在可视化窗口中显示点集*/
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);
}
}
【解释说明】
首先，我们进行命令行解析，然后，读取点云（或者如果没有提供，则创建一个点云），最后，创建深度图像并使其可视化。提取边界信息时很重要的一点是区分深度图像中的当前视点不可见点集合和应该可见但处于传感器获取距离范围外的点集，后者可以标记为典型边界，然而当前视点不可见点则不能成为边界。因此，如果后者的测量值存在，则提供那些超出传感器距离获取范围外的数据对于边界提取是非常有用的，我们希望找到另外的包含这些值的pcd文件，这里代码中用far_range.pcd作为这类数据的实例。
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";
}
使用以下代码之后将它们并入到深度图像中：
range_image.integrateFarRanges (far_ranges);
如果用户没有这些远距离的点云，则可采用命令行参数-m，这样设置所有不能观察到的点都为远距离的，以下代码来执行这一步：
if (setUnseenToMaxRange)
range_image.setUnseenToMaxRange ();
接下来是与实际边界抽取相关的这一部分：
pcl::RangeImageBorderExtractor border_extractor (&range_image);
pcl::PointCloud<pcl::BorderDescription> border_descriptions;
border_extractor.compute (border_descriptions);
此部分创建了RangeImageBorderExtractor这个对象，给了它深度图像，并且计算后存储边界信息在border_descriptions中（详见BorderDescription结构中的common/include/pcl/point_types.h），余的代码只是用来可视化的。
【编译和运行程序】
在工作空间根目录range_image_border_extraction下，编写CMakeLists.txt文件如下：
cmake_minimum_required(VERSION 2.6 FATAL_ERROR)
project(range_image_border_extraction)
find_package(PCL 1.3 REQUIRED)
include_directories(${
PCL_INCLUDE_DIRS})
link_directories(${
PCL_LIBRARY_DIRS})
add_definitions(${
PCL_DEFINITIONS})
add_executable (${
PROJECT_NAME}_node src/range_image_border_extraction.cpp)
target_link_libraries (${
PROJECT_NAME}_node ${
PCL_LIBRARIES})
在工作空间根目录range_image_border_extraction下创建一个build文件夹，用于存放编译过程中产生的文件，然后执行编译：
mkdir build
cd build
cmake ..
make
此时，会在build文件夹下生成一个可执行文件range_image_border_extraction_node，运行该可执行文件：
./range_image_border_extraction_node -m
这将使用一个自动生成的、矩形状浮点型点云，运行结果如下图所示，检测出的边缘点用绿色较大的点表示，其他点用默认的黑色普通大小点表示：
4 点云到深度图的变换与曲面重建
有时我们获取的点云数据是从一个视点获取的，为了使用深度图相关的计算方法，以提高效率等，我们需要将点云数据转化为深度图。这两种数据的主要区别在于，点云数据需要通过k-d tree等索引来对数据进行检索，而深度图和图像类似，可以通过上下左右等近邻来直接进行索引。下面将介绍如何将点云数据转化为深度图像，进而使用PCL内部只适用于深度图的算法来进行曲面重建等。
首先创建一个工作空间greedy_projection，然后再在工作空间创建一个文件夹src用于存放源代码：
mkdir -p greedy_projection/src
接着，在greedy_projection/src路径下，创建一个文件并命名为greedy_projection.cpp，拷贝如下代码：
#include 【解释说明】
首先声明了RangeImage结构和RangeImagePlanar等类对应的头文件：
#include 随后，我们首先定义从点云变换到深度图像时所需要的变量，并让用户可以通过命令行参数进行设置，这里包含深度图像的宽度、高度，光轴在深度图像上的坐标点，成像的焦距等，其他参数是后面进行曲面重建时候的参数。在实际使用时，用户需要根据自己的点云数据来源来确定这些参数，本文给出的参数是根据Xtion Pro live深度相机参数确定的。
int width=640,height=480,size=2,type=0;
float fx=525,fy=525,cx=320,cy=240;
parse_argument (argc, argv, "-w", width); // 深度图像宽度
parse_argument (argc, argv, "-h", height); // 深度图像高度
parse_argument (argc, argv, "-cx", cx); // 光轴在深度图像上的x坐标
parse_argument (argc, argv, "-cy", cy); // 光轴在深度图像上的y坐标
parse_argument (argc, argv, "-fx", fx); // 水平方向焦距
parse_argument (argc, argv, "-fy", fy); // 垂直方向焦距
parse_argument (argc, argv, "-type", type); // 曲面重建时三角化的方式
parse_argument (argc, argv, "-size", size); // 曲面重建时的面片的大小
下面是对原始点云进行加载，同时创建了RangeImagePlanar对象，利用该对象的函数createFromPointCloudWithFixedSize()进行深度图像的生成。这里的参数除了上面用户提供的参数外，需要提供相机成像时相机的位姿，以及成像时遵循的坐标系统。
/* 读取原始点云 */
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::io::loadPCDFile ("../pcd/1.pcd", *cloud);
print_info ("Read pcd file successfully\n");
/* 原始点云数据可视化*/
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer1 (new pcl::visualization::PCLVisualizer ("original point cloud"));
viewer1->setBackgroundColor (0.5,0.5,0.5);
viewer1->addCoordinateSystem ();
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZRGB> point_cloud_color_handler (cloud, 0, 0, 0);
viewer1->addPointCloud (cloud, point_cloud_color_handler, "original point cloud");
/* 点云数据转化为深度图像 */
Eigen::Affine3f sensorPose;
sensorPose.setIdentity();
pcl::RangeImage::CoordinateFrame coordinate_frame = pcl::RangeImage::CAMERA_FRAME;
float noiseLevel=0.00; // 成像时模拟噪声的水平
float minRange = 0.0f; // 成像时考虑该阈值外的点
pcl::RangeImagePlanar::Ptr rangeImage(new pcl::RangeImagePlanar);
rangeImage->createFromPointCloudWithFixedSize(*cloud,width,height,cx,cy,fx,fy,sensorPose,coordinate_frame);
std::cout << rangeImage << "\n";
下面使用RangeImageVisualizer 对象来对创建的深度图像进行可视化。
pcl::visualization::RangeImageVisualizer range_image_widget ("RangeImage");
range_image_widget.showRangeImage (*rangeImage);
完成从点云到深度图像的生成后，利用该深度图像作为输入，来使用曲面重建模块中的简单三角化类生成曲面模型。创建OrganizedFastMesh对象，该算法的输入参数有size，通过setTrianglePixelSize()函数接口来实现，其控制重建曲面的精细程度。另外一个参数是setTriangulationType()方法设置的三角化类型，是个枚举变量，包含三角形、四边形等。
pcl::OrganizedFastMesh<pcl::PointWithRange>::Ptr tri(new pcl::OrganizedFastMesh<pcl::PointWithRange>);
pcl::search::KdTree<pcl::PointWithRange>::Ptr tree (new pcl::search::KdTree<pcl::PointWithRange>);
tree->setInputCloud(rangeImage);
pcl::PolygonMesh triangles;
tri->setTrianglePixelSize(size);
tri->setInputCloud(rangeImage);
tri->setSearchMethod(tree);
tri->setTriangulationType((pcl::OrganizedFastMesh<pcl::PointWithRange>::TriangulationType)type);
tri->reconstruct(triangles);
最后，对重建结果进行可视化。
【编译和运行程序】
在工作空间根目录greedy_projection下，编写CMakeLists.txt文件如下：
cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
project(greedy_projection)
find_package(PCL 1.2 REQUIRED)
include_directories(${
PCL_INCLUDE_DIRS})
link_directories(${
PCL_LIBRARY_DIRS})
add_definitions(${
PCL_DEFINITIONS})
add_executable (${
PROJECT_NAME}_node src/greedy_projection.cpp)
target_link_libraries (${
PROJECT_NAME}_node ${
PCL_LIBRARIES})
在工作空间根目录greedy_projection下创建一个build文件夹，用于存放编译过程中产生的文件，然后执行编译：
mkdir build
cd build
cmake ..
make
此时，会在build文件夹下生成一个可执行文件greedy_projection_node，运行该可执行文件：
./greedy_projection_node -size 5
此时会看到类似下面三幅图的结果，其中第一幅图为原始点云数据的可视化结果，第二幅图为深度图像可视化结果，第三幅图为曲面重建结果。
