Skip to content

Commit

Permalink
Correct format
Browse files Browse the repository at this point in the history
  • Loading branch information
@yaoyx689
yaoyx689 committed Dec 17, 2024
1 parent 213e84f commit f2f6dea
Show file tree
Hide file tree
Showing 2 changed files with 92 additions and 82 deletions.
124 changes: 62 additions & 62 deletions ...tration/include/pcl/registration/impl/transformation_estimation_point_to_point_robust.hpp
100755 → 100644
View file
Original file line number Diff line number Diff line change
Expand Up @@ -56,7 +56,8 @@ TransformationEstimationPointToPointRobust<PointSource, PointTarget, Scalar>::
{
const auto nr_points = cloud_src.size();
if (cloud_tgt.size() != nr_points) {
PCL_ERROR("[pcl::TransformationEstimationPointToPointRobust::estimateRigidTransformation] Number "
PCL_ERROR("[pcl::TransformationEstimationPointToPointRobust::"
"estimateRigidTransformation] Number "
"or points in source (%zu) differs than target (%zu)!\n",
static_cast<std::size_t>(nr_points),
static_cast<std::size_t>(cloud_tgt.size()));
Expand Down Expand Up @@ -133,58 +134,56 @@ TransformationEstimationPointToPointRobust<PointSource, PointTarget, Scalar>::
{
// Convert to Eigen format
const int npts = static_cast<int>(source_it.size());
source_it.reset();
target_it.reset();
Eigen::Matrix<Scalar, Eigen::Dynamic, 1> weights(npts);
Eigen::Matrix<Scalar, Eigen::Dynamic, 1> square_distances(npts);
for(int i = 0; i < npts; i++)
{
Scalar dx = source_it->x - target_it->x;
Scalar dy = source_it->y - target_it->y;
Scalar dz = source_it->z - target_it->z;
Scalar dist2 = dx*dx + dy*dy + dz*dz;
square_distances[i] = dist2;

source_it++;
target_it++;
}
source_it.reset();
target_it.reset();
Eigen::Matrix<Scalar, Eigen::Dynamic, 1> weights(npts);
Eigen::Matrix<Scalar, Eigen::Dynamic, 1> square_distances(npts);
for (int i = 0; i < npts; i++) {
Scalar dx = source_it->x - target_it->x;
Scalar dy = source_it->y - target_it->y;
Scalar dz = source_it->z - target_it->z;
Scalar dist2 = dx * dx + dy * dy + dz * dz;
square_distances[i] = dist2;

Scalar sigma2;
if(sigma_ < 0)
sigma2 = square_distances.maxCoeff()/9.0;
else
sigma2 = sigma_*sigma_;

for(int i = 0; i < npts; i++)
{
weights[i] = std::exp(-square_distances[i]/(2.0*sigma2));
}
weights = weights/weights.sum();

source_it.reset();
target_it.reset();
// <cloud_src,cloud_src> is the source dataset
transformation_matrix.setIdentity();

Eigen::Matrix<Scalar, 4, 1> centroid_src, centroid_tgt;
// Estimate the centroids of source, target
computeWeighted3DCentroid(source_it, weights, centroid_src);
computeWeighted3DCentroid(target_it, weights, centroid_tgt);
source_it.reset();
target_it.reset();

// Subtract the centroids from source, target
Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> cloud_src_demean,
cloud_tgt_demean;
demeanPointCloud(source_it, centroid_src, cloud_src_demean);
demeanPointCloud(target_it, centroid_tgt, cloud_tgt_demean);

getTransformationFromCorrelation(cloud_src_demean,
centroid_src,
cloud_tgt_demean,
centroid_tgt,
weights,
transformation_matrix);
source_it++;
target_it++;
}

Scalar sigma2;
if (sigma_ < 0)
sigma2 = square_distances.maxCoeff() / 9.0;
else
sigma2 = sigma_ * sigma_;

for (int i = 0; i < npts; i++) {
weights[i] = std::exp(-square_distances[i] / (2.0 * sigma2));
}
weights = weights / weights.sum();

source_it.reset();
target_it.reset();
// <cloud_src,cloud_src> is the source dataset
transformation_matrix.setIdentity();

Eigen::Matrix<Scalar, 4, 1> centroid_src, centroid_tgt;
// Estimate the centroids of source, target
computeWeighted3DCentroid(source_it, weights, centroid_src);
computeWeighted3DCentroid(target_it, weights, centroid_tgt);
source_it.reset();
target_it.reset();

// Subtract the centroids from source, target
Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> cloud_src_demean,
cloud_tgt_demean;
demeanPointCloud(source_it, centroid_src, cloud_src_demean);
demeanPointCloud(target_it, centroid_tgt, cloud_tgt_demean);

getTransformationFromCorrelation(cloud_src_demean,
centroid_src,
cloud_tgt_demean,
centroid_tgt,
weights,
transformation_matrix);
}

template <typename PointSource, typename PointTarget, typename Scalar>
Expand All @@ -202,7 +201,8 @@ TransformationEstimationPointToPointRobust<PointSource, PointTarget, Scalar>::

// Assemble the correlation matrix H = source * weights * target'
Eigen::Matrix<Scalar, 3, 3> H =
(cloud_src_demean * weights.asDiagonal() * cloud_tgt_demean.transpose()).template topLeftCorner<3, 3>();
(cloud_src_demean * weights.asDiagonal() * cloud_tgt_demean.transpose())
.template topLeftCorner<3, 3>();

// Compute the Singular Value Decomposition
Eigen::JacobiSVD<Eigen::Matrix<Scalar, 3, 3>> svd(
Expand Down Expand Up @@ -232,22 +232,22 @@ TransformationEstimationPointToPointRobust<PointSource, PointTarget, Scalar>::
}
}


template <typename PointSource, typename PointTarget, typename Scalar> inline unsigned int
TransformationEstimationPointToPointRobust<PointSource, PointTarget, Scalar>::computeWeighted3DCentroid(ConstCloudIterator<PointSource> &cloud_iterator, Eigen::Matrix<Scalar, Eigen::Dynamic, 1>& weights,
Eigen::Matrix<Scalar, 4, 1> &centroid) const
template <typename PointSource, typename PointTarget, typename Scalar>
inline unsigned int
TransformationEstimationPointToPointRobust<PointSource, PointTarget, Scalar>::
computeWeighted3DCentroid(ConstCloudIterator<PointSource>& cloud_iterator,
Eigen::Matrix<Scalar, Eigen::Dynamic, 1>& weights,
Eigen::Matrix<Scalar, 4, 1>& centroid) const
{
Eigen::Matrix<Scalar, 4, 1> accumulator {0, 0, 0, 0};
Eigen::Matrix<Scalar, 4, 1> accumulator{0, 0, 0, 0};

unsigned int cp = 0;

// For each point in the cloud
// If the data is dense, we don't need to check for NaN
while (cloud_iterator.isValid ())
{
while (cloud_iterator.isValid()) {
// Check if the point is invalid
if (pcl::isFinite (*cloud_iterator))
{
if (pcl::isFinite(*cloud_iterator)) {
accumulator[0] += weights[cp] * cloud_iterator->x;
accumulator[1] += weights[cp] * cloud_iterator->y;
accumulator[2] += weights[cp] * cloud_iterator->z;
Expand Down
50 changes: 30 additions & 20 deletions registration/include/pcl/registration/transformation_estimation_point_to_point_robust.h
100755 → 100644
View file
Original file line number Diff line number Diff line change
Expand Up @@ -47,9 +47,9 @@
namespace pcl {
namespace registration {
/** @b TransformationEstimationPointToPointRobust implements SVD-based estimation of
* the transformation aligning the given correspondences for minimizing the Welsch function instead of L2-norm,
* For additional details, see
* "Fast and Robust Iterative Closest Point", Juyong Zhang, Yuxin Yao, Bailin Deng, 2022.
* the transformation aligning the given correspondences for minimizing the Welsch
* function instead of L2-norm, For additional details, see "Fast and Robust Iterative
* Closest Point", Juyong Zhang, Yuxin Yao, Bailin Deng, 2022.
* \note The class is templated on the source and target point types as well as on the
* output scalar of the transformation matrix (i.e., float or double). Default: float.
* \author Yuxin Yao
Expand All @@ -59,9 +59,12 @@ template <typename PointSource, typename PointTarget, typename Scalar = float>
class TransformationEstimationPointToPointRobust
: public TransformationEstimation<PointSource, PointTarget, Scalar> {
public:
using Ptr = shared_ptr<TransformationEstimationPointToPointRobust<PointSource, PointTarget, Scalar>>;
using Ptr = shared_ptr<
TransformationEstimationPointToPointRobust<PointSource, PointTarget, Scalar>>;
using ConstPtr =
shared_ptr<const TransformationEstimationPointToPointRobust<PointSource, PointTarget, Scalar>>;
shared_ptr<const TransformationEstimationPointToPointRobust<PointSource,
PointTarget,
Scalar>>;

using Matrix4 =
typename TransformationEstimation<PointSource, PointTarget, Scalar>::Matrix4;
Expand Down Expand Up @@ -123,7 +126,11 @@ class TransformationEstimationPointToPointRobust
const pcl::Correspondences& correspondences,
Matrix4& transformation_matrix) const override;

void set_sigma(Scalar sigma) { sigma_=sigma; };
void
set_sigma(Scalar sigma)
{
sigma_ = sigma;
};

protected:
/** \brief Estimate a rigid rotation transformation between a source and a target
Expand Down Expand Up @@ -152,23 +159,26 @@ class TransformationEstimationPointToPointRobust
const Eigen::Matrix<Scalar, Eigen::Dynamic, 1>& weights,
Matrix4& transformation_matrix) const;

/** \brief Compute the 3D (X-Y-Z) centroid of a set of weighted points and return it as a 3D vector.
* \param[in] cloud_iterator an iterator over the input point cloud
* \param[in] weights the weights corresponding to points in the input point cloud
* \param[out] centroid the output centroid
* \return number of valid points used to determine the centroid. In case of dense point clouds, this is the same as the size of input cloud.
* \note if return value is 0, the centroid is not changed, thus not valid.
* The last component of the vector is set to 1, this allows to transform the centroid vector with 4x4 matrices.
* \ingroup common
*/
inline unsigned int
computeWeighted3DCentroid(ConstCloudIterator<PointSource> &cloud_iterator, Eigen::Matrix<Scalar, Eigen::Dynamic, 1>& weights, Eigen::Matrix<Scalar, 4, 1> &centroid) const;
/** \brief Compute the 3D (X-Y-Z) centroid of a set of weighted points and return it
* as a 3D vector.
* \param[in] cloud_iterator an iterator over the input point cloud
* \param[in] weights the weights corresponding to points in the input point cloud
* \param[out] centroid the output centroid
* \return number of valid points used to determine the centroid. In case of dense
* point clouds, this is the same as the size of input cloud.
* \note if return value is 0, the centroid is not changed, thus not valid.
* The last component of the vector is set to 1, this allows to transform the centroid
* vector with 4x4 matrices.
* \ingroup common
*/
inline unsigned int
computeWeighted3DCentroid(ConstCloudIterator<PointSource>& cloud_iterator,
Eigen::Matrix<Scalar, Eigen::Dynamic, 1>& weights,
Eigen::Matrix<Scalar, 4, 1>& centroid) const;

/** parameter for the Welsch function */
Scalar sigma_ = -1;


};
};

} // namespace registration
} // namespace pcl
Expand Down

0 comments on commit f2f6dea

Please sign in to comment.