Skip to content

Commit

Permalink
feat: merge map change checker features to the main branch (#60)
Browse files Browse the repository at this point in the history
* first commit

* Adding map change checker

* Map change checker

* Applied clang

Signed-off-by: Anh Nguyen <[email protected]>

* Applied clang format 17.0.5

Signed-off-by: Anh Nguyen <[email protected]>

---------

Signed-off-by: Anh Nguyen <[email protected]>
Co-authored-by: konishu <[email protected]>
Co-authored-by: SakodaShintaro <[email protected]>
  • Loading branch information
3 people authored Aug 13, 2024
1 parent c5016db commit e32ec1b
Show file tree
Hide file tree
Showing 4 changed files with 183 additions and 96 deletions.
38 changes: 37 additions & 1 deletion include/pclomp/ndt_omp.h
Original file line number Diff line number Diff line change
Expand Up @@ -54,6 +54,16 @@
namespace pclomp
{

struct EigenCmp
{
bool operator()(const Eigen::Vector3i & a, const Eigen::Vector3i & b) const
{
return (
a(0) < b(0) || (a(0) == b(0) && a(1) < b(1)) ||
(a(0) == b(0) && a(1) == b(1) && a(2) < b(2)));
}
};

/** \brief A 3D Normal Distribution Transform registration implementation for point cloud data.
* \note For more information please see
* <b>Magnusson, M. (2009). The Three-Dimensional Normal-Distributions Transform —
Expand Down Expand Up @@ -211,9 +221,28 @@ class NormalDistributionsTransform : public pcl::Registration<PointSource, Point
trans = _affine.matrix();
}

// add at 20220721 konishi
inline const std::vector<double> getScores() const { return scores_; }

inline const TargetGrid getTargetCells() const { return target_cells_; }

inline const std::map<size_t, double> getScoreMap() const { return voxel_score_map_; }

inline const std::map<size_t, size_t> getNoPointMap() const { return nomap_points_num_; }

std::set<Eigen::Vector3i, EigenCmp> & getEmptyVoxels() { return empty_voxels_; }

// For debug
void cleanScores()
{
voxel_score_map_.clear();
nomap_points_num_.clear();
}
// End

// negative log likelihood function
// lower is better
double calculateScore(const PointCloudSource & cloud) const;
double calculateScore(const PointCloudSource & cloud);
double calculateTransformationProbability(const PointCloudSource & cloud) const;
double calculateNearestVoxelTransformationLikelihood(const PointCloudSource & cloud) const;

Expand Down Expand Up @@ -510,6 +539,13 @@ class NormalDistributionsTransform : public pcl::Registration<PointSource, Point
boost::optional<Eigen::Matrix4f> regularization_pose_;
Eigen::Vector3f regularization_pose_translation_;

// add at 20220721 konishi
std::vector<double> scores_;
std::map<size_t, double> voxel_score_map_;
std::map<size_t, size_t> nomap_points_num_;
// For recording voxels that contain no map point
std::set<Eigen::Vector3i, EigenCmp> empty_voxels_;

NdtParams params_;

public:
Expand Down
131 changes: 73 additions & 58 deletions include/pclomp/ndt_omp_impl.hpp
Original file line number Diff line number Diff line change
@@ -1,6 +1,3 @@
#include "ndt_omp.h"

#include <cmath>
/*
* Software License Agreement (BSD License)
*
Expand Down Expand Up @@ -44,7 +41,10 @@
#ifndef PCL_REGISTRATION_NDT_OMP_IMPL_H_
#define PCL_REGISTRATION_NDT_OMP_IMPL_H_

#include "ndt_omp.h"

#include <algorithm>
#include <cmath>
#include <vector>

//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
Expand Down Expand Up @@ -102,7 +102,7 @@ void pclomp::NormalDistributionsTransform<PointSource, PointTarget>::computeTran
// Initialise final transformation to the guessed one
final_transformation_ = guess;
// Apply guessed transformation prior to search for neighbours
transformPointCloud(output, output, guess);
transformPointCloud(*input_, output, guess);
}

Eigen::Transform<float, 3, Eigen::Affine, Eigen::ColMajor> eig_transformation;
Expand Down Expand Up @@ -299,10 +299,7 @@ double pclomp::NormalDistributionsTransform<PointSource, PointTarget>::computeDe
Eigen::Matrix<double, 6, 6> hessian_pt = Eigen::Matrix<double, 6, 6>::Zero();
int neighborhood_count = 0;

for (typename std::vector<TargetGridLeafConstPtr>::iterator neighborhood_it =
neighborhood.begin();
neighborhood_it != neighborhood.end(); ++neighborhood_it) {
cell = *neighborhood_it;
for (auto & cell : neighborhood) {
x_pt = input_->points[idx];
x = Eigen::Vector3d(x_pt.x, x_pt.y, x_pt.z);

Expand Down Expand Up @@ -751,29 +748,23 @@ void pclomp::NormalDistributionsTransform<PointSource, PointTarget>::computeHess
break;
}

for (typename std::vector<TargetGridLeafConstPtr>::iterator neighborhood_it =
neighborhood.begin();
neighborhood_it != neighborhood.end(); ++neighborhood_it) {
cell = *neighborhood_it;
for (auto & cell : neighborhood) {
x_pt = input_->points[idx];
x = Eigen::Vector3d(x_pt.x, x_pt.y, x_pt.z);

{
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);

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();

// 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, point_gradient, point_hessian);
// Update hessian, lines 21 in Algorithm 2, according to Equations 6.10, 6.12 and 6.13,
// respectively [Magnusson 2009]
updateHessian(hessian, point_gradient, point_hessian, x_trans, c_inv);
}
// 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 hessian, lines 21 in Algorithm 2, according to Equations 6.10, 6.12 and 6.13,
// respectively [Magnusson 2009]
updateHessian(hessian, point_gradient, point_hessian, x_trans, c_inv);
}
}
}
Expand Down Expand Up @@ -1087,11 +1078,13 @@ double pclomp::NormalDistributionsTransform<PointSource, PointTarget>::computeSt
return (a_t);
}

// change at 20220721 konishi
template <typename PointSource, typename PointTarget>
double pclomp::NormalDistributionsTransform<PointSource, PointTarget>::calculateScore(
const PointCloudSource & trans_cloud) const
const PointCloudSource & trans_cloud)
{
double score = 0;
std::map<size_t, size_t> voxel_points_num;

for (std::size_t idx = 0; idx < trans_cloud.points.size(); idx++) {
PointSource x_trans_pt = trans_cloud.points[idx];
Expand All @@ -1115,32 +1108,62 @@ double pclomp::NormalDistributionsTransform<PointSource, PointTarget>::calculate
break;
}

for (typename std::vector<TargetGridLeafConstPtr>::iterator neighborhood_it =
neighborhood.begin();
neighborhood_it != neighborhood.end(); ++neighborhood_it) {
TargetGridLeafConstPtr cell = *neighborhood_it;
// add at 20220218 by konishi
size_t voxel_idx;

Eigen::Vector3d x_trans = Eigen::Vector3d(x_trans_pt.x, x_trans_pt.y, x_trans_pt.z);
if (neighborhood.size() == 0) {
// Compute the 3D index of the voxel containing the query point
Eigen::Vector3i vid;

// 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();
vid(0) = static_cast<int>(floor(x_trans_pt.x / params_.resolution));
vid(1) = static_cast<int>(floor(x_trans_pt.y / params_.resolution));
vid(2) = static_cast<int>(floor(x_trans_pt.z / params_.resolution));

// 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 - gauss_d3_;
empty_voxels_.insert(vid);

if (nomap_points_num_.count(voxel_idx) == 0) {
nomap_points_num_[voxel_idx] = 0;
}
nomap_points_num_[voxel_idx] += 1;

} else {
for (auto & cell : neighborhood) {
PointSource x_pt = input_->points[idx];
Eigen::Vector3d x = Eigen::Vector3d(x_pt.x, x_pt.y, x_pt.z);

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 / neighborhood.size();
score += score_inc;

voxel_idx = target_cells_.getLeafIndex(cell->getMean());

if (voxel_points_num.count(voxel_idx) == 0) {
voxel_points_num[voxel_idx] = 0;
voxel_score_map_[voxel_idx] = 0;
}

voxel_score_map_[voxel_idx] += score_inc;
voxel_points_num[voxel_idx] += 1;
}
}
}

double output_score = 0;
if (!trans_cloud.points.empty()) {
output_score = (score) / static_cast<double>(trans_cloud.size());
for (auto & voxel_score_output : voxel_score_map_) {
if (voxel_points_num[voxel_score_output.first] != 0) {
voxel_score_output.second /= (voxel_points_num[voxel_score_output.first]);
}
}
return output_score;

return (score) / static_cast<double>(trans_cloud.size());
}

template <typename PointSource, typename PointTarget>
Expand Down Expand Up @@ -1172,11 +1195,7 @@ pclomp::NormalDistributionsTransform<PointSource, PointTarget>::calculateTransfo
break;
}

for (typename std::vector<TargetGridLeafConstPtr>::iterator neighborhood_it =
neighborhood.begin();
neighborhood_it != neighborhood.end(); ++neighborhood_it) {
TargetGridLeafConstPtr cell = *neighborhood_it;

for (auto & cell : neighborhood) {
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]
Expand Down Expand Up @@ -1230,11 +1249,7 @@ double pclomp::NormalDistributionsTransform<PointSource, PointTarget>::
break;
}

for (typename std::vector<TargetGridLeafConstPtr>::iterator neighborhood_it =
neighborhood.begin();
neighborhood_it != neighborhood.end(); ++neighborhood_it) {
TargetGridLeafConstPtr cell = *neighborhood_it;

for (auto & cell : neighborhood) {
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]
Expand Down
49 changes: 43 additions & 6 deletions include/pclomp/voxel_grid_covariance_omp.h
Original file line number Diff line number Diff line change
Expand Up @@ -101,13 +101,15 @@ class VoxelGridCovariance : public pcl::VoxelGrid<PointT>
struct Leaf
{
/** \brief Constructor.
* Sets \ref nr_points, \ref icov_, \ref mean_ and \ref evals_ to 0 and \ref cov_ and \ref
* Sets \ref nr_points_, \ref icov_, \ref mean_ and \ref evals_ to 0 and \ref cov_ and \ref
* evecs_ to the identity matrix
*/
Leaf()
: nr_points(0),
: nr_points_(0),
mean_(Eigen::Vector3d::Zero()),
centroid(),
// add 20220721 konishi
voxelXYZ_(Eigen::Vector3d::Zero()),
centroid_(),
cov_(Eigen::Matrix3d::Identity()),
icov_(Eigen::Matrix3d::Zero()),
evecs_(Eigen::Matrix3d::Identity()),
Expand All @@ -129,6 +131,8 @@ class VoxelGridCovariance : public pcl::VoxelGrid<PointT>
* \return centroid
*/
Eigen::Vector3d getMean() const { return (mean_); }
// add at 20220721 by konishi
Eigen::Vector3d getLeafCenter() const { return (voxelXYZ_); }

/** \brief Get the eigen vectors of the voxel covariance.
* \note Order corresponds with \ref getEvals
Expand All @@ -145,18 +149,21 @@ class VoxelGridCovariance : public pcl::VoxelGrid<PointT>
/** \brief Get the number of points contained by this voxel.
* \return number of points
*/
int getPointCount() const { return (nr_points); }
int getPointCount() const { return (nr_points_); }

/** \brief Number of points contained by voxel */
int nr_points;
int nr_points_;

/** \brief 3D voxel centroid */
Eigen::Vector3d mean_;

// add at 20220721 by konishi
Eigen::Vector3d voxelXYZ_;

/** \brief Nd voxel centroid
* \note Differs from \ref mean_ when color data is used
*/
Eigen::VectorXf centroid;
Eigen::VectorXf centroid_;

/** \brief Voxel covariance matrix */
Eigen::Matrix3d cov_;
Expand Down Expand Up @@ -329,6 +336,36 @@ class VoxelGridCovariance : public pcl::VoxelGrid<PointT>
return NULL;
}

// add at 20220218 by konishi
inline size_t getLeafIndex(const Eigen::Vector3d & p)
{
// Generate index associated with p
int ijk0 = static_cast<int>(floor(p[0] * inverse_leaf_size_[0]) - min_b_[0]);
int ijk1 = static_cast<int>(floor(p[1] * inverse_leaf_size_[1]) - min_b_[1]);
int ijk2 = static_cast<int>(floor(p[2] * inverse_leaf_size_[2]) - min_b_[2]);

// Compute the centroid leaf index
return (ijk0 * divb_mul_[0] + ijk1 * divb_mul_[1] + ijk2 * divb_mul_[2]);
}

// add at 20220721 by konishi
inline Eigen::Vector3d
// getLeafCenterの命名ほうがよい
getLeafCenter(const size_t index)
{
int ijk2 = index / divb_mul_[2];
int ijk1 = (index % divb_mul_[2]) / divb_mul_[1];
int ijk0 = ((index % divb_mul_[2]) % divb_mul_[1]) / divb_mul_[0];

Eigen::Vector3d p;

p[0] = (static_cast<double>(ijk0 + min_b_[0]) + 0.5) / inverse_leaf_size_[0];
p[1] = (static_cast<double>(ijk1 + min_b_[1]) + 0.5) / inverse_leaf_size_[1];
p[2] = (static_cast<double>(ijk2 + min_b_[2]) + 0.5) / inverse_leaf_size_[2];

return p;
}

/** \brief Get the voxels surrounding point p, not including the voxel containing point p.
* \note Only voxels containing a sufficient number of points are used (slower than radius search
* in practice). \param[in] reference_point the point to get the leaf structure at \param[out]
Expand Down
Loading

0 comments on commit e32ec1b

Please sign in to comment.