简介
PCL全称Point Cloud Library,官方网站为(https://pointclouds.org),是一个专门用于点云处理的C++开源库。其功能包括了点云数据的基本操作,如保存、读取、显示;包括了点云的进阶操作,如空间刚体变换(旋转、平移)、仿射变换(旋转、平移、缩放)等;包括了点云的更进阶操作,如ICP(Iterative Closest Point)点云配准、3D特征描述子的计算、3D物体识别等。
本工作的背景是人体躯干姿态识别,采用ICP(Iterative Closest Point)方法,根据人体躯干模板,对深度相机所拍摄的人体躯干进行位置和姿态估计。本工作对PCL的使用包括了人体躯干模板的导入、场景点云导入、机器人行走路径导入、ICP方法、点云显示。
PCL的模型导入方法
ply格式导入人体躯干模板
人体躯干模板来自于免费的人体全身模型,在ICP开始之前,首先采用Blender进行处理和导出,导出的文件格式为ply格式。对应ply格式的人体躯干模板,工作中调用PCL的ply文件导入函数,函数名为pcl::io::loadPLYFile
,具体代码如下,
typedef pcl::PointXYZ PointT;
typedef pcl::PointCloud<PointT> PointCloudT;
PointCloudT::Ptr cloud_icp (new PointCloudT); // ICP output point cloud
//
// load and get the model
//
if (pcl::io::loadPLYFile (argv[1], *cloud_icp) < 0)
{
PCL_ERROR ("Error loading cloud %s.\n", argv[1]);
return (-1);
}
cout<<"Model Point cloud size : "<<cloud_icp->points.size()<<endl;
ply格式导入机器人行走路径
机器人行走路径是本工作的关注对象,本工作首先在人体躯干模板中制作这个路径,再根据模板到场景人体的空间刚体变换,计算得到场景人体的行走路径。在上文“行走路径操作”的第一步中,人体躯干模板也在Blender中制作,以ply格式导出,供本工作计算使用,而ply文件的导入方式与人体躯干模板的导入相同,也通过调用PCL的ply文件导入函数完成,具体函数如下,
obj格式导入场景点云
场景点云的原始数据为ply格式,但在本工作中,需要进行一步点云修剪、空间变换的操作,考虑场景点云只由点组成,没有面,被Blender按照ply格式导出后,ply文件中不会存在任何点的数据,因此不能采用ply格式。本工作采用obj格式导出场景点云,对应的,需要调用PCL的obj文件导入函数,函数名为pcl::io::loadPolygonFileOBJ
,具体代码如下,
typedef pcl::PointXYZ PointT;
typedef pcl::PointCloud<PointT> PointCloudT;
PointCloudT::Ptr cloud_in (new PointCloudT); // Original point cloud
//
// load and get the scene, which is the target
//
// Load obj file
pcl::PolygonMesh mesh;
if (pcl::io::loadPolygonFileOBJ (argv[2], mesh) < 0)
{
PCL_ERROR ("Error loading cloud %s.\n", argv[2]);
return (-1);
}
// Transform the mesh to point cloud
pcl::fromPCLPointCloud2(mesh.cloud, *cloud_in);
cout<<"Scene Point cloud size : "<<cloud_in->points.size()<<endl;
obj格式文件的导入方法包含两个步骤:首先以pcl::PolygonMesh
的数据类型保存obj文件内的点云数据,然后将pcl::PolygonMesh
的对象转换为pcl::PointCloud<pcl::PointXYZ>::Ptr
数据格式。
源代码
#include <iostream>
#include <string>
#include <random>
#include <ctime>
#include <pcl/io/obj_io.h>
#include <pcl/io/ply_io.h>
#include <pcl/io/vtk_lib_io.h>
#include <pcl/point_types.h>
#include <pcl/registration/icp.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/console/time.h> // TicToc
#include <pcl/filters/voxel_grid.h> // sparse point cloud
using namespace std;
using namespace Eigen;
typedef pcl::PointXYZ PointT;
typedef pcl::PointCloud<PointT> PointCloudT;
bool next_iteration = false;
void
print4x4Matrix (const Eigen::Matrix4d & matrix)
{
printf ("Rotation matrix :\n");
printf (" | %6.3f %6.3f %6.3f | \n", matrix (0, 0), matrix (0, 1), matrix (0, 2));
printf ("R = | %6.3f %6.3f %6.3f | \n", matrix (1, 0), matrix (1, 1), matrix (1, 2));
printf (" | %6.3f %6.3f %6.3f | \n", matrix (2, 0), matrix (2, 1), matrix (2, 2));
printf ("Translation vector :\n");
printf ("t = < %6.3f, %6.3f, %6.3f >\n\n", matrix (0, 3), matrix (1, 3), matrix (2, 3));
}
void
keyboardEventOccurred (const pcl::visualization::KeyboardEvent& event,
void* nothing