点云配准之icp的简单实现

C++实现经典的点云配准算法——ICP，根据高博视觉14讲的推导（具体过程看书，还是很好理解的），ICP的计算过程如下所示：

具体的C++代码实现如下：

void pose_estimation_3d3d (const vector<Point3f>& pts1,const vector<Point3f>& pts2,Mat& R, Mat& t
)
{Point3f p1, p2;     // center of massint N = pts1.size();for ( int i=0; i<N; i++ ){p1 += pts1[i];p2 += pts2[i];}p1 = Point3f( Vec3f(p1) /  N);p2 = Point3f( Vec3f(p2) / N);vector<Point3f>     q1 ( N ), q2 ( N ); // remove the centerfor ( int i=0; i<N; i++ ){q1[i] = pts1[i] - p1;q2[i] = pts2[i] - p2;}// compute q1*q2^TEigen::Matrix3d W = Eigen::Matrix3d::Zero();for ( int i=0; i<N; i++ ){W += Eigen::Vector3d ( q1[i].x, q1[i].y, q1[i].z ) * Eigen::Vector3d ( q2[i].x, q2[i].y, q2[i].z ).transpose();}cout<<"W="<<W<<endl;// SVD on WEigen::JacobiSVD<Eigen::Matrix3d> svd ( W, Eigen::ComputeFullU|Eigen::ComputeFullV );Eigen::Matrix3d U = svd.matrixU();Eigen::Matrix3d V = svd.matrixV();if (U.determinant() * V.determinant() < 0){for (int x = 0; x < 3; ++x){U(x, 2) *= -1;}}cout<<"U="<<U<<endl;cout<<"V="<<V<<endl;Eigen::Matrix3d R_ = U* ( V.transpose() );Eigen::Vector3d t_ = Eigen::Vector3d ( p1.x, p1.y, p1.z ) - R_ * Eigen::Vector3d ( p2.x, p2.y, p2.z );// convert to cv::MatR = ( Mat_<double> ( 3,3 ) <<R_ ( 0,0 ), R_ ( 0,1 ), R_ ( 0,2 ),R_ ( 1,0 ), R_ ( 1,1 ), R_ ( 1,2 ),R_ ( 2,0 ), R_ ( 2,1 ), R_ ( 2,2 ));t = ( Mat_<double> ( 3,1 ) << t_ ( 0,0 ), t_ ( 1,0 ), t_ ( 2,0 ) );
}

点云配准之icp的简单实现相关推荐

点云配准NDT+ICP
点云配准NDT+ICP #include <iostream> #include <pcl/io/pcd_io.h> #include <pcl/point_types. ...
干货 | 三维点云配准：ICP 算法原理及推导
编者荐语点云配准可以分为粗配准(Coarse Registration)和精配准(Fine Registration)两步.粗配准指的是在两幅点云之间的变换完全未知的情况下进行较为粗糙的配准,目的主 ...
点云配准算法ICP及其各种变体
点击上方"3D视觉工坊",选择"星标" 干货第一时间送达作者丨流川峰来源丨深蓝AI 介绍点云配准(Point Cloud Registration)算法指 ...
PCL点云配准（ICP）
/*********************************** 点云配准 *****************************************/ #include <pc ...
点云配准（二）— python open3d ICP方法
上一节中介绍了点云配准的基础知识.本节将采用python open3d来进行点云配准. open3d安装和点云配准介绍,请参考: Open3d读写ply点云文件_Coding的叶子的博客-CSDN博客 ...
点云配准网络 PCRNet: Point Cloud Registration Network using PointNet Encoding 2019
本文使用PointNet对点云提取全局特征,不需要计算点云之间的一一对应关系,因此能够实现快速的点云配准. PCRNet根本没有用到局部特征,因此在速度上有优势,精度上会有所妥协.加入了局部特征信息的 ...
点云配准各种方法总结[不定时更新]
本文主要用来记录我在学习过程中接触到的一些或新或旧的点云配准的方法,有些方法我可能还不怎么理解,如果有错误欢迎大家指出. 2018/04/15 添加LORAX 2018/04/08 添加DO法和结合法 ...
简述3D点云配准算法
蝶恋花·槛菊愁烟兰泣露槛菊愁烟兰泣露,罗幕轻寒,燕子双飞去. 明月不谙离恨苦,斜光到晓穿朱户. 昨夜西风凋碧树,独上高楼,望尽天涯路. 欲寄彩笺兼尺素.山长水阔知何处? --晏殊导读: 3D点 ...
点云配准各种方法总结
粗配准方案 LORAX 4点法(4-Points Congruent Sets,4PCS) Super 4PCS(Super 4-Points Congruent Sets) SK-4PCS(Sema ...

点云配准之icp的简单实现

点云配准之icp的简单实现相关推荐

最新文章

热门文章