icp算法原理与实现

ICP算法原理

ICP是一种经典的点云匹配算法，用于估计两个点云PsP_sPs、PtP_tPt之间的位姿变换。
点云配准的问题可以描述为：
R∗,t∗=arg min⁡R,t1∣Ps∣∑i=1Ps∣∣pti−(R∗psi+t)∣∣2R^*,t^* = \argmin_{R,t}\frac{1}{|P_s|}\sum_{i=1}^{P_s}||p_t^i-(R*p_s^i+t)||^2R∗,t∗=R,targmin∣Ps∣1i=1∑Ps∣∣pti−(R∗psi+t)∣∣2
ICP的一般流程如下：

1、获得初始位姿(初始位姿对最终的匹配结果有较大影响)
2、迭代运行如下步骤：
- 为点云PtP_tPt中的点寻找PsP_sPs中的匹配点
- 依据匹配，计算PsP_sPs、PtP_tPt之间的位姿

ICP算法C++实现

point2point 解析法

#include <bits/stdc++.h>
#include <eigen3/Eigen/Dense>
#include <eigen3/Eigen/Geometry>
#include <ceres/ceres.h>
#include <ceres/rotation.h>
#include <sophus/so3.hpp>using namespace std;
using namespace Eigen;void pointToPoint(Matrix3Xd& src, Matrix3Xd& dst, Isometry3d& trans){int N = src.cols(),M = dst.cols();assert(M==N);Vector3d ps_mean = src.rowwise().mean();Vector3d qs_mean = dst.rowwise().mean();Matrix3Xd ps_centered = src.colwise() - ps_mean;Matrix3Xd qs_centered = dst.colwise() - qs_mean;Matrix3d k = qs_centered*ps_centered.transpose();JacobiSVD<Matrix3d> svd(k,ComputeFullU|ComputeFullV);Matrix3d r = svd.matrixU()*svd.matrixV().transpose();if(r.determinant()<0) r.col(2)*=-1;trans.linear() = r;trans.translation() = qs_mean-r*ps_mean;return;
}int main()
{vector<Vector3d> modelP;double a = 1,b = 2.,c = 3.;//椭球参数for(auto i = -1.4;i<=1.4;i+=0.28){for(auto j= -3.1;j<=3.1;j+=0.62){double x = a*cos(i)*cos(j);double y = b*cos(i)*sin(j);double z = c*sin(i);modelP.push_back(Vector3d(x,y,z));}}cout<<modelP.size()<<endl;Vector3d angles = {0.5,0.5,0.5};Vector3d translation = {0.5,0.5,0.5};std::default_random_engine dre;std::normal_distribution<double> noise(0,0.001);vector<Vector3d> modelT;Matrix3d R;R = AngleAxisd(angles(0), Vector3d::UnitX())* AngleAxisd(angles(1),  Vector3d::UnitY())* AngleAxisd(angles(2), Vector3d::UnitZ());cout<< "original rotation: \n"<< R <<endl;cout<< "original translation: "<< translation[0]<<" "<<translation[1]<<" "<<translation[2]<<endl;for(int i=0;i<modelP.size();i++){Vector3d xyz = R*modelP[i] + translation + Vector3d(noise(dre),noise(dre),noise(dre));modelT.push_back(xyz);}Quaterniond q = Quaterniond::Identity();q = q*AngleAxisd(0.1,Vector3d::UnitX());q = q*AngleAxisd(0.1,Vector3d::UnitY());q = q*AngleAxisd(0.1,Vector3d::UnitZ());Vector4d rot(q.x(),q.y(),q.z(),q.w());Vector3d trans(0.1,0.1,0.1);Isometry3d pose = Translation3d(trans)*Quaterniond(rot);//初始位姿Map<Matrix<double,3,Dynamic,ColMajor>> modelP_mat(&modelP[0].x(),3,modelP.size());Map<Matrix<double,3,Dynamic,ColMajor>> modelT_mat(&modelT[0].x(),3,modelT.size());Matrix3Xd src = modelP_mat;Matrix3Xd dst = modelT_mat;int max_iter = 5, iter = 0;while (iter++<max_iter){pointToPoint(src,dst,pose);}std::cout<<"optimized transform: \n "<<pose.matrix()<<endl;return 0;
}

利用ceres求解：

