transformation_estimation_point_to_plane_lls_weighted.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.
*
* $Id$
*
*/
#ifndef PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_POINT_TO_PLANE_LLS_WEIGHTED_HPP_
#define PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_POINT_TO_PLANE_LLS_WEIGHTED_HPP_
#include <pcl/cloud_iterator.h>
//////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointSource, typename PointTarget, typename Scalar> inline void
pcl::registration::TransformationEstimationPointToPlaneLLSWeighted<PointSource, PointTarget, Scalar>::
estimateRigidTransformation (const pcl::PointCloud<PointSource> &cloud_src,
const pcl::PointCloud<PointTarget> &cloud_tgt,
Matrix4 &transformation_matrix) const
{
std::size_t nr_points = cloud_src.points.size ();
if (cloud_tgt.points.size () != nr_points)
{
PCL_ERROR ("[pcl::TransformationEstimationPointToPlaneLLSWeighted::estimateRigidTransformation] Number or points in source (%lu) differs than target (%lu)!\n", nr_points, cloud_tgt.points.size ());
return;
}
if (weights_.size () != nr_points)
{
PCL_ERROR ("[pcl::TransformationEstimationPointToPlaneLLSWeighted::estimateRigidTransformation] Number or weights from the number of correspondences! Use setWeights () to set them.\n");
return;
}
ConstCloudIterator<PointSource> source_it (cloud_src);
ConstCloudIterator<PointTarget> target_it (cloud_tgt);
typename std::vector<Scalar>::const_iterator weights_it = weights_.begin ();
estimateRigidTransformation (source_it, target_it, weights_it, transformation_matrix);
}
//////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointSource, typename PointTarget, typename Scalar> void
pcl::registration::TransformationEstimationPointToPlaneLLSWeighted<PointSource, PointTarget, Scalar>::
estimateRigidTransformation (const pcl::PointCloud<PointSource> &cloud_src,
const std::vector<int> &indices_src,
const pcl::PointCloud<PointTarget> &cloud_tgt,
Matrix4 &transformation_matrix) const
{
std::size_t nr_points = indices_src.size ();
if (cloud_tgt.points.size () != nr_points)
{
PCL_ERROR ("[pcl::TransformationEstimationPointToPlaneLLSWeighted::estimateRigidTransformation] Number or points in source (%lu) differs than target (%lu)!\n", indices_src.size (), cloud_tgt.points.size ());
return;
}
if (weights_.size () != nr_points)
{
PCL_ERROR ("[pcl::TransformationEstimationPointToPlaneLLSWeighted::estimateRigidTransformation] Number or weights from the number of correspondences! Use setWeights () to set them.\n");
return;
}
ConstCloudIterator<PointSource> source_it (cloud_src, indices_src);
ConstCloudIterator<PointTarget> target_it (cloud_tgt);
typename std::vector<Scalar>::const_iterator weights_it = weights_.begin ();
estimateRigidTransformation (source_it, target_it, weights_it, transformation_matrix);
}
//////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointSource, typename PointTarget, typename Scalar> inline void
pcl::registration::TransformationEstimationPointToPlaneLLSWeighted<PointSource, PointTarget, Scalar>::
estimateRigidTransformation (const pcl::PointCloud<PointSource> &cloud_src,
const std::vector<int> &indices_src,
const pcl::PointCloud<PointTarget> &cloud_tgt,
const std::vector<int> &indices_tgt,
Matrix4 &transformation_matrix) const
{
std::size_t nr_points = indices_src.size ();
if (indices_tgt.size () != nr_points)
{
PCL_ERROR ("[pcl::TransformationEstimationPointToPlaneLLSWeighted::estimateRigidTransformation] Number or points in source (%lu) differs than target (%lu)!\n", indices_src.size (), indices_tgt.size ());
return;
}
if (weights_.size () != nr_points)
{
PCL_ERROR ("[pcl::TransformationEstimationPointToPlaneLLSWeighted::estimateRigidTransformation] Number or weights from the number of correspondences! Use setWeights () to set them.\n");
return;
}
ConstCloudIterator<PointSource> source_it (cloud_src, indices_src);
ConstCloudIterator<PointTarget> target_it (cloud_tgt, indices_tgt);
typename std::vector<Scalar>::const_iterator weights_it = weights_.begin ();
estimateRigidTransformation (source_it, target_it, weights_it, transformation_matrix);
}
//////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointSource, typename PointTarget, typename Scalar> inline void
pcl::registration::TransformationEstimationPointToPlaneLLSWeighted<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);
std::vector<Scalar> weights (correspondences.size ());
for (std::size_t i = 0; i < correspondences.size (); ++i)
weights[i] = correspondences[i].weight;
typename std::vector<Scalar>::const_iterator weights_it = weights.begin ();
estimateRigidTransformation (source_it, target_it, weights_it, transformation_matrix);
}
//////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointSource, typename PointTarget, typename Scalar> inline void
pcl::registration::TransformationEstimationPointToPlaneLLSWeighted<PointSource, PointTarget, Scalar>::
constructTransformationMatrix (const double & alpha, const double & beta, const double & gamma,
const double & tx, const double & ty, const double & tz,
Matrix4 &transformation_matrix) const
{
// Construct the transformation matrix from rotation and translation
transformation_matrix = Eigen::Matrix<Scalar, 4, 4>::Zero ();
transformation_matrix (0, 0) = static_cast<Scalar> ( std::cos (gamma) * std::cos (beta));
transformation_matrix (0, 1) = static_cast<Scalar> (-sin (gamma) * std::cos (alpha) + std::cos (gamma) * sin (beta) * sin (alpha));
transformation_matrix (0, 2) = static_cast<Scalar> ( sin (gamma) * sin (alpha) + std::cos (gamma) * sin (beta) * std::cos (alpha));
transformation_matrix (1, 0) = static_cast<Scalar> ( sin (gamma) * std::cos (beta));
transformation_matrix (1, 1) = static_cast<Scalar> ( std::cos (gamma) * std::cos (alpha) + sin (gamma) * sin (beta) * sin (alpha));
transformation_matrix (1, 2) = static_cast<Scalar> (-std::cos (gamma) * sin (alpha) + sin (gamma) * sin (beta) * std::cos (alpha));
transformation_matrix (2, 0) = static_cast<Scalar> (-sin (beta));
transformation_matrix (2, 1) = static_cast<Scalar> ( std::cos (beta) * sin (alpha));
transformation_matrix (2, 2) = static_cast<Scalar> ( std::cos (beta) * std::cos (alpha));
transformation_matrix (0, 3) = static_cast<Scalar> (tx);
transformation_matrix (1, 3) = static_cast<Scalar> (ty);
transformation_matrix (2, 3) = static_cast<Scalar> (tz);
transformation_matrix (3, 3) = static_cast<Scalar> (1);
}
//////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointSource, typename PointTarget, typename Scalar> inline void
pcl::registration::TransformationEstimationPointToPlaneLLSWeighted<PointSource, PointTarget, Scalar>::
estimateRigidTransformation (ConstCloudIterator<PointSource>& source_it,
ConstCloudIterator<PointTarget>& target_it,
typename std::vector<Scalar>::const_iterator& weights_it,
Matrix4 &transformation_matrix) const
{
using Vector6d = Eigen::Matrix<double, 6, 1>;
using Matrix6d = Eigen::Matrix<double, 6, 6>;
Matrix6d ATA;
Vector6d ATb;
ATA.setZero ();
ATb.setZero ();
while (source_it.isValid () && target_it.isValid ())
{
if (!std::isfinite (source_it->x) ||
!std::isfinite (source_it->y) ||
!std::isfinite (source_it->z) ||
!std::isfinite (target_it->x) ||
!std::isfinite (target_it->y) ||
!std::isfinite (target_it->z) ||
!std::isfinite (target_it->normal_x) ||
!std::isfinite (target_it->normal_y) ||
!std::isfinite (target_it->normal_z))
{
++ source_it;
++ target_it;
++ weights_it;
continue;
}
const float & sx = source_it->x;
const float & sy = source_it->y;
const float & sz = source_it->z;
const float & dx = target_it->x;
const float & dy = target_it->y;
const float & dz = target_it->z;
const float & nx = target_it->normal[0] * (*weights_it);
const float & ny = target_it->normal[1] * (*weights_it);
const float & nz = target_it->normal[2] * (*weights_it);
double a = nz*sy - ny*sz;
double b = nx*sz - nz*sx;
double c = ny*sx - nx*sy;
// 0 1 2 3 4 5
// 6 7 8 9 10 11
// 12 13 14 15 16 17
// 18 19 20 21 22 23
// 24 25 26 27 28 29
// 30 31 32 33 34 35
ATA.coeffRef (0) += a * a;
ATA.coeffRef (1) += a * b;
ATA.coeffRef (2) += a * c;
ATA.coeffRef (3) += a * nx;
ATA.coeffRef (4) += a * ny;
ATA.coeffRef (5) += a * nz;
ATA.coeffRef (7) += b * b;
ATA.coeffRef (8) += b * c;
ATA.coeffRef (9) += b * nx;
ATA.coeffRef (10) += b * ny;
ATA.coeffRef (11) += b * nz;
ATA.coeffRef (14) += c * c;
ATA.coeffRef (15) += c * nx;
ATA.coeffRef (16) += c * ny;
ATA.coeffRef (17) += c * nz;
ATA.coeffRef (21) += nx * nx;
ATA.coeffRef (22) += nx * ny;
ATA.coeffRef (23) += nx * nz;
ATA.coeffRef (28) += ny * ny;
ATA.coeffRef (29) += ny * nz;
ATA.coeffRef (35) += nz * nz;
double d = nx*dx + ny*dy + nz*dz - nx*sx - ny*sy - nz*sz;
ATb.coeffRef (0) += a * d;
ATb.coeffRef (1) += b * d;
ATb.coeffRef (2) += c * d;
ATb.coeffRef (3) += nx * d;
ATb.coeffRef (4) += ny * d;
ATb.coeffRef (5) += nz * d;
++ source_it;
++ target_it;
++ weights_it;
}
ATA.coeffRef (6) = ATA.coeff (1);
ATA.coeffRef (12) = ATA.coeff (2);
ATA.coeffRef (13) = ATA.coeff (8);
ATA.coeffRef (18) = ATA.coeff (3);
ATA.coeffRef (19) = ATA.coeff (9);
ATA.coeffRef (20) = ATA.coeff (15);
ATA.coeffRef (24) = ATA.coeff (4);
ATA.coeffRef (25) = ATA.coeff (10);
ATA.coeffRef (26) = ATA.coeff (16);
ATA.coeffRef (27) = ATA.coeff (22);
ATA.coeffRef (30) = ATA.coeff (5);
ATA.coeffRef (31) = ATA.coeff (11);
ATA.coeffRef (32) = ATA.coeff (17);
ATA.coeffRef (33) = ATA.coeff (23);
ATA.coeffRef (34) = ATA.coeff (29);
// Solve A*x = b
Vector6d x = static_cast<Vector6d> (ATA.inverse () * ATb);
// Construct the transformation matrix from x
constructTransformationMatrix (x (0), x (1), x (2), x (3), x (4), x (5), transformation_matrix);
}
#endif /* PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_POINT_TO_PLANE_LLS_WEIGHTED_HPP_ */
pcl::ConstCloudIterator::isValid
bool isValid() const
Definition: cloud_iterator.hpp:551
pcl::PointCloud::points
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:397
pcl::registration::TransformationEstimation::Matrix4
Eigen::Matrix< Scalar, 4, 4 > Matrix4
Definition: transformation_estimation.h:65
pcl::PointCloud< PointSource >
pcl::registration::TransformationEstimationPointToPlaneLLSWeighted::estimateRigidTransformation
void estimateRigidTransformation(const pcl::PointCloud< PointSource > &cloud_src, const pcl::PointCloud< PointTarget > &cloud_tgt, Matrix4 &transformation_matrix) const
Estimate a rigid rotation transformation between a source and a target point cloud using SVD.
Definition: transformation_estimation_point_to_plane_lls_weighted.hpp:46
pcl::registration::TransformationEstimationPointToPlaneLLSWeighted::constructTransformationMatrix
void constructTransformationMatrix(const double &alpha, const double &beta, const double &gamma, const double &tx, const double &ty, const double &tz, Matrix4 &transformation_matrix) const
Construct a 4 by 4 transformation matrix from the provided rotation and translation.
Definition: transformation_estimation_point_to_plane_lls_weighted.hpp:146
pcl::ConstCloudIterator
Iterator class for point clouds with or without given indices.
Definition: cloud_iterator.h:120
pcl::Correspondences
std::vector< pcl::Correspondence, Eigen::aligned_allocator< pcl::Correspondence > > Correspondences
Definition: correspondence.h:88