pcl 点云配准 ICP SAC和LM_ICP

2023-11-23 14:30
文章标签 pcl 点云 配准 icp lm sac

本文主要是介绍pcl 点云配准 ICP SAC和LM_ICP，希望对大家解决编程问题提供一定的参考价值，需要的开发者们随着小编来一起学习吧！

一、ICP推导 

 

 

 

#include <fstream>
#include <sstream>
#include <iostream>
#include <vector>
#include <Eigen/Eigen>void ICP(const std::vector<Eigen::Vector3f>& p1, const std::vector<Eigen::Vector3f>& p2,Eigen::Matrix3f& R_12, Eigen::Vector3f& t_12) {assert(p1.size() == p2.size());// center of masssize_t N = p1.size();Eigen::Vector3f p1_center, p2_center;for (int i = 0; i < N; ++i) {p1_center += p1.at(i);p2_center += p2.at(i);}p1_center /= N;p2_center /= N;// remove the centerstd::vector<Eigen::Vector3f> q1(N), q2(N);for (int i = 0; i < N; ++i) {q1[i] = p1.at(i) - p1_center;q2[i] = p2.at(i) - p2_center;}// compute q2*q1^TEigen::Matrix3f H = Eigen::Matrix3f::Zero();for (int i = 0; i < N; ++i) {H += q2.at(i) * q1.at(i).transpose();}// SVD on HEigen::JacobiSVD<Eigen::Matrix3f> svd(H, Eigen::ComputeFullU | Eigen::ComputeFullV);Eigen::Matrix3f U = svd.matrixU();Eigen::Matrix3f V = svd.matrixV();R_12 = V * U.transpose();t_12 = p1_center - R_12 * p2_center;
}int main(int argc, char** argv) {Eigen::AngleAxisf angle_axis(M_PI / 3, Eigen::Vector3f(0, 0, 1));Eigen::Matrix3f R_12_ = angle_axis.matrix();Eigen::Vector3f t_12_(1, 2, 3);std::cout << "except R_12:\n" << R_12_ << std::endl;std::cout << "except t_12:\n" << t_12_.transpose() << std::endl;std::vector<Eigen::Vector3f> p1, p2;Eigen::Vector3f point;std::ifstream fin("/tmp/bunny.txt");std::string line;while (getline(fin, line)) {std::stringstream ss(line);ss >> point.x();ss >> point.y();ss >> point.z();p2.push_back(point);p1.push_back(R_12_ * point + t_12_);}fin.close();Eigen::Matrix3f R_12;Eigen::Vector3f t_12;ICP(p1, p2, R_12, t_12);std::cout << "result R_12:\n" << R_12_ << std::endl;std::cout << "result t_12:\n" << t_12.transpose() << std::endl;return 0;
}

迭代最近点 ICP 详细推导（C++实现）_icp c++_Alan Lan的博客-CSDN博客 

二、ICP 的变种


