dgit.raspbian.org Git - pcl.git/blob

   1 /*
   2  * Software License Agreement (BSD License)
   3  *
   4  *  Point Cloud Library (PCL) - www.pointclouds.org
   5  *  Copyright (c) 2019-, Open Perception, Inc.
   6  *
   7  *  All rights reserved.
   8  *
   9  *  Redistribution and use in source and binary forms, with or without
  10  *  modification, are permitted provided that the following conditions
  11  *  are met:
  12  *
  13  *   * Redistributions of source code must retain the above copyright
  14  *     notice, this list of conditions and the following disclaimer.
  15  *   * Redistributions in binary form must reproduce the above
  16  *     copyright notice, this list of conditions and the following
  17  *     disclaimer in the documentation and/or other materials provided
  18  *     with the distribution.
  19  *   * Neither the name of the copyright holder(s) nor the names of its
  20  *     contributors may be used to endorse or promote products derived
  21  *     from this software without specific prior written permission.
  22  *
  23  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
  24  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
  25  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
  26  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
  27  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
  28  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
  29  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
  30  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
  31  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
  32  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
  33  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  34  *  POSSIBILITY OF SUCH DAMAGE.
  35  *
  36  */
  37
  38 #pragma once
  39
  40 #include <pcl/cloud_iterator.h>
  41
  42
  43 namespace pcl
  44 {
  45
  46 namespace registration
  47 {
  48
  49 template <typename PointSource, typename PointTarget, typename Scalar> inline void
  50 TransformationEstimationSymmetricPointToPlaneLLS<PointSource, PointTarget, Scalar>::
  51 estimateRigidTransformation (const pcl::PointCloud<PointSource> &cloud_src,
  52                              const pcl::PointCloud<PointTarget> &cloud_tgt,
  53                              Matrix4 &transformation_matrix) const
  54 {
  55   const auto nr_points = cloud_src.size ();
  56   if (cloud_tgt.size () != nr_points)
  57   {
  58     PCL_ERROR("[pcl::TransformationEstimationSymmetricPointToPlaneLLS::"
  59               "estimateRigidTransformation] Number or points in source (%zu) differs "
  60               "from target (%zu)!\n",
  61               static_cast<std::size_t>(nr_points),
  62               static_cast<std::size_t>(cloud_tgt.size()));
  63     return;
  64   }
  65
  66   ConstCloudIterator<PointSource> source_it (cloud_src);
  67   ConstCloudIterator<PointTarget> target_it (cloud_tgt);
  68   estimateRigidTransformation (source_it, target_it, transformation_matrix);
  69 }
  70
  71
  72 template <typename PointSource, typename PointTarget, typename Scalar> void
  73 TransformationEstimationSymmetricPointToPlaneLLS<PointSource, PointTarget, Scalar>::
  74 estimateRigidTransformation (const pcl::PointCloud<PointSource> &cloud_src,
  75                              const std::vector<int> &indices_src,
  76                              const pcl::PointCloud<PointTarget> &cloud_tgt,
  77                              Matrix4 &transformation_matrix) const
  78 {
  79   const auto nr_points = indices_src.size ();
  80   if (cloud_tgt.size () != nr_points)
  81   {
  82     PCL_ERROR("[pcl::TransformationEstimationSymmetricPointToPlaneLLS::"
  83               "estimateRigidTransformation] Number or points in source (%zu) differs "
  84               "than target (%zu)!\n",
  85               indices_src.size(),
  86               static_cast<std::size_t>(cloud_tgt.size()));
  87     return;
  88   }
  89
  90   ConstCloudIterator<PointSource> source_it (cloud_src, indices_src);
  91   ConstCloudIterator<PointTarget> target_it (cloud_tgt);
  92   estimateRigidTransformation (source_it, target_it, transformation_matrix);
  93 }
  94
  95
  96 template <typename PointSource, typename PointTarget, typename Scalar> inline void
  97 TransformationEstimationSymmetricPointToPlaneLLS<PointSource, PointTarget, Scalar>::
  98 estimateRigidTransformation (const pcl::PointCloud<PointSource> &cloud_src,
  99                              const std::vector<int> &indices_src,
 100                              const pcl::PointCloud<PointTarget> &cloud_tgt,
 101                              const std::vector<int> &indices_tgt,
 102                              Matrix4 &transformation_matrix) const
 103 {
 104   const auto nr_points = indices_src.size ();
 105   if (indices_tgt.size () != nr_points)
 106   {
 107     PCL_ERROR("[pcl::TransformationEstimationSymmetricPointToPlaneLLS::"
 108               "estimateRigidTransformation] Number or points in source (%zu) differs "
 109               "than target (%zu)!\n",
 110               indices_src.size(),
 111               indices_tgt.size());
 112     return;
 113   }
 114
 115   ConstCloudIterator<PointSource> source_it (cloud_src, indices_src);
 116   ConstCloudIterator<PointTarget> target_it (cloud_tgt, indices_tgt);
 117   estimateRigidTransformation (source_it, target_it, transformation_matrix);
 118 }
 119
 120
 121 template <typename PointSource, typename PointTarget, typename Scalar> inline void
 122 TransformationEstimationSymmetricPointToPlaneLLS<PointSource, PointTarget, Scalar>::
 123 estimateRigidTransformation (const pcl::PointCloud<PointSource> &cloud_src,
 124                              const pcl::PointCloud<PointTarget> &cloud_tgt,
 125                              const pcl::Correspondences &correspondences,
 126                              Matrix4 &transformation_matrix) const
 127 {
 128   ConstCloudIterator<PointSource> source_it (cloud_src, correspondences, true);
 129   ConstCloudIterator<PointTarget> target_it (cloud_tgt, correspondences, false);
 130   estimateRigidTransformation (source_it, target_it, transformation_matrix);
 131 }
 132
 133
 134 template <typename PointSource, typename PointTarget, typename Scalar> inline void
 135 TransformationEstimationSymmetricPointToPlaneLLS<PointSource, PointTarget, Scalar>::
 136 constructTransformationMatrix (const Vector6 &parameters,
 137                                Matrix4 &transformation_matrix) const
 138 {
 139   // Construct the transformation matrix from rotation and translation
 140   const Eigen::AngleAxis<Scalar> rotation_z (parameters (2), Eigen::Matrix<Scalar, 3, 1>::UnitZ ());
 141   const Eigen::AngleAxis<Scalar> rotation_y (parameters (1), Eigen::Matrix<Scalar, 3, 1>::UnitY ());
 142   const Eigen::AngleAxis<Scalar> rotation_x (parameters (0), Eigen::Matrix<Scalar, 3, 1>::UnitX ());
 143   const Eigen::Translation<Scalar, 3> translation (parameters (3), parameters (4), parameters (5));
 144   const Eigen::Transform<Scalar, 3, Eigen::Affine> transform = rotation_z * rotation_y * rotation_x *
 145                                                                translation *
 146                                                                rotation_z * rotation_y * rotation_x;
 147   transformation_matrix = transform.matrix ();
 148 }
 149
 150
 151 template <typename PointSource, typename PointTarget, typename Scalar> inline void
 152 TransformationEstimationSymmetricPointToPlaneLLS<PointSource, PointTarget, Scalar>::
 153 estimateRigidTransformation (ConstCloudIterator<PointSource>& source_it, ConstCloudIterator<PointTarget>& target_it, Matrix4 &transformation_matrix) const
 154 {
 155   using Matrix6 = Eigen::Matrix<Scalar, 6, 6>;
 156   using Vector3 = Eigen::Matrix<Scalar, 3, 1>;
 157
 158   Matrix6 ATA;
 159   Vector6 ATb;
 160   ATA.setZero ();
 161   ATb.setZero ();
 162   auto M = ATA.template selfadjointView<Eigen::Upper> ();
 163
 164   // Approximate as a linear least squares problem
 165   source_it.reset ();
 166   target_it.reset ();
 167   for (; source_it.isValid () && target_it.isValid (); ++source_it, ++target_it)
 168   {
 169     const Vector3 p (source_it->x, source_it->y, source_it->z);
 170     const Vector3 q (target_it->x, target_it->y, target_it->z);
 171     const Vector3 n1 (source_it->getNormalVector3fMap().template cast<Scalar>());
 172     const Vector3 n2 (target_it->getNormalVector3fMap().template cast<Scalar>());
 173     Vector3 n;
 174     if (enforce_same_direction_normals_)
 175     {
 176         if (n1.dot (n2) >= 0.)
 177             n = n1 + n2;
 178         else
 179             n = n1 - n2;
 180     }
 181     else
 182     {
 183         n = n1 + n2;
 184     }
 185
 186     if (!p.array().isFinite().all() ||
 187         !q.array().isFinite().all() ||
 188         !n.array().isFinite().all())
 189     {
 190       continue;
 191     }
 192
 193     Vector6 v;
 194     v << (p + q).cross (n), n;
 195     M.rankUpdate (v);
 196
 197     ATb += v * (q - p).dot (n);
 198   }
 199
 200   // Solve A*x = b
 201   const Vector6 x = M.ldlt ().solve (ATb);
 202
 203   // Construct the transformation matrix from x
 204   constructTransformationMatrix (x, transformation_matrix);
 205 }
 206
 207
 208 template <typename PointSource, typename PointTarget, typename Scalar> inline void
 209 TransformationEstimationSymmetricPointToPlaneLLS<PointSource, PointTarget, Scalar>::
 210 setEnforceSameDirectionNormals (bool enforce_same_direction_normals)
 211 {
 212     enforce_same_direction_normals_ = enforce_same_direction_normals;
 213 }
 214
 215
 216 template <typename PointSource, typename PointTarget, typename Scalar> inline bool
 217 TransformationEstimationSymmetricPointToPlaneLLS<PointSource, PointTarget, Scalar>::
 218 getEnforceSameDirectionNormals ()
 219 {
 220     return enforce_same_direction_normals_;
 221 }
 222
 223 } // namespace registration
 224 } // namespace pcl
 225