一、声明

本人作为初学者，才开始接触点云配准这一块，如有错误地方，望大家指出，我将及时修改，共同进步。其次这一部分主要是点云数据配准的第一个程序，也是官方代码比较直观的一次icp配准实验。

二、数据来源

本次数据来源于斯坦福大学的兔子，龙，和猴子。在此注明一下，官方代码只能用于三者重建好了点云数据。

斯坦福大学的兔子和龙，猴子点云数据下载地址（麻烦大家打一个小星星）：

StarStar-666/PointData (github.com)

三、代码，

由于是在windows上面运行的代码，需对官方代码稍作修改，修改如下即可（把对应的数据路劲修改即可）：

       //读取ply文件if (pcl::io::loadPLYFile<pcl::PointXYZ>("E:/LX/PLC/点云数据/dragon_stand/dragon_recon.tar/dragon_recon/dragon_vrip_res2.ply", *cloud_in) == -1){PCL_ERROR("Couldn't read file1 \n");return (-2);}std::cout << "Loaded " << cloud_in->size() << " data points from file1" << std::endl;if (pcl::io::loadPLYFile<pcl::PointXYZ>("E:/LX/PLC/点云数据/dragon_stand/dragon_recon.tar/dragon_recon/dragon_vrip_res2.ply", *cloud_icp) == -1){PCL_ERROR("Couldn't read file1 \n");return (-2);}

3.1 完整代码

#include <iostream>
#include <string>
#include <pcl/io/ply_io.h> //ply格式读取
#include <pcl/io/pcd_io.h> //pcd格式读取
#include <pcl/point_types.h> //PCL中支持的点类型头文件
#include <pcl/registration/icp.h>
#include <pcl/visualization/pcl_visualizer.h>  //可视化头文件
#include <pcl/console/time.h>   // TicToc 计时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)//使用空格键来增加迭代次数，并更新显示
{if (event.getKeySym() == "space" && event.keyDown())next_iteration = true;
}int
main(int argc,char* argv[])
{// 将会出现的三个点云模型PointCloudT::Ptr cloud_in(new PointCloudT);  // （原始点云）Original point cloudPointCloudT::Ptr cloud_tr(new PointCloudT);  // （位姿变化点云）Transformed point cloudPointCloudT::Ptr cloud_icp(new PointCloudT);  // （ICP迭代输出点云）ICP output point cloudint iterations = 1;  // 默认的ICP迭代次数pcl::console::TicToc time;time.tic();//读取ply文件if (pcl::io::loadPLYFile<pcl::PointXYZ>("E:/LX/PLC/点云数据/dragon_stand/dragon_recon.tar/dragon_recon/dragon_vrip_res2.ply", *cloud_in) == -1){PCL_ERROR("Couldn't read file1 \n");return (-2);}std::cout << "Loaded " << cloud_in->size() << " data points from file1" << std::endl;if (pcl::io::loadPLYFile<pcl::PointXYZ>("E:/LX/PLC/点云数据/dragon_stand/dragon_recon.tar/dragon_recon/dragon_vrip_res2.ply", *cloud_icp) == -1){PCL_ERROR("Couldn't read file1 \n");return (-2);}std::cout << "Loaded " << cloud_icp->size() << " data points from file1" << std::endl;// 定义旋转矩阵和平移向量Matrix4d是4*4的矩阵Eigen::Matrix4d transformation_matrix = Eigen::Matrix4d::Identity();// 旋转矩阵 (请参考 https://en.wikipedia.org/wiki/Rotation_matrix)double theta = M_PI / 8;  // // 旋转的角度用弧度的表示方法transformation_matrix(0, 0) = std::cos(theta);transformation_matrix(0, 1) = -sin(theta);transformation_matrix(1, 0) = sin(theta);transformation_matrix(1, 1) = std::cos(theta);// 平移向量在X,Y,Z方向的位移 (meters)transformation_matrix(0, 3) = 1;transformation_matrix(1, 3) = 2;transformation_matrix(2, 3) = 4;// 在终端显示转换矩阵std::cout << "Applying this rigid transformation to: cloud_in -> cloud_icp" << std::endl;print4x4Matrix(transformation_matrix);// 执行点云转化pcl::transformPointCloud(*cloud_icp, *cloud_icp, transformation_matrix);*cloud_tr = *cloud_icp;  // 备份cloud_icp赋值给cloud_tr为后期使用// icp算法配准time.tic();pcl::IterativeClosestPoint<PointT, PointT> icp;icp.setMaximumIterations(iterations); //设置要执行的初始迭代次数（默认值为1）icp.setInputSource(cloud_icp);//设置输入点云icp.setInputTarget(cloud_in); //设置目标点云（输入点云进行仿射变换，得到目标点云）icp.align(*cloud_icp);//匹配后源点云icp.setMaximumIterations(1);  // 设置为1以便下次调用std::cout << "Applied " << iterations << " ICP iteration(s) in " << time.toc() << " ms" << std::endl;if (icp.hasConverged())   //输出变换矩阵的适合性评估，检查ICP算法是否收敛；否则退出程序。如果成功，我们将转换矩阵存储在4x4矩阵中，然后打印刚性矩阵转换{std::cout << "\nICP has converged, score is " << icp.getFitnessScore() << std::endl;std::cout << "\nICP transformation " << iterations << " : cloud_icp -> cloud_in" << std::endl;transformation_matrix = icp.getFinalTransformation().cast<double>();print4x4Matrix(transformation_matrix);}else{PCL_ERROR("\nICP has not converged.\n");return (-1);}// //可视化pcl::visualization::PCLVisualizer viewer("ICP demo");// 创建两个独立垂直观察视点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);// 定义显示的颜色信息float bckgr_gray_level = 0.0;  // 黑色float txt_gray_lvl = 1.0 - bckgr_gray_level;// 原始的点云设置为白色的pcl::visualization::PointCloudColorHandlerCustom<PointT> 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<PointT> 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<PointT> cloud_icp_color_h(cloud_icp, 180, 20, 20);viewer.addPointCloud(cloud_icp, cloud_icp_color_h, "cloud_icp_v2", v2);// 加入文本的描述在各自的视口界面viewer.addText("White: Original point cloud\nGreen: Matrix 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\nRed: ICP aligned point cloud", 10, 15, 16, txt_gray_lvl, txt_gray_lvl, txt_gray_lvl, "icp_info_2", v2);std::stringstream ss;  //需要使用字符串流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){// The Iterative Closest Point algorithm（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");  // Go up 11 lines in terminal output.printf("\nICP has converged, score is %+.0e\n", icp.getFitnessScore());std::cout << "\nICP transformation " << ++iterations << " : cloud_icp -> cloud_in" << std::endl;transformation_matrix *= icp.getFinalTransformation().cast<double>();  // WARNING /!\ This is not accurate! For "educational" purpose only!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;}return (0);
}

四实验结果

4.1 龙

随着迭代的进行，发现icp对于其配准效果不好(迭代47step)，龙头和龙尾是相反的：

迭代104step

4.2 兔子

迭代31sep就可

当目标点云和源电源初始状态差距太大，

迭代327epoch

4.3 猴子

五、结论

1、点之间的重复性越大，icp配准效果越好

2、icp适合精配准，如果差异性太大，效果不是很好（比较龙）

3、容易陷入局部最优

点云配准 3- icp-交互式ICP点云配准相关推荐

在医学图像分析中使用ICP算法进行点云配准
点击上方"3D视觉工坊",选择"星标" 干货第一时间送达本文转载自「计算机视觉工坊」,该公众号重点在于介绍深度学习.智能驾驶等领域,一个小众的公众号. 论文标 ...
点云配准的传统算法ICP与NDT概述
公众号致力于分享点云处理,SLAM,三维视觉,高精地图相关的文章与技术,欢迎各位加入我们,一起交流一起进步,有兴趣的可联系微信:920177957.本文来自点云PCL博主的分享,未经作者允许请勿转载, ...
PCL点云处理之点面ICP配准（附代码，参数设置，实验结果）（六十七）
PCL点云处理之点面ICP配准(附代码,参数设置,实验结果)(六十七) 前言一.点面ICP是什么? 二.使用步骤 1.代码效果总结前言学习点云配准,我辈义不容辞一.点面ICP是什么? 将之 ...
ICP算法进行点云匹配
[原文:http://www.cnblogs.com/yhlx125/p/5234156.html] 上一篇:http://www.cnblogs.com/yhlx125/p/4924283.html ...
3Ｄ点云配准（二多幅点云配准）
原文首发于公众号「3D视觉工坊」:3D点云配准(二多幅点云配准) 在上一篇文章点云配准(一两两配准)中我们介绍了两两点云之间的配准原理.本篇文章,我们主要介绍一下PCL中对于多幅点云连续配准的实现 ...
保存点云数据_3Ｄ点云配准（二多幅点云配准）
本文首发于微信公众号「3D视觉工坊」:3D点云配准(二多幅点云配准) 在上一篇文章点云配准(一两两配准)中我们介绍了两两点云之间的配准原理.本篇文章,我们主要介绍一下PCL中对于多幅点云连续配准的 ...
【手写ICP】ICP -SVD 手动实现与例程（上）
文章目录 1. 写在前面 1. 配和代码的 SVD-ICP 流程梳理 1.1 SVD-ICP 算法 5 步走 1.2 预处理 1.3 点云变换 1.4 通过 KD-tree 找最近匹配点 1.5 SV ...
python点云快速配准_分享一个V-SLAM中点云配准算法改进的方法
近年来,随着自主导航的广泛应用,视觉同时定位与地图构建(Visual simultaneous localization and mapping, V-SLAM)通过自身携带的视觉传感器对周围环境进行 ...
ICP、ICP备案、ICP证是什么意思？有什么区别和联系？
ICP是什么意思呢?相信大多站长并不清楚ICP是什么意思,就算是网站已经备案的也不了解ICP到底为何物.大家往往搞不清楚ICP备案和ICP证之间的区别.今天小编为大家讲解一下ICP是什么意思,ICP备 ...

点云配准 3- icp-交互式ICP点云配准

一、声明

二、数据来源

三、代码，

3.1 完整代码

四实验结果

4.1 龙

4.2 兔子

4.3 猴子

五、结论

点云配准 3- icp-交互式ICP点云配准相关推荐

最新文章

热门文章

点云配准 3- icp-交互式ICP点云配准

一、声明

二、数据来源

三、代码，

3.1 完整代码

四 实验结果

4.1 龙

4.2 兔子

4.3 猴子

五、结论

点云配准 3- icp-交互式ICP点云配准相关推荐

最新文章

热门文章

四实验结果