SVD分解求解
SVD分解法网上原理有很多文章,例如:ICP算法公式推导和PCL源码解析
#include <iostream> #include <ctime> #include <pcl/io/pcd_io.h> #include <pcl/point_types.h> #include <pcl/common/transforms.h> #include <pcl/search/kdtree.h> #include <pcl/correspondence.h> #include <pcl/visualization/pcl_visualizer.h> int main(int argc, char** argv) {
pcl::PointCloud<pcl::PointXYZ>::Ptr source_cloud(new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::PointXYZ>::Ptr target_cloud(new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::PointXYZ>::Ptr source_cloud_mid(new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::PointXYZ>::Ptr target_cloud_mid(new pcl::PointCloud<pcl::PointXYZ>); //加载点云 pcl::io::loadPCDFile("bunny1.pcd", *source_cloud); pcl::io::loadPCDFile("bunny3.pcd", *target_cloud); //建立kd树 pcl::search::KdTree<pcl::PointXYZ>::Ptr kdtree(new pcl::search::KdTree<pcl::PointXYZ>); kdtree->setInputCloud(target_cloud); //初始化变换矩阵等参数 int iters = 0; double error = std::numeric_limits<double>::infinity(); Eigen::Matrix3f R = Eigen::Matrix3f::Identity(); Eigen::Vector3f T = Eigen::Vector3f::Zero(); Eigen::Matrix4f H = Eigen::Matrix4f::Identity(); Eigen::Matrix4f H_final = H; Eigen::Matrix4f transformation_matrix = Eigen::Matrix4f::Identity(); pcl::copyPointCloud(*source_cloud, *source_cloud_mid); //开始迭代,直到满足条件 clock_t start = clock(); while (error > 0.0001 && iters < 100) {
iters++; double last_error = error; double err = 0.0; pcl::transformPointCloud(*source_cloud_mid, *source_cloud_mid, H); std::vector<int>indexs(source_cloud_mid->size()); for (int i = 0; i < source_cloud_mid->size(); ++i) {
std::vector<int>index(1); std::vector<float>distance(1); kdtree->nearestKSearch(source_cloud_mid->points[i], 1, index, distance); err = err + sqrt(distance[0]); indexs[i] = index[0]; } pcl::copyPointCloud(*target_cloud, indexs, *target_cloud_mid); error = err / source_cloud->size(); std::cout << "iters:" << iters << " " << "error:" << error << std::endl; if (fabs(last_error - error) < 1e-6) break; //计算点云中心坐标 Eigen::Vector4f source_centroid, target_centroid_mid; pcl::compute3DCentroid(*source_cloud_mid, source_centroid); pcl::compute3DCentroid(*target_cloud_mid, target_centroid_mid); //去中心化 Eigen::MatrixXf souce_cloud_demean, target_cloud_demean; pcl::demeanPointCloud(*source_cloud_mid, source_centroid, souce_cloud_demean); pcl::demeanPointCloud(*target_cloud_mid, target_centroid_mid, target_cloud_demean); //计算W=q1*q2^T Eigen::MatrixXf W = (souce_cloud_demean * target_cloud_demean.transpose()).topLeftCorner(3, 3); //SVD分解得到新的旋转矩阵和平移矩阵 Eigen::JacobiSVD<Eigen::MatrixXf> svd(W, Eigen::ComputeFullU | Eigen::ComputeFullV); Eigen::MatrixXf U = svd.matrixU(); Eigen::MatrixXf V = svd.matrixV(); if (U.determinant() * V.determinant() < 0) {
for (int x = 0; x < 3; ++x) V(x, 2) *= -1; } R = V * U.transpose(); T = target_centroid_mid.head(3) - R * source_centroid.head(3); H << R, T, 0, 0, 0, 1; H_final = H * H_final; //更新变换矩阵 } transformation_matrix << H_final; clock_t end = clock(); std::cout << end - start << "ms" << std::endl; std::cout << transformation_matrix << std::endl; //配准结果 pcl::PointCloud<pcl::PointXYZ>::Ptr icp_cloud(new pcl::PointCloud<pcl::PointXYZ>); pcl::transformPointCloud(*source_cloud, *icp_cloud, transformation_matrix); pcl::io::savePCDFileBinary("icp_cloud.pcd", *icp_cloud); //可视化 pcl::visualization::PCLVisualizer viewer("registration Viewer"); pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> src_h(source_cloud, 0, 255, 0); //原始点云绿色 pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> tgt_h(target_cloud, 255, 0, 0); //目标点云红色 pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> final_h(icp_cloud, 0, 0, 255); //匹配好的点云蓝色 viewer.setBackgroundColor(255, 255, 255); viewer.addPointCloud(source_cloud, src_h, "source cloud"); viewer.addPointCloud(target_cloud, tgt_h, "target cloud"); viewer.addPointCloud(icp_cloud, final_h, "result cloud"); while (!viewer.wasStopped()) {
viewer.spinOnce(100); } return 0; }
讯享网
GN法求解
原理参考:ICP的不同解法:SVD分解,高斯牛顿法。代码实现
讯享网#include <iostream> #include <ctime> #include <pcl/io/pcd_io.h> #include <pcl/point_types.h> #include <pcl/common/transforms.h> #include <pcl/search/kdtree.h> #include <pcl/correspondence.h> #include <pcl/visualization/pcl_visualizer.h> #include "sophus/se3.hpp" int main(int argc, char** argv) {
pcl::PointCloud<pcl::PointXYZ>::Ptr source_cloud(new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::PointXYZ>::Ptr target_cloud(new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::PointXYZ>::Ptr source_cloud_mid(new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::PointXYZ>::Ptr target_cloud_mid(new pcl::PointCloud<pcl::PointXYZ>); //加载点云 pcl::io::loadPCDFile("bunny1.pcd", *source_cloud); pcl::io::loadPCDFile("bunny3.pcd", *target_cloud); //建立kd树 pcl::search::KdTree<pcl::PointXYZ>::Ptr kdtree(new pcl::search::KdTree<pcl::PointXYZ>); kdtree->setInputCloud(target_cloud); //初始化变换矩阵等参数 int iters = 0; Sophus::SE3f pose_gn; double error = std::numeric_limits<double>::infinity(); Eigen::Matrix3f R = Eigen::Matrix3f::Identity(); Eigen::Vector3f T = Eigen::Vector3f::Zero(); Eigen::Matrix4f H = Eigen::Matrix4f::Identity(); Eigen::Matrix4f H_final = H; Eigen::Matrix4f transformation_matrix = Eigen::Matrix4f::Identity(); pcl::copyPointCloud(*source_cloud, *source_cloud_mid); //开始迭代,直到满足条件 clock_t start = clock(); while (error > 0.0001 && iters < 100) {
iters++; double err = 0.0; pcl::transformPointCloud(*source_cloud_mid, *source_cloud_mid, H); std::vector<int>indexs(source_cloud_mid->size()); for (int i = 0; i < source_cloud_mid->size(); ++i) {
std::vector<int>index(1); std::vector<float>distance(1); kdtree->nearestKSearch(source_cloud_mid->points[i], 1, index, distance); err = err + sqrt(distance[0]); indexs[i] = index[0]; } pcl::copyPointCloud(*target_cloud, indexs, *target_cloud_mid); error = err / source_cloud->size(); std::cout << "iters:" << iters << " " << "error:" << error << std::endl; Eigen::Matrix<float, 6, 6> h = Eigen::Matrix<float, 6, 6>::Zero(); Eigen::Matrix<float, 6, 1> g = Eigen::Matrix<float, 6, 1>::Zero(); for (int i = 0; i < target_cloud->size(); ++i) {
Eigen::Vector3f p1_i = target_cloud_mid->points[i].getVector3fMap(); Eigen::Vector3f p2_i = source_cloud_mid->points[i].getVector3fMap(); Eigen::Vector3f p_n = pose_gn * p2_i; // 经过pose转化后的点 Eigen::Matrix<float, 3, 6> J = Eigen::Matrix<float, 3, 6>::Zero(); // 计算J J.block(0, 0, 3, 3) = Eigen::Matrix3f::Identity(); // 推导的jacobin中左边单位阵 J.rightCols<3>() = -Sophus::SO3f::hat(p_n); // 右边的李群 J = -J; Eigen::Vector3f e = p1_i - p_n; // 计算e h += J.transpose() * J; g += -J.transpose() * e; } Eigen::Matrix<float, 6, 1> dx = h.ldlt().solve(g); // 求解dx pose_gn = Sophus::SE3f::exp(dx) * pose_gn; // 进行更新,这里的dx是李代数 H = pose_gn.matrix(); H_final = H * H_final; //更新变换矩阵 //std::cout << H << std::endl; } transformation_matrix << H_final; clock_t end = clock(); std::cout << end - start << "ms" << std::endl; std::cout << transformation_matrix << std::endl; //配准结果 pcl::PointCloud<pcl::PointXYZ>::Ptr icp_cloud(new pcl::PointCloud<pcl::PointXYZ>); pcl::transformPointCloud(*source_cloud, *icp_cloud, transformation_matrix); pcl::io::savePCDFileBinary("icp.pcd", *icp_cloud); //可视化 pcl::visualization::PCLVisualizer viewer("registration Viewer"); pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> src_h(source_cloud, 0, 255, 0); //原始点云绿色 pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> tgt_h(target_cloud, 255, 0, 0); //目标点云红色 pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> final_h(icp_cloud, 0, 0, 255); //匹配好的点云蓝色 viewer.setBackgroundColor(255, 255, 255); viewer.addPointCloud(source_cloud, src_h, "source cloud"); viewer.addPointCloud(target_cloud, tgt_h, "target cloud"); viewer.addPointCloud(icp_cloud, final_h, "result cloud"); while (!viewer.wasStopped()) {
viewer.spinOnce(100); } return 0; }
四元数求解
参考文献:
【1】改进的ICP算法在点云配准中的应用
【2】基于KDTree改进的ICP算法在点云配准中的应用研究
#include <iostream> #include <ctime> #include <Eigen/Dense> #include <Eigen/Eigenvalues> #include <pcl/io/pcd_io.h> #include <pcl/point_types.h> #include <pcl/common/transforms.h> #include <pcl/search/kdtree.h> #include <pcl/correspondence.h> #include <pcl/visualization/pcl_visualizer.h> int main(int argc, char** argv) {
pcl::PointCloud<pcl::PointXYZ>::Ptr source_cloud(new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::PointXYZ>::Ptr target_cloud(new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::PointXYZ>::Ptr source_cloud_mid(new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::PointXYZ>::Ptr target_cloud_mid(new pcl::PointCloud<pcl::PointXYZ>); //加载点云 pcl::io::loadPCDFile("bunny1.pcd", *source_cloud); pcl::io::loadPCDFile("bunny3.pcd", *target_cloud); //建立kd树 pcl::search::KdTree<pcl::PointXYZ>::Ptr kdtree(new pcl::search::KdTree<pcl::PointXYZ>); kdtree->setInputCloud(target_cloud); //初始化变换矩阵等参数 int iters = 0; double error = std::numeric_limits<double>::infinity(); Eigen::Matrix3f R = Eigen::Matrix3f::Identity(); Eigen::Vector3f T = Eigen::Vector3f::Zero(); Eigen::Matrix4f H = Eigen::Matrix4f::Identity(); Eigen::Matrix4f H_final = H; Eigen::Matrix4f transformation_matrix = Eigen::Matrix4f::Identity(); pcl::copyPointCloud(*source_cloud, *source_cloud_mid); //开始迭代,直到满足条件 clock_t start = clock(); while (error > 0.0001 && iters < 100) {
iters++; double last_error = error; double err = 0.0; pcl::transformPointCloud(*source_cloud_mid, *source_cloud_mid, H); std::vector<int>indexs(source_cloud_mid->size()); for (int i = 0; i < source_cloud_mid->size(); ++i) {
std::vector<int>index(1); std::vector<float>distance(1); kdtree->nearestKSearch(source_cloud_mid->points[i], 1, index, distance); err = err + sqrt(distance[0]); indexs[i] = index[0]; } pcl::copyPointCloud(*target_cloud, indexs, *target_cloud_mid); error = err / source_cloud->size(); std::cout << "iters:" << iters << " " << "error:" << error << std::endl; if (fabs(last_error - error) < 1e-6) break; //计算点云中心坐标 Eigen::Vector4f source_centroid, target_centroid_mid; pcl::compute3DCentroid(*source_cloud_mid, source_centroid); pcl::compute3DCentroid(*target_cloud_mid, target_centroid_mid); //去中心化 Eigen::MatrixXf souce_cloud_demean, target_cloud_demean; pcl::demeanPointCloud(*source_cloud_mid, source_centroid, souce_cloud_demean); pcl::demeanPointCloud(*target_cloud_mid, target_centroid_mid, target_cloud_demean); Eigen::Matrix3f W = (souce_cloud_demean * target_cloud_demean.transpose()).topLeftCorner(3, 3); Eigen::Matrix3f WT = (target_cloud_demean * souce_cloud_demean.transpose()).topLeftCorner(3, 3); Eigen::Matrix3f delta = W - WT; Eigen::Matrix4f Q = Eigen::Matrix4f::Zero(); Q.topLeftCorner(1, 1) << W.trace(); Q.topRightCorner(1, 3) << delta(1, 2), delta(2, 0), delta(0, 1); Q.bottomLeftCorner(3, 1) << delta(1, 2), delta(2, 0), delta(0, 1); Q.bottomRightCorner(3, 3) << W + WT - W.trace() * Eigen::Matrix3f::Identity(); //for (int i = 0; i < source_cloud_mid->size(); ++i) //{
// float x_mi = source_cloud_mid->points[i].x; // float x_ni = target_cloud_mid->points[i].x; // float y_mi = source_cloud_mid->points[i].y; // float y_ni = target_cloud_mid->points[i].y; // float z_mi = source_cloud_mid->points[i].z; // float z_ni = target_cloud_mid->points[i].z; // Q(0, 0) += x_mi * x_ni + y_mi * y_ni + z_mi * z_ni; // Q(1, 0) += y_mi * z_ni - z_mi * y_ni; // Q(2, 0) += z_mi * x_ni - x_mi * z_ni; // Q(3, 0) += x_mi * y_ni - y_mi * x_ni; // Q(0, 1) += y_mi * z_ni - z_mi * y_ni; // Q(1, 1) += x_mi * x_ni - y_mi * y_ni - z_mi * z_ni; // Q(2, 1) += x_mi * y_ni + y_mi * x_ni; // Q(3, 1) += x_mi * z_ni + z_mi * x_ni; // Q(0, 2) += z_mi * x_ni - x_mi * z_ni; // Q(1, 2) += x_mi * y_ni + y_mi * x_ni; // Q(2, 2) += - x_mi * x_ni + y_mi * y_ni - z_mi * z_ni; // Q(3, 2) += y_mi * z_ni + z_mi * y_ni; // Q(0, 3) += x_mi * y_ni - y_mi * x_ni; // Q(1, 3) += x_mi * z_ni + z_mi * x_ni; // Q(2, 3) += y_mi * z_ni + z_mi * y_ni; // Q(3, 3) += - x_mi * x_ni - y_mi * y_ni + z_mi * z_ni; //} Eigen::SelfAdjointEigenSolver<Eigen::Matrix4f> eigen_solver(Q); Eigen::Vector4f eigenvalues = eigen_solver.eigenvalues(); //特征值一般按从小到大排列 Eigen::Matrix4f eigenvectors = eigen_solver.eigenvectors(); Eigen::Vector4f eigenvector = eigenvectors.col(3); float w = eigenvector(0), m = eigenvector(1), n = eigenvector(2), p = eigenvector(3); Eigen::Matrix3f R; R << 1 - 2 * (p * p + n * n), 2 * (m * n - w * p), 2 * (w * n + m * p), 2 * (m * n + w * p), 1 - 2 * (m * m + p * p), 2 * (n * p - w * m), 2 * (m * p - w * n), 2 * (n * p + w * m), 1 - 2 * (m * m + n * n); T = target_centroid_mid.head(3) - R * source_centroid.head(3); H << R, T, 0, 0, 0, 1; H_final = H * H_final; //更新变换矩阵 } transformation_matrix << H_final; clock_t end = clock(); std::cout << end - start << "ms" << std::endl; std::cout << transformation_matrix << std::endl; //配准结果 pcl::PointCloud<pcl::PointXYZ>::Ptr icp_cloud(new pcl::PointCloud<pcl::PointXYZ>); pcl::transformPointCloud(*source_cloud, *icp_cloud, transformation_matrix); pcl::io::savePCDFileBinary("icp_cloud.pcd", *icp_cloud); //可视化 pcl::visualization::PCLVisualizer viewer("registration Viewer"); pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> src_h(source_cloud, 0, 255, 0); //原始点云绿色 pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> tgt_h(target_cloud, 255, 0, 0); //目标点云红色 pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> final_h(icp_cloud, 0, 0, 255); //匹配好的点云蓝色 viewer.setBackgroundColor(255, 255, 255); viewer.addPointCloud(source_cloud, src_h, "source cloud"); viewer.addPointCloud(target_cloud, tgt_h, "target cloud"); viewer.addPointCloud(icp_cloud, final_h, "result cloud"); while (!viewer.wasStopped()) {
viewer.spinOnce(100); } return 0; }
BA求解
原理参考:基于高斯牛顿的点-点,点-线,点到面ICP过程推导
视觉SLAM笔记(39) 求解 ICP
代码参考:slam14讲高博的代码https://github.com/gaoxiang12/slambook2/blob/master/ch7/pose_estimation_3d3d.cpp
在此基础上修改,直接读入pcd点云,加入了迭代求解。
讯享网#include <iostream> #include <g2o/core/base_vertex.h> #include <g2o/core/base_unary_edge.h> #include <g2o/core/block_solver.h> #include <g2o/core/optimization_algorithm_gauss_newton.h> #include <g2o/solvers/eigen/linear_solver_eigen.h> #include <g2o/types/sba/types_six_dof_expmap.h> #include <pcl/io/pcd_io.h> #include <pcl/point_types.h> #include <pcl/common/transforms.h> #include <pcl/search/kdtree.h> #include <pcl/visualization/pcl_visualizer.h> // g2o edge class EdgeProjectXYZRGBDPoseOnly : public g2o::BaseUnaryEdge<3, Eigen::Vector3d, g2o::VertexSE3Expmap> {
public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW; EdgeProjectXYZRGBDPoseOnly(const Eigen::Vector3d& point) : _point(point) {
} virtual void computeError() {
const g2o::VertexSE3Expmap* pose = static_cast<const g2o::VertexSE3Expmap*> (_vertices[0]); // measurement is p, point is p' _error = _measurement - pose->estimate().map(_point); } virtual void linearizeOplus() {
g2o::VertexSE3Expmap* pose = static_cast<g2o::VertexSE3Expmap*>(_vertices[0]); g2o::SE3Quat T(pose->estimate()); Eigen::Vector3d xyz_trans = T.map(_point); double x = xyz_trans[0]; double y = xyz_trans[1]; double z = xyz_trans[2]; _jacobianOplusXi(0, 0) = 0; _jacobianOplusXi(0, 1) = -z; _jacobianOplusXi(0, 2) = y; _jacobianOplusXi(0, 3) = -1; _jacobianOplusXi(0, 4) = 0; _jacobianOplusXi(0, 5) = 0; _jacobianOplusXi(1, 0) = z; _jacobianOplusXi(1, 1) = 0; _jacobianOplusXi(1, 2) = -x; _jacobianOplusXi(1, 3) = 0; _jacobianOplusXi(1, 4) = -1; _jacobianOplusXi(1, 5) = 0; _jacobianOplusXi(2, 0) = -y; _jacobianOplusXi(2, 1) = x; _jacobianOplusXi(2, 2) = 0; _jacobianOplusXi(2, 3) = 0; _jacobianOplusXi(2, 4) = 0; _jacobianOplusXi(2, 5) = -1; } bool read(std::istream& in) {
return true; } bool write(std::ostream& out) const {
return true; } protected: Eigen::Vector3d _point; }; void bundleAdjustment(const pcl::PointCloud<pcl::PointXYZ>::Ptr& pts1, const pcl::PointCloud<pcl::PointXYZ>::Ptr& pts2, Eigen::Matrix4d& T) {
// 初始化g2o typedef g2o::BlockSolver< g2o::BlockSolverTraits<6, 3> > Block; // pose维度为 6, landmark 维度为 3 Block::LinearSolverType* linearSolver = new g2o::LinearSolverEigen<Block::PoseMatrixType>(); // 线性方程求解器 Block* solver_ptr = new Block(linearSolver); // 矩阵块求解器 g2o::OptimizationAlgorithmGaussNewton* solver = new g2o::OptimizationAlgorithmGaussNewton(solver_ptr); g2o::SparseOptimizer optimizer; optimizer.setAlgorithm(solver); // vertex g2o::VertexSE3Expmap* pose = new g2o::VertexSE3Expmap(); // camera pose pose->setId(0); pose->setEstimate(g2o::SE3Quat(Eigen::Matrix3d::Identity(), Eigen::Vector3d(0, 0, 0))); optimizer.addVertex(pose); // edges int index = 1; std::vector<EdgeProjectXYZRGBDPoseOnly*> edges; for (size_t i = 0; i < pts1->size(); i++) {
EdgeProjectXYZRGBDPoseOnly* edge = new EdgeProjectXYZRGBDPoseOnly(Eigen::Vector3d(pts2->points[i].x, pts2->points[i].y, pts2->points[i].z)); edge->setId(index); edge->setVertex(0, dynamic_cast<g2o::VertexSE3Expmap*> (pose)); edge->setMeasurement(Eigen::Vector3d(pts1->points[i].x, pts1->points[i].y, pts1->points[i].z)); edge->setInformation(Eigen::Matrix3d::Identity() * 1e4); optimizer.addEdge(edge); index++; edges.push_back(edge); } optimizer.setVerbose(true); optimizer.initializeOptimization(); optimizer.optimize(10); T = Eigen::Isometry3d(pose->estimate()).matrix(); } int main(int argc, char** argv) {
pcl::PointCloud<pcl::PointXYZ>::Ptr source_cloud(new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::PointXYZ>::Ptr target_cloud(new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::PointXYZ>::Ptr target_copy(new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::PointXYZ>::Ptr icp_cloud(new pcl::PointCloud<pcl::PointXYZ>); pcl::io::loadPCDFile("bunny1.pcd", *source_cloud); pcl::io::loadPCDFile("bunny3.pcd", *target_cloud); pcl::copyPointCloud(*source_cloud, *icp_cloud); pcl::copyPointCloud(*target_cloud, *target_copy); pcl::search::KdTree<pcl::PointXYZ>::Ptr kdtree(new pcl::search::KdTree<pcl::PointXYZ>); kdtree->setInputCloud(target_cloud); std::vector<int>indexs(source_cloud->size()); int iters = 0; double error = std::numeric_limits<double>::infinity(); while (error > 0.0001 && iters < 100) {
iters++; double last_error = error; double err = 0.0; Eigen::Matrix4d transformation_matrix; bundleAdjustment(target_copy, icp_cloud, transformation_matrix); pcl::transformPointCloud(*icp_cloud, *icp_cloud, transformation_matrix); for (int i = 0; i < icp_cloud->size(); ++i) {
std::vector<int>index(1); std::vector<float>distance(1); kdtree->nearestKSearch(icp_cloud->points[i], 1, index, distance); err = err + sqrt(distance[0]); indexs[i] = index[0]; } pcl::copyPointCloud(*target_cloud, indexs, *target_copy); error = err / source_cloud->size(); std::cout << "iters:" << iters << " " << "error:" << error << std::endl << std::endl; if (fabs(last_error - error) < 1e-6) break; } pcl::io::savePCDFileBinary("icp_cloud.pcd", *icp_cloud); //可视化 pcl::visualization::PCLVisualizer viewer("registration Viewer"); pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> src_h(source_cloud, 0, 255, 0); //原始点云绿色 pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> tgt_h(target_cloud, 255, 0, 0); //目标点云红色 pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> final_h(icp_cloud, 0, 0, 255); //匹配好的点云蓝色 viewer.setBackgroundColor(255, 255, 255); viewer.addPointCloud(source_cloud, src_h, "source cloud"); viewer.addPointCloud(target_cloud, tgt_h, "target cloud"); viewer.addPointCloud(icp_cloud, final_h, "result cloud"); while (!viewer.wasStopped()) {
viewer.spinOnce(100); } return 0; }

版权声明:本文内容由互联网用户自发贡献,该文观点仅代表作者本人。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如发现本站有涉嫌侵权/违法违规的内容,请联系我们,一经查实,本站将立刻删除。
如需转载请保留出处:https://51itzy.com/kjqy/76433.html