#if  1  
using pcl::NormalEstimation;
using pcl::search::KdTree;
typedef pcl::PointXYZ MyPointT;
typedef pcl::PointCloud<MyPointT> MyPointCloud;
typedef pcl::PointCloud<pcl::PointNormal> PointCloudWithNormals;
int main()
{string strfilepath = "C:\\Users\\Albert\\Desktop\\Binary.pcd";string strfilepath2 = "C:\\Users\\Albert\\Desktop\\Binary2.pcd";string strfilepath3 = "C:\\Users\\Albert\\Desktop\\Binary4.pcd";string strfilepath4 = "C:\\Users\\Albert\\Desktop\\BinaryTransform.pcd";//加载点云文件MyPointCloud::Ptr cloud_src(new MyPointCloud);//原点云，待配准pcl::io::loadPCDFile(strfilepath, *cloud_src);MyPointCloud::Ptr cloud_target(new MyPointCloud);//目标点云pcl::io::loadPCDFile(strfilepath2, *cloud_target);clock_t start = clock();//去除NAN点std::vector<int> indices_src;  // 去除无效点pcl::removeNaNFromPointCloud(*cloud_src,*cloud_src, indices_src);cout << "下采样之前的点云大小 ：" << cloud_src->size() << endl;//为了加速。，我们需要下采样pcl::VoxelGrid<pcl::PointXYZ> voxel_grid;voxel_grid.setLeafSize(0.01,0.01,0.01);voxel_grid.setInputCloud(cloud_src);MyPointCloud::Ptr cloud(new MyPointCloud);voxel_grid.filter(*cloud);pcl::io::savePCDFileASCII(strfilepath3, *cloud);cout << "下采样之后的点云大小 ："<<cloud->size() << endl;// 计算表面法向量pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal>  nc_src;nc_src.setInputCloud(cloud);pcl::search::KdTree<pcl::PointXYZ>::Ptr tree_src(new pcl::search::KdTree<pcl::PointXYZ>());nc_src.setSearchMethod(tree_src);pcl::PointCloud<pcl::Normal>::Ptr  normal_src(new  pcl::PointCloud<pcl::Normal>());nc_src.setRadiusSearch(0.01);nc_src.compute(*normal_src);  // 计算得到法向量cout << "下采样之后的法向量的      点云大小 ：" << normal_src->size() << endl;cout << "目标==========================================================================" << endl;//  同理 将target 的点云也std::vector<int> indices_target;pcl::removeNaNFromPointCloud(*cloud_target,*cloud_target, indices_target);//  下采样MyPointCloud::Ptr cloud_tar(new MyPointCloud);pcl::VoxelGrid<pcl::PointXYZ>  v_target;v_target.setInputCloud(cloud_target);v_target.setLeafSize(0.005,0.005,0.005);v_target.filter(*cloud_tar);cout << "目标------下采样之前的   点云大小 ：" << cloud_target->size() << endl;cout << "目标------下采样之后的     点云大小 ：" << cloud_tar->size() << endl;// 计算法向量pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal>  n_c_target;pcl::search::KdTree<pcl::PointXYZ>::Ptr  tree_target(new pcl::search::KdTree<pcl::PointXYZ>());n_c_target.setInputCloud(cloud_tar);n_c_target.setRadiusSearch(0.01);n_c_target.setSearchMethod(tree_target);pcl::PointCloud<pcl::Normal>::Ptr normal_targ(new pcl::PointCloud<pcl::Normal>());n_c_target.compute(*normal_targ);cout << "目标------下采样之后的法向量的      点云大小 ：" << normal_targ->size() << endl;cout << "===============================计算FPFH   ===========================================" << endl;// 计算FPFHpcl::FPFHEstimationOMP<pcl::PointXYZ, pcl::Normal, pcl::FPFHSignature33>  fpfh_src;fpfh_src.setInputCloud(cloud);fpfh_src.setInputNormals(normal_src); // pcl::search::KdTree<pcl::PointXYZ> ::Ptr  tree_fpfh_src(new pcl::search::KdTree<pcl::PointXYZ>());fpfh_src.setSearchMethod(tree_fpfh_src);fpfh_src.setRadiusSearch(0.05);pcl::PointCloud<pcl::FPFHSignature33>::Ptr  Signature33_src(new pcl::PointCloud<pcl::FPFHSignature33>());fpfh_src.compute(*Signature33_src);cout << "计算FPFH    Signature33_src    点云大小 ：" << Signature33_src->size() << endl;pcl::FPFHEstimationOMP<pcl::PointXYZ, pcl::Normal, pcl::FPFHSignature33>  fpfh_target;fpfh_target.setInputCloud(cloud_tar);fpfh_target.setInputNormals(normal_targ);// pcl::search::KdTree<pcl::PointXYZ> ::Ptr  tree_fpfh_target(new pcl::search::KdTree<pcl::PointXYZ>());fpfh_target.setSearchMethod(tree_fpfh_target);fpfh_target.setRadiusSearch(0.05);pcl::PointCloud<pcl::FPFHSignature33>::Ptr  Signature33_target(new pcl::PointCloud<pcl::FPFHSignature33>());fpfh_target.compute(*Signature33_target);cout << "计算FPFH    tree_fpfh_target    点云大小 ：" << Signature33_target->size() << endl;cout << "==================================SAC========================================" << endl;// 计算SACpcl::SampleConsensusInitialAlignment<pcl::PointXYZ, pcl::PointXYZ, pcl::FPFHSignature33>Sac;Sac.setInputSource(cloud);Sac.setInputTarget(cloud_tar);Sac.setSourceFeatures(Signature33_src);Sac.setTargetFeatures(Signature33_target);pcl::PointCloud<pcl::PointXYZ>::Ptr  sac_result(new pcl::PointCloud<pcl::PointXYZ>());Sac.align(*sac_result);cout << "sac_result----------------" << endl;cout << "Sac.hasConverged()       " << Sac.hasConverged() << "              score :     " << Sac.getFitnessScore() << endl;cout<<" 旋转矩阵 ：     "<< endl;cout << Sac.getFinalTransformation() << endl;// 保存匹配的点云pcl::io::savePCDFileASCII(strfilepath4, *sac_result);cout << "==========================ICP 匹配================================================" << endl;// ICP 匹配pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;pcl::PointCloud<pcl::PointXYZ>::Ptr  icp_cloud_result(new  pcl::PointCloud<pcl::PointXYZ>());icp.setInputSource(cloud);icp.setInputTarget(cloud_tar);
//	icp.setMaxCorrespondenceDistance(0.4);icp.setMaximumIterations(50);icp.setTransformationEpsilon(0.001);icp.setEuclideanFitnessEpsilon(0.2);icp.align(*icp_cloud_result);cout << "icp   ----------------" << endl;cout << "icp.hasConverged()       " << icp.hasConverged() << "              score :     " << icp.getFitnessScore() << endl;cout << "icp  旋转矩阵 ：     " << endl;cout << icp.getFinalTransformation() << endl;cout << "==========================非线性迭代================================================" << endl;pcl::PointCloud<pcl::PointXYZ>::Ptr  result2(new pcl::PointCloud<pcl::PointXYZ>());//  非线性迭代 pcl::IterativeClosestPointNonLinear<pcl::PointXYZ, pcl::PointXYZ> NonLinear_icp;NonLinear_icp.setInputSource(cloud);NonLinear_icp.setInputTarget(cloud_tar);NonLinear_icp.setTransformationEpsilon(1e-10);NonLinear_icp.setMaxCorrespondenceDistance(100);NonLinear_icp.setEuclideanFitnessEpsilon(0.01);NonLinear_icp.setMaxCorrespondenceDistance(150);NonLinear_icp.setMaximumIterations(50);NonLinear_icp.align(*result2);cout << "计算    result    点云大小 ：" << result2->size() << endl;string strfilepath5 = "C:\\Users\\Albert\\Desktop\\BinaryMain.pcd";pcl::io::savePCDFileBinaryCompressed(strfilepath5, *result2);cout << "NonLinear_icp   ----------------" << endl;cout << "NonLinear_icp.hasConverged()       " << NonLinear_icp.hasConverged() << "              score :     " << NonLinear_icp.getFitnessScore() << endl;cout << "NonLinear_icp  旋转矩阵 ：     " << endl;cout << NonLinear_icp.getFinalTransformation() << endl;cout << "==========================LM ================================================" << endl;//  非线性迭代 pcl::PointCloud<pcl::PointXYZ>::Ptr icp_result(new pcl::PointCloud<pcl::PointXYZ>);pcl::IterativeClosestPointNonLinear<pcl::PointXYZ, pcl::PointXYZ> lm_icp;lm_icp.setInputSource(cloud_src);lm_icp.setInputTarget(cloud_target);lm_icp.setTransformationEpsilon(1e-10);    //为终止条件设置最小转换差异lm_icp.setMaxCorrespondenceDistance(150);   //设置对应点对之间的最大距离（此值对配准结果影响较大）。lm_icp.setEuclideanFitnessEpsilon(1e-10); //设置收敛条件是均方误差和小于阈值， 停止迭代；lm_icp.setMaximumIterations(50);           //最大迭代次数； lm_icp.align(*icp_result);cout << "计算    result    点云大小 ：" << icp_result->size() << endl;string strfilepath7 = "C:\\Users\\Albert\\Desktop\\LM_BinaryMain.pcd";pcl::io::savePCDFileBinaryCompressed(strfilepath7, *icp_result);cout << "lm_icp   ----------------" << endl;cout << "lm_icp.hasConverged()       " << lm_icp.hasConverged() << "              score :     " << lm_icp.getFitnessScore() << endl;cout << "lm_icp  旋转矩阵 ：     " << endl;cout << lm_icp.getFinalTransformation() << endl;cout << "==========================使用法向量匹配===============================================" << endl;MyPointCloud::Ptr cloud_src2(new MyPointCloud);//原点云，待配准MyPointCloud::Ptr cloud_target2(new MyPointCloud);//原点云，待配准pcl::copyPointCloud(*cloud_src, *cloud_src2);pcl::copyPointCloud(*cloud_target,* cloud_target2);pcl::PointCloud<pcl::PointNormal>::Ptr  points_with_normals_src(new pcl::PointCloud<pcl::PointNormal>());pcl::PointCloud<pcl::PointNormal>::Ptr  points_with_normals_tgt(new pcl::PointCloud<pcl::PointNormal>());pcl::PointCloud<pcl::PointNormal>::Ptr  result(new pcl::PointCloud<pcl::PointNormal>());pcl::NormalEstimation<pcl::PointXYZ, pcl::PointNormal> normalest;pcl::search::KdTree<pcl::PointXYZ>::Ptr  tree_src_pointnormal(new  pcl::search::KdTree<pcl::PointXYZ>);normalest.setSearchMethod(tree_src_pointnormal);//normalest.setKSearch(30);normalest.setRadiusSearch(0.02);normalest.setInputCloud(cloud_src2);normalest.compute(*points_with_normals_src);pcl::NormalEstimation<pcl::PointXYZ, pcl::PointNormal> normalest2;pcl::search::KdTree<pcl::PointXYZ>::Ptr  tree_tar_pointnormal(new  pcl::search::KdTree<pcl::PointXYZ>);normalest2.setSearchMethod(tree_tar_pointnormal);//normalest2.setKSearch(30);normalest2.setRadiusSearch(0.02);normalest2.setInputCloud(cloud_target2);normalest2.compute(*points_with_normals_tgt);cout << "计算    points_with_normals_tgt    点云大小 ：" << points_with_normals_tgt->size() << endl;cout << "计算    points_with_normals_src    点云大小 ：" << points_with_normals_src->size() << endl;pcl::IterativeClosestPointWithNormals<pcl::PointNormal, pcl::PointNormal>ptoplane_icp;ptoplane_icp.setInputSource(points_with_normals_src);ptoplane_icp.setInputTarget(points_with_normals_tgt);ptoplane_icp.setTransformationEpsilon(1e-10);ptoplane_icp.setMaxCorrespondenceDistance(100);ptoplane_icp.setEuclideanFitnessEpsilon(0.1);ptoplane_icp.setMaxCorrespondenceDistance(500); ptoplane_icp.setMaximumIterations(50);ptoplane_icp.align(*result);cout << "计算    result    点云大小 ：" << result->size() << endl;//string strfilepath7 = "C:\\Users\\Albert\\Desktop\\LM_BinaryMain.pcd";//pcl::io::savePCDFileBinaryCompressed(strfilepath7, *icp_result);cout << "ptoplane_icp   ----------------" << endl;cout << "ptoplane_icp.hasConverged()       " << ptoplane_icp.hasConverged() << "              score :     " << ptoplane_icp.getFitnessScore() << endl;cout << "ptoplane_icp  旋转矩阵 ：     " << endl;cout << ptoplane_icp.getFinalTransformation() << endl;system("pause");return 0;
}#endif#if 0   // 点云的精配准
int main()
{system("pause");return 0;
}
#endif

