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) 2012-, 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  * $Id$
  37  *
  38  */
  39
  40 #ifndef PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_POINT_TO_PLANE_LLS_WEIGHTED_HPP_
  41 #define PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_POINT_TO_PLANE_LLS_WEIGHTED_HPP_
  42
  43 #include <pcl/cloud_iterator.h>
  44
  45
  46 namespace pcl
  47 {
  48
  49 namespace registration
  50 {
  51
  52 template <typename PointSource, typename PointTarget, typename Scalar> inline void
  53 TransformationEstimationPointToPlaneLLSWeighted<PointSource, PointTarget, Scalar>::
  54 estimateRigidTransformation (const pcl::PointCloud<PointSource> &cloud_src,
  55                              const pcl::PointCloud<PointTarget> &cloud_tgt,
  56                              Matrix4 &transformation_matrix) const
  57 {
  58   const auto nr_points = cloud_src.size ();
  59   if (cloud_tgt.size () != nr_points)
  60   {
  61     PCL_ERROR("[pcl::TransformationEstimationPointToPlaneLLSWeighted::"
  62               "estimateRigidTransformation] Number or points in source (%zu) differs "
  63               "than target (%zu)!\n",
  64               static_cast<std::size_t>(nr_points),
  65               static_cast<std::size_t>(cloud_tgt.size()));
  66     return;
  67   }
  68
  69   if (weights_.size () != nr_points)
  70   {
  71     PCL_ERROR ("[pcl::TransformationEstimationPointToPlaneLLSWeighted::estimateRigidTransformation] Number or weights from the number of correspondences! Use setWeights () to set them.\n");
  72     return;
  73   }
  74
  75   ConstCloudIterator<PointSource> source_it (cloud_src);
  76   ConstCloudIterator<PointTarget> target_it (cloud_tgt);
  77   typename std::vector<Scalar>::const_iterator weights_it = weights_.begin ();
  78   estimateRigidTransformation (source_it, target_it, weights_it, transformation_matrix);
  79 }
  80
  81
  82 template <typename PointSource, typename PointTarget, typename Scalar> void
  83 TransformationEstimationPointToPlaneLLSWeighted<PointSource, PointTarget, Scalar>::
  84 estimateRigidTransformation (const pcl::PointCloud<PointSource> &cloud_src,
  85                              const std::vector<int> &indices_src,
  86                              const pcl::PointCloud<PointTarget> &cloud_tgt,
  87                              Matrix4 &transformation_matrix) const
  88 {
  89   const std::size_t nr_points = indices_src.size ();
  90   if (cloud_tgt.size () != nr_points)
  91   {
  92     PCL_ERROR("[pcl::TransformationEstimationPointToPlaneLLSWeighted::"
  93               "estimateRigidTransformation] Number or points in source (%zu) differs "
  94               "than target (%zu)!\n",
  95               indices_src.size(),
  96               static_cast<std::size_t>(cloud_tgt.size()));
  97     return;
  98   }
  99
 100   if (weights_.size () != nr_points)
 101   {
 102     PCL_ERROR ("[pcl::TransformationEstimationPointToPlaneLLSWeighted::estimateRigidTransformation] Number or weights from the number of correspondences! Use setWeights () to set them.\n");
 103     return;
 104   }
 105
 106
 107   ConstCloudIterator<PointSource> source_it (cloud_src, indices_src);
 108   ConstCloudIterator<PointTarget> target_it (cloud_tgt);
 109   typename std::vector<Scalar>::const_iterator weights_it = weights_.begin ();
 110   estimateRigidTransformation (source_it, target_it, weights_it, transformation_matrix);
 111 }
 112
 113
 114 template <typename PointSource, typename PointTarget, typename Scalar> inline void
 115 TransformationEstimationPointToPlaneLLSWeighted<PointSource, PointTarget, Scalar>::
 116 estimateRigidTransformation (const pcl::PointCloud<PointSource> &cloud_src,
 117                              const std::vector<int> &indices_src,
 118                              const pcl::PointCloud<PointTarget> &cloud_tgt,
 119                              const std::vector<int> &indices_tgt,
 120                              Matrix4 &transformation_matrix) const
 121 {
 122   const std::size_t nr_points = indices_src.size ();
 123   if (indices_tgt.size () != nr_points)
 124   {
 125     PCL_ERROR ("[pcl::TransformationEstimationPointToPlaneLLSWeighted::estimateRigidTransformation] Number or points in source (%lu) differs than target (%lu)!\n", indices_src.size (), indices_tgt.size ());
 126     return;
 127   }
 128
 129   if (weights_.size () != nr_points)
 130   {
 131     PCL_ERROR ("[pcl::TransformationEstimationPointToPlaneLLSWeighted::estimateRigidTransformation] Number or weights from the number of correspondences! Use setWeights () to set them.\n");
 132     return;
 133   }
 134
 135   ConstCloudIterator<PointSource> source_it (cloud_src, indices_src);
 136   ConstCloudIterator<PointTarget> target_it (cloud_tgt, indices_tgt);
 137   typename std::vector<Scalar>::const_iterator weights_it = weights_.begin ();
 138   estimateRigidTransformation (source_it, target_it, weights_it, transformation_matrix);
 139 }
 140
 141
 142 template <typename PointSource, typename PointTarget, typename Scalar> inline void
 143 TransformationEstimationPointToPlaneLLSWeighted<PointSource, PointTarget, Scalar>::
 144 estimateRigidTransformation (const pcl::PointCloud<PointSource> &cloud_src,
 145                              const pcl::PointCloud<PointTarget> &cloud_tgt,
 146                              const pcl::Correspondences &correspondences,
 147                              Matrix4 &transformation_matrix) const
 148 {
 149   ConstCloudIterator<PointSource> source_it (cloud_src, correspondences, true);
 150   ConstCloudIterator<PointTarget> target_it (cloud_tgt, correspondences, false);
 151   std::vector<Scalar> weights (correspondences.size ());
 152   for (std::size_t i = 0; i < correspondences.size (); ++i)
 153     weights[i] = correspondences[i].weight;
 154   typename std::vector<Scalar>::const_iterator weights_it = weights.begin ();
 155   estimateRigidTransformation (source_it, target_it, weights_it, transformation_matrix);
 156 }
 157
 158
 159 template <typename PointSource, typename PointTarget, typename Scalar> inline void
 160 TransformationEstimationPointToPlaneLLSWeighted<PointSource, PointTarget, Scalar>::
 161 constructTransformationMatrix (const double & alpha, const double & beta, const double & gamma,
 162                                const double & tx,    const double & ty,   const double & tz,
 163                                Matrix4 &transformation_matrix) const
 164 {
 165   // Construct the transformation matrix from rotation and translation
 166   transformation_matrix = Eigen::Matrix<Scalar, 4, 4>::Zero ();
 167   transformation_matrix (0, 0) = static_cast<Scalar> ( std::cos (gamma) * std::cos (beta));
 168   transformation_matrix (0, 1) = static_cast<Scalar> (-sin (gamma) * std::cos (alpha) + std::cos (gamma) * sin (beta) * sin (alpha));
 169   transformation_matrix (0, 2) = static_cast<Scalar> ( sin (gamma) * sin (alpha) + std::cos (gamma) * sin (beta) * std::cos (alpha));
 170   transformation_matrix (1, 0) = static_cast<Scalar> ( sin (gamma) * std::cos (beta));
 171   transformation_matrix (1, 1) = static_cast<Scalar> ( std::cos (gamma) * std::cos (alpha) + sin (gamma) * sin (beta) * sin (alpha));
 172   transformation_matrix (1, 2) = static_cast<Scalar> (-std::cos (gamma) * sin (alpha) + sin (gamma) * sin (beta) * std::cos (alpha));
 173   transformation_matrix (2, 0) = static_cast<Scalar> (-sin (beta));
 174   transformation_matrix (2, 1) = static_cast<Scalar> ( std::cos (beta) * sin (alpha));
 175   transformation_matrix (2, 2) = static_cast<Scalar> ( std::cos (beta) * std::cos (alpha));
 176
 177   transformation_matrix (0, 3) = static_cast<Scalar> (tx);
 178   transformation_matrix (1, 3) = static_cast<Scalar> (ty);
 179   transformation_matrix (2, 3) = static_cast<Scalar> (tz);
 180   transformation_matrix (3, 3) = static_cast<Scalar> (1);
 181 }
 182
 183
 184 template <typename PointSource, typename PointTarget, typename Scalar> inline void
 185 TransformationEstimationPointToPlaneLLSWeighted<PointSource, PointTarget, Scalar>::
 186 estimateRigidTransformation (ConstCloudIterator<PointSource>& source_it,
 187                              ConstCloudIterator<PointTarget>& target_it,
 188                              typename std::vector<Scalar>::const_iterator& weights_it,
 189                              Matrix4 &transformation_matrix) const
 190 {
 191   using Vector6d = Eigen::Matrix<double, 6, 1>;
 192   using Matrix6d = Eigen::Matrix<double, 6, 6>;
 193
 194   Matrix6d ATA;
 195   Vector6d ATb;
 196   ATA.setZero ();
 197   ATb.setZero ();
 198
 199   while (source_it.isValid () && target_it.isValid ())
 200   {
 201     if (!std::isfinite (source_it->x) ||
 202         !std::isfinite (source_it->y) ||
 203         !std::isfinite (source_it->z) ||
 204         !std::isfinite (target_it->x) ||
 205         !std::isfinite (target_it->y) ||
 206         !std::isfinite (target_it->z) ||
 207         !std::isfinite (target_it->normal_x) ||
 208         !std::isfinite (target_it->normal_y) ||
 209         !std::isfinite (target_it->normal_z))
 210     {
 211       ++ source_it;
 212       ++ target_it;
 213       ++ weights_it;
 214       continue;
 215     }
 216
 217     const float & sx = source_it->x;
 218     const float & sy = source_it->y;
 219     const float & sz = source_it->z;
 220     const float & dx = target_it->x;
 221     const float & dy = target_it->y;
 222     const float & dz = target_it->z;
 223     const float & nx = target_it->normal[0] * (*weights_it);
 224     const float & ny = target_it->normal[1] * (*weights_it);
 225     const float & nz = target_it->normal[2] * (*weights_it);
 226
 227     double a = nz*sy - ny*sz;
 228     double b = nx*sz - nz*sx;
 229     double c = ny*sx - nx*sy;
 230
 231     //    0  1  2  3  4  5
 232     //    6  7  8  9 10 11
 233     //   12 13 14 15 16 17
 234     //   18 19 20 21 22 23
 235     //   24 25 26 27 28 29
 236     //   30 31 32 33 34 35
 237
 238     ATA.coeffRef (0) += a * a;
 239     ATA.coeffRef (1) += a * b;
 240     ATA.coeffRef (2) += a * c;
 241     ATA.coeffRef (3) += a * nx;
 242     ATA.coeffRef (4) += a * ny;
 243     ATA.coeffRef (5) += a * nz;
 244     ATA.coeffRef (7) += b * b;
 245     ATA.coeffRef (8) += b * c;
 246     ATA.coeffRef (9) += b * nx;
 247     ATA.coeffRef (10) += b * ny;
 248     ATA.coeffRef (11) += b * nz;
 249     ATA.coeffRef (14) += c * c;
 250     ATA.coeffRef (15) += c * nx;
 251     ATA.coeffRef (16) += c * ny;
 252     ATA.coeffRef (17) += c * nz;
 253     ATA.coeffRef (21) += nx * nx;
 254     ATA.coeffRef (22) += nx * ny;
 255     ATA.coeffRef (23) += nx * nz;
 256     ATA.coeffRef (28) += ny * ny;
 257     ATA.coeffRef (29) += ny * nz;
 258     ATA.coeffRef (35) += nz * nz;
 259
 260     double d = nx*dx + ny*dy + nz*dz - nx*sx - ny*sy - nz*sz;
 261     ATb.coeffRef (0) += a * d;
 262     ATb.coeffRef (1) += b * d;
 263     ATb.coeffRef (2) += c * d;
 264     ATb.coeffRef (3) += nx * d;
 265     ATb.coeffRef (4) += ny * d;
 266     ATb.coeffRef (5) += nz * d;
 267
 268     ++ source_it;
 269     ++ target_it;
 270     ++ weights_it;
 271   }
 272
 273   ATA.coeffRef (6) = ATA.coeff (1);
 274   ATA.coeffRef (12) = ATA.coeff (2);
 275   ATA.coeffRef (13) = ATA.coeff (8);
 276   ATA.coeffRef (18) = ATA.coeff (3);
 277   ATA.coeffRef (19) = ATA.coeff (9);
 278   ATA.coeffRef (20) = ATA.coeff (15);
 279   ATA.coeffRef (24) = ATA.coeff (4);
 280   ATA.coeffRef (25) = ATA.coeff (10);
 281   ATA.coeffRef (26) = ATA.coeff (16);
 282   ATA.coeffRef (27) = ATA.coeff (22);
 283   ATA.coeffRef (30) = ATA.coeff (5);
 284   ATA.coeffRef (31) = ATA.coeff (11);
 285   ATA.coeffRef (32) = ATA.coeff (17);
 286   ATA.coeffRef (33) = ATA.coeff (23);
 287   ATA.coeffRef (34) = ATA.coeff (29);
 288
 289   // Solve A*x = b
 290   Vector6d x = static_cast<Vector6d> (ATA.inverse () * ATb);
 291
 292   // Construct the transformation matrix from x
 293   constructTransformationMatrix (x (0), x (1), x (2), x (3), x (4), x (5), transformation_matrix);
 294 }
 295
 296 } // namespace registration
 297 } // namespace pcl
 298
 299 #endif /* PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_POINT_TO_PLANE_LLS_WEIGHTED_HPP_ */
 300