diff --git a/registration/include/pcl/registration/impl/transformation_estimation_dq.hpp b/registration/include/pcl/registration/impl/transformation_estimation_dq.hpp deleted file mode 100644 index fce6079d3da..00000000000 --- a/registration/include/pcl/registration/impl/transformation_estimation_dq.hpp +++ /dev/null @@ -1,225 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Point Cloud Library (PCL) - www.pointclouds.org - * Copyright (c) 2010, Willow Garage, Inc. - * 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_DQ_HPP_ -#define PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_DQ_HPP_ - -#include -PCL_DEPRECATED_HEADER(1, - 15, - "TransformationEstimationDQ has been renamed to " - "TransformationEstimationDualQuaternion."); - -namespace pcl { - -namespace registration { - -template -inline void -TransformationEstimationDQ:: - estimateRigidTransformation(const pcl::PointCloud& cloud_src, - const pcl::PointCloud& cloud_tgt, - Matrix4& transformation_matrix) const -{ - const auto nr_points = cloud_src.size(); - if (cloud_tgt.size() != nr_points) { - PCL_ERROR("[pcl::TransformationEstimationDQ::estimateRigidTransformation] Number " - "or points in source (%zu) differs than target (%zu)!\n", - static_cast(nr_points), - static_cast(cloud_tgt.size())); - return; - } - - ConstCloudIterator source_it(cloud_src); - ConstCloudIterator target_it(cloud_tgt); - estimateRigidTransformation(source_it, target_it, transformation_matrix); -} - -template -void -TransformationEstimationDQ:: - estimateRigidTransformation(const pcl::PointCloud& cloud_src, - const pcl::Indices& indices_src, - const pcl::PointCloud& cloud_tgt, - Matrix4& transformation_matrix) const -{ - if (indices_src.size() != cloud_tgt.size()) { - PCL_ERROR("[pcl::TransformationDQ::estimateRigidTransformation] Number or points " - "in source (%zu) differs than target (%zu)!\n", - indices_src.size(), - static_cast(cloud_tgt.size())); - return; - } - - ConstCloudIterator source_it(cloud_src, indices_src); - ConstCloudIterator target_it(cloud_tgt); - estimateRigidTransformation(source_it, target_it, transformation_matrix); -} - -template -inline void -TransformationEstimationDQ:: - estimateRigidTransformation(const pcl::PointCloud& cloud_src, - const pcl::Indices& indices_src, - const pcl::PointCloud& cloud_tgt, - const pcl::Indices& indices_tgt, - Matrix4& transformation_matrix) const -{ - if (indices_src.size() != indices_tgt.size()) { - PCL_ERROR("[pcl::TransformationEstimationDQ::estimateRigidTransformation] Number " - "or points in source (%zu) differs than target (%zu)!\n", - indices_src.size(), - indices_tgt.size()); - return; - } - - ConstCloudIterator source_it(cloud_src, indices_src); - ConstCloudIterator target_it(cloud_tgt, indices_tgt); - estimateRigidTransformation(source_it, target_it, transformation_matrix); -} - -template -void -TransformationEstimationDQ:: - estimateRigidTransformation(const pcl::PointCloud& cloud_src, - const pcl::PointCloud& cloud_tgt, - const pcl::Correspondences& correspondences, - Matrix4& transformation_matrix) const -{ - ConstCloudIterator source_it(cloud_src, correspondences, true); - ConstCloudIterator target_it(cloud_tgt, correspondences, false); - estimateRigidTransformation(source_it, target_it, transformation_matrix); -} - -template -inline void -TransformationEstimationDQ:: - estimateRigidTransformation(ConstCloudIterator& source_it, - ConstCloudIterator& target_it, - Matrix4& transformation_matrix) const -{ - const int npts = static_cast(source_it.size()); - - transformation_matrix.setIdentity(); - - // dual quaternion optimization - Eigen::Matrix C1 = Eigen::Matrix::Zero(); - Eigen::Matrix C2 = Eigen::Matrix::Zero(); - Scalar* c1 = C1.data(); - Scalar* c2 = C2.data(); - - for (int i = 0; i < npts; i++) { - const PointSource& a = *source_it; - const PointTarget& b = *target_it; - const Scalar axbx = a.x * b.x; - const Scalar ayby = a.y * b.y; - const Scalar azbz = a.z * b.z; - const Scalar axby = a.x * b.y; - const Scalar aybx = a.y * b.x; - const Scalar axbz = a.x * b.z; - const Scalar azbx = a.z * b.x; - const Scalar aybz = a.y * b.z; - const Scalar azby = a.z * b.y; - c1[0] += axbx - azbz - ayby; - c1[5] += ayby - azbz - axbx; - c1[10] += azbz - axbx - ayby; - c1[15] += axbx + ayby + azbz; - c1[1] += axby + aybx; - c1[2] += axbz + azbx; - c1[3] += aybz - azby; - c1[6] += azby + aybz; - c1[7] += azbx - axbz; - c1[11] += axby - aybx; - - c2[1] += a.z + b.z; - c2[2] -= a.y + b.y; - c2[3] += a.x - b.x; - c2[6] += a.x + b.x; - c2[7] += a.y - b.y; - c2[11] += a.z - b.z; - source_it++; - target_it++; - } - - c1[4] = c1[1]; - c1[8] = c1[2]; - c1[9] = c1[6]; - c1[12] = c1[3]; - c1[13] = c1[7]; - c1[14] = c1[11]; - c2[4] = -c2[1]; - c2[8] = -c2[2]; - c2[12] = -c2[3]; - c2[9] = -c2[6]; - c2[13] = -c2[7]; - c2[14] = -c2[11]; - - C1 *= -2.0f; - C2 *= 2.0f; - - const Eigen::Matrix A = - (0.25f / float(npts)) * C2.transpose() * C2 - C1; - - const Eigen::EigenSolver> es(A); - - ptrdiff_t i; - es.eigenvalues().real().maxCoeff(&i); - const Eigen::Matrix qmat = es.eigenvectors().col(i).real(); - const Eigen::Matrix smat = -(0.5f / float(npts)) * C2 * qmat; - - const Eigen::Quaternion q(qmat(3), qmat(0), qmat(1), qmat(2)); - const Eigen::Quaternion s(smat(3), smat(0), smat(1), smat(2)); - - const Eigen::Quaternion t = s * q.conjugate(); - - const Eigen::Matrix R(q.toRotationMatrix()); - - for (int i = 0; i < 3; ++i) - for (int j = 0; j < 3; ++j) - transformation_matrix(i, j) = R(i, j); - - transformation_matrix(0, 3) = -t.x(); - transformation_matrix(1, 3) = -t.y(); - transformation_matrix(2, 3) = -t.z(); -} - -} // namespace registration -} // namespace pcl - -#endif /* PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_DQ_HPP_ */ diff --git a/registration/include/pcl/registration/transformation_estimation_dq.h b/registration/include/pcl/registration/transformation_estimation_dq.h deleted file mode 100644 index 5ad76cc6cc8..00000000000 --- a/registration/include/pcl/registration/transformation_estimation_dq.h +++ /dev/null @@ -1,138 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Point Cloud Library (PCL) - www.pointclouds.org - * Copyright (c) 2010-2011, Willow Garage, Inc. - * 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. - * - * - */ - -#pragma once - -#include -#include -PCL_DEPRECATED_HEADER(1, - 15, - "TransformationEstimationDQ has been renamed to " - "TransformationEstimationDualQuaternion."); - -namespace pcl { -namespace registration { -/** @b TransformationEstimationDQ implements dual quaternion based estimation of - * the transformation aligning the given correspondences. - * - * \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 Sergey Zagoruyko - * \ingroup registration - */ -template -class TransformationEstimationDQ -: public TransformationEstimation { -public: - using Ptr = shared_ptr>; - using ConstPtr = - shared_ptr>; - - using Matrix4 = - typename TransformationEstimation::Matrix4; - - TransformationEstimationDQ(){}; - - /** \brief Estimate a rigid rotation transformation between a source and a target - * point cloud using dual quaternion optimization \param[in] cloud_src the source - * point cloud dataset \param[in] cloud_tgt the target point cloud dataset \param[out] - * transformation_matrix the resultant transformation matrix - */ - inline void - estimateRigidTransformation(const pcl::PointCloud& cloud_src, - const pcl::PointCloud& cloud_tgt, - Matrix4& transformation_matrix) const; - - /** \brief Estimate a rigid rotation transformation between a source and a target - * point cloud using dual quaternion optimization \param[in] cloud_src the source - * point cloud dataset \param[in] indices_src the vector of indices describing the - * points of interest in \a cloud_src - * \param[in] cloud_tgt the target point cloud dataset - * \param[out] transformation_matrix the resultant transformation matrix - */ - inline void - estimateRigidTransformation(const pcl::PointCloud& cloud_src, - const pcl::Indices& indices_src, - const pcl::PointCloud& cloud_tgt, - Matrix4& transformation_matrix) const; - - /** \brief Estimate a rigid rotation transformation between a source and a target - * point cloud using dual quaternion optimization \param[in] cloud_src the source - * point cloud dataset \param[in] indices_src the vector of indices describing the - * points of interest in \a cloud_src - * \param[in] cloud_tgt the target point cloud dataset - * \param[in] indices_tgt the vector of indices describing the correspondences of the - * interest points from \a indices_src - * \param[out] transformation_matrix the resultant transformation matrix - */ - inline void - estimateRigidTransformation(const pcl::PointCloud& cloud_src, - const pcl::Indices& indices_src, - const pcl::PointCloud& cloud_tgt, - const pcl::Indices& indices_tgt, - Matrix4& transformation_matrix) const; - - /** \brief Estimate a rigid rotation transformation between a source and a target - * point cloud using dual quaternion optimization \param[in] cloud_src the source - * point cloud dataset \param[in] cloud_tgt the target point cloud dataset \param[in] - * correspondences the vector of correspondences between source and target point cloud - * \param[out] transformation_matrix the resultant transformation matrix - */ - void - estimateRigidTransformation(const pcl::PointCloud& cloud_src, - const pcl::PointCloud& cloud_tgt, - const pcl::Correspondences& correspondences, - Matrix4& transformation_matrix) const; - -protected: - /** \brief Estimate a rigid rotation transformation between a source and a target - * \param[in] source_it an iterator over the source point cloud dataset - * \param[in] target_it an iterator over the target point cloud dataset - * \param[out] transformation_matrix the resultant transformation matrix - */ - void - estimateRigidTransformation(ConstCloudIterator& source_it, - ConstCloudIterator& target_it, - Matrix4& transformation_matrix) const; -}; - -} // namespace registration -} // namespace pcl - -#include diff --git a/registration/src/transformation_estimation_dq.cpp b/registration/src/transformation_estimation_dq.cpp deleted file mode 100644 index a6d388fcf9e..00000000000 --- a/registration/src/transformation_estimation_dq.cpp +++ /dev/null @@ -1,39 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011, Alexandru-Eugen Ichim - * Willow Garage, 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 Willow Garage, Inc. 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: transformation_estimation_svd.cpp 7153 2012-09-16 22:24:29Z aichim $ - * - */ - -#include