3D NDT [20151126]

[Magnusson 2009]
NDT的核心即，栅格内正态分布拟合来表征数据点的分布。
优化用Newton-Raphson算法，因其source point set与target point set之间没有对应关系，目标函数为指数函数和形式，无法转化为最小二乘估计问题来求解。

NDT方法的关键应为line 3~6。
而对于其程序实现，本质上也是基于邻近搜索的方法。

  // Update gradient and hessian for each point, line 17 in Algorithm 2 [Magnusson 2009]  for (size_t idx = 0; idx < input_->points.size (); idx++)  {    x_trans_pt = trans_cloud.points[idx];    // Find nieghbors (Radius search has been experimentally faster than direct neighbor checking.    std::vector<TargetGridLeafConstPtr> neighborhood;    std::vector<float> distances;    target_cells_.radiusSearch (x_trans_pt, resolution_, neighborhood, distances);    for (typename std::vector<TargetGridLeafConstPtr>::iterator neighborhood_it = neighborhood.begin (); neighborhood_it != neighborhood.end (); neighborhood_it++)    {      cell = *neighborhood_it;      x_pt = input_->points[idx];      x = Eigen::Vector3d (x_pt.x, x_pt.y, x_pt.z);      x_trans = Eigen::Vector3d (x_trans_pt.x, x_trans_pt.y, x_trans_pt.z);      // Denorm point, x_k' in Equations 6.12 and 6.13 [Magnusson 2009]      x_trans -= cell->getMean ();      // Uses precomputed covariance for speed.      c_inv = cell->getInverseCov ();      // Compute derivative of transform function w.r.t. transform vector, J_E and H_E in Equations 6.18 and 6.20 [Magnusson 2009]      computePointDerivatives (x);      // Update score, gradient and hessian, lines 19-21 in Algorithm 2, according to Equations 6.10, 6.12 and 6.13, respectively [Magnusson 2009]      score += updateDerivatives (score_gradient, hessian, x_trans, c_inv, compute_hessian);    }  }