版权声明:本文为博主原创文章,未经博主允许不得转载。 https://blog.csdn.net/u014801811/article/details/79979764
**代码出处:**http://pointclouds.org/documentation/tutorials/interactive_icp.php#interactive-icp
交互迭代最近法允许用户每次按下“空格”键,一次ICP迭代算法执行,并更新视图。
此程序主要步骤:
1、导入点云数据
PCL提供IO模块直接读入点云文件。
pcl::io::loadPCDFile("dragon_source.pcd", *cloud_in)
注:控制台参数输入时,cd到exe路径,执行 "XX arg1 arg2 …"
2、旋转平移获取待配准点云数据
pcl中矩阵的定义及赋值:
//定义旋转矩阵和平移矩阵
Eigen::Matrix4d transformation_matrix = Eigen::Matrix4d::Identity();
double theta = M_PI / 8;//旋转角度
transformation_matrix(0,0) = cos(theta);
transformation_matrix(0,1) = -sin(theta);
transformation_matrix(1,0) = sin(theta);
transformation_matrix(1,1) = cos(theta);
transformation_matrix(0, 3) = 0;//沿x轴移动0.m
transformation_matrix(1, 3) = 0;//沿y轴移动0.m
transformation_matrix(2, 3) = 0.04;//沿z轴移动0.04m
pcl::transformPointCloud(*cloud_in, *cloud_icp, transformation_matrix);//执行变换
3、icp配准
pcl::IterativeClosestPoint<pcl::PointXYZ,pcl::PointXYZ> icp;//配准对象
icp.setInputSource(cloud_icp);
icp.setInputTarget(cloud_in);
/*icp.setEuclideanFitnessEpsilon(0.1);
icp.setRANSACIterations(10);
icp.setTransformationEpsilon(1e-6);*/
icp.align(*cloud_icp);
icp.setMaximumIterations(1);//迭代一次
4、点云可视化
//可视化
pcl::visualization::PCLVisualizer viewer("ICP demo");
//创建两个视口
int v1(0);
int v2(1);
viewer.createViewPort(0, 0, 0.5, 1, v1);
viewer.createViewPort(0.5, 0, 1, 1, v2);
//颜色
float bckgr_gray_level = 0.0;//黑色
float txt_gray_lvl = 1.0 - bckgr_gray_level;
//原始点云 白色
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> cloud_in_color_h
(cloud_in, (int)255 * txt_gray_lvl, (int)255 * txt_gray_lvl, (int)255 * txt_gray_lvl);
viewer.addPointCloud(cloud_in, cloud_in_color_h, "cloud_in_v1", v1);
viewer.addPointCloud(cloud_in, cloud_in_color_h, "cloud_in_v2", v2);
//目标变换点云 绿色
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> cloud_tr_color_h
(cloud_tr, 20,180,20);
viewer.addPointCloud(cloud_tr, cloud_tr_color_h, "cloud_tr_v1", v1);
//ICP 配准点云 红色
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> cloud_icp_color_h
(cloud_tr, 180, 20, 20);
viewer.addPointCloud(cloud_icp, cloud_icp_color_h, "cloud_icp_v2", v2);
完整代码
/** Filename: interaction_icp.cpp
** Date: 2018-4-17
**Description:
**/
#include "stdafx.h"
#include <iostream>
#include <string>
#include <vector>
#include <pcl\visualization\pcl_plotter.h>
#include <pcl\visualization\pcl_visualizer.h>
#include <pcl\point_types.h>
#include <pcl\point_cloud.h>
#include <pcl\range_image\range_image.h>
#include <pcl\visualization\range_image_visualizer.h>
#include <pcl\io\ply_io.h>
#include <pcl\io\pcd_io.h>
#include <pcl\registration\icp.h>
#include <pcl\console\time.h>
bool next_iteration = false;
void print4x4Matrix(const Eigen::Matrix4d& matrix)
{
printf("旋转矩阵R:\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("平移矩阵T: \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)
{
if (event.getKeySym() == "space" && event.keyDown())
next_iteration = true;
}
int main(int argc, char* argv[])
{
//定义点云类型
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in(new pcl::PointCloud<pcl::PointXYZ>);//目标点云
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_tr(new pcl::PointCloud<pcl::PointXYZ>);//变换点云
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_icp(new pcl::PointCloud<pcl::PointXYZ>);//icp配准后点云
//检查程序参数
if (argc < 2)
{
printf("Usage:\n");
printf("\t\t%s file.ply number_of_ICP_iterations\n",argv[0]);
PCL_ERROR("Provide one ply file. \n");
return -1;
}
int iterations = 1;//默认迭代次数
if (argc>2)
{
iterations = atoi(argv[1]);
if (iterations < 1)
{
PCL_ERROR("Number of initial iterations must be >= 1\n");
return -1;
}
}
pcl::console::TicToc time;
time.tic();
//加载点云数据
if (pcl::io::loadPCDFile("dragon_source.pcd", *cloud_in) < 0)
{
PCL_ERROR("Error loading cloud %s.\n", argv[1]);
return -1;
}
std::cout << "\n Loaded file " << argv[1] << "(" << cloud_in->points.size() << "points) in " << time.toc() << "ms\n\n";
//定义旋转矩阵和平移矩阵
Eigen::Matrix4d transformation_matrix = Eigen::Matrix4d::Identity();
print4x4Matrix(transformation_matrix);
std::cout << transformation_matrix << std::endl;
double theta = M_PI / 8;//旋转角度
transformation_matrix(0,0) = cos(theta);
transformation_matrix(0,1) = -sin(theta);
transformation_matrix(1,0) = sin(theta);
transformation_matrix(1,1) = cos(theta);
transformation_matrix(0, 3) = 0;//沿x轴移动0.m
transformation_matrix(1, 3) = 0;//沿y轴移动0.m
transformation_matrix(2, 3) = 0.04;//沿z轴移动0.04m
//在控制台显示变换矩阵
printf("刚性变换矩阵:\n");
std::cout << transformation_matrix << std::endl;
pcl::transformPointCloud(*cloud_in, *cloud_icp, transformation_matrix);//执行变换
*cloud_tr = *cloud_icp;//备份目标点云
//ICP算法
std::cout << "ICP 配准开始" << std::endl;
time.tic();
pcl::IterativeClosestPoint<pcl::PointXYZ,pcl::PointXYZ> icp;
icp.setInputSource(cloud_icp);
icp.setInputTarget(cloud_in);
/*icp.setEuclideanFitnessEpsilon(0.1);
icp.setRANSACIterations(10);
icp.setTransformationEpsilon(1e-6);*/
icp.align(*cloud_icp);
icp.setMaximumIterations(1);
std::cout << "Applied" << iterations << "ICP iteration(s) in " << time.toc() << "ms"<<std::endl;
if (icp.hasConverged())
{
std::cout << "\n ICP has converged, score is " << icp.getFitnessScore() << std::endl;
std::cout << "\n ICP transformation " << iterations << ": cloud_icp -> cloud_in" << std::endl;
transformation_matrix = icp.getFinalTransformation().cast<double>();;
print4x4Matrix(transformation_matrix);
}
else
{
PCL_ERROR(" \n ICP has not converged.\n");
return -1;
}
//可视化
pcl::visualization::PCLVisualizer viewer("ICP demo");
//创建两个视口
int v1(0);
int v2(1);
viewer.createViewPort(0, 0, 0.5, 1, v1);
viewer.createViewPort(0.5, 0, 1, 1, v2);
//颜色
float bckgr_gray_level = 0.0;//黑色
float txt_gray_lvl = 1.0 - bckgr_gray_level;
//原始点云 白色
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> cloud_in_color_h
(cloud_in, (int)255 * txt_gray_lvl, (int)255 * txt_gray_lvl, (int)255 * txt_gray_lvl);
viewer.addPointCloud(cloud_in, cloud_in_color_h, "cloud_in_v1", v1);
viewer.addPointCloud(cloud_in, cloud_in_color_h, "cloud_in_v2", v2);
//目标变换点云 绿色
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> cloud_tr_color_h
(cloud_tr, 20,180,20);
viewer.addPointCloud(cloud_tr, cloud_tr_color_h, "cloud_tr_v1", v1);
//ICP 配准点云 红色
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> cloud_icp_color_h
(cloud_tr, 180, 20, 20);
viewer.addPointCloud(cloud_icp, cloud_icp_color_h, "cloud_icp_v2", v2);
//添加文本描述
viewer.addText("White: Original point cloud\n Green :Matix transformed point cloud", 10, 15, 16,
txt_gray_lvl, txt_gray_lvl, txt_gray_lvl,"icp_info_1",v1);
viewer.addText("White: Original point cloud\n Red: ICP aligned point cloud", 10, 15, 16,
txt_gray_lvl, txt_gray_lvl, txt_gray_lvl, "icp_info_2", v2);
std::stringstream ss;
ss << iterations;
std::string iterations_cnt = "ICP iterations = " + ss.str();
viewer.addText(iterations_cnt, 10, 60, 16, txt_gray_lvl, txt_gray_lvl, txt_gray_lvl, "iterations_cnt", v2);
//设置背景颜色
viewer.setBackgroundColor(bckgr_gray_level, bckgr_gray_level, bckgr_gray_level, v1);
viewer.setBackgroundColor(bckgr_gray_level, bckgr_gray_level, bckgr_gray_level, v2);
//设置相机位置和方向
viewer.setCameraPosition(-3.68332, 2.94092, 5.71266, 0.289847, 0.921947, -0.256907, 0);
//窗口大小
viewer.setSize(1280, 1024);
viewer.registerKeyboardCallback(&keyboardEventOccurred, (void*)NULL);
while (!viewer.wasStopped())
{
viewer.spinOnce();
//按下空格键
if (next_iteration)
{
//ICP 算法
time.tic();
icp.align(*cloud_icp);
std::cout << "Applied 1 ICP iteration in " << time.toc() << "ms" << std::endl;
if (icp.hasConverged())
{
printf("\033[11A");
printf("\n ICP has converged ,score is %+.0e\n", icp.getFitnessScore());
std::cout << "\n ICP transformation" << ++iterations << " :cloud_icp -> cloud_in" << std::endl;
transformation_matrix *= icp.getFinalTransformation().cast<double>();
print4x4Matrix(transformation_matrix);
ss.str("");
ss << iterations;
std::string iterations_cnt = "ICP iterations = " + ss.str();
viewer.updateText(iterations_cnt, 10, 60, 16, txt_gray_lvl, txt_gray_lvl, txt_gray_lvl, "iterations_cnt");
viewer.updatePointCloud(cloud_icp, cloud_icp_color_h, "cloud_icp_v2");
}
else
{
PCL_ERROR("\nICP has not converged.\n");
return (-1);
}
}
next_iteration = false;
}
system("pause");
return 0;
}
可视化结果