三、法向量和点云配合做匹配

#if  1  // 点云的精配准
int main()
{string strfilepath   = "C:\\Users\\Albert\\Desktop\\pcd\\1.pcd";string strfilepath2 = "C:\\Users\\Albert\\Desktop\\pcd\\2.pcd";string strfilepath4 = "C:\\Users\\Albert\\Desktop\\3.pcd";//加载点云文件pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_src(new pcl::PointCloud<pcl::PointXYZ>);//原点云，待配准pcl::io::loadPCDFile(strfilepath, *cloud_src);pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_target(new pcl::PointCloud<pcl::PointXYZ>);//目标点云pcl::io::loadPCDFile(strfilepath2, *cloud_target);cout << "原始点云的 cloud_src 大小:         " << cloud_src->size() << endl;cout << "原始点云的 cloud_target大小:     " << cloud_target->size() << endl;// 分别计算两个点云的法向量pcl::PointCloud<pcl::Normal> ::Ptr  normal_src(new pcl::PointCloud<pcl::Normal>());pcl::PointCloud<pcl::Normal> ::Ptr  normal_target(new pcl::PointCloud<pcl::Normal>());compute_normal(cloud_src, normal_src, 0.03);compute_normal(cloud_target, normal_target, 0.03);//for (size_t i = 0; i < 10; i++)//{//	cout << "   normal_src x : " << normal_src->points[i].normal_x << endl;//	cout << "   normal_src y : "<< normal_src->points[i].normal_y << endl;//	cout << "   normal_src z : " << normal_src->points[i].normal_z << endl;//	cout << "   normal_target x : " << normal_target->points[i].normal_x << endl;//	cout << "   normal_target y : " << normal_target->points[i].normal_y << endl;//	cout << "   normal_target z : " << normal_target->points[i].normal_z << endl;//}cout << "法向量  计算完成！！！" << endl;// 使用N4PCS进行粗配准pcl::registration::FPCSInitialAlignment<pcl::PointXYZ, pcl::PointXYZ, pcl::Normal, float> fpcs;fpcs.setInputSource(cloud_src);fpcs.setInputTarget(cloud_target);fpcs.setSourceNormals(normal_src);fpcs.setTargetNormals(normal_target);fpcs.setMaxNormalDifference(30);// 最大法线差值用度数来表示fpcs.setApproxOverlap(0.6);  // 设置source 和 target 点云之间的重叠部分fpcs.setDelta(0.03);  //  设置配准后源点云和目标点云的距离fpcs.setNumberOfSamples(200);     // 采样的数量fpcs.setMaxComputationTime(50);  // 最大的计算时间pcl::PointCloud<pcl::PointXYZ>::Ptr pcs(new pcl::PointCloud<pcl::PointXYZ>);fpcs.align(*pcs);cout << "变换矩阵：\n" << fpcs.getFinalTransformation() << endl;pcl::transformPointCloud(*cloud_src, *cloud_target, fpcs.getFinalTransformation());cout << "匹配   计算完成！！！" << endl;system("pause");return 0;
}
#endif

这篇关于pcl 点云配准 ICP SAC和LM_ICP的文章就介绍到这儿，希望我们推荐的文章对编程师们有所帮助！


http://www.chinasem.cn/article/418580。

相关文章

2、PF-Net点云补全

2、PF-Net点云补全

2、PF-Net 点云补全 PF-Net论文链接：PF-Net PF-Net （Point Fractal Network for 3D Point Cloud Completion）是一种专门为三维点云补全设计的深度学习模型。点云补全实际上和图片补全是一个逻辑，都是采用GAN模型的思想来进行补全，在图片补全中，将部分像素点删除并且标记，然后卷积特征提取预测、判别器判别，来训练模型，生成的像
阅读更多...
【LVI-SAM】激光雷达点云处理特征提取LIO-SAM 之FeatureExtraction实现细节

【LVI-SAM】激光雷达点云处理特征提取LIO-SAM 之FeatureExtraction实现细节

激光雷达点云处理特征提取LIO-SAM 之FeatureExtraction实现细节 1. 特征提取实现过程总结1.0 特征提取过程小结1.1 类 `FeatureExtraction` 的整体结构与作用1.2 详细特征提取的过程1. 平滑度计算（`calculateSmoothness()`）2. 标记遮挡点（`markOccludedPoints()`）3. 特征提取（`extractF
阅读更多...
点云数据常见的坐标系有哪些，如何进行转换？

点云数据常见的坐标系有哪些，如何进行转换？

文章目录 一、点云坐标系分类1. 世界坐标系2. 相机坐标系3. 极坐标系4. 笛卡尔坐标系（直角坐标系）：5. 传感器坐标系6. 地理坐标系 二、坐标系转换方法1. 地理坐标系与投影坐标系之间的转换2. 投影坐标系与局部坐标系之间的转换3. 局部坐标系与3D模型坐标系之间的转换4. 相机坐标系与其他坐标系之间的转换5. 传感器坐标系与其他坐标系之间的转换 三、坐标系转换工具 一
阅读更多...
三维激光扫描点云配准外业棋盘的布设与棋盘坐标测量

三维激光扫描点云配准外业棋盘的布设与棋盘坐标测量

文章目录 一、棋盘标定板准备二、棋盘标定板布设三、棋盘标定板坐标测量 一、棋盘标定板准备 三维激光扫描棋盘是用来校准和校正激光扫描仪的重要工具，主要用于提高扫描精度。棋盘标定板通常具有以下特点： 高对比度图案：通常是黑白相间的棋盘格，便于识别。已知尺寸：每个格子的尺寸是已知的，可以用于计算比例和调整。平面标定：帮助校准相机和激光扫描仪之间的位置关系。 使用方法 扫描棋盘：
阅读更多...
基于opencv实现双目立体匹配点云距离

基于opencv实现双目立体匹配点云距离

双目相机或两个单目相机。 一、相机标定 MATLAB软件，打开双目标定app。 点击add images，弹出加载图像的窗口，分别导入左图和右图，设置黑白格长度（标定板的长度一般为20）。 点击确定，弹出加载好的图像。 在菜单栏勾选2 Coefficients（鱼眼相机勾选3 Coefficients）、Tangential Distinction，点击Calibrate，显示
阅读更多...
CUDAPCL ROR点云滤波

CUDAPCL ROR点云滤波

文章目录 一、简介二、实现代码三、实现效果参考资料 一、简介 该方法的具体原理为输入的点云中每一个点设定一个范围(半径为r的圆)，如果在该范围内没有达到某一个设定的点数值，则该数据点将会被删除，重复上述此过程直到最后一个数据点，即完成该滤波过程。 二、实现代码 ROR.cuh #ifndef ROR_GPU_CUH#define ROR_GPU_CU
阅读更多...
2d激光点云识别退化场景（长走廊）

2d激光点云识别退化场景（长走廊）

注：算法只适用于静态场景，在有动态场景（行人）的环境下不适用 退化场景描述 场景一：长走廊 激光探测距离有限，在长走廊环境下，激光在某些位置无法探测到走廊尽头，会出现如上图情况，激光轮廓为红色的两条平行线。对于这种情况，我们只需寻找到只有两个平行线，即位退化场景 场景二：单一墙面 通常情况下，退化场景为如上两种情况，当然多条平行线也是符合的。 算法思路为，如果激光雷达点云构成的特征都是平行线
阅读更多...
Open3D 点云添加均匀分布的随机噪声

Open3D 点云添加均匀分布的随机噪声

目录 一、概述 1.1实现步骤 1.2应用场景 二、代码实现 三、实现效果 3.1原始点云 3.2添加噪声后点云 Open3D点云算法汇总及实战案例汇总的目录地址： Open3D点云算法与点云深度学习案例汇总（长期更新）-CSDN博客 一、概述         在 Open3D 中，可以通过向点云的每个点添加随机噪声来模拟实际的测量误差或环境噪声。均匀分布的随机噪声是
阅读更多...
Pangolin多视口显示——按钮面板，图片，三维点云

Pangolin多视口显示——按钮面板，图片，三维点云

这篇博客记录： Pangolin的几类显示方法，包括按钮面板，彩色图片，三维点云（可变换视角）Pangolin显示cv::Mat格式图片的方法原本倒置的图片自下而上渲染（变正:） 1.按钮面板 //新建按钮和选择框pangolin::CreatePanel("按钮面板").SetBounds(0.0,1.0,0.0,0.2);//第一个参数为按钮的名字，第二个为默认状态，第三个为是
阅读更多...
点云配准之ICP和NDT算法的高斯牛顿法求解

点云配准之ICP和NDT算法的高斯牛顿法求解

ICP算法 NDT算法 代码：https://github.com/taifyang/pointcloud-registration 参考：高翔《自动驾驶与机器人中的SLAM技术》
阅读更多...

Copyright China编程 23002807@qq.com

沪ICP备2023033072号-2