From 403f0d0884ca5c6b7f1b6624c511b444ccc6cc60 Mon Sep 17 00:00:00 2001 From: Yamato Ando Date: Tue, 22 Mar 2022 14:28:26 +0900 Subject: [PATCH] Feature/nearest voxel transformation probability (#10) * WIP * add calculateTransformationProbability funcs * avoid zero division * restored the condition * avoid zero division Signed-off-by: YamatoAndo --- include/pclomp/ndt_omp.h | 9 ++ include/pclomp/ndt_omp_impl.hpp | 159 ++++++++++++++++++++++++++++++-- 2 files changed, 159 insertions(+), 9 deletions(-) diff --git a/include/pclomp/ndt_omp.h b/include/pclomp/ndt_omp.h index c2e103ac..c9e19c1f 100644 --- a/include/pclomp/ndt_omp.h +++ b/include/pclomp/ndt_omp.h @@ -213,6 +213,12 @@ namespace pclomp return (trans_probability_); } + inline double + getNearestVoxelTransformationProbability() const + { + return nearest_voxel_transformation_probability_; + } + /** \brief Get the number of iterations required to calculate alignment. * \return final number of iterations */ @@ -264,6 +270,8 @@ namespace pclomp // negative log likelihood function // lower is better double calculateScore(const PointCloudSource& cloud) const; + double calculateTransformationProbability(const PointCloudSource& cloud) const; + double calculateNearestVoxelTransformationProbability(const PointCloudSource& cloud) const; inline void setRegularizationScaleFactor(float regularization_scale_factor) { @@ -540,6 +548,7 @@ namespace pclomp Eigen::Matrix hessian_; std::vector> transformation_array_; + double nearest_voxel_transformation_probability_; float regularization_scale_factor_; boost::optional regularization_pose_; diff --git a/include/pclomp/ndt_omp_impl.hpp b/include/pclomp/ndt_omp_impl.hpp index 812bc590..b4775eb1 100644 --- a/include/pclomp/ndt_omp_impl.hpp +++ b/include/pclomp/ndt_omp_impl.hpp @@ -211,13 +211,19 @@ pclomp::NormalDistributionsTransform::computeDerivativ hessian.setZero(); double score = 0; int total_neighborhood_count = 0; + double nearest_voxel_score = 0; + size_t found_neigborhood_voxel_num = 0; std::vector scores(input_->points.size()); + std::vector nearest_voxel_scores(input_->points.size()); + std::vector found_neigborhood_voxel_nums(input_->points.size()); std::vector, Eigen::aligned_allocator>> score_gradients(input_->points.size()); std::vector, Eigen::aligned_allocator>> hessians(input_->points.size()); std::vector neighborhood_counts(input_->points.size()); for (std::size_t i = 0; i < input_->points.size(); i++) { scores[i] = 0; + nearest_voxel_scores[i] = 0; + found_neigborhood_voxel_nums[i] = 0; score_gradients[i].setZero(); hessians[i].setZero(); neighborhood_counts[i] = 0; @@ -273,7 +279,8 @@ pclomp::NormalDistributionsTransform::computeDerivativ break; } - double score_pt = 0; + double sum_score_pt = 0; + double nearest_voxel_score_pt = 0; Eigen::Matrix score_gradient_pt = Eigen::Matrix::Zero(); Eigen::Matrix hessian_pt = Eigen::Matrix::Zero(); int neighborhood_count = 0; @@ -294,12 +301,20 @@ pclomp::NormalDistributionsTransform::computeDerivativ // 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, point_gradient_, point_hessian_); // 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_pt += updateDerivatives(score_gradient_pt, hessian_pt, point_gradient_, point_hessian_, x_trans, c_inv, compute_hessian); - + double score_pt = updateDerivatives(score_gradient_pt, hessian_pt, point_gradient_, point_hessian_, x_trans, c_inv, compute_hessian); neighborhood_count++; + sum_score_pt += score_pt; + if (score_pt > nearest_voxel_score_pt) { + nearest_voxel_score_pt = score_pt; + } } - scores[idx] = score_pt; + if(!neighborhood.empty()) { + ++found_neigborhood_voxel_nums[idx]; + } + + scores[idx] = sum_score_pt; + nearest_voxel_scores[idx] = nearest_voxel_score_pt; score_gradients[idx].noalias() = score_gradient_pt; hessians[idx].noalias() = hessian_pt; neighborhood_counts[idx] += neighborhood_count; @@ -308,6 +323,8 @@ pclomp::NormalDistributionsTransform::computeDerivativ // Ensure that the result is invariant against the summing up order for (std::size_t i = 0; i < input_->points.size(); i++) { score += scores[i]; + nearest_voxel_score += nearest_voxel_scores[i]; + found_neigborhood_voxel_num += found_neigborhood_voxel_nums[i]; score_gradient += score_gradients[i]; hessian += hessians[i]; total_neighborhood_count += neighborhood_counts[i]; @@ -340,6 +357,13 @@ pclomp::NormalDistributionsTransform::computeDerivativ hessian += regularization_hessian; } + if (found_neigborhood_voxel_num != 0) { + nearest_voxel_transformation_probability_ = nearest_voxel_score / static_cast(found_neigborhood_voxel_num); + } + else { + nearest_voxel_transformation_probability_ = 0.0; + } + return (score); } @@ -989,7 +1013,6 @@ pclomp::NormalDistributionsTransform::computeStepLengt return (a_t); } - template double pclomp::NormalDistributionsTransform::calculateScore(const PointCloudSource & trans_cloud) const { @@ -1038,13 +1061,131 @@ double pclomp::NormalDistributionsTransform::calculate } } - if (trans_cloud.points.empty()) { - return 0.0f; + double output_score = 0; + if (!trans_cloud.points.empty()) { + output_score = (score) / static_cast (trans_cloud.size()); } - else { - return (score) / static_cast (trans_cloud.points.size()); + return output_score; +} + +template +double pclomp::NormalDistributionsTransform::calculateTransformationProbability(const PointCloudSource & trans_cloud) const +{ + double score = 0; + + for (std::size_t idx = 0; idx < trans_cloud.points.size(); idx++) + { + PointSource x_trans_pt = trans_cloud.points[idx]; + + // Find neighbors (Radius search has been experimentally faster than direct neighbor checking. + std::vector neighborhood; + std::vector distances; + switch (search_method) { + case KDTREE: + target_cells_.radiusSearch(x_trans_pt, resolution_, neighborhood, distances); + break; + case DIRECT26: + target_cells_.getNeighborhoodAtPoint(x_trans_pt, neighborhood); + break; + default: + case DIRECT7: + target_cells_.getNeighborhoodAtPoint7(x_trans_pt, neighborhood); + break; + case DIRECT1: + target_cells_.getNeighborhoodAtPoint1(x_trans_pt, neighborhood); + break; + } + + for (typename std::vector::iterator neighborhood_it = neighborhood.begin(); neighborhood_it != neighborhood.end(); neighborhood_it++) + { + TargetGridLeafConstPtr cell = *neighborhood_it; + + Eigen::Vector3d 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. + Eigen::Matrix3d c_inv = cell->getInverseCov(); + + // e^(-d_2/2 * (x_k - mu_k)^T Sigma_k^-1 (x_k - mu_k)) Equation 6.9 [Magnusson 2009] + double e_x_cov_x = exp(-gauss_d2_ * x_trans.dot(c_inv * x_trans) / 2); + // Calculate probability of transformed points existence, Equation 6.9 [Magnusson 2009] + double score_inc = -gauss_d1_ * e_x_cov_x; + + score += score_inc; + } + } + + double output_score = 0; + if (!trans_cloud.points.empty()) { + output_score = (score) / static_cast (trans_cloud.points.size()); } + return output_score; +} + +template +double pclomp::NormalDistributionsTransform::calculateNearestVoxelTransformationProbability(const PointCloudSource & trans_cloud) const +{ + double nearest_voxel_score = 0; + size_t found_neigborhood_voxel_num = 0; + for (std::size_t idx = 0; idx < trans_cloud.points.size(); idx++) + { + double nearest_voxel_score_pt = 0; + PointSource x_trans_pt = trans_cloud.points[idx]; + + // Find neighbors (Radius search has been experimentally faster than direct neighbor checking. + std::vector neighborhood; + std::vector distances; + switch (search_method) { + case KDTREE: + target_cells_.radiusSearch(x_trans_pt, resolution_, neighborhood, distances); + break; + case DIRECT26: + target_cells_.getNeighborhoodAtPoint(x_trans_pt, neighborhood); + break; + default: + case DIRECT7: + target_cells_.getNeighborhoodAtPoint7(x_trans_pt, neighborhood); + break; + case DIRECT1: + target_cells_.getNeighborhoodAtPoint1(x_trans_pt, neighborhood); + break; + } + + for (typename std::vector::iterator neighborhood_it = neighborhood.begin(); neighborhood_it != neighborhood.end(); neighborhood_it++) + { + TargetGridLeafConstPtr cell = *neighborhood_it; + + Eigen::Vector3d 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. + Eigen::Matrix3d c_inv = cell->getInverseCov(); + + // e^(-d_2/2 * (x_k - mu_k)^T Sigma_k^-1 (x_k - mu_k)) Equation 6.9 [Magnusson 2009] + double e_x_cov_x = exp(-gauss_d2_ * x_trans.dot(c_inv * x_trans) / 2); + // Calculate probability of transformed points existence, Equation 6.9 [Magnusson 2009] + double score_inc = -gauss_d1_ * e_x_cov_x; + + if (score_inc > nearest_voxel_score_pt) { + nearest_voxel_score_pt = score_inc; + } + } + + if (!neighborhood.empty()) { + ++found_neigborhood_voxel_num; + nearest_voxel_score += nearest_voxel_score_pt; + } + + } + + double output_score = 0; + if (found_neigborhood_voxel_num != 0) { + output_score = nearest_voxel_score / static_cast (found_neigborhood_voxel_num); + } + return output_score; } #endif // PCL_REGISTRATION_NDT_IMPL_H_