下面代码的功能是：把一个文件夹中所有的pcd文件进行ICP点云配准，并且把每帧结果使用PCL的cloud_viewer进行显示。因为是在ROS下使用，所以还有一个ROS的发布操作(可忽略)。
源码如下：
#include <iostream>
#include <fstream>
#include <sys/types.h>
#include <dirent.h>
#include <vector>
#include <cstring>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/registration/icp.h>
#include <ros/ros.h>
#include <pcl/visualization/cloud_viewer.h>
#include "position/Position.h"
using namespace std;
#define NUMBER 50000
#define ITERATIONS 5
typedef pcl::PointXYZ PointT;
typedef pcl::PointCloud<PointT> PointCloudT;
// function：获取路径下所有文件名，存在filenames中
void getFiles(string path, vector<string>& filenames)
{
DIR *pDir;
struct dirent* ptr;
if(!(pDir = opendir(path.c_str()))){
cout<<"Folder doesn't Exist!"<<endl;
return;
}
while((ptr = readdir(pDir))!=0) {
if (strcmp(ptr->d_name, ".") != 0 && strcmp(ptr->d_name, "..") != 0){
filenames.push_back(path + "/" + ptr->d_name);
}
}
closedir(pDir);
}
// function：读取整个文件夹下pcd文件，通过PCL将其中的坐标存储在position_x和position_y中
void Get_position_data(float *position_x, float *position_y, int &position_num,int &file_count) {
string filePath = "/home/lyn/pcd"; // 待读取文件夹路径
string pwd = get_current_dir_name();
string logPath = pwd + "/pcd.log";
vector<string> files;
//获取该路径下的所有文件
getFiles(filePath, files);
float bckgr_gray_level = 0.0; // Black
float txt_gray_lvl = 1.0 - bckgr_gray_level;
PointCloudT::Ptr first_cloud (new PointCloudT);
PointCloudT::Ptr second_cloud (new PointCloudT);
PointCloudT::Ptr cloud_icp(new PointCloudT);
pcl::visualization::PCLVisualizer viewer("viewer");
fstream log;
log.open(logPath,ios::app);
if(log.fail())
{
cout<<"open fail"<<endl;
}
else{
log<<"file size is "<<(int)files.size()<<endl;
log<<"begin reading,file count is "<<file_count<<endl;
}
if(file_count==(int)files.size())
{
log<<"stop reading"<<endl;
return;
}
if(pcl::io::loadPCDFile<PointT>(files[file_count],*first_cloud)==-1)
{
PCL_ERROR("Couldn't read file test_pcd.pcd\n");
return ;
}
else{
log<<"file 0 is "<<files[file_count]<<endl;
}
if(pcl::io::loadPCDFile<PointT>(files[file_count+1],*second_cloud)==-1)
{
PCL_ERROR("Couldn't read file test_pcd.pcd\n");
return ;
}
else{
log<<"file 1 is "<<files[file_count+1]<<endl;
}
file_count += 2;
pcl::IterativeClosestPoint<PointT,PointT> icp;
icp.setMaximumIterations(ITERATIONS);
icp.setInputSource(first_cloud);
icp.setInputTarget(second_cloud);
icp.align(*cloud_icp);
if(icp.hasConverged())
{
std::cout<<"\nICP has converged" << icp.getFitnessScore()<<std::endl;
}
else
{
PCL_ERROR ("\nICP has not converged.\n");
}
int v1 (0);
int v2 (1);
viewer.createViewPort (0.0, 0.0, 0.5, 1.0, v1);
viewer.createViewPort (0.5, 0.0, 1.0, 1.0, v2);
pcl::visualization::PointCloudColorHandlerCustom<PointT> cloud_color(first_cloud,(int) 255 * txt_gray_lvl, (int) 255 * txt_gray_lvl,
(int) 255 * txt_gray_lvl);
viewer.addPointCloud(first_cloud,cloud_color,"cloud_in_v1",v1);
viewer.addPointCloud(first_cloud,cloud_color,"cloud_in_v2",v2);
pcl::visualization::PointCloudColorHandlerCustom<PointT> cloud_tr_color(second_cloud, 20, 180, 20);
viewer.addPointCloud(second_cloud,cloud_tr_color,"cloud_tr",v1);
pcl::visualization::PointCloudColorHandlerCustom<PointT> cloud_icp_color(cloud_icp, 180, 20, 20);
viewer.addPointCloud(cloud_icp,cloud_icp_color,"cloud_icp",v2);
viewer.setCameraPosition (-3.68332, 2.94092, 12.71266, 0.289847, 0.921947, -0.256907, 0);
viewer.setSize (1280, 1960); // Visualiser window size
for (int i=2; i<(int)files.size(); i++) {
PointCloudT::Ptr cloud_in (new PointCloudT);
PointCloudT::Ptr cloud_temp (new PointCloudT);
if(pcl::io::loadPCDFile<PointT>(files[i], *cloud_in)==-1) {
//*打开点云文件
PCL_ERROR("Couldn't read file test_pcd.pcd\n");
return ;
}
else
{
log<<"target file is "<<files[i]<<endl;
}
// The Iterative Closest Point algorithm
*cloud_temp = *cloud_icp;
pcl::IterativeClosestPoint<PointT,PointT> icp;
icp.setMaximumIterations(ITERATIONS);
icp.setInputSource(cloud_icp);
icp.setInputTarget(cloud_in);
icp.align(*cloud_icp);
if(icp.hasConverged())
{
std::cout<<"\nICP has converged" << icp.getFitnessScore()<<std::endl;
}
else
{
PCL_ERROR ("\nICP has not converged.\n");
}
viewer.updatePointCloud(cloud_in,cloud_color,"cloud_in_v1");
viewer.updatePointCloud(cloud_in,cloud_color,"cloud_in_v2");
viewer.updatePointCloud(cloud_temp,cloud_tr_color,"cloud_tr");
viewer.updatePointCloud(cloud_icp,cloud_icp_color,"cloud_icp");
for(size_t t=0; t<cloud_icp->points.size(); ++t) {
position_x[position_num] = cloud_icp->points[t].x;
position_y[position_num] = cloud_icp->points[t].y;
position_num++;
}
file_count++;
}
log<<"end reading,file count is"<<file_count<<endl;
log.close();
return ;
}
int main(int argc, char** argv) {
float *position_x = (float *)malloc(sizeof(float) * NUMBER);
float *position_y = (float *)malloc(sizeof(float) * NUMBER);
// ROS节点初始化
ros::init(argc, argv, "position_publisher");
// 创建节点句柄
ros::NodeHandle n;
// 创建一个Publisher，发布名为/person_info的topic，消息类型为learning_topic::Person，队列长度10
ros::Publisher position_info_pub = n.advertise<position::Position>("/position_info", 10);
//设置循环的频率
ros::Rate loop_rate(1);
int count = 0;
int file_count = 0;
while (ros::ok())
{
int position_num = 0;
Get_position_data(position_x, position_y, position_num,file_count);
// 初始化learning_topic::Person类型的消息
position::Position position_msg;
for (int i=0; i<position_num; i++) {
position_msg.position_x[i] = position_x[i];
position_msg.position_y[i] = position_y[i];
}
position_msg.position_num = position_num;
// 发布消息
position_info_pub.publish(posirtion_msg);
ROS_INFO("Publish Successfully!");
cout << "point num is:" << position_num << endl;
// 按照循环频率延时
loop_rate.sleep();
sleep(5);
}
return 0;
}