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

八、NDT概览

正态分布变换(NDT)的第一部分是创建概率密度函数(PDF):
求簇区域的质心
求聚类的协方差
利用聚类区域的质心及其协方差来定义概率函数

创建概率密度函数(PDF)的方程

牛顿法

NDT的下一部分是利用牛顿法:

牛顿的方法给出了一个方向，以达到一个函数的根或更高或更小的值

利用多变量函数，利用梯度法和黑森矩阵求解牛顿法

牛顿法(2D)方程

g为梯度向量:

 H矩阵为Hessian矩阵:

如果H是正定的，我们可以用梯度和Hessian矩阵来计算位置p的变化:

HΔp=−g 

PDF特性和NDT方程

梯度和黑森值依赖于当前点、均值和协方差
exp变量可以计算一次，然后在g和H方程中多次出现
梯度法和黑森法都使用偏导数
黑森函数使用二阶偏导数，它是<0,0>，除非i和j都是3，这意味着都是theta项
应用牛顿法

每个单元格都有自己的PDF，该PDF由位于其中的目标点定义

根据PDF所在的单元格，将每个源点分配给PDF

每个有PDF的源点都会导致g, H的计算

所有的g和H矩阵相加然后计算整个变换

要得到最终的变换，需要步长和确保H是正定的

九、创建NDT

在本练习中，你将从头开始实现NDT，即正态分布变换算法。NDT的创建涉及到从目标点云创建一个概率密度函数PDF，然后使用牛顿方法找到一个变换，使该概率域中源点值的总体总和最大化。为了创建概率密度函数PDF，将网格空间离散为单元格，每个单元格都有自己的二维高斯分布，这些高斯分布来自位于该单元格区域的目标云点。二维高斯分布表示在整个单元区域内找到一个点的概率，并基于单元内点的均值和协方差计算。在本练习的第一部分中，将完成ndt-main.cpp中的概率密度函数PDF和概率Probability，以生成单个单元格的概率密度函数，然后将其可视化。单元格将是x和y方向上从0到10的区域，包含一组5个设置点。当第一次开始练习时，检视器会在其他任何地方显示蓝色和红色的五个点。这是因为目前概率Probability只是返回零，概率密度函数PDF返回零矩阵，还不是这五个点的合适均值和协方差的。如下所示：

五个单元格点用蓝点表示，其他地方用红色表示，因为输出为零。 

一旦在PDF中计算出正确的2 × 1均值矩阵和2 × 2协方差矩阵，并在Probability中输出正确的概率计算，将看到单元格的概率密度函数如下所示。

在pcl察看器看到带3D强度点云的单元格PDF。绕点云旋转，也可以看到带颜色强度的二维高斯的三维结构。

创建一个PDF

完成这个练习的一个非常有用的资源是论文：《正态分布变换:激光扫描匹配的新方法》https://www.researchgate.net/publication/4045903_The_Normal_Distributions_Transform_A_New_Approach_to_Laser_Scan_Matching，在第三节包含了构建NDT的所有计算。正态分布变换步骤1 - 3详解方程的使用，以计算均值矩阵，协方差矩阵，并输出正确的概率值。下面也总结了这些步骤：

  1.在PDF中计算平均矩阵，将所有(x,y)点相加，然后除以点的总数n，得到1/n[x点总和，y点总和]。在这个例子中n是5。

  2.在PDF中计算协方差矩阵，对每个点的(point - mean)*(point - mean).transpose()的所有矩阵乘积进行求和，然后除以总点数。2 × 1矩阵和1 × 2矩阵的乘积是2 × 2矩阵

  3.在Probability概率中计算点的概率，使用平均值和协方差，计算exp(-0.5 *(point - mean).transpose()*covariance.inverse()*(point - mean))。1 × 2矩阵与2 × 2矩阵和2 × 1矩阵的乘积是1 × 1矩阵。取矩阵中单个的1 × 1元素乘以-0.5然后取e的值，得到最终的值。

验证您的结果，完成PDF和Probability概率，确保您的结果匹配上面的PDF图像。

牛顿法