#include <bits/stdc++.h>
#include <eigen3/Eigen/Dense>
#include <eigen3/Eigen/Geometry>
#include <ceres/ceres.h>
#include <ceres/rotation.h>
#include <sophus/so3.hpp>using namespace std;
using namespace Eigen;
struct ICPError
{ICPError(const Vector3d& p,const Vector3d& q):p_(p),q_(q){}template<typename T>bool operator()(const T* const rt,T*residual) const{T rot[3*3];ceres::AngleAxisToRotationMatrix(rt, rot);Eigen::Matrix<T, 3, 3> R = Eigen::Matrix<T, 3, 3>::Identity();for (int i = 0; i < 3; ++i) {for (int j = 0; j < 3; ++j) {R(i, j) = rot[i+3*j];}}Eigen::Map<const Matrix<T,3,1>> trans(rt+3);residual[0] = (q_.template cast<T>() - (R * p_.template cast<T>() + trans)).norm();return true;}const Vector3d p_;const Vector3d q_;
};int main()
{vector<Vector3d> modelP;double a = 1,b = 2.,c = 3.;//椭球参数for(auto i = -1.4;i<=1.4;i+=0.28){for(auto j= -3.1;j<=3.1;j+=0.62){double x = a*cos(i)*cos(j);double y = b*cos(i)*sin(j);double z = c*sin(i);modelP.push_back(Vector3d(x,y,z));}}cout<<modelP.size()<<endl;Vector3d angles = {0.5,0.5,0.5};Vector3d translation = {0.5,0.5,0.5};std::default_random_engine dre;std::normal_distribution<double> noise(0,0.001);vector<Vector3d> modelT;Matrix3d R;R = AngleAxisd(angles(0), Vector3d::UnitX())* AngleAxisd(angles(1),  Vector3d::UnitY())* AngleAxisd(angles(2), Vector3d::UnitZ());cout<< "original rotation: \n"<< R <<endl;cout<< "original translation: "<< translation[0]<<" "<<translation[1]<<" "<<translation[2]<<endl;for(int i=0;i<modelP.size();i++){Vector3d xyz = R*modelP[i] + translation + Vector3d(noise(dre),noise(dre),noise(dre));modelT.push_back(xyz);}double rt[6] = {0.,0.,0.,0,0,0};ceres::Problem problem;for(int i=0;i<modelP.size();i++){ceres::CostFunction* pCostFunction = new ceres::AutoDiffCostFunction<ICPError,1,6>(new ICPError(modelP[i],modelT[i]));problem.AddResidualBlock(pCostFunction, nullptr, rt);}ceres::Solver::Options options;options.minimizer_progress_to_stdout = false;options.linear_solver_type = ceres::DENSE_QR;options.max_num_iterations = 200;ceres::Solver::Summary summary;ceres::Solve(options, &problem,&summary);Eigen::Map<const Eigen::Vector3d> f(rt);Eigen::Matrix3d Rf = Sophus::SO3d::exp(f).matrix();cout<< "optimized rotation: \n "<< Rf <<endl;cout<< "optimized translation: "<< rt[3]<<" "<<rt[4]<<" "<<rt[5]<<endl;return 0;
}

icp算法原理与实现相关推荐

【动手学MVG】ICP算法原理和代码实现
介绍本文介绍了相同尺度的点云ICP算法,若点云之间尺度不同,则需要估计尺度,可以看一下另一篇博文<通过ICP配准两组尺度不同点云, 统一坐标系(附代码)> 本文介绍的ICP问题,是在3D ...
干货 | 三维点云配准：ICP 算法原理及推导
编者荐语点云配准可以分为粗配准(Coarse Registration)和精配准(Fine Registration)两步.粗配准指的是在两幅点云之间的变换完全未知的情况下进行较为粗糙的配准,目的主 ...
ICP算法实现（C++）
用C++实现的ICP(Iterative Closest Point,迭代最近点)算法,借助了PCL库实现点云基本变换.KD-tree以及可视化部分,核心迭代部分没有调用PCL的api.代码在KD-t ...
ICP算法的原理与实现
一.背景与意义点云数据能够以较小的存储成本获得物体准确的拓扑结构和几何结构,因而获得越来越广泛的关注.在实际的采集过程中,因为被测物体尺寸过大,物体表面被遮挡或者三维扫描设备的扫描角度等因素,单次的 ...
VTK修炼之道58：图形基本操作进阶_点云配准技术（迭代最近点ICP算法）
1.Iterative Closest Points算法点云数据配准最经典的方法是迭代最近点算法(Iterative Closest Points,ICP).ICP算法是一个迭代的过程,每次迭代中对 ...
经典ICP算法的问题
最近可能要用三维点云实现一个三维场景重建的功能,从经典的ICP算法开始,啃了一些文档,对其原理也是一知半解. 迭代最近点算法综述大致参考了这份文档之后,照流程用MATLAB实现了一个简单的ICP算法 ...
PCL点云库：ICP算法
ICP(Iterative Closest Point迭代最近点)算法是一种点集对点集配准方法.在VTK.PCL.MRPT.MeshLab等C++库或软件中都有实现,可以参见维基百科中的ICP Alg ...
点云配准2：icp算法在PCL1.10.0上的实现+源码解析
目录本文最后实现的配准实例点云配准系列准备程序结构主程序 1.为什么要降采样 2.体素降采样原理 3.点云更新 icp 配准前的参数设置 icp配准算法内部对应点对确定(determine ...
ICP算法学习记录（包括基础概念，计算推导）
首先,ICP,全称叫做(Iterative Closest Point,迭代最近点),在slam中用来做点云匹配,点云匹配就是将同一个物体,在不同视角下的两组或多组点云,通过旋转加平移让他们匹配起来. ...

icp算法原理与实现

ICP算法原理

ICP算法C++实现

icp算法原理与实现相关推荐

最新文章

热门文章