点云配准 ICP SAC和LM"/>
pcl 点云配准 ICP SAC和LM
一、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_srcpute(*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_targetpute(*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_srcpute(*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_targetpute(*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);normalestpute(*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);normalest2pute(*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
发布评论