template<typename PointSource, typename PointTarget> double
pcl::NormalDistributionsTransform<PointSource, PointTarget>::computeDerivatives (Eigen::Matrix<double, 6, 1> &score_gradient,
Eigen::Matrix<double, 6, 6> &hessian,
PointCloudSource &trans_cloud,
Eigen::Matrix<double, 6, 1> &p,
bool compute_hessian)
{
// Original Point and Transformed Point
PointSource x_pt, x_trans_pt;
// Original Point and Transformed Point (for math)
Eigen::Vector3d x, x_trans;
// Occupied Voxel
TargetGridLeafConstPtr cell;
// Inverse Covariance of Occupied Voxel
Eigen::Matrix3d c_inv;
score_gradient.setZero ();
hessian.setZero ();
double score = 0;
// Precompute Angular Derivatives (eq. 6.19 and 6.21)[Magnusson 2009]
computeAngleDerivatives (p);
// 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);
}
}
return (score);
}
void Filter::updateFilter(measurement_vector z, double an_update_time)
{
updateDerivatives(z, an_update_time);
nu = z-z_hat;
x_hat = x_hat_bar + W*nu;
updateKFCovariance();
ROS_INFO("\nPosition gain KF = %f \nVelocity gain KF = %f\n", getPositionGainX(),getVelocityGainX());
x_hat_bar = F*x_hat;
z_hat = H*x_hat_bar;
//ROS_INFO("Updating Filter");
}
