ICP学习笔记

最新推荐文章于 2022-12-05 13:49:04 发布

磊磊哈哈

最新推荐文章于 2022-12-05 13:49:04 发布

阅读量204

点赞数

原文链接：https://editor.csdn.net/md?articleId=110368642

版权

SLAM十四讲专栏收录该内容

37 篇文章 8 订阅

订阅专栏

来自slam14讲

ICP（Iterative Closest Point,ICP）

求解3D3D 3维点之间的对应关系。
$P=\{p_1,p_2,...,p_3\}:一组3D点$
$P'=\{p_1',p'_2,...,p_3,\}:与P对应的一组3D点$
求解 $P 与 P^{'}$ 之间的对应关系即求解 $R, T$ :
$P'_i=R_iP_i+T$
两种方法：
（1）：构建线性最优化问题，SVD求解
（2）：使用非线性方法，BA求解方程

构建线性最优化问题，SVD求解

构建最小二乘
$min_{R,t}=\frac{1}{2}\sum^n_{i=1}||p_i-(Rp_i'+t)||^2_2$
化简：
$p=\frac{1}{n}\sum^n_{i=1}(p_i)$
$p'=\frac{1}{n}\sum_{i=1}^n(p_i')$
$\sum^n_{i=1}||p_i-(Rp_i'+t)||^2_2=\sum^n_{i=1}||p_i-Rp_i-t-p+Rp'+p-Rp'||^2\\ =\sum^n_{i=1}||((p_i-p)-R(p_i-p'))+(p-Rp'-t)||^2\\ =\sum^n_{i=1}(||(p_i-p)-R(p_i-p')||^2+||(p-Rp'-t)||^2+2(p_i-p-R(p_i-p')^T(p-Rp'-t))\\ =\sum^n_{i=1}(||(p_i-p)-R(p_i-p')||^2+||(p-Rp'-t)||^2\\ st:\\ \sum^n_{i=1}(p_i-p-R(p_i-p')^T(p-Rp'-t))\\ \sum^n_{i=1}(p_i-\frac{1}{n}\sum^n_{i=1}(p_i)-R(p_i'-\frac{1}{n}\sum_{i=1}^n(p_i')))\\ =0$
得到求解步骤如下：
（1）：计算质心位置
（2）：求解旋转矩阵的优化解：
$R*=argmin_R\frac{1}{2}\sum_{i=1}^n||q_i-Rq_i'||^2$
化简：
$argmin_R\sum_{i=1}^n||q_i-Rq_i'||^2=argmin_R\sum_{i=1}^n(q_i^Tq_i+{q'}_i^TR^TRq_i-2q^T_iRq'_i)\\ 因为：\\ q_i^Tq_i与R无关的常数项;R^TR=I,{q'}_i^TR^TRq_i也与R无关\\ \iff argmin_R\sum_{i=1}^n(-q^T_iRq'_i)\\ \\argmin_R\sum_{i=1}^n(-q^T_iRq'_i)\\ =argmin_R\left[\begin{matrix}x&y&z\end{matrix}\right]\left[\begin{matrix}R_{11}&R_{12}&R_{13}\\R_{21}&R_{22}&R_{23}\\R_{31}&R_{32}&R_{33}\end{matrix}\right]\left[\begin{matrix}x'\\y'\\z'\end{matrix}\right]\\ =argmin_R-((R_{11}+R_{12}+R_{13})xx'+(R_{21}+R_{22}+R_{23})yy'+(R_{31}+R_{32}+R_{33}))zz'\\ =-tr(R\sum^n_{i=1}q'_iq'^{T}_i)\\$
求解: $tr(R\sum^n_{i=1}q'_iq'^{T}_i)$
$W=\sum_{i=1}^nq_iq'^{T}_i\\ 令：W=U\sum V^T(svd分解)\\ R=UV^T(W满秩)$
（3）：根据求解出的R计算t：
$t * = p - R p^{'}$

非线性求解

使用李代数求解

目标函数
$min_{\xi}=\frac{1}{2}\sum^n_{i=1}||(p_i-e^{\xi×}p_i')||_2^2$
一阶导数（雅克比矩阵）：
$\frac{\partial e}{\partial \delta\xi}=-(e^{\xi×}p_i')^\vee=\left[\begin{matrix}-I &hat{(Rp+t)}\end{matrix}\right]$

实现

slams十四讲

#include <iostream>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/SVD>
#include <opencv/cv.hpp>
#include <g2o/core/base_binary_edge.h>
#include <g2o/core/base_edge.h>
#include <g2o/core/base_multi_edge.h>
#include <g2o/core/base_vertex.h>
#include <g2o/core/block_solver.h>
#include <sophus/se3.hpp>
#include <g2o/core/optimization_algorithm_gauss_newton.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/solvers/dense/linear_solver_dense.h>
#include <g2o/core/base_unary_edge.h>
#include <chrono>
using namespace std;
using namespace Eigen;
using namespace g2o;
using namespace cv;
void find_feature(const Mat img1,const Mat img2, vector<KeyPoint> &p1, vector<KeyPoint> &p2,vector<DMatch> &Matchers);
Point2d pix2cam( Point2d pt,Mat K){
    return Point2d((pt.x-K.at<double>(0,2))/K.at<double>(0,0),(pt.y-K.at<double>(1,2))/K.at<double>(1,2));
};
void ICP_SVD(vector<Point3d> &point_3d_1,vector<Point3d> &point_3d_2,Matrix3d &R,Vector3d &t);
void ICP_BA(vector<Point3d> &point_3d_1,vector<Point3d> &point_3d_2,Matrix3d &R,Vector3d &t);
int main(int argc, const char** argv) {
    Mat K = (Mat_<double>(3, 3) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1);
    Matrix3d R1,R2;Vector3d T1,T2;
    Mat img1=imread("/home/n1/notes/3d3d/ICP/1.png",CV_LOAD_IMAGE_COLOR);
    Mat img2=imread("/home/n1/notes/3d3d/ICP/2.png",CV_LOAD_IMAGE_COLOR);
    Mat img1_depth=imread("/home/n1/notes/3d3d/ICP/1_depth.png",CV_LOAD_IMAGE_UNCHANGED);
    Mat img2_depth=imread("/home/n1/notes/3d3d/ICP/2_depth.png",CV_LOAD_IMAGE_UNCHANGED);
    vector<DMatch> Matchers;
    vector<KeyPoint> Point1,Point2;
    find_feature(img1,img2,Point1,Point2,Matchers);
     cout << "一共找到了" << Matchers.size() << "组匹配点" << endl;
    vector<Point3d> point_3d_1,point_3d_2;
    for(DMatch m:Matchers){
        ushort depth_1=img1_depth.at<unsigned short>(int(Point1[m.queryIdx].pt.y),int(Point1[m.queryIdx].pt.x));
        ushort depth_2=img1_depth.at<unsigned short>(int(Point1[m.trainIdx].pt.y),int(Point1[m.trainIdx].pt.x));
        double dd1=depth_1/5000;
        double dd2=depth_2/5000;
        if (depth_1 == 0 || depth_2 == 0)   // bad depth
            continue;
        Point2d p2d_1=pix2cam(Point1[m.queryIdx].pt,K);
        Point2d p2d_2=pix2cam(Point2[m.trainIdx].pt,K);
        Point3d Point1_i=Point3d(p2d_1.x*dd1,p2d_1.y*dd1,dd1);//单位化
        Point3d Point2_i=Point3d(p2d_2.x*dd2,p2d_2.y*dd2,dd2);
        
        point_3d_1.push_back(Point1_i);
        point_3d_2.push_back(Point2_i);
    }

    ICP_SVD(point_3d_1,point_3d_2,R1,T1);
    cout<<"R1"<<R1<<endl;
    cout<<"T1"<<T1<<endl;
    ICP_BA(point_3d_1,point_3d_2,R2,T2);
    cout<<"R2"<<R2<<endl;
    cout<<"T2"<<T2<<endl;
    //验证
      for (int i = 0; i < 5; i++) {
        cout << "p1 = " << point_3d_1[i] << endl;
        cout << "p2 = " << point_3d_2[i] << endl;
        Matrix<double,3,1> p2;
        p2<<point_3d_2[i].x, point_3d_2[i].y, point_3d_2[i].z;

        cout << "(R*p2+t) = " <<R1*p2 + T2<< endl;
        cout << endl;
        }
    waitKey();
    return 0;
}
bool cmp( DMatch &m1, DMatch &m2) { return m1.distance < m2.distance; }
void find_feature(const Mat img1,const Mat img2, vector<KeyPoint> &p1, vector<KeyPoint> &p2,vector<DMatch> &Matchers){
    Mat description1,description2;
    Ptr<FeatureDetector> Detector=ORB::create();
    Ptr<DescriptorExtractor> Descriptor=ORB::create();
    Ptr<DescriptorMatcher> Macher= DescriptorMatcher::create("BruteForce-Hamming");

    Detector->detect(img1,p1);
    Detector->detect(img2,p2);

    Descriptor->compute(img1,p1,description1);
    Descriptor->compute(img2,p2,description2);
    vector<DMatch> match;
    double min_dist = 10000, max_dist = 0;
    Macher->match(description1,description2,match);
   
    for(int i=0;i<match.size();i++){
        if(min_dist>match[i].distance)min_dist=match[i].distance;
        if(max_dist<match[i].distance)max_dist=match[i].distance;
    }
    cout<<"max_value:"<<max_dist<<endl;
    cout<<"min_dist:"<<min_dist<<endl;
    for(int dist=0;dist<match.size();dist++){
        if(match[dist].distance<=max(2*min_dist,30.0)){
            Matchers.push_back(match[dist]);
        }
    }
    Mat Match_img;
    drawMatches(img1,p1,img2,p2,Matchers,Match_img);
   // imshow("1",Match_img);
}
void ICP_SVD(vector<Point3d> &point_3d_1,vector<Point3d> &point_3d_2,Matrix3d &R,Vector3d &t){
    Point3d center_1,center_2;//两组向量质心坐标
    Point3d sum_p1,sum_p2;//两组坐标的和
    vector<Point3d> p1,p2;//两组向量去质心坐标后的值
    //获取质心坐标
    int size_points=point_3d_1.size();//数组的大小
   
    for(int i=0;i<size_points;i++){
        sum_p1+=point_3d_1[i];
        sum_p2+=point_3d_2[i];
    }
    //cout<<sum_p1<<endl;
    ;
    center_1=sum_p1/size_points;
    center_2=sum_p2/size_points;
    //cout<<center_1<<endl;cout<<center_2<<endl;
   
    //获取去质心坐标
    for(int i=0;i<size_points;i++){
        p1.push_back(point_3d_1[i]-center_1);
        p2.push_back(point_3d_2[i]-center_2);
    }

    //计算矩阵M
    Eigen::Matrix3d W=Matrix3d::Zero();
    for(int i=0;i<size_points;i++){
        W +=Eigen::Vector3d(p1[i].x,p1[i].y,p1[i].z)*(Eigen::Vector3d(p2[i].x,p2[i].y,p2[i].z).transpose());
    };
    Eigen::JacobiSVD<Eigen::Matrix3d> svd(W,ComputeFullU|ComputeFullV);
    Matrix3d U=svd.matrixU();
    Matrix3d V=svd.matrixV();
    cout << "U=" << U << endl;
    cout << "V=" << V << endl;
     R=U*(V.transpose());
     if(R.determinant()<0){R=-R;}
     t=Vector3d(center_1.x,center_1.y,center_1.z)-R*Vector3d(center_2.x,center_2.y,center_2.z);
}

class Vertpose:public g2o::BaseVertex<6,Sophus::SE3d>{
    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
        virtual void setToOriginImpl() override{
            _estimate=Sophus::SE3d();
        };
        virtual void oplusImpl(const double *update){
             Eigen::Matrix<double,6,1> M_update;
             M_update<<update[0],update[1],update[2],update[3],update[4],update[5];
             _estimate=Sophus::SE3d::exp(M_update)*_estimate;
        };
        virtual bool read(istream &in)override{};
        virtual bool write(ostream &out)const override{};
};
class MyEdge:public g2o::BaseUnaryEdge<3,Eigen::Vector3d,Vertpose>{
    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
        virtual bool read(istream &in)override{};
        virtual bool write(ostream &out)const override{};
        MyEdge(const Eigen::Vector3d &point):_point(point){};
        virtual void  computeError()override{
            const Vertpose *pose=static_cast<const Vertpose *>(_vertices[0]);
            _error=_measurement-pose->estimate()*_point;
        }
          virtual void linearizeOplus() override {
            Vertpose *pose = static_cast<Vertpose *>(_vertices[0]);
            Sophus::SE3d T = pose->estimate();
            Eigen::Vector3d xyz_trans = T * _point;
            _jacobianOplusXi.block<3, 3>(0, 0) = -Eigen::Matrix3d::Identity();
            _jacobianOplusXi.block<3, 3>(0, 3) = Sophus::SO3d::hat(xyz_trans);
        }
    private:
        Eigen::Vector3d _point;
};
void ICP_BA(vector<Point3d> &point_3d_1,vector<Point3d> &point_3d_2,Matrix3d &R,Vector3d &t){
    typedef g2o::BlockSolver<g2o::BlockSolverTraits<6,3> > BlockType;
    typedef g2o::LinearSolverDense<BlockType::PoseMatrixType > linearType;//定义线性求解器
    auto slover= new g2o::OptimizationAlgorithmLevenberg(
    g2o::make_unique<BlockType>(g2o::make_unique<linearType>()));
    g2o::SparseOptimizer Optimer;//图模型
    Optimer.setAlgorithm(slover);//添加求解器
    Optimer.setVerbose(true);    //设置显示输出
    
    //添加顶点
    Vertpose *pose=new Vertpose();
    pose->setId(0);
    pose->setEstimate(Sophus::SE3d());
    Optimer.addVertex(pose);

    //添加边
    int index=1;
    for(int i=0;i<point_3d_1.size();i++){
        MyEdge *edge=new MyEdge(Vector3d(point_3d_2[i].x,point_3d_2[i].y,point_3d_2[i].z));
        edge->setId(index);
        edge->setVertex(0,pose);
        edge->setMeasurement(Vector3d(point_3d_1[i].x,point_3d_1[i].y,point_3d_1[i].z));
        edge->setInformation(Eigen::Matrix3d::Identity());//设置协防差矩阵初值
        Optimer.addEdge(edge);
    }
    chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
    Optimer.initializeOptimization();//创建实例
    Optimer.optimize(10);//迭代次数
      chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
    chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
    cout << "optimization costs time: " << time_used.count() << " seconds." << endl;
    cout << endl << "after optimization:" << endl;
    cout << "T=\n" << pose->estimate().matrix() << endl;
    R=pose->estimate().rotationMatrix();
    t=pose->estimate().translation();
}

磊磊哈哈

关注

0
点赞
踩
0

收藏

觉得还不错? 一键收藏
0
评论
ICP学习笔记

来自slam14讲ICP（Iterative Closest Point,ICP）求解3D3D 3维点之间的对应关系。P={p1,p2,...,p3}:一组3D点P=\{p_1,p_2,...,p_3\}:一组3D点P={p1,p2,...,p3}:一组3D点P′={p1′,p2′,...,p3,}:与P对应的一组3D点P'=\{p_1',p'_2,...,p_3,\}:与P对应的一组3D点P′={p1′,p2′,...,p3,}:与P对应的一组3D点求解P与P′P与P'P与P′之间的
复制链接

扫一扫

专栏目录