PCL迭代最近点算法（ICP）的C++实现

简介：
在*中是这样介绍迭代最近点算法。迭代最近点（ICP）是一种用于最小化两点云之间差异的算法。给定P、Q两个点集，求解旋转矩阵R和平移矩阵T使得min{distance(P,Q)}.
C++算法流程图：

四元数求解方法


C++代码

/**

** Filename:icp.h

** Copyright (c) 2017-2018 

** Author:Rson

** Date:2018/04/03

** Modifier:

** Date:

** Description: 

**

** Version:

**/

#ifndef _ICP_H
#define _ICP_H

#include <vector>
#include <string>

#include <pcl\point_cloud.h>
#include <pcl\point_types.h>
#include <pcl\visualization\pcl_visualizer.h>

struct Vertex
{
    double coord[3];
};

struct Point
{
    float x;
    float y;
    float z;
};

class ICP
{
public:
    ICP();
    ICP(int controlnum=1000,double thre=0.01,int iter=100);
    virtual ~ICP();

    void readfile(std::string firstname, std::string secondname);
    void run();
    void writefile(std::string name);
    void showcloud(std::string firstname, std::string secondname,std::string thirdname);

private:
    void initransmat();//初始化旋转矩阵和平移矩阵
    void sample();//采样控制点
    double closest();//找出最近点并返回误差
    void getcenter();//获取两个控制点的中心
    void rmcontcenter();//移动两个控制点的中心
    void transform();//将四元数转换成矩阵并更新整个旋转矩阵
    void uprotate();// 更新变换矩阵
    void uptranslate();
    void updata();//更新控制点坐标
    void applyall();

private:
    double distance(Vertex a, Vertex b);
    void printTT();
    void printTR();


private:
    int conNum;//控制点数目
    int iterate;//迭代次数
    double threshold;//阈值
    std::vector<Vertex> VarrP;//起始点
    std::vector<Vertex> VarrQ;
    Vertex meanP;//控制点中心
    Vertex meanQ;
    Vertex *contP;//P控制点
    Vertex *contQ;
    Vertex *rmcoP;//移动后的控制点
    Vertex *rmcoQ;

    int *index; //在采样控制点和寻找相应的点索引时使用

    double TT[3];//平移向量
    double TR[3][3];//旋转矩阵
    double Rw[3][3];//旋转的步距
    double Tw[3];//平移的步距
    double quad[4];//四元数


};

#endif  /* ICP_H */

/**

** Filename:icp.cpp

** Copyright (c) 

** Author:Rson

** Date:2018/04/03

** Modifier:

** Date:

** Description:

**

** Version:

**/
#include "stdafx.h"
#include <iostream>
#include <sstream>
#include <fstream>
#include <cassert>
#include <math.h>
#include <time.h>
#include <newmat10/newmat.h>
#include <newmat10/newmatap.h>
#include "ICP.h"


ICP::ICP()
{
}

ICP::ICP(int controlnum, double thre, int iter)
{
    conNum = controlnum;
    threshold = thre;
    iterate = iter;

    contP = new Vertex[conNum];
    assert(contP != NULL);
    contQ = new Vertex[conNum];
    assert(contQ != NULL);
    rmcoP = new Vertex[conNum];
    assert(rmcoP != NULL);
    rmcoQ = new Vertex[conNum];
    assert(rmcoQ != NULL);
    index = new int[conNum];
    assert(index != NULL);
}

ICP::~ICP()
{
    delete[] contP;
    delete[] contQ;
    delete[] rmcoP;
    delete[] rmcoQ;
    delete[] index;
}

void ICP::readfile(std::string firstname, std::string secondname)
{
    std::cout << "读取两个点云文件！！" <<std:: endl;
    ifstream in;
    in.open(firstname.c_str(), std::ios::in);
    if (!in.is_open())
    {
        std::cout << "error open!" <<std:: endl;
        system("pause");
    }
    Vertex v;
    while(in>>v.coord[0]>>v.coord[1]>>v.coord[2])
    {
        VarrP.push_back(v);
    }
    std::cout << "点云A的大小：" << VarrP.size() << std::endl;
    in.close();

    //
    in.open(secondname.c_str(), std::ios::in);
    if (!in.is_open())
    {
        cout << "error open!" << endl;
        system("pause");
    }

    //Vertex v;
    while (in >> v.coord[0] >> v.coord[1] >> v.coord[2])
    {
        VarrQ.push_back(v);
    }
    std::cout << "点云B的大小：" << VarrQ.size() << std::endl;
    in.close();
}
void ICP::run()
{
    initransmat();
    sample();
    //
    double err = closest();
    std::cout << "初始误差：error = " << err << std::endl;
    //
    for (int i = 0; i<iterate; i++)
    {
        getcenter();
        rmcontcenter();
        transform();
        uprotate();
        uptranslate();
        updata();
        double newerr = closest();
        std::cout << "迭代次数 times = " << i << std::endl;
        std::cout << "error = " << newerr << std::endl;
        double delta = fabs(err - newerr) / conNum;
        std::cout << "delta = " << delta << std::endl;
        if (delta<threshold)
            break;
        err = newerr;

    }
    printTR();
    printTT();
    applyall();
}
void ICP::writefile(std::string name)
{
    ofstream outobj;
    outobj.open(name.c_str());
    //outobj << "# Geomagic Studio" << endl;
    int num = 1;
    for (vector<Vertex>::const_iterator p = VarrP.begin(); p != VarrP.end(); p++)
    {
        Vertex v;
        v = *p;
        outobj << v.coord[0] << " " << v.coord[1] << " " << v.coord[2] << endl;
        //outobj << "p " << num++ << endl;

        //outobj << "v " << v.coord[0] << " " << v.coord[1] << " " << v.coord[2] << endl;
        //outobj << "p " << num++ << endl;
    }
    //
    outobj.close();
}

//初始化变换矩阵
//    -
//    | 1.0  0.0 0.0 | 0.0  |
//    | 0.0  1.0 0.0 | 0.0  |
//    | 0.0  0.0 1.0 | 0.0  |
//    | -------------|----- |
//    | 0.0 0.0 0.0  | 1.0  |
void ICP::initransmat()//初始化变换矩阵
{
    std::cout << "初始化变换矩阵" << endl;
    for (int i = 0; i < 3; i++)
        TT[i] = 0;
    for (int i = 0; i < 3; i++)
    {
        for (int j = 0; j < 3; j++)
        {
            if (i != j)
                TR[i][j] = 0.0;
            else
                TR[i][j] = 1.0;
        }
    }
}


//随机选取控制点,并存储在contP中

void ICP::sample()
{
    std::cout<<"随机选取控制点,并存储在contP中"<<std::endl;
    int N = VarrP.size();
    bool *flag = new bool[N];
    assert(flag != NULL);
    for (int i = 0; i < N; i++)
        flag[i] = false;
    //随机选择一个控制点，并记录其索引
    srand((unsigned)time(NULL));
    for (int i = 0; i < conNum; i++)
    {
        while (true)
        {
            int sam = rand() % N;
            if (!flag[sam])
            {
                index[i] = sam;
                flag[sam] = true;
                break;
            }
        }
    }
    //cout<<"store control points into contP"<<endl;
    for (int i = 0; i<conNum; i++)
    {
        Vertex v = VarrP[index[i]];//
        for (int j = 0; j<3; j++)
        {
            contP[i].coord[j] = v.coord[j];
        }
    }
    delete[] flag;
}

//找出最近点并计算误差之和
double ICP::closest()
{
    //find closest points and error
    double error = 0.0;
    for (int i = 0; i < conNum; i++)
    {
        double mindist = 100.0;
        index[i] = 0;
        for (unsigned int j = 0; j < VarrQ.size(); j++)
        {
            double dist = distance(contP[i], VarrQ[j]);
            if (dist < mindist)
            {
                mindist = dist;
                index[i] = j;
            }
        }
        Vertex v = VarrQ[index[i]];
        for (int j = 0; j < 3; j++)
        {
            contQ[i].coord[j] = v.coord[j];
        }
        error += mindist;
    }
    return error;
}

//求取两个点云控制点的重心
void ICP::getcenter()
{
    //首先初始化重心坐标0，0，0
    for (int i = 0; i < 3; i++)
        meanP.coord[i] = 0.0;
    //求取每个分量之和
    for (int i = 0; i < conNum; i++)
    {
        for (int j = 0; j < 3; j++)
        {
            meanP.coord[j] += contP[i].coord[j];
        }
    }
    //求取平均值
    for (int i = 0; i < 3; i++)
        meanP.coord[i] = meanP.coord[i] / conNum;

    //
    for (int i = 0; i < 3; i++)
        meanQ.coord[i] = 0.0;
    //求取每个分量之和
    for (int i = 0; i < conNum; i++)
    {
        for (int j = 0; j < 3; j++)
        {
            meanQ.coord[j] += contQ[i].coord[j];
        }
    }
    //求取平均值
    for (int i = 0; i < 3; i++)
        meanQ.coord[i] = meanQ.coord[i] / conNum;
}

//点集中心化，生成新的点云数据
void ICP::rmcontcenter()
{
    std::cout << "点集中心化，生成新的点云数据" << std::endl;
    for (int i = 0; i < conNum; i++)
    {
        for (int j = 0; j < 3; j++)
        {
            rmcoP[i].coord[j] = contP[i].coord[j] - meanP.coord[j];
            rmcoQ[i].coord[j] = contQ[i].coord[j] - meanQ.coord[j];
        }
    }
}

void ICP::transform()
{
    std::cout << "获取变换矩阵" << std::endl;
    Matrix B(4, 4);
    B = 0;
    double u[3];//di+di'
    double d[3];//di-di'
    //计算协方差
    for (int i = 0; i < conNum; i++)
    {
        for (int j = 0; j < 3; j++)
        {
            u[j] = rmcoP[i].coord[j] + rmcoQ[i].coord[j];
            d[j] = rmcoP[i].coord[j] - rmcoQ[i].coord[j];
        }
        double uM[16] = { 
               0, -d[0], -d[1], -d[2],
            d[0],     0, -u[2], -u[1],
            d[1], -u[2],     0,  u[0],
            d[2],  u[1], -u[0],    0 };

        Matrix Ai(4, 4);
        Ai << uM;
        B += Ai * Ai.t();
    }

    Matrix U;
    Matrix V;
    DiagonalMatrix D;
    SVD(B, D, U, V);

    for (int i = 0; i < 4; i++)
    {
        quad[i] = V.element(i, 3);
    }
    B.Release();
    U.Release();
    V.Release();
    D.Release();
}

void ICP::uprotate()
{
    //根据四元数求解选择矩阵
    Rw[0][0] = quad[0] * quad[0] + quad[1] * quad[1] - quad[2] * quad[2] - quad[3] * quad[3];
    Rw[0][1] = 2 * (-quad[0] * quad[3] + quad[1] * quad[2]);
    Rw[0][2] = 2 * (quad[0] * quad[2] + quad[1] * quad[3]);
    Rw[1][0] = 2 * (quad[0] * quad[3] + quad[1] * quad[2]);
    Rw[1][1] = quad[0] * quad[0] - quad[1] * quad[1] + quad[2] * quad[2] - quad[3] * quad[3];
    Rw[1][2] = 2 * (-quad[0] * quad[1] + quad[2] * quad[3]);
    Rw[2][0] = 2 * (-quad[0] * quad[2] + quad[1] * quad[3]);
    Rw[2][1] = 2 * (quad[0] * quad[1] + quad[2] * quad[3]);
    Rw[2][2] = quad[0] * quad[0] - quad[1] * quad[1] - quad[2] * quad[2] + quad[3] * quad[3];

    //Rn+1 = R * Rn
    double tmp[3][3];
    for (int i = 0; i < 3; i++)
    {
        for (int j = 0; j < 3; j++)
        {
            tmp[i][j] = 0;
        }
    }

    for (int i = 0; i < 3; i++)
    {
        for (int j = 0; j < 3; j++)
        {
            for (int k = 0; k < 3; k++)
            {
                tmp[i][j] += Rw[i][k] * TR[k][j];
            }
        }
    }

    for (int i = 0; i < 3; i++)
    {
        for (int j = 0; j < 3; j++)
            TR[i][j] = tmp[i][j];
    }
}

void ICP::uptranslate()
{
    //Tw = P'-Rw * P
    double tmp[3] = { 0, 0, 0 };
    for (int i = 0; i < 3; i++)
    {
        for (int j = 0; j < 3; j++)
        {
            tmp[i] += Rw[i][j] * meanP.coord[j];
        }
    }
    for (int i = 0; i < 3; i++)
    {
        Tw[i] = meanQ.coord[i] - tmp[i];
    }

    double temp[3] = { 0, 0, 0 };
    for (int i = 0; i < 3; i++)
    {
        for (int j = 0; j < 3; j++)
        {
            temp[i] += Rw[i][j] * TT[j];
        }
    }
    for (int i = 0; i < 3; i++)
    {
        TT[i] = temp[i] + Tw[i];
    }
}

void ICP::updata()
{
    for (int i = 0; i < conNum; i++)
    {
        double tmp[3] = { 0, 0, 0 };
        for (int j = 0; j < 3; j++)
        {
            for (int k = 0; k < 3; k++)
            {
                tmp[j] += Rw[j][k] * contP[i].coord[k];
            }
        }
        for (int j = 0; j < 3; j++)
            contP[i].coord[j] = tmp[j] + Tw[j];
    }
}

void ICP::applyall()
{
    for (vector<Vertex>::iterator p = VarrP.begin(); p != VarrP.end(); p++)
    {
        Vertex v = *p;
        double tmp[3] = { 0, 0, 0 };
        for (int i = 0; i < 3; i++)
        {
            for (int k = 0; k < 3; k++)
            {
                tmp[i] += TR[i][k] * v.coord[k];
            }
        }
        for (int i = 0; i < 3; i++)
        {
            v.coord[i] = tmp[i] + TT[i];
        }
        *p = v;
    }
}

double ICP::distance(Vertex a, Vertex b)
{
    double dist = 0.0;
    for (int i = 0; i < 3; i++)
    {
        dist += (a.coord[i] - b.coord[i])*(a.coord[i] - b.coord[i]);
    }
    return dist;
}

void ICP::printTT()
{
    std::cout << "Translate Matrix = " << std::endl;
    for (int i = 0; i < 3; i++)
    {
        std::cout << TT[i] << " ";
    }
    std::cout << std::endl;
}

void ICP::printTR()
{
    std::cout << "Rotate Matrix = " << std::endl;
    for (int i = 0; i < 3; i++)
    {
        for (int j = 0; j < 3; j++)
        {
            std::cout << TR[i][j] << " ";
        }
        std::cout << std::endl;
    }
}

void ICP::showcloud(std::string firstname, std::string secondname,std::string thirdname)
{
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_Target(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_Source(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloudOut(new pcl::PointCloud<pcl::PointXYZ>);

    std::cout << "显示两个点云：" << std::endl;
    //第一个点云数据
    ifstream in;
    in.open(firstname.c_str(), std::ios::in);
    if (!in.is_open())
    {
        std::cout << "error open!" << std::endl;
        system("pause");
    }
    vector<Point> points;
    points.clear();
    Point tmp;
    while (in >> tmp.x >> tmp.y >> tmp.z)
    {
        points.push_back(tmp);
    }
    pcl::PointXYZ cltmp;

    for (size_t i = 0; i != points.size();i++)
    {
        cltmp.x = points[i].x;
        cltmp.y = points[i].y;
        cltmp.z = points[i].z;
        cloud_Target->points.push_back(cltmp);
    }
    std::cout << "点云A的大小：" << cloud_Target->size() << std::endl;
    in.close();

    //第二个点云数据
    in.open(secondname.c_str(), std::ios::in);
    if (!in.is_open())
    {
        std::cout << "error open!" << std::endl;
        system("pause");
    }
    points.clear();
    while (in >> tmp.x >> tmp.y >> tmp.z)
    {
        points.push_back(tmp);
    }
    //pcl::PointXYZ cltmp;

    for (size_t i = 0; i != points.size(); i++)
    {
        cltmp.x = points[i].x;
        cltmp.y = points[i].y;
        cltmp.z = points[i].z;
        cloud_Source->points.push_back(cltmp);
    }
    std::cout << "点云B的大小：" << cloud_Source->size() << std::endl;
    in.close();

    //第三个点云数据
    in.open(thirdname.c_str(), std::ios::in);
    if (!in.is_open())
    {
        std::cout << "error open!" << std::endl;
        system("pause");
    }
    points.clear();
    while (in >> tmp.x >> tmp.y >> tmp.z)
    {
        points.push_back(tmp);
    }
    //pcl::PointXYZ cltmp;

    for (size_t i = 0; i != points.size(); i++)
    {
        cltmp.x = points[i].x;
        cltmp.y = points[i].y;
        cltmp.z = points[i].z;
        cloudOut->points.push_back(cltmp);
    }
    std::cout << "点云C的大小：" << cloudOut->size() << std::endl;
    in.close();


    //可视化初始化
    pcl::visualization::PCLVisualizer viewer;
    viewer.setCameraFieldOfView(0.785398);//fov 45°  视场角
    viewer.setBackgroundColor(0.2, 0.2, 0.2);
    viewer.setCameraPosition(
        0, 0, 0,
        0, 0, -1,
        0, 0, 0);
    //点云可视化
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> TargetHandler(cloud_Target, 255, 0, 0);
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> SourceHandler(cloud_Source, 0, 0, 255);
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> OutHandler(cloudOut, 0, 255, 0);
    viewer.addPointCloud(cloud_Target, TargetHandler, "cloud_Target");
    viewer.addPointCloud(cloud_Source, SourceHandler, "cloud_Source");
    viewer.addCoordinateSystem(0.1, "cloud", 0);


    int v2 = 1;
    viewer.createViewPort(0.5, 0, 1, 1, v2);
    viewer.createViewPortCamera(v2);
    viewer.setCameraFieldOfView(0.785398, v2);//fov 45°
    viewer.setBackgroundColor(0.2, 0.2, 0.2,v2);
    viewer.setCameraPosition(
        0, 0, 0,
        0, 0, -1,
        0, 0, 0,v2);
    //点云可视化
    viewer.addPointCloud(cloud_Target, TargetHandler, "cloud222", v2);
    viewer.addPointCloud(cloudOut, OutHandler, "cloudOut",v2);
    viewer.addCoordinateSystem(0.1, "cloud1", v2);
    while(!viewer.wasStopped())
    {
        viewer.spinOnce();
    }
}

//主程序
#include "stdafx.h"
#include <iostream>
#include <pcl\common\transforms.h>
#include <pcl\io\pcd_io.h>
#include <pcl\visualization\pcl_visualizer.h>
#include <pcl\registration\icp.h>
#include "ICP.h">
#include <iostream>
#include <time.h>
int main()
{
    clock_t start, finish;
    start = clock();
    ICP myicp(1000, 0.00001, 50);
    myicp.readfile("bunny_0.asc", "bunny_1.asc");
    myicp.run();
    myicp.writefile("out.asc");
    finish = clock();
    std::cout << "运行时间：" << (finish - start) / 1000 << "s" << std::endl;
    myicp.showcloud("bunny_0.asc", "bunny_1.asc", "out.asc");
    system("pause");
    return 0;
}