学习笔记：创建扫描匹配算法（一）

一、简介

本文主要介绍两个强大的扫描匹配算法---迭代最近点(ICP)和正态分布变换(NDT)，对于每一种算法，我们将首先从概念的层面进行介绍，然后将在c++代码中从头构建每一种算法。

二、扫描匹配概览

扫描匹配比较两个非常相似的激光雷达扫描，以获得运动中的转换；然后将转换缝合在一起进行定位，这些变换可以用一个变换矩阵在数学上表示:

 

ICP有一个目标扫描和一个源扫描。  
 1.在源点和目标点之间建立关联  
 2.执行一种使关联距离之和最小化的变换  
步骤1和步骤2重复，直到关联没有改变，并且ICP已经收敛或已经完成了一定数量的迭代。

三、ICP简介

在第一个练习中，将使用ICP来恢复机器人在一个只有一个激光雷达传感器的房间中行走所采取的路径。在这个练习中，一切都将尽可能简化，所以环境将是2D的，这意味着激光雷达也扫描2D点云。下图是机器人从房间中央开始移动并扫描环境后的样子：

机器人从中心驶出的第一张扫描图是蓝色的。然后，机器人从绿色线段所显示的中心移动，并进行第二次扫描，显示为红色。两种扫描都是从机器人局部坐标系的角度，这意味着沿着x轴的0度是机器人直接看到的。这就是为什么红色扫描看起来与蓝色扫描不对齐的原因。机器人局部坐标系示意图如下图所示。

 由于这个例子是2D的，只需要关注左边显示机器人的局部坐标系。机器人在它前面看到的东西沿着x轴被映射为0度。右边红色的角就等于2D中的平角。
上面蓝色和红色扫描之间的错位实际上转换为绿色线段所示的机器人所采取的步骤的距离和方向。通过调整红色和蓝色扫描你可以在全局地图参考框架中恢复机器人的新姿态。这正是使用ICP所要做的。

用PCL实现ICP功能

构建函数：

Eigen::Matrix4d ICP(PointCloudT::Ptr target, PointCloudT::Ptr source, Pose startingPose, int iterations)
source源表示是第二次红色扫描，target目标表示是试图将扫描对齐的对象，在本例中是房间的第一次蓝色点云扫描。通过以下步骤可以完成ICP功能：

1. 将source源转换为startingPose（出发位置）

2. 创建PCL icp对象

3.设置icp对象的值

4. 在icp对象上调用align（对齐）

5. 如果icp收敛得到icp对象输出变换，并通过startingPose（出发位置）调整它，返回调整后的变换

6. 如果icp没有收敛，则记录消息并返回原始的单位矩阵

可以通过使用pcl::transformPointCloud并提供一个4 x4转换矩阵来完成点云上的转换。例如:

#INPUT `pcl::PointCloud<pcl::PointXYZ>` input point cloud

#OUTPUT `pcl::PointCloud<pcl::PointXYZ>` output point cloud

#TRANSFORM `Eigen::Matrix4d` transformation matrix

pcl::transformPointCloud (#INPUT, #OUTPUT,  #TRANSFORM );

设置ICP的超参数（Hyper-parameters）

ICP的主要超参数如下所示。除了这些设置之外，还可以更改其他参数并进行实验，以查看哪些值会产生最佳结果。有关ICP参数的完整列表，请参阅此链接：pcl: pcl::IterativeClosestPoint< PointSource, PointTarget, Scalar > Class Template Reference

##ITERATIONS `int` set from the function header value **iterations**

##SOURCE `pcl::PointCloud<pcl::PointXYZ>::Ptr` set from the function header value **source**

##TARGET `pcl::PointCloud<pcl::PointXYZ>::Ptr` set from the function header value **target**

##DIS `double` if this value is too small then correspondences can't be made

icp.setMaximumIterations (##ITERATIONS);
icp.setInputSource (##SOURCE);
icp.setInputTarget (##TARGET);
icp.setMaxCorrespondenceDistance (##DIS);

恢复机器人的位置

完成ICP函数后，将main下面调用的ICP迭代计数超参数设置为大于0的值。然后，为了完成本练习的第1部分，使用ICP的输出变换对第二个红色扫描执行变换，并呈现扫描。您可以使用PCL转换函数和renderPointCloud点云函数来完成这一点。

// perform the transformation on the scan using transform from ICP
pcl::transformPointCloud (#INPUT, #OUTPUT,  #TRANSFORM );


##CLOUD`pcl::PointCloud<pcl::PointXYZ>::Ptr`
##VIS_NAME string (name of object to visualize)

// Create a new point cloud Ptr 
PointCloudT::Ptr  #CLOUD_NAME (new PointCloudT);


// render the correct scan ( function from helper.cpp )
renderPointCloud(viewer, #CLOUD, #VIS_NAME, Color(0,1,0));

最终的结果如下图所示，绿色扫描图显示的是经过ICP转换后的红色扫描图，现在与蓝色扫描图对齐良好。蓝色的点和线显示了机器人从ICP变换中恢复的位置，注意它与真实值非常接近(绿色的点和线)。

 第1部分的最终结果

第2及第3部分        

一旦你可以让机器人像上面的图片一样定位，通过尝试第2部分和第3部分来完成练习。只需将这些部分的if条件设置为true就可以激活它们。第二部分将让机器人再移动2步。在这里你可以看到初始姿态对于ICP的收敛非常重要。如果使用的起始姿态总是机器人从房间中间开始的初始姿态，那么ICP将无法在最后一步收敛。因此，开始的姿势总是需要接近实际的位置，以便ICP工作良好。下面的图片显示了第3部分，使用开始姿势作为初始姿势vs上一步的估计姿势。

开始姿势总是机器人的初始位置。包含不正确的ICP收敛估计。

ICP每次使用上一步估计的位置作为开始姿势。

一旦你的机器人可以完成三步定位，尝试第3部分来完成练习。在第三部分中，机器人将以随机的动作围绕房间完成一个完整的旋转。

为了完成练习，您应该能够获得上面的图像，其估计路径与实际路径非常接近。同样，所有扫描正确转换的最终图像创建了绿色的正方形，在整个房间移动后几乎完全填补。注意，做扫描的2D激光雷达并不总是100%准确，有时可能会有一些干扰，看看绿色方块上没有的绿色点。ICP足够可靠，即使测量有一些小的差异，仍然能够定位。

附文：

***如何使用ICP(迭代最近点)***

本文档演示了在代码中使用迭代最近点算法，该算法可以通过最小化两个点云点之间的距离，并对它们进行刚性转换，来确定一个点云是否是另一个点云的刚性转换。
The code & The explanation
#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/registration/icp.h>
//这些头文件包含我们将使用的所有类的定义。//
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in (new pcl::PointCloud<pcl::PointXYZ>(5,1));
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_out (new pcl::PointCloud<pcl::PointXYZ>);
//创建两个pcl::PointCloud增强共享指针并初始化它们。在pcl命名空间中，每个点的类型设置为PointXYZ，即:
struct PointXYZ
{
  float x;
  float y;
  float z;
};//
The lines:
   // Fill in the CloudIn data
  for (auto& point : *cloud_in)
  {
    point.x = 1024 * rand() / (RAND_MAX + 1.0f);
    point.y = 1024 * rand() / (RAND_MAX + 1.0f);
    point.z = 1024 * rand() / (RAND_MAX + 1.0f);
  }
   std::cout << "Saved " << cloud_in->size () << " data points to input:" << std::endl;
    for (auto& point : *cloud_in)
    std::cout << point << std::endl;
   *cloud_out = *cloud_in;
  std::cout << "size:" << cloud_out->size() << std::endl;
  for (auto& point : *cloud_out)
    point.x += 0.7f;
//用随机的点值填充PointCloud结构，并设置适当的参数(宽度、高度、is_dense)。此外，它们还输出保存的点数和它们的实际数据值。//
std::cout << "Transformed " << cloud_in->size () << " data points:" << std::endl;
   for (auto& point : *cloud_out)
    std::cout << point << std::endl;
  pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
//在点云上执行简单的刚性转换，并再次输出数据值。//
 icp.setInputSource(cloud_in);
 icp.setInputTarget(cloud_out);
//这样创建一个IterativeClosestPoint的实例，并向它提供一些有用的信息。" icp.setInputSource(cloud_in); "设置cloud_in作为PointCloud开始；" icp.setInputTarget(cloud_out); "设置cloud_out作为cloud_in看起来像的PointCloud。//
pcl::PointCloud<pcl::PointXYZ> Final;
icp.align(Final);
std::cout << "has converged:" << icp.hasConverged() << " score: " <<
icp.getFitnessScore() << std::endl;
//创建一个pcl::PointCloud<pcl::PointXYZ>，在应用算法后 IterativeClosestPoint可以将结果云保存到该处。如果两个PointClouds正确对齐(意味着它们都是相同的云，只是对其中一个应用了某种刚性转换)，那么icp.hasConverged() = 1 (true)。然后输出最终转换的适应度分数和一些相关信息。//
原文参见：https://pointclouds.org/documentation/tutorials/iterative_closest_point.html

四、ICP练习及代码

using namespace std;

#include <string>
#include <sstream>
#include "helper.h"

#include <pcl/registration/icp.h>
#include <pcl/console/time.h>   // TicToc

Eigen::Matrix4d ICP(PointCloudT::Ptr target, PointCloudT::Ptr source, Pose startingPose, int iterations){

    Eigen::Matrix4d transformation_matrix = Eigen::Matrix4d::Identity ();

    // align source with starting pose
      Eigen::Matrix4d initTransform = transform2D(startingPose.theta, startingPose.position.x, startingPose.position.y);
      PointCloudT::Ptr transformSource (new PointCloudT); 
      pcl::transformPointCloud (*source, *transformSource, initTransform);

    pcl::console::TicToc time;
      time.tic ();
      pcl::IterativeClosestPoint<PointT, PointT> icp;
      icp.setMaximumIterations (iterations);
      icp.setInputSource (transformSource);
      icp.setInputTarget (target);
      PointCloudT::Ptr cloud_icp (new PointCloudT);  // ICP output point cloud
      icp.align (*cloud_icp);

      if (icp.hasConverged ())
      {
          std::cout << "\nICP has converged, score is " << icp.getFitnessScore () << std::endl;
          transformation_matrix = icp.getFinalTransformation ().cast<double>();
          transformation_matrix =  transformation_matrix * initTransform;
          return transformation_matrix;
      }
      cout << "WARNING: ICP did not converge" << endl;

    return transformation_matrix;

}

int main(){

    pcl::visualization::PCLVisualizer::Ptr viewer (new pcl::visualization::PCLVisualizer ("2D Viewer"));
    viewer->setBackgroundColor (0, 0, 0);
    viewer->addCoordinateSystem (1.0);

    // create a room
    double lowerX = -5;
    double upperX = 5;
    double lowerY = -5;
    double upperY = 5;
    vector<LineSegment> room;
    LineSegment top(0, 1, upperY, lowerX, upperX);
    room.push_back(top);
    LineSegment bottom(0, 1, lowerY, lowerX, upperX);
    room.push_back(bottom);
    LineSegment right(1, 0, upperX, lowerY, upperY);
    room.push_back(right);
    LineSegment left(1, 0, lowerX, lowerY, upperY);
    room.push_back(left);

    // create lidar
    Lidar lidar(0, 0, 0, 100, 128);

    PointCloudT::Ptr poses (new PointCloudT);     // ground truth
    PointCloudT::Ptr locator (new PointCloudT); // estimated locations

    // starting location
    poses->points.push_back(PointT(lidar.x, lidar.y, 0));
    locator->points.push_back(PointT(lidar.x, lidar.y, 0));

    // get map of room
    PointCloudT::Ptr map = lidar.scan(room);
    cout << "map captured " << map->points.size() << " points" << endl;

    // move around the room

    // Part 1. TODO: localize from single step
    vector<Vect2> movement = {Vect2(0.5,pi/12)};

    // Part 2. TODO: localize after several steps
    if(true){ // Change to true
        movement.push_back(Vect2(0.8, pi/10));
        movement.push_back(Vect2(1.0, pi/6));
    }
    // Part 3. TODO: localize after randomly moving around the whole room
    if(true){ // Change to true
        srand(time(0));
        for(int i = 0; i < 10; i++){
            double mag = 0.5 * ((double) rand() / (RAND_MAX)) + 0.5;
            double angle = pi/8 * ((double) rand() / (RAND_MAX)) + pi/8;
            movement.push_back(Vect2(mag, angle));
        }
    }

    renderPointCloud(viewer, map, "map", Color(0,0,1)); // render map
    Pose location(Point(0,0), 0);
    PointCloudT::Ptr scan;
    int count = 0;
    for( Vect2 move : movement ){

        // exectue move
        lidar.Move(move.mag, move.theta);
        poses->points.push_back(PointT(lidar.x, lidar.y, 0));

        // scan the room
        scan = lidar.scan(room);
        cout << "scan captured " << scan->points.size() << " points" << endl;
        renderPointCloud(viewer, scan, "scan_"+to_string(count), Color(1,0,0)); // render scan

        // perform localization
        Eigen::Matrix4d transform = ICP(map, scan, location, 50); //TODO: make the iteration count greater than zero
        Pose estimate = getPose(transform);
        // TODO: save estimate location and use it as starting pose for ICP next time
        location = estimate;
        locator->points.push_back(PointT(estimate.position.x, estimate.position.y, 0));

        // view transformed scan
          PointCloudT::Ptr transformed_scan (new PointCloudT);
          pcl::transformPointCloud (*scan, *transformed_scan, transform);
          renderPointCloud(viewer, transformed_scan, "icp_scan_"+to_string(count), Color(0,1,0)); // render corrected scan

        count++;
    }

    // display ground truth poses vs estimated pose
    renderPointCloud(viewer, poses, "poses", Color(0,1,0), 8);
    renderPath(viewer, poses, "posePath", Color(0,1,0) );
    renderPointCloud(viewer, locator, "locator", Color(0,0,1), 6);
    renderPath(viewer, locator, "locPath", Color(0,0,1) );

    while (!viewer->wasStopped ())
    {
        viewer->spinOnce ();
    }

    return 0;
}

五、创建ICP概述

在开始使用ICP时，我们首先使用最近邻关联来匹配点。为了提高效率，我们在目标扫描中使用KD树来快速关联源点和目标点。KD树是一种类似于二叉树的数据结构，但对多维空间中的点(如点云)很有帮助。
接下来，通过使用居中向量和最近的邻居(来自半径搜索)将关联组合成一个矩阵。
最后，对关联矩阵进行奇异值分解(将矩阵分解为奇异向量或奇异值)，得到源上的旋转和平移，使与目标的关联距离最小。

ICP方程

这里的重点是，从向量加起来得到总的X和Y，然后得到旋转R和平移t：

最后的变换矩阵

六、创建ICP -代码

在下一个练习中，您将从头开始实现ICP(迭代最近点)算法。正如您在上一个练习中看到的，ICP在迭代中收敛。在每次迭代过程中，源点云中的每个点都与目标点云中最近的点相关联。一旦建立了关联，转换就完成了，它使每个关联对之间的最小平方距离最小化。当源目标点关联在转换后没有变化时，ICP会收敛到它不能再变化的点。每个关联对的距离之和可以计算出一个得分值。得分值让您了解源点云与目标点云重叠的情况。分数越低，两个点云重叠得越好。在本练习的第一部分中，您将编写一个计算关联对的函数，完成关联对后，您可以通过在score函数的查看器中查看score来测试它。下图显示了在按下空格键后，源和目标之间用分数值建立关联时的样子。

在查看器上按空格键后从关联计算得分。目标点云是蓝色的，源点云在按空格键后首先显示为红色，然后显示为绿色。 

为了创建关联，您将完成icp2-main.cpp中的NN功能。在开始使用NN函数之前，让我们先看看Score函数，以了解如何访问点云中的点，以及如何使用关联来计算分数。下面是来自Score的代码。

double Score(vector<int> pairs, PointCloudT::Ptr target, PointCloudT::Ptr source, Eigen::Matrix4d transform){
    double score = 0;
    int index = 0;
    for(int i : pairs){
        Eigen::MatrixXd p(4, 1);
        p(0,0) = (*source)[index].x;
        p(1,0) = (*source)[index].y;
        p(2,0) = 0.0;
        p(3,0) = 1.0;
        Eigen::MatrixXd p2 = transform * p;
        PointT association = (*target)[i];
        score += sqrt( (p2(0,0) - association.x) * (p2(0,0) - association.x) + (p2(1,0) - association.y) * (p2(1,0) - association.y) );
        index++;
    }
    return score;
}

代码概述

1.Score被初始化为0
2.对每个关联对执行以下操作。
A.源点进行变换，并访问对应的目标点
B.计算变换后的源点与目标点之间的距离，并添加Score
注意:这个函数可以修改为距离的平方和，只需去掉平方和的平方根。这里重要的是，得分值为您提供了一个指标，用于比较某些转换与目标的重叠程度。

制造关联度

为了完成NN最近邻函数，这里有一个关于将做什么的高水平概述。首先，源将通过initTransform进行转换，回想一下ICP简介：如何转换PCL点云。然后循环遍历每个转换后的源点，得到与该转换后的源点最近的目标点索引。只需将目标点索引推入vector<int>关联。关联的索引是转换后的源点的索引，它的值是目标点的索引。例如，如果您想知道目标中离源点索引15最近的点，您可以只执行关联[15]来获得目标索引，然后使用该索引来访问目标并查看点(*target)[target index]。该函数的有趣之处在于有效地获取最近的点，要做到这一点，您需要使用KDTree。PCL有一个可以使用的KDTree对象，下面是如何创建和使用一个KDTree对象。

// Create KDTree object in PCL
pcl::KdTreeFLANN<PointT> kdtree;
kdtree.setInputCloud (Point Cloud); // use the target point cloud as input

//Use KDTree object to get closest point
vector<int> pointIdxRadiusSearch;
vector<float> pointRadiusSquaredDistance;
#POINT `pcl::PointXYZ` (input point from transformed source) 
#DISTANCE `double` use distance parameter from `NN` header
kdtree.radiusSearch (#POINT, #DISTANCE, pointIdxRadiusSearch, pointRadiusSquaredDistance) // if no points found within distance returns -1.
// otherwise closest target point index is pointIdxRadiusSearch[0]

有关在PCL中创建和使用KDTree的进一步参考，请查看此页面：https://pointclouds.org/documentation/tutorials/kdtree_search.html

可视化关联

在下一节中，一旦您能够创建索引关联，您将获取匹配索引的这些关联，并将它们转换为匹配点。匹配点将保存为向量对中的Pair。在本节中，您还可以通过使用PCL查看器输入参数和renderRay函数来可视化实际的关联。下面展示了关联的样子，其中每个关联被可视化为连接源点和最近的目标点的线段。

将关联可视化为将每个源点连接到最近的目标点的线段

通过每个关联完成PairPoints函数循环，得到源点及其对应的目标点。这里有一些建议。循环遍历每个源点的一种可能方法是执行for(PointT point: source->points)，也执行PointT association = (*target)[i]，其中i是来自给定源索引的关联的值。下面展示了如何创建一对点以及如何使用renderRay函数。

// create a Pair containing two pcl::PointXYZ
Pair(Point(point.x, point.y,0), Point(association.x, association.y,0))

viewer->removeShape(to_string(index)); // remove object name from viewer so can be used next time. An error message appears if trying to add it and its already contained.
renderRay(viewer, Point(point.x, point.y,0), Point(association.x, association.y,0), to_string(index), Color(0,1,0)); // render the associations line segments

移动变换

一旦您完成了PairPoints函数，您可以通过启动程序来测试它，按下空格键，然后使用方向键移动变换和(K,L)键旋转变换。这样做的结果是看到关联线段如何根据转换和评分值发生变化。一个有趣的事情是尝试手动移动您自己的转换，以找到最小的分数值。在本练习的最后一部分，您将了解到哪些数学运算可以找到适合您的最佳转换。

计算最优变换

你现在已经完成了实施ICP的一半。您找到了源和目标之间的关联，最后一部分是计算最佳转换，它为关联对提供最低的可能分数。寻找最优变换涉及到线性代数，其中点关联以及源和目标的平均点位被转换为矩阵。然后找到最优变换涉及到使用SVD https://en.wikipedia.org/wiki/Singular_value_decomposition奇异值分解。所有的内容都在这个文档中https://igl.ethz.ch/projects/ARAP/svd_rot.pdf。为了完成这个练习并完成ICP函数，你将使用刚动计算中的步骤1 - 5中的方程-链接总结。下面总结一下如何完成ICP功能:
完成ICP函数

1.通过startingPose转换源，可以使用helper.cpp中的transform3D，将startingPose转换为一个矩阵

2.使用PairPoints函数计算关联

3.使用步骤1 - 5中的刚性运动计算-总结，以找到最佳的旋转和平移

4.根据最优旋转和平移设置transformation_matrix

矩阵与线性代数函数

本节将介绍如何使用c++实现上述步骤3和文档中的5个步骤。本练习中使用 Eigen特征函数的依赖关系已经建立，它是c++中处理矩阵和线性代数函数的优秀库。下面展示了一个如何创建任意大小的双精度数组的示例。

// Creating matrices of n x m, if (n,m) is not included then matrix will be resized later based on what it is assigned to
Eigen::MatrixXd X(n,m) // where n and m are any values that you define. This matrix will be called X

// Accessing and modifying matrix values
X( row, col ) // where row and col are the indices of the matrix to access or modify

// Getting the size of matrix
X.rows() // how many rows the matrix contains
X.cols() // how many columns the matrix contains

// Setting all values in a matrix to zero
X << Eigen::MatrixXd::Zero(n ,m);

// Take transpose of matrix
X.transpose()

// Taking the SVD of X, which gives svd.matrixV and svd.matrixU
JacobiSVD<MatrixXd> svd(X, ComputeFullV | ComputeFullU);

// Creating Identity matrix of arbitrary size n x m
Eigen::MatrixXd A
A.setIdentity(n, m)
// Could also do
Eigen::MatrixXd A = Eigen::MatrixXd::Identity(n, m)

使用最优化旋转和平移

一旦你计算出了最佳旋转和平移，你就可以将它们结合起来，得到最终的transformation_matrix。

七、创建ICP-完整代码

using namespace std;

#include <string>
#include <sstream>
#include "helper.h"

#include <Eigen/Core>
#include <Eigen/SVD>
using namespace Eigen;
#include <pcl/registration/icp.h>
#include <pcl/console/time.h>   // TicToc

Pose pose(Point(0,0,0), Rotate(0,0,0));
Pose upose = pose;
vector<int> associations;
vector<int> bestAssociations = {5,6,7,8,9,10,11,12,13,14,15,16,16,17,18,19,20,21,22,23,24,25,26,26,27,28,29,30,31,32,33,34,35,36,37,38,39,40,42,43,44,45,46,47,48,49,50,51,52,53,54,55,56,57,58,59,60,62,63,64,65,66,67,68,69,70,71,72,74,75,76,77,78,79,80,81,82,83,84,86,87,88,89,90,91,92,93,94,95,96,97,98,99,100,101,102,103,104,105,106,107,108,109,110,111,112,113,114,115,116,117,118,119,120,121,122,122,123,124,125,126,127,0,1,2,3,4,4};
bool init = false;
bool matching = false;
bool update = false;
void keyboardEventOccurred(const pcl::visualization::KeyboardEvent &event, void* viewer)
{

      //boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer = *static_cast<boost::shared_ptr<pcl::visualization::PCLVisualizer> *>(viewer_void);
    if (event.getKeySym() == "Right" && event.keyDown()){
        update = true;
        upose.position.x += 0.1;
      }
    else if (event.getKeySym() == "Left" && event.keyDown()){
        update = true;
        upose.position.x -= 0.1;
      }
      if (event.getKeySym() == "Up" && event.keyDown()){
        update = true;
        upose.position.y += 0.1;
      }
    else if (event.getKeySym() == "Down" && event.keyDown()){
        update = true;
        upose.position.y -= 0.1;
      }
    else if (event.getKeySym() == "k" && event.keyDown()){
        update = true;
        upose.rotation.yaw += 0.1;
        while( upose.rotation.yaw > 2*pi)
            upose.rotation.yaw -= 2*pi;  
      }
    else if (event.getKeySym() == "l" && event.keyDown()){
        update = true;
        upose.rotation.yaw -= 0.1;
        while( upose.rotation.yaw < 0)
            upose.rotation.yaw += 2*pi; 
      }
    else if (event.getKeySym() == "space" && event.keyDown()){
        matching = true;
        update = false;
      }
    else if (event.getKeySym() == "n" && event.keyDown()){
        pose = upose;
        cout << "Set New Pose" << endl;
      }
    else if (event.getKeySym() == "b" && event.keyDown()){
        cout << "Do ICP With Best Associations" << endl;
        matching = true;
        update = true;
      }
}

double Score(vector<int> pairs, PointCloudT::Ptr target, PointCloudT::Ptr source, Eigen::Matrix4d transform){
    double score = 0;
    int index = 0;
    for(int i : pairs){
        Eigen::MatrixXd p(4, 1);
        p(0,0) = (*source)[index].x;
        p(1,0) = (*source)[index].y;
        p(2,0) = 0.0;
        p(3,0) = 1.0;
        Eigen::MatrixXd p2 = transform * p;
        PointT association = (*target)[i];
        score += sqrt( (p2(0,0) - association.x) * (p2(0,0) - association.x) + (p2(1,0) - association.y) * (p2(1,0) - association.y) );
        index++;
    }
    return score;
}
vector<Pair> estimations;

vector<int> NN(PointCloudT::Ptr target, PointCloudT::Ptr source, Eigen::Matrix4d initTransform, double dist){

    PointCloudT::Ptr transformSource (new PointCloudT); 
      pcl::transformPointCloud (*source, *transformSource, initTransform);

    pcl::KdTreeFLANN<PointT> kdtree;
    kdtree.setInputCloud (target);

    vector<int> associations;

    int index = 0;
    for(PointT point : transformSource->points ){

        vector<int> pointIdxRadiusSearch;
          vector<float> pointRadiusSquaredDistance;
        if ( kdtree.radiusSearch (point, dist, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 0 )
         {
            associations.push_back(pointIdxRadiusSearch[0]);
        }
        else{
            associations.push_back(-1);
        }
        index++;
    }

    return associations;
}

vector<Pair> PairPoints(vector<int> associations, PointCloudT::Ptr target, PointCloudT::Ptr source, bool render, pcl::visualization::PCLVisualizer::Ptr& viewer){

    vector<Pair> pairs;

    int index = 0;
    for(PointT point : source->points ){
        int i = associations[index];
        if( i >= 0)
        {
            PointT association = (*target)[i];
            if( render){
                viewer->removeShape(to_string(index));
                renderRay(viewer, Point(point.x, point.y,0), Point(association.x, association.y,0), to_string(index), Color(0,1,0));
            }
            pairs.push_back(Pair(Point(point.x, point.y,0), Point(association.x, association.y,0)) );
        }
        index++;
    }
    return pairs;
}

ICP函数

Eigen::Matrix4d ICP(vector<int> associations, PointCloudT::Ptr target, PointCloudT::Ptr source, Pose startingPose, int iterations, pcl::visualization::PCLVisualizer::Ptr& viewer){

      // align source with starting pose
      Eigen::Matrix4d initTransform = transform3D(startingPose.rotation.yaw, startingPose.rotation.pitch, startingPose.rotation.roll, startingPose.position.x, startingPose.position.y, startingPose.position.z);
      PointCloudT::Ptr transformSource (new PointCloudT); 
      pcl::transformPointCloud (*source, *transformSource, initTransform);

    vector<Pair> pairs = PairPoints(associations, target, transformSource, true, viewer);

    //cout << "score is " << Score(pairs, Eigen::MatrixXd::Identity(4,4) ) << endl;

    Eigen::MatrixXd X(2,pairs.size());
    Eigen::MatrixXd Y(2,pairs.size());
    Eigen::MatrixXd P(2,1);
    P << Eigen::MatrixXd::Zero(2,1);
    Eigen::MatrixXd Q(2,1);
    Q << Eigen::MatrixXd::Zero(2,1);

    for(Pair pair : pairs){
        P(0,0) += pair.p1.x;
        P(1,0) += pair.p1.y;

        Q(0,0) += pair.p2.x;
        Q(1,0) += pair.p2.y;
    }
    P(0,0) = P(0,0)/pairs.size();
    P(1,0) = P(1,0)/pairs.size();

    Q(0,0) = Q(0,0)/pairs.size();
    Q(1,0) = Q(1,0)/pairs.size();
    int index = 0;
    for(Pair pair : pairs){
        X(0,index) = pair.p1.x - P(0,0);
        X(1,index) = pair.p1.y - P(1,0);

        Y(0,index) = pair.p2.x - Q(0,0);
        Y(1,index) = pair.p2.y - Q(1,0);
        index++;
    }

    // compute best R and t from using SVD
    Eigen::MatrixXd S  = X * Y.transpose();
    JacobiSVD<MatrixXd> svd(S, ComputeFullV | ComputeFullU);
    Eigen::MatrixXd D;
    D.setIdentity(svd.matrixV().cols(), svd.matrixV().cols());
    D(svd.matrixV().cols()-1,svd.matrixV().cols()-1) = (svd.matrixV() * svd.matrixU().transpose() ).determinant();

    Eigen::MatrixXd R  = svd.matrixV() * D * svd.matrixU().transpose();
    Eigen::MatrixXd t  = Q - R * P;

    Eigen::Matrix4d transformation_matrix;
    transformation_matrix << Eigen::MatrixXd::Identity(4,4);

    transformation_matrix(0,0) = R(0,0);
    transformation_matrix(0,1) = R(0,1);
    transformation_matrix(1,0) = R(1,0);
    transformation_matrix(1,1) = R(1,1);
    transformation_matrix(0,3) = t(0,0);
    transformation_matrix(1,3) = t(1,0);

    //cout << "score is " << Score(pairs, transformation_matrix ) << endl;

    //cout << transformation_matrix << endl;

    estimations = pairs;
    transformation_matrix =  transformation_matrix * initTransform;
      return transformation_matrix;

}

int main(){

    pcl::visualization::PCLVisualizer::Ptr viewer (new pcl::visualization::PCLVisualizer ("ICP Creation"));
      viewer->setBackgroundColor (0, 0, 0);
      viewer->addCoordinateSystem (1.0);
    viewer->registerKeyboardCallback(keyboardEventOccurred, (void*)&viewer);

    // Load target
    PointCloudT::Ptr target(new PointCloudT);
      pcl::io::loadPCDFile("target.pcd", *target);

    // Load source
    PointCloudT::Ptr source(new PointCloudT);
      pcl::io::loadPCDFile("source.pcd", *source);

    renderPointCloud(viewer, target, "target", Color(0,0,1));
    renderPointCloud(viewer, source, "source", Color(1,0,0));
    viewer->addText("Score", 200, 200, 32, 1.0, 1.0, 1.0, "score",0);

      while (!viewer->wasStopped ())
      {
        if(matching){

            init = true;

            viewer->removePointCloud("usource");

            Eigen::Matrix4d transformInit = transform3D(pose.rotation.yaw, pose.rotation.pitch, pose.rotation.roll, pose.position.x, pose.position.y, pose.position.z);
            PointCloudT::Ptr transformed_scan (new PointCloudT);
              pcl::transformPointCloud (*source, *transformed_scan, transformInit);
            viewer->removePointCloud("source");
              renderPointCloud(viewer, transformed_scan, "source", Color(1,0,0));

            if(!update)
                associations = NN(target, source, transformInit, 5);
            else
                associations = bestAssociations;

            Eigen::Matrix4d transform = ICP(associations, target, source, pose, 1, viewer);

            pose = getPose(transform);
              pcl::transformPointCloud (*source, *transformed_scan, transform);
            viewer->removePointCloud("icp_scan");
              renderPointCloud(viewer, transformed_scan, "icp_scan", Color(0,1,0));

            double score = Score(associations,  target, source, transformInit);
            viewer->removeShape("score");
            viewer->addText("Score: "+to_string(score), 200, 200, 32, 1.0, 1.0, 1.0, "score",0);
            double icpScore = Score(associations,  target, source, transform);
            viewer->removeShape("iscore");
            viewer->addText("ICP Score: "+to_string(icpScore), 200, 150, 32, 1.0, 1.0, 1.0, "iscore",0);

            matching = false;
            update = false;
            upose = pose;

        }
        else if(update && init){

            Eigen::Matrix4d userTransform = transform3D(upose.rotation.yaw, upose.rotation.pitch, upose.rotation.roll, upose.position.x, upose.position.y, upose.position.z);

            PointCloudT::Ptr transformed_scan (new PointCloudT);
              pcl::transformPointCloud (*source, *transformed_scan, userTransform);
            viewer->removePointCloud("usource");
            renderPointCloud(viewer, transformed_scan, "usource", Color(0,1,1));

            vector<Pair> pairs = PairPoints(associations, target, transformed_scan, true, viewer);

            double score = Score(associations,  target, source, userTransform);
            viewer->removeShape("score");
            viewer->addText("Score: "+to_string(score), 200, 200, 32, 1.0, 1.0, 1.0, "score",0);

            update = false;

        }

          viewer->spinOnce ();
      }

    return 0;
}