void CGrid::bayesOccupancyFilter() { // PREDICTION BY MOTION MODEL computeLikelihood(polar_array.predicted, predicted_true_likelihood_, predicted_false_likelihood_); updateGridProbability(prior, predicted_true_likelihood_, predicted_false_likelihood_, predicted_posterior_); // UPDATE BY OBSERVATION computeLikelihood(polar_array.current, true_likelihood_, false_likelihood_); updateGridProbability(predicted_posterior_, true_likelihood_, false_likelihood_, posterior); prior = posterior; polar_array.past = polar_array.current; max_probability_ = posterior.at(maxProbCellIndex()); }
void PlaneSupportedCuboidEstimator::likelihood(pcl::PointCloud<pcl::PointXYZ>::ConstPtr input, pcl::tracking::ParticleCuboid& p) { p.weight = computeLikelihood(p, candidate_cloud_, tree_, viewpoint_, polygons_, latest_polygon_msg_->likelihood, config_); }
double NGSTree::optimizeAllBranches(int my_iterations, double tolerance, int maxNRStep) { return computeLikelihood(); }