Skip to content

Commit

Permalink
Applied clang
Browse files Browse the repository at this point in the history
Signed-off-by: Anh Nguyen <[email protected]>
  • Loading branch information
anhnv3991 committed Aug 8, 2024
1 parent 95ac770 commit 8edf9cf
Show file tree
Hide file tree
Showing 8 changed files with 353 additions and 218 deletions.
98 changes: 78 additions & 20 deletions include/multigrid_pclomp/multigrid_ndt_omp.h
Original file line number Diff line number Diff line change
Expand Up @@ -125,7 +125,9 @@ class MultiGridNormalDistributionsTransform : public pcl::Registration<PointSour
MultiGridNormalDistributionsTransform && other) noexcept;

/** \brief Empty destructor */
virtual ~MultiGridNormalDistributionsTransform() {}
virtual ~MultiGridNormalDistributionsTransform()
{
}

void setNumThreads(int n)
{
Expand All @@ -134,7 +136,10 @@ class MultiGridNormalDistributionsTransform : public pcl::Registration<PointSour
target_cells_.setThreadNum(params_.num_threads);
}

inline int getNumThreads() const { return params_.num_threads; }
inline int getNumThreads() const
{
return params_.num_threads;
}

inline void setInputSource(const PointCloudSourceConstPtr & input)
{
Expand Down Expand Up @@ -165,7 +170,10 @@ class MultiGridNormalDistributionsTransform : public pcl::Registration<PointSour
target_cells_.setInputCloudAndFilter(cloud, target_id);
}

inline void removeTarget(const std::string target_id) { target_cells_.removeCloud(target_id); }
inline void removeTarget(const std::string target_id)
{
target_cells_.removeCloud(target_id);
}

inline void createVoxelKdtree()
{
Expand All @@ -188,32 +196,50 @@ class MultiGridNormalDistributionsTransform : public pcl::Registration<PointSour
/** \brief Get voxel grid resolution.
* \return side length of voxels
*/
inline float getResolution() const { return (params_.resolution); }
inline float getResolution() const
{
return (params_.resolution);
}

/** \brief Get the newton line search maximum step length.
* \return maximum step length
*/
inline double getStepSize() const { return (params_.step_size); }
inline double getStepSize() const
{
return (params_.step_size);
}

/** \brief Set/change the newton line search maximum step length.
* \param[in] step_size maximum step length
*/
inline void setStepSize(double step_size) { params_.step_size = step_size; }
inline void setStepSize(double step_size)
{
params_.step_size = step_size;
}

/** \brief Get the point cloud outlier ratio.
* \return outlier ratio
*/
inline double getOutlierRatio() const { return (outlier_ratio_); }
inline double getOutlierRatio() const
{
return (outlier_ratio_);
}

/** \brief Set/change the point cloud outlier ratio.
* \param[in] outlier_ratio outlier ratio
*/
inline void setOutlierRatio(double outlier_ratio) { outlier_ratio_ = outlier_ratio; }
inline void setOutlierRatio(double outlier_ratio)
{
outlier_ratio_ = outlier_ratio;
}

/** \brief Get the registration alignment probability.
* \return transformation probability
*/
inline double getTransformationProbability() const { return (trans_probability_); }
inline double getTransformationProbability() const
{
return (trans_probability_);
}

inline double getNearestVoxelTransformationLikelihood() const
{
Expand All @@ -223,10 +249,16 @@ class MultiGridNormalDistributionsTransform : public pcl::Registration<PointSour
/** \brief Get the number of iterations required to calculate alignment.
* \return final number of iterations
*/
inline int getFinalNumIteration() const { return (nr_iterations_); }
inline int getFinalNumIteration() const
{
return (nr_iterations_);
}

/** \brief Return the hessian matrix */
inline Eigen::Matrix<double, 6, 6> getHessian() const { return hessian_; }
inline Eigen::Matrix<double, 6, 6> getHessian() const
{
return hessian_;
}

/** \brief Return the transformation array */
inline const std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f>> &
Expand Down Expand Up @@ -273,7 +305,10 @@ class MultiGridNormalDistributionsTransform : public pcl::Registration<PointSour
regularization_pose_ = regularization_pose;
}

inline void unsetRegularizationPose() { regularization_pose_ = boost::none; }
inline void unsetRegularizationPose()
{
regularization_pose_ = boost::none;
}

NdtResult getResult()
{
Expand All @@ -296,22 +331,34 @@ class MultiGridNormalDistributionsTransform : public pcl::Registration<PointSour
* transformation epsilon in order for an optimization to be considered as having
* converged to the final solution.
*/
inline void setTransformationEpsilon(double epsilon) { params_.trans_epsilon = epsilon; }
inline void setTransformationEpsilon(double epsilon)
{
params_.trans_epsilon = epsilon;
}

/** \brief Get the transformation epsilon (maximum allowable translation squared
* difference between two consecutive transformations) as set by the user.
*/
inline double getTransformationEpsilon() { return (params_.trans_epsilon); }
inline double getTransformationEpsilon()
{
return (params_.trans_epsilon);
}

inline void setMaximumIterations(int max_iterations)
{
params_.max_iterations = max_iterations;
max_iterations_ = params_.max_iterations;
}

inline int getMaxIterations() const { return params_.max_iterations; }
inline int getMaxIterations() const
{
return params_.max_iterations;
}

inline int getMaxIterations() { return params_.max_iterations; }
inline int getMaxIterations()
{
return params_.max_iterations;
}

void setParams(const NdtParams & ndt_params)
{
Expand All @@ -321,11 +368,20 @@ class MultiGridNormalDistributionsTransform : public pcl::Registration<PointSour
target_cells_.setThreadNum(params_.num_threads);
}

NdtParams getParams() const { return params_; }
NdtParams getParams() const
{
return params_;
}

pcl::PointCloud<PointTarget> getVoxelPCD() const { return target_cells_.getVoxelPCD(); }
pcl::PointCloud<PointTarget> getVoxelPCD() const
{
return target_cells_.getVoxelPCD();
}

std::vector<std::string> getCurrentMapIDs() const { return target_cells_.getCurrentMapIDs(); }
std::vector<std::string> getCurrentMapIDs() const
{
return target_cells_.getCurrentMapIDs();
}

protected:
using BaseRegType::converged_;
Expand Down Expand Up @@ -357,7 +413,9 @@ class MultiGridNormalDistributionsTransform : public pcl::Registration<PointSour
virtual void computeTransformation(PointCloudSource & output, const Eigen::Matrix4f & guess);

/** \brief Initiate covariance voxel structure. */
void inline init() {}
void inline init()
{
}

/** \brief Compute derivatives of probability function w.r.t. the transformation vector.
* \note Equation 6.10, 6.12 and 6.13 [Magnusson 2009].
Expand Down
30 changes: 24 additions & 6 deletions include/pclomp/gicp_omp.h
Original file line number Diff line number Diff line change
Expand Up @@ -217,33 +217,51 @@ class GeneralizedIterativeClosestPoint : public pcl::IterativeClosestPoint<Point
* converged to the final solution.
* \param epsilon the rotation epsilon
*/
inline void setRotationEpsilon(double epsilon) { rotation_epsilon_ = epsilon; }
inline void setRotationEpsilon(double epsilon)
{
rotation_epsilon_ = epsilon;
}

/** \brief Get the rotation epsilon (maximum allowable difference between two
* consecutive rotations) as set by the user.
*/
inline double getRotationEpsilon() { return (rotation_epsilon_); }
inline double getRotationEpsilon()
{
return (rotation_epsilon_);
}

/** \brief Set the number of neighbors used when selecting a point neighbourhood
* to compute covariances.
* A higher value will bring more accurate covariance matrix but will make
* covariances computation slower.
* \param k the number of neighbors to use when computing covariances
*/
void setCorrespondenceRandomness(int k) { k_correspondences_ = k; }
void setCorrespondenceRandomness(int k)
{
k_correspondences_ = k;
}

/** \brief Get the number of neighbors used when computing covariances as set by
* the user
*/
int getCorrespondenceRandomness() { return (k_correspondences_); }
int getCorrespondenceRandomness()
{
return (k_correspondences_);
}

/** set maximum number of iterations at the optimization step
* \param[in] max maximum number of iterations for the optimizer
*/
void setMaximumOptimizerIterations(int max) { max_inner_iterations_ = max; }
void setMaximumOptimizerIterations(int max)
{
max_inner_iterations_ = max;
}

///\return maximum number of iterations at the optimization step
int getMaximumOptimizerIterations() { return (max_inner_iterations_); }
int getMaximumOptimizerIterations()
{
return (max_inner_iterations_);
}

protected:
/** \brief The number of neighbors used for covariances computation.
Expand Down
Loading

0 comments on commit 8edf9cf

Please sign in to comment.