您现在已经完成了本练习第1部分的一半，并通过生成单元格概率密度函数PDF来创建NDT。接下来的一半是使用牛顿法找到一个变换，给到源点在概率场内的最大值。为了委婉地介绍这个思想，本练习的下一部分将使用单个测试点的源点云。然后，目标将是使用牛顿方法迭代地移动测试点，越来越接近概率密度函数的峰值。要理解牛顿的方法是如何做到这一点的，最好从一个简单的入门问题开始。牛顿法的一个很常见的介绍是用它来找到一个特定值的平方根。牛顿法可以通过跟踪每个起始点与x轴的切线交点来做到这一点。接下来的每一个交点都将成为下一个起始点，并通过迭代来实现这个点的收敛。在本质上，它遵循函数的曲率由导数定义，使牛顿方法收敛。这里有一个链接：Newton’s Method，展示了如何使用牛顿的方法来找到一个函数的根，以及如何计算一个值的平方根。

用牛顿法，通过函数的导数定义的切线来求函数的根。 

多元函数牛顿法

用同样的方法处理多参数函数，沿着函数的曲率锁定一个最大值/最小值点，考虑梯度下降。这个函数将是之前创建的概率密度函数PDF,它是一个关于x,y位置的函数。用牛顿法来找到二元PDF函数的变换的过程,将会转化为一个线性代数问题,用矩阵表示PDF的一阶和二阶偏导数。在下一部分中，完成ndt-main.cpp中的牛顿方法函数。早期的NDT论文<优化使用牛顿算法>的第五节:https://www.researchgate.net/publication/4045903_The_Normal_Distributions_Transform_A_New_Approach_to_Laser_Scan_Matching,概述了如何计算最优增量变化的变换，以获得更高的PDF曲线高度。 NewtonsMethod的函数头如下所示。函数是void，不返回任何东西，但是请注意，输入g_previous和H_previous是通过引用传递的，所以函数内部对这些变量的任何更改也会出现在函数外部。

void NewtonsMethod(PointT point, double theta, Cell cell, Eigen::MatrixBase<Derived>& g_previous, Eigen:: MatrixBase<Derived>& H_previous)

函数的变换所需的主要部分和给定点均来自于H矩阵，函数的黑森矩阵和g矩阵，函数的转置梯度。一旦计算出H和g，就可以通过-H.inverse()*g来计算变换。因为H是3 x 3，g是3 x 1，变换是3 x 1，它的元素分别是x偏移，y偏移和theta旋转。H和g的计算步骤总结如下，参考《优化牛顿算法》第V部分。

1.计算矩阵q的转置点减去均值，q应该是2 × 1。

2.定义3个偏导数矩阵，每个是2 x 1。X偏导= < 1,0 > ,y偏导= < 0,1 >,theta偏导= <-xsin(theta)-ycos(theta), xcos(theta)-ysin(theta)>

3.通过使用从PDF部分计算的单元格均值和协方差计算指数部分，计算exp(0.5 * q.tranpose() * covariance .inverse() * q)

4.通过计算exp(0.5 * q.tranpose() * covariance .inverse() * q) * (q.tranpose() * covaraince.inverse() * < x偏导数，y偏导数，θ偏导数>，得到g值

5.计算二阶偏导数即< -xcos(theta)+ysin(theta), -xsin(theta)-ycos(theta) >

6.计算H矩阵，即Hessian。H是3x3，使用了所有你在上述步骤中计算过的其他矩阵，除了g矩阵，也就是转置梯度

一旦完成了牛顿方法的这些步骤，这个函数将把你计算的矩阵加到输入参考值H和g上。为了完成这个练习，填写PosDeffunction，从一些正值开始，包括一个max。这个函数是为了确保H矩阵是正定的，然后用-H.inverse()*g计算T矩阵。牛顿算法优化。最后，通过变换矩阵T < 3 x 1 >定义的变换来计算新点，该点应该比前一个点高。通过牛顿法，对测试点进行20次迭代变换，结果如下图所示：

牛顿法迭代地将测试点向概率得分最高的峰值移动 

上述完成了对单个单元格PDF和单个测试点的牛顿方法，对于多个单元格，每个单元格都有自己的PDF及多个点的输入，仍然是相同的机制。为了完成练习，将ndt-main.cpp中的第一部分设置为false。现在，练习将从使用设定的的5个点和单一测试点，改为使用在之前的ICP练习中使用的相同的目标点和源点云。现在看起来就像下图一样，它可视化了PDF单元格的组合，以及目标整体PDF中源点云的当前分数，大约是9.1。总体得分只是目标PDF中每个源点概率/高度的总和。得分越高，源和目标重叠越好。

 十、创建NDT参考代码

using namespace std;

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

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;
bool matching = false;
bool update = false;
void keyboardEventOccurred(const pcl::visualization::KeyboardEvent &event, void* viewer)
{

    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.02;
        while( upose.rotation.yaw > 2*pi)
            upose.rotation.yaw -= 2*pi;  
      }
    else if (event.getKeySym() == "l" && event.keyDown()){
        update = true;
        upose.rotation.yaw -= 0.02;
        while( upose.rotation.yaw < 0)
            upose.rotation.yaw += 2*pi; 
      }
    if (event.getKeySym() == "space" && event.keyDown()){
        matching = true;

      }
    else if (event.getKeySym() == "n" && event.keyDown()){
        pose = upose;
        cout << "Set New Pose" << endl;
      }


}

double Probability(Eigen::MatrixXd X, Eigen::MatrixXd Q, Eigen::MatrixXd S){
    // TODO: calculate the probibility of the point given mean and standard deviation
    return exp( -0.5 * ( (X-Q).transpose() * S.inverse() * (X-Q) )(0,0) );
}

struct Cell{
    PointCloudT::Ptr cloud;
    Eigen::MatrixXd Q;
    Eigen::MatrixXd S;

    Cell(){
        PointCloudT::Ptr input(new PointCloudT);
        cloud = input;
        Q = Eigen::MatrixXd::Zero(2,1);
        S = Eigen::MatrixXd::Zero(2,2);
    }
};

struct Grid{

    // each cell is a square res x res and total grid size is (2 x width) x (2 x height)
    double res;
    int width;
    int height;

    vector<vector<Cell>> grid;

    Grid(double setRes, int setWidth, int setHeight){

        res = setRes;
        width = setWidth;
        height = setHeight;

        for(int r = 0; r < height*2; r++ ){
            vector<Cell> row;
            for(int c = 0; c < width*2; c++){
                row.push_back(Cell());
            }
            grid.push_back(row);
        }
    }

    void addPoint(PointT point){

        //cout << point.x << "," << point.y << endl;

        int c = int((point.x + width * res  ) / res);
        int r = int((point.y + height * res ) / res);

        //cout << r << "," << c << endl;

        if( (c >= 0 && c < width*2) && (r >= 0 && r < height*2) ){
            grid[r][c].cloud->points.push_back(point);
        }
    } 

    void Build(){

        for(int r = 0; r < height*2; r++ ){
            for(int c = 0; c < width*2; c++){

                PointCloudT::Ptr input = grid[r][c].cloud;
                if(input->points.size() > 2){

                    // Calculate the mean
                    Eigen::MatrixXd Q(2,1);
                    Q << Eigen::MatrixXd::Zero(2,1);
                    for(PointT point : input->points){
                        Q(0,0) += point.x;
                        Q(1,0) += point.y;
                    }
                    Q(0,0) = Q(0,0)/input->points.size();
                    Q(1,0) = Q(1,0)/input->points.size();

                    grid[r][c].Q = Q;

                    // Calculate sigma
                    Eigen::MatrixXd S(2,2);
                    S << Eigen::MatrixXd::Zero(2,2);
                    for(PointT point : input->points){
                        Eigen::MatrixXd X(2,1);
                        X(0,0) = point.x;
                        X(1,0) = point.y;

                        S += (X-Q) * (X-Q).transpose();
                    }
                    S(0,0) = S(0,0)/input->points.size();
                    S(0,1) = S(0,1)/input->points.size();
                    S(1,0) = S(1,0)/input->points.size();
                    S(1,1) = S(1,1)/input->points.size();

                    grid[r][c].S = S;
                }

            }
        }
    }

    Cell getCell(PointT point){
        int c = int((point.x + width * res  ) / res);
        int r = int((point.y + height * res ) / res);

        if( (c >= 0 && c < width*2) && (r >= 0 && r < height*2) ){
            return grid[r][c];
        }
        return Cell();
    }

    double Value(PointT point){

        Eigen::MatrixXd X(2,1);
        X(0,0) = point.x;
        X(1,0) = point.y;

        double value = 0;
        for(int r = 0; r < height*2; r++ ){
            for(int c = 0; c < width*2; c++){
                if(grid[r][c].cloud->points.size() > 2)
                    value += Probability(X, grid[r][c].Q, grid[r][c].S );
            }
        }
        return value;
    }
};

Cell PDF(PointCloudT::Ptr input, int res, pcl::visualization::PCLVisualizer::Ptr& viewer){

    // Calculate the mean
    Eigen::MatrixXd Q(2,1);
    Q << Eigen::MatrixXd::Zero(2,1);
    for(PointT point : input->points){
        Q(0,0) += point.x;
        Q(1,0) += point.y;
    }
    Q(0,0) = Q(0,0)/input->points.size();
    Q(1,0) = Q(1,0)/input->points.size();

    // Calculate sigma
    Eigen::MatrixXd S(2,2);
    S << Eigen::MatrixXd::Zero(2,2);
    for(PointT point : input->points){
        Eigen::MatrixXd X(2,1);
        X(0,0) = point.x;
        X(1,0) = point.y;

        S += (X-Q) * (X-Q).transpose();
    }
    S(0,0) = S(0,0)/input->points.size();
    S(0,1) = S(0,1)/input->points.size();
    S(1,0) = S(1,0)/input->points.size();
    S(1,1) = S(1,1)/input->points.size();

    PointCloudTI::Ptr pdf(new PointCloudTI);
    for(double i = 0.0; i <= 10.0; i += 10.0/double(res)){
        for(double j = 0.0; j <= 10.0; j += 10.0/double(res)){
            Eigen::MatrixXd X(2,1);
            X(0,0) = i;
            X(1,0) = j;

            PointTI point;
            point.x = i;
            point.y = j;
            point.z = Probability(X,Q,S);
            point.intensity = point.z;
            pdf->points.push_back(point);
        }
    }
    renderPointCloudI(viewer, pdf, "pdf");

    Cell cell = Cell();
    cell.S = S;
    cell.Q = Q;
    cell.cloud = input;
    return cell;
}

template<typename Derived>
void NewtonsMethod(PointT point, double theta, Cell cell, Eigen::MatrixBase<Derived>& g_previous, Eigen:: MatrixBase<Derived>& H_previous){

    Eigen::MatrixXd Q = cell.Q;
    Eigen::MatrixXd Si = cell.S.inverse();

    Eigen::MatrixXd X(2,1);
    X(0,0) = point.x;
    X(1,0) = point.y;

    Eigen::MatrixXd q = X - Q;

    Eigen::MatrixXd q_p1(2,1);
    q_p1(0,0) = 1.0;
    q_p1(1,0) = 0.0;
    Eigen::MatrixXd q_p2(2,1);
    q_p2(0,0) = 0.0;
    q_p2(1,0) = 1.0;
    Eigen::MatrixXd q_p3(2,1);
    q_p3(0,0) = -point.x*sin(theta)-point.y*cos(theta);
    q_p3(1,0) =  point.x*cos(theta)-point.y*sin(theta);

    Eigen::MatrixXd q_pp(2,1);
    q_pp(0,0) = -point.x*cos(theta)+point.y*sin(theta);
    q_pp(1,0) = -point.x*sin(theta)-point.y*cos(theta);

    Eigen::MatrixXd EXP(1,1);
    EXP(0,0) = exp(-0.5 * (q.transpose() * Si * q)(0,0) );

    Eigen::MatrixXd g(3,1);
    g(0,0) = (q.transpose() * Si * q_p1 * EXP)(0,0);
    g(1,0) = (q.transpose() * Si * q_p2 * EXP)(0,0);
    g(2,0) = (q.transpose() * Si * q_p3 * EXP)(0,0);

    Eigen::MatrixXd H(3,3);
    H(0,0) = (-EXP*( (-q.transpose()*Si*q_p1)*(-q.transpose()*Si*q_p1)+(-q_p1.transpose()*Si*q_p1)))(0,0);
    H(0,1) = (-EXP*( (-q.transpose()*Si*q_p1)*(-q.transpose()*Si*q_p2)+(-q_p2.transpose()*Si*q_p1)))(0,0);
    H(0,2) = (-EXP*( (-q.transpose()*Si*q_p1)*(-q.transpose()*Si*q_p3)+(-q_p3.transpose()*Si*q_p1)))(0,0);
    H(1,0) = (-EXP*( (-q.transpose()*Si*q_p2)*(-q.transpose()*Si*q_p1)+(-q_p1.transpose()*Si*q_p2)))(0,0);
    H(1,1) = (-EXP*( (-q.transpose()*Si*q_p2)*(-q.transpose()*Si*q_p2)+(-q_p2.transpose()*Si*q_p2)))(0,0);
    H(1,2) = (-EXP*( (-q.transpose()*Si*q_p2)*(-q.transpose()*Si*q_p3)+(-q_p3.transpose()*Si*q_p2)))(0,0);
    H(2,0) = (-EXP*( (-q.transpose()*Si*q_p3)*(-q.transpose()*Si*q_p1)+(-q_p1.transpose()*Si*q_p3)))(0,0);
    H(2,1) = (-EXP*( (-q.transpose()*Si*q_p3)*(-q.transpose()*Si*q_p2)+(-q_p2.transpose()*Si*q_p3)))(0,0);
    H(2,2) = (-EXP*( (-q.transpose()*Si*q_p3)*(-q.transpose()*Si*q_p3)+(-q.transpose()*Si*q_pp)+(-q_p3.transpose()*Si*q_p3)))(0,0);

    H_previous += H;
    g_previous += g;

}
double Score(PointCloudT::Ptr cloud, Grid grid){
    double score = 0;
    for(PointT point:cloud->points){
        Cell cell = grid.getCell(point);
        if(cell.cloud->points.size() > 2){
            score += grid.Value(point);
        }
    }
    return score;
}

double AdjustmentScore(double alpha, Eigen::MatrixXd T, PointCloudT::Ptr source, Pose pose, Grid grid){

    T *= alpha;
    pose.position.x += T(0,0);
    pose.position.y += T(1,0);
    pose.rotation.yaw += T(2,0);
    while(pose.rotation.yaw > 2*pi)
        pose.rotation.yaw -= 2*pi;


    Eigen::Matrix4d transform = transform3D(pose.rotation.yaw, 0, 0, pose.position.x, pose.position.y, 0);

    PointCloudT::Ptr transformed_scan (new PointCloudT);
    pcl::transformPointCloud (*source, *transformed_scan, transform);

    double score = Score(transformed_scan, grid);

    //cout << "score would be " << score << endl;

    return score;

}

double computeStepLength(Eigen::MatrixXd T, PointCloudT::Ptr source, Pose pose, Grid grid, double currScore){
    double maxParam = max( max( T(0,0), T(1,0)), T(2,0) );
    double mlength = 1.0;
    if(maxParam > 0.2){
        mlength =  0.1/maxParam;
        T *= mlength;
    }

    double bestAlpha = 0;

    //Try smaller steps
    double alpha = 1.0;
    for(int i = 0; i < 40; i++){
        //cout << "Adjusting alpha smaller" << endl;
        double adjScore = AdjustmentScore(alpha, T, source, pose, grid);
        if( adjScore > currScore){
            bestAlpha = alpha;
            currScore = adjScore;
        }
        alpha *= .7;
    }
    if(bestAlpha == 0){
        //Try larger steps
        alpha = 2.0;
        for(int i = 0; i < 10; i++){
            //cout << "Adjusting alpha bigger" << endl;
            double adjScore = AdjustmentScore(alpha, T, source, pose, grid);
            if( adjScore > currScore){
                bestAlpha = alpha;
                currScore = adjScore;
            }
            alpha *= 2;
        }
    }

    return bestAlpha * mlength;
}

template<typename Derived>
double PosDef(Eigen::MatrixBase<Derived>& A, double start, double increment, int maxIt){

    bool pass = false;
    int count = 0;

    A += start * Eigen::Matrix3d::Identity ();

    while(!pass && count < maxIt){

        Eigen::LLT<Eigen::MatrixXd> lltOfA(A); // compute the Cholesky decomposition of A

        if(lltOfA.info() == Eigen::NumericalIssue){
            A += increment * Eigen::Matrix3d::Identity ();
            count++;
        }
        else{
            pass = true;
        }
    }

    return  start + increment * count;
}

int main(){

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

    // Part 1: Visualize a PDF cell
    bool part1 = false;
    if(part1){
        // Create Input from set of points in (x,y) range 0 - 10
        PointCloudT::Ptr input(new PointCloudT);
          input->points.push_back(PointT(4, 4, 0));
        input->points.push_back(PointT(4, 9, 0));
        input->points.push_back(PointT(5, 5, 0));
        input->points.push_back(PointT(5, 2, 0));
        input->points.push_back(PointT(7, 5, 0));

        // render cell border
        renderRay(viewer, PointT(0,0,0), PointT(0,10,0), "left", Color(0,0,1));
        renderRay(viewer, PointT(0,10,0), PointT(10,10,0), "top", Color(0,0,1));
        renderRay(viewer, PointT(10,10,0), PointT(10,0,0), "right", Color(0,0,1));
        renderRay(viewer, PointT(0,0,0), PointT(10,0,0), "bottom", Color(0,0,1));

        // TODO: finish writing the PDF function to visualize the 2D gaussian
        Cell cell = PDF(input, 200, viewer);

        // Test points
        PointT point(1,2,1); // CANDO: change test point to see how it converges

        input->points.push_back(PointT(point.x, point.y, 1.0));
        for(int iteration = 0; iteration < 20; iteration++){ // TODO: increase the iteration count to get convergence

            Eigen::MatrixXd g(3,1);
            g << Eigen::MatrixXd::Zero(3,1);

            Eigen::MatrixXd H(3,3);
            H << Eigen::MatrixXd::Zero(3,3);

            // TODO: finish writing the NewtonsMethod function
            NewtonsMethod(point, 0, cell, g, H);
            PosDef(H, 0, 5, 100); // TODO: change the increment and max values to nonzero values
            // TODO: calculate the 3 x 1 matrix T by using H inverse and g
            Eigen::MatrixXd T = -H.inverse()*g;

            // TODO: calculate the new point by transforming point by the T matrix which is [x translation, y translation, theta rotation]
            //        pointT(new x, new y, 1), values below should be nonzero
            PointT pointT(point.x * cos(T(2,0)) - point.y * sin(T(2,0)) + T(0,0) - point.x ,  point.x * sin(T(2,0)) + point.y * cos(T(2,0)) + T(1,0) - point.y , 1);
            double magT = sqrt(pointT.x*pointT.x + pointT.y*pointT.y);
            double maxDelta = 0.5; // CANDO: change the step size value
            pointT.x *= maxDelta/magT;
            pointT.y *= maxDelta/magT;
            PointT point2(point.x+pointT.x, point.y+pointT.y,1);

            renderRay(viewer, point, point2, "gradient_"+to_string(iteration), Color(1,1,1));
            input->points.push_back(PointT(point2.x, point2.y, 1.0));
            point = point2;
        }

        renderPointCloud(viewer, input, "input", Color(0,0,1));

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

    // Part 2: Multiple PDF cells for target
    else{
        // Load target
        PointCloudT::Ptr target(new PointCloudT);
          pcl::io::loadPCDFile("target.pcd", *target);

        renderPointCloud(viewer, target, "target", Color(0,0,1));

        Grid ndtGrid(3.0, 2, 2); //CANDO: change grid dimension parameters

        for(PointT point : target->points){
            ndtGrid.addPoint(point);
        }
        ndtGrid.Build();

        // Draw grid
        int rowc = 0;
        for(double y = -ndtGrid.height * ndtGrid.res; y <= ndtGrid.height * ndtGrid.res; y += ndtGrid.res){
            renderRay(viewer, PointT(-ndtGrid.width * ndtGrid.res,y,0), PointT(ndtGrid.width * ndtGrid.res,y,0), "grid_row_"+to_string(rowc), Color(0,0,1));
            rowc++;
        }
        int colc = 0;
        for(double x = -ndtGrid.width * ndtGrid.res; x <= ndtGrid.width * ndtGrid.res; x += ndtGrid.res){
            renderRay(viewer, PointT(x,-ndtGrid.height * ndtGrid.res,0), PointT(x,ndtGrid.height * ndtGrid.res,0), "grid_col_"+to_string(colc), Color(0,0,1));
            colc++;
        }

        // Draw total PDF from all cells

        PointCloudTI::Ptr pdf(new PointCloudTI);
        int res = 10;
        for(double y = -ndtGrid.height * ndtGrid.res; y <= ndtGrid.height * ndtGrid.res; y += ndtGrid.res/double(res)){
            for(double x = -ndtGrid.width * ndtGrid.res; x <= ndtGrid.width * ndtGrid.res; x += ndtGrid.res/double(res)){
                Eigen::MatrixXd X(2,1);
                X(0,0) = x;
                X(1,0) = y;

                PointTI point;
                point.x = x;
                point.y = y;
                double value = ndtGrid.Value(PointT(x,y,0));
                point.z = value;
                point.intensity = value;
                if(value > 0.01)
                    pdf->points.push_back(point);
            }
        }

        renderPointCloudI(viewer, pdf, "pdf");

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

        renderPointCloud(viewer, source, "source", Color(1,0,0));

        double sourceScore = Score(source, ndtGrid);

        viewer->addText("Score: "+to_string(sourceScore), 200, 200, 32, 1.0, 1.0, 1.0, "score",0);

        double currentScore = sourceScore;

        int iteration = 0;
          while (!viewer->wasStopped ())
          {

            if(matching){

                viewer->removeShape("score");
                viewer->addText("Score: "+to_string(currentScore), 200, 200, 32, 1.0, 1.0, 1.0, "score",0);

                Eigen::MatrixXd g(3,1);
                g << Eigen::MatrixXd::Zero(3,1);

                Eigen::MatrixXd H(3,3);
                H << Eigen::MatrixXd::Zero(3,3);

                for(PointT point:source->points){
                    Cell cell = ndtGrid.getCell(point);
                    if(cell.cloud->points.size() > 2){
                        double theta = pose.rotation.yaw;
                        double x = pose.position.x;
                        double y = pose.position.y;

                        // TODO: calculate the new point pointTran, by transforming point by x, y, and theta 
                        //        pointTran(new x, new y, point.z), values below should be nonzero
                        PointT pointTran(point.x*cos(theta)-point.y*sin(theta)+x, point.x*sin(theta)+point.y*cos(theta)+y, point.z);
                        NewtonsMethod(pointTran, theta, cell, g, H);
                    }
                }

                PosDef(H, 0, 5, 100); // TODO: change the increment and max values to nonzero values

                Eigen::MatrixXd T = -H.inverse()*g;

                double alpha = computeStepLength(T, source, pose, ndtGrid, currentScore); // CANDO: tweek the computeStepLength function, not optimized

                T *= alpha;

                pose.position.x += T(0,0);
                pose.position.y += T(1,0);
                pose.rotation.yaw += T(2,0);
                while(pose.rotation.yaw > 2*pi)
                    pose.rotation.yaw -= 2*pi;

                Eigen::Matrix4d transform = transform3D(pose.rotation.yaw, 0, 0, pose.position.x, pose.position.y, 0);

                PointCloudT::Ptr transformed_scan (new PointCloudT);
                pcl::transformPointCloud (*source, *transformed_scan, transform);

                double ndtScore = Score(transformed_scan, ndtGrid);

                viewer->removeShape("nscore");
                viewer->addText("NDT Score: "+to_string(ndtScore), 200, 150, 32, 1.0, 1.0, 1.0, "nscore",0);
                currentScore = ndtScore;
                iteration++;

                viewer->removePointCloud("ndt_scan");
                renderPointCloud(viewer, transformed_scan, "ndt_scan", Color(0,1,0));

                matching = false;
            }
            else if(update){

                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));


                double score = Score(transformed_scan, ndtGrid);
                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;
}