第二周：01 ICP迭代交互

80 阅读 0 评论 53 点赞

我是靠谱客的博主冷傲樱桃，最近开发中收集的这篇文章主要介绍第二周：01 ICP迭代交互，觉得挺不错的，现在分享给大家，希望可以做个参考。

概述

本周主要任务01：利用PCL库函数，ICP融合两个角度的点云

任务时间：2014年9月8日-2014年9月14日

任务完成情况：可以使用键盘交互，显示每次ICP迭代结果

任务涉及基本方法：

　　1.PCL库中ICP相关函数，ICP交互迭代参考官方教程 http://pointclouds.org/documentation/tutorials/interactive_icp.php#interactive-icp

程序文件：

　　1.icpInteractive.cpp


1 //icpInteractive.cpp

2 //函数：main()

3 //功能：导入两个视角的点云数据，第一个点云数据作为源，第二个点云数据作为目标，进行icp配准，

4 //
并显示配准结果，按空格进行每次迭代的交互

5 //输入：导入文件名，输出文件名 例：可执行文件根目录下，命令行输入 xml2pcd file1.pcd file2.pcd

6 //创建时间：2014/09/10

7 //最近更新时间：2014/09/17

8 //创建者：肖泽东

9
 10 #include <iostream>
 11
 12 #include <pcl/io/pcd_io.h>
 13 #include <pcl/point_cloud.h>
 14 #include <pcl/console/parse.h>
 15 #include <pcl/visualization/pcl_visualizer.h>
 16 #include <pcl/registration/icp.h>
 17
 18 bool next_iteration = false;
 19
 20 //显示帮助
 21 void showHelp(char* program_name)
 22 {
 23
std::cout << std::endl;
 24
std::cout << "Usage: " << program_name << " sourcefile.pcd targetfile.pcd" << std::endl;
 25
std::cout << "-h: Show this help." << std::endl;
 26 }
 27
 28 //键盘交互
 29 void keyboardEventOccurred (const pcl::visualization::KeyboardEvent& event,
 30
void* nothing)
 31 {
 32
if (event.getKeySym () == "space" && event.keyDown ())
//判别是否按下空格按键
 33
next_iteration = true;
//允许下一次迭代
 34 }
 35
 36 int main(int argc, char** argv)
 37 {
 38
// 显示帮助文档
 39
if(pcl::console::find_switch (argc,argv,"-h") || pcl::console::find_switch(argc,argv,"--help"))
 40 
{
 41
showHelp(argv[0]);
 42
return 0;
 43 
}
 44
 45
std::vector<int> filenames;
//输入参数 文件名向量
 46
 47
filenames = pcl::console::parse_file_extension_argument(argc, argv, ".pcd");
//查找输入中的pcd文件
 48
if (filenames.size() != 2)
//限定输入两个pcd点云文件
 49 
{
 50
std::cout << "input pcd file format wrong" << std::endl;
//不是两个，显示出错
 51
showHelp(argv[0]);
 52
return -1;
 53 
}
 54
//定义配准源点云和目标点云
 55
pcl::PointCloud<pcl::PointXYZ>::Ptr source_cloud (new pcl::PointCloud<pcl::PointXYZ> ());
//源点云
 56
pcl::PointCloud<pcl::PointXYZ>::Ptr target_cloud (new pcl::PointCloud<pcl::PointXYZ> ());
//目标点云
 57
pcl::PointCloud<pcl::PointXYZ>::Ptr aligned_cloud(new pcl::PointCloud<pcl::PointXYZ> ());;
//配准后点云
 58
 59
if(pcl::io::loadPCDFile(argv[filenames[0]],*source_cloud) < 0)
//导入源点云
 60 
{
 61
std::cout << "Error loading point cloud " << argv[filenames[0]] << std::endl;
 62
showHelp(argv[0]);
 63
return -1;
 64 
}
 65
if(pcl::io::loadPCDFile(argv[filenames[1]],*target_cloud) < 0)
//导入目标点云
 66 
{
 67
std::cout << "Error loading point cloud " << argv[filenames[1]] << std::endl;
 68
showHelp(argv[0]);
 69
return -1;
 70 
}
 71
 72
//ICP 配准定义
 73
pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
//定义ICP类型实例icp
 74
*aligned_cloud = *source_cloud;
//初始化配准点云为源点云
 75
icp.setInputCloud(aligned_cloud);
//初始化源点云
 76
icp.setInputTarget(target_cloud);
//初始化目标点云
 77
//设置ICP基本参数
 78
icp.setMaxCorrespondenceDistance(100); //设置对应点容忍最大距离
 79
icp.setMaximumIterations(1);
//设置每次按键触发最大迭代次数
 80
icp.setRANSACIterations(0);
//不进行RANSAC迭代
 81
 82
// Visualization显示结果
 83
printf(
"nPoint cloud colors :
white
= original point cloudn"
 84
"
red
= transformed point cloudn");
 85
 86
 87
pcl::visualization::PCLVisualizer viewer ("ICP test example");
//定义显示窗口
 88
int v1 (0);
//分隔窗口v1
 89
int v2 (1);
//分隔窗口v2
 90
viewer.createViewPort(0.0, 0.0, 0.5, 1.0, v1);
//...viewPort(x_min,y_min,x_max,y_max,int ..)
 91
viewer.createViewPort(0.5, 0.0, 1.0, 1.0, v2);
//
 92
//std::cout << "v1=" << v1 << "v2=" << v2<< std::endl;
 93
 94
// Define R,G,B colors for the point cloud
 95
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> source_cloud_color_handler (source_cloud, 255, 255, 255);//White
 96
// We add the point cloud to the viewer and pass the color handler
 97
viewer.addPointCloud (source_cloud, source_cloud_color_handler, "source_cloud_v1", v1);
 98
 99
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> target_cloud_color_handler (target_cloud, 230, 20, 20); // Red
100
viewer.addPointCloud (target_cloud, target_cloud_color_handler, "target_cloud_v1", v1);
101
102
//viewer.addCoordinateSystem (1.0, "cloud", 0);
103
//viewer.setBackgroundColor(0.05, 0.05, 0.05, 0); // Setting background to a dark grey
104
viewer.setBackgroundColor(0.05, 0.05, 0.05, v1);
105
viewer.setBackgroundColor(0.05, 0.05, 0.05, v2);
106
viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "source_cloud_v1");
107
viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "target_cloud_v1");
108
//viewer.setPosition(800, 400); // Setting visualiser window position
109
110
// Define R,G,B colors for the point cloud
111
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> aligned_cloud_color_handler (aligned_cloud, 255, 255, 255); //White
112
// We add the point cloud to the viewer and pass the color handler
113
viewer.addPointCloud (aligned_cloud, aligned_cloud_color_handler, "aligned_cloud_v2", v2);
114
viewer.addPointCloud (target_cloud, target_cloud_color_handler, "target_cloud_v2", v2);
115
116
viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "aligned_cloud_v2");
117
viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "target_cloud_v2");
118
//viewer.setPosition(800, 400); // Setting visualiser window position
119
// Register keyboard callback :
120
viewer.registerKeyboardCallback (&keyboardEventOccurred, (void*) NULL);
//对齐响应键盘事件
121
122
int iterations = 0;
//迭代次数
123
while (!viewer.wasStopped ())
124
{ // Display the visualiser until 'q' key is pressed
125
viewer.spinOnce ();
//运行视图
126
if(next_iteration)
127 
{
128
icp.align(*aligned_cloud);
//ICP配准对齐结果
129
std::cout << "has converged:" << icp.hasConverged() << " score: " <<
130
icp.getFitnessScore() << std::endl;
//配准分析...icp.hasConverge()=1,表示收敛
131
std::cout << icp.getFinalTransformation() << std::endl;
//获取变换矩阵
132
std::cout << "Iteration = " << ++iterations; //迭代次数加1
133
if(iterations ==100)
//迭代满100次停止迭代
134
return 0;
135
viewer.updatePointCloud (aligned_cloud, aligned_cloud_color_handler, "aligned_cloud_v2");
//更新点云
136 
}
137
next_iteration = false;
//本次迭代结束，不直接进行下一次迭代，等待下次迭代触发
138 
}
139
return 0;
140 }

　　2.CMakeLists.txt

cmake_minimum_required(VERSION 2.6 FATAL_ERROR)
project(pcl_icpInteractive)
find_package(PCL 1.6 REQUIRED)
include_directories(${PCL_INCLUDE_DIRS})
link_directories(${PCL_LIBRARY_DIRS})
add_definitions(${PCL_DEFINITIONS})
add_executable (icpInteractive icpInteractive.cpp)
target_link_libraries (icpInteractive ${PCL_LIBRARIES})

转载于:https://www.cnblogs.com/xzd1575/p/3977960.html