Loading...
Searching...
No Matches
transformation_estimation_2D.hpp
/*
* Software License Agreement (BSD License)
*
* Point Cloud Library (PCL) - www.pointclouds.org
* Copyright (c) 2012-, Open Perception Inc.
*
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the copyright holder(s) nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
*/
#ifndef PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_2D_HPP_
#define PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_2D_HPP_
namespace pcl {
namespace registration {
template <typename PointSource, typename PointTarget, typename Scalar>
inline void
TransformationEstimation2D<PointSource, PointTarget, Scalar>::
estimateRigidTransformation(const pcl::PointCloud<PointSource>& cloud_src,
const pcl::PointCloud<PointTarget>& cloud_tgt,
Matrix4& transformation_matrix) const
{
const auto nr_points = cloud_src.size();
if (cloud_tgt.size() != nr_points) {
PCL_ERROR("[pcl::TransformationEstimation2D::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()));
return;
}
ConstCloudIterator<PointSource> source_it(cloud_src);
ConstCloudIterator<PointTarget> target_it(cloud_tgt);
estimateRigidTransformation(source_it, target_it, transformation_matrix);
}
template <typename PointSource, typename PointTarget, typename Scalar>
void
TransformationEstimation2D<PointSource, PointTarget, Scalar>::
estimateRigidTransformation(const pcl::PointCloud<PointSource>& cloud_src,
const pcl::Indices& indices_src,
const pcl::PointCloud<PointTarget>& cloud_tgt,
Matrix4& transformation_matrix) const
{
if (indices_src.size() != cloud_tgt.size()) {
PCL_ERROR("[pcl::Transformation2D::estimateRigidTransformation] Number or points "
"in source (%zu) differs than target (%zu)!\n",
indices_src.size(),
static_cast<std::size_t>(cloud_tgt.size()));
return;
}
ConstCloudIterator<PointSource> source_it(cloud_src, indices_src);
ConstCloudIterator<PointTarget> target_it(cloud_tgt);
estimateRigidTransformation(source_it, target_it, transformation_matrix);
}
template <typename PointSource, typename PointTarget, typename Scalar>
inline void
TransformationEstimation2D<PointSource, PointTarget, Scalar>::
estimateRigidTransformation(const pcl::PointCloud<PointSource>& cloud_src,
const pcl::Indices& indices_src,
const pcl::PointCloud<PointTarget>& cloud_tgt,
const pcl::Indices& indices_tgt,
Matrix4& transformation_matrix) const
{
if (indices_src.size() != indices_tgt.size()) {
PCL_ERROR("[pcl::TransformationEstimation2D::estimateRigidTransformation] Number "
"or points in source (%lu) differs than target (%lu)!\n",
indices_src.size(),
indices_tgt.size());
return;
}
ConstCloudIterator<PointSource> source_it(cloud_src, indices_src);
ConstCloudIterator<PointTarget> target_it(cloud_tgt, indices_tgt);
estimateRigidTransformation(source_it, target_it, transformation_matrix);
}
template <typename PointSource, typename PointTarget, typename Scalar>
void
TransformationEstimation2D<PointSource, PointTarget, Scalar>::
estimateRigidTransformation(const pcl::PointCloud<PointSource>& cloud_src,
const pcl::PointCloud<PointTarget>& cloud_tgt,
const pcl::Correspondences& correspondences,
Matrix4& transformation_matrix) const
{
ConstCloudIterator<PointSource> source_it(cloud_src, correspondences, true);
ConstCloudIterator<PointTarget> target_it(cloud_tgt, correspondences, false);
estimateRigidTransformation(source_it, target_it, transformation_matrix);
}
template <typename PointSource, typename PointTarget, typename Scalar>
inline void
TransformationEstimation2D<PointSource, PointTarget, Scalar>::
estimateRigidTransformation(ConstCloudIterator<PointSource>& source_it,
ConstCloudIterator<PointTarget>& target_it,
Matrix4& transformation_matrix) const
{
source_it.reset();
target_it.reset();
Eigen::Matrix<Scalar, 4, 1> centroid_src, centroid_tgt;
// Estimate the centroids of source, target
compute3DCentroid(source_it, centroid_src);
compute3DCentroid(target_it, centroid_tgt);
source_it.reset();
target_it.reset();
// ignore z component
centroid_src[2] = 0.0f;
centroid_tgt[2] = 0.0f;
// 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,
transformation_matrix);
}
template <typename PointSource, typename PointTarget, typename Scalar>
void
TransformationEstimation2D<PointSource, PointTarget, Scalar>::
getTransformationFromCorrelation(
const Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>& cloud_src_demean,
const Eigen::Matrix<Scalar, 4, 1>& centroid_src,
const Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>& cloud_tgt_demean,
const Eigen::Matrix<Scalar, 4, 1>& centroid_tgt,
Matrix4& transformation_matrix) const
{
transformation_matrix.setIdentity();
// Assemble the correlation matrix H = source * target'
Eigen::Matrix<Scalar, 3, 3> H =
(cloud_src_demean * cloud_tgt_demean.transpose()).topLeftCorner(3, 3);
float angle = std::atan2((H(0, 1) - H(1, 0)), (H(0, 0) + H(1, 1)));
Eigen::Matrix<Scalar, 3, 3> R(Eigen::Matrix<Scalar, 3, 3>::Identity());
R(0, 0) = R(1, 1) = std::cos(angle);
R(0, 1) = -std::sin(angle);
R(1, 0) = std::sin(angle);
// Return the correct transformation
transformation_matrix.topLeftCorner(3, 3).matrix() = R;
const Eigen::Matrix<Scalar, 3, 1> Rc(R * centroid_src.head(3).matrix());
transformation_matrix.block(0, 3, 3, 1).matrix() = centroid_tgt.head(3) - Rc;
}
} // namespace registration
} // namespace pcl
#endif // PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_2D_HPP_
pcl::ConstCloudIterator
Iterator class for point clouds with or without given indices.
Definition cloud_iterator.h:121
pcl::ConstCloudIterator::reset
void reset()
Definition cloud_iterator.hpp:540
pcl::PointCloud
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition point_cloud.h:173
pcl::PointCloud::size
std::size_t size() const
Definition point_cloud.h:443
pcl::registration::TransformationEstimation2D::getTransformationFromCorrelation
void getTransformationFromCorrelation(const Eigen::Matrix< Scalar, Eigen::Dynamic, Eigen::Dynamic > &cloud_src_demean, const Eigen::Matrix< Scalar, 4, 1 > &centroid_src, const Eigen::Matrix< Scalar, Eigen::Dynamic, Eigen::Dynamic > &cloud_tgt_demean, const Eigen::Matrix< Scalar, 4, 1 > &centroid_tgt, Matrix4 &transformation_matrix) const
Obtain a 4x4 rigid transformation matrix from a correlation matrix H = src.
Definition transformation_estimation_2D.hpp:158
pcl::registration::TransformationEstimation2D::estimateRigidTransformation
void estimateRigidTransformation(const pcl::PointCloud< PointSource > &cloud_src, const pcl::PointCloud< PointTarget > &cloud_tgt, Matrix4 &transformation_matrix) const
Estimate a rigid transformation between a source and a target point cloud in 2D.
Definition transformation_estimation_2D.hpp:48
pcl::registration::TransformationEstimation2D::Matrix4
typename TransformationEstimation< PointSource, PointTarget, Scalar >::Matrix4 Matrix4
Definition transformation_estimation_2D.h:66
pcl::demeanPointCloud
void demeanPointCloud(ConstCloudIterator< PointT > &cloud_iterator, const Eigen::Matrix< Scalar, 4, 1 > &centroid, pcl::PointCloud< PointT > &cloud_out, int npts=0)
Subtract a centroid from a point cloud and return the de-meaned representation.
Definition centroid.hpp:933
pcl::compute3DCentroid
unsigned int compute3DCentroid(ConstCloudIterator< PointT > &cloud_iterator, Eigen::Matrix< Scalar, 4, 1 > &centroid)
Compute the 3D (X-Y-Z) centroid of a set of points and return it as a 3D vector.
Definition centroid.hpp:57
pcl
Definition convolution.h:46
pcl::Correspondences
std::vector< pcl::Correspondence, Eigen::aligned_allocator< pcl::Correspondence > > Correspondences
Definition correspondence.h:89
pcl::Indices
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition types.h:133