八、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;
}
2932
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
