transformation_estimation_point_to_plane_lls_weighted.hpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Point Cloud Library (PCL) - www.pointclouds.org
00005  *  Copyright (c) 2012-, Open Perception, Inc.
00006  *
00007  *  All rights reserved.
00008  *
00009  *  Redistribution and use in source and binary forms, with or without
00010  *  modification, are permitted provided that the following conditions
00011  *  are met:
00012  *
00013  *   * Redistributions of source code must retain the above copyright
00014  *     notice, this list of conditions and the following disclaimer.
00015  *   * Redistributions in binary form must reproduce the above
00016  *     copyright notice, this list of conditions and the following
00017  *     disclaimer in the documentation and/or other materials provided
00018  *     with the distribution.
00019  *   * Neither the name of the copyright holder(s) nor the names of its
00020  *     contributors may be used to endorse or promote products derived
00021  *     from this software without specific prior written permission.
00022  *
00023  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00024  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00025  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00026  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00027  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00028  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00029  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00030  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00031  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00032  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00033  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00034  *  POSSIBILITY OF SUCH DAMAGE.
00035  *
00036  * $Id$
00037  *
00038  */
00039 #ifndef PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_POINT_TO_PLANE_LLS_WEIGHTED_HPP_
00040 #define PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_POINT_TO_PLANE_LLS_WEIGHTED_HPP_
00041 #include <pcl/cloud_iterator.h>
00042 
00044 template <typename PointSource, typename PointTarget, typename Scalar> inline void
00045 pcl::registration::TransformationEstimationPointToPlaneLLSWeighted<PointSource, PointTarget, Scalar>::
00046 estimateRigidTransformation (const pcl::PointCloud<PointSource> &cloud_src,
00047                              const pcl::PointCloud<PointTarget> &cloud_tgt,
00048                              Matrix4 &transformation_matrix) const
00049 {
00050   size_t nr_points = cloud_src.points.size ();
00051   if (cloud_tgt.points.size () != nr_points)
00052   {
00053     PCL_ERROR ("[pcl::TransformationEstimationPointToPlaneLLSWeighted::estimateRigidTransformation] Number or points in source (%zu) differs than target (%zu)!\n", nr_points, cloud_tgt.points.size ());
00054     return;
00055   }
00056 
00057   if (weights_.size () != nr_points)
00058   {
00059     PCL_ERROR ("[pcl::TransformationEstimationPointToPlaneLLSWeighted::estimateRigidTransformation] Number or weights from the number of correspondences! Use setWeights () to set them.\n");
00060     return;
00061   }
00062 
00063   ConstCloudIterator<PointSource> source_it (cloud_src);
00064   ConstCloudIterator<PointTarget> target_it (cloud_tgt);
00065   typename std::vector<Scalar>::const_iterator weights_it = weights_.begin ();
00066   estimateRigidTransformation (source_it, target_it, weights_it, transformation_matrix);
00067 }
00068 
00070 template <typename PointSource, typename PointTarget, typename Scalar> void
00071 pcl::registration::TransformationEstimationPointToPlaneLLSWeighted<PointSource, PointTarget, Scalar>::
00072 estimateRigidTransformation (const pcl::PointCloud<PointSource> &cloud_src,
00073                              const std::vector<int> &indices_src,
00074                              const pcl::PointCloud<PointTarget> &cloud_tgt,
00075                              Matrix4 &transformation_matrix) const
00076 {
00077   size_t nr_points = indices_src.size ();
00078   if (cloud_tgt.points.size () != nr_points)
00079   {
00080     PCL_ERROR ("[pcl::TransformationEstimationPointToPlaneLLSWeighted::estimateRigidTransformation] Number or points in source (%zu) differs than target (%zu)!\n", indices_src.size (), cloud_tgt.points.size ());
00081     return;
00082   }
00083 
00084   if (weights_.size () != nr_points)
00085   {
00086     PCL_ERROR ("[pcl::TransformationEstimationPointToPlaneLLSWeighted::estimateRigidTransformation] Number or weights from the number of correspondences! Use setWeights () to set them.\n");
00087     return;
00088   }
00089 
00090 
00091   ConstCloudIterator<PointSource> source_it (cloud_src, indices_src);
00092   ConstCloudIterator<PointTarget> target_it (cloud_tgt);
00093   typename std::vector<Scalar>::const_iterator weights_it = weights_.begin ();
00094   estimateRigidTransformation (source_it, target_it, weights_it, transformation_matrix);
00095 }
00096 
00097 
00099 template <typename PointSource, typename PointTarget, typename Scalar> inline void
00100 pcl::registration::TransformationEstimationPointToPlaneLLSWeighted<PointSource, PointTarget, Scalar>::
00101 estimateRigidTransformation (const pcl::PointCloud<PointSource> &cloud_src,
00102                              const std::vector<int> &indices_src,
00103                              const pcl::PointCloud<PointTarget> &cloud_tgt,
00104                              const std::vector<int> &indices_tgt,
00105                              Matrix4 &transformation_matrix) const
00106 {
00107   size_t nr_points = indices_src.size ();
00108   if (indices_tgt.size () != nr_points)
00109   {
00110     PCL_ERROR ("[pcl::TransformationEstimationPointToPlaneLLSWeighted::estimateRigidTransformation] Number or points in source (%zu) differs than target (%zu)!\n", indices_src.size (), indices_tgt.size ());
00111     return;
00112   }
00113 
00114   if (weights_.size () != nr_points)
00115   {
00116     PCL_ERROR ("[pcl::TransformationEstimationPointToPlaneLLSWeighted::estimateRigidTransformation] Number or weights from the number of correspondences! Use setWeights () to set them.\n");
00117     return;
00118   }
00119 
00120   ConstCloudIterator<PointSource> source_it (cloud_src, indices_src);
00121   ConstCloudIterator<PointTarget> target_it (cloud_tgt, indices_tgt);
00122   typename std::vector<Scalar>::const_iterator weights_it = weights_.begin ();
00123   estimateRigidTransformation (source_it, target_it, weights_it, transformation_matrix);
00124 }
00125 
00127 template <typename PointSource, typename PointTarget, typename Scalar> inline void
00128 pcl::registration::TransformationEstimationPointToPlaneLLSWeighted<PointSource, PointTarget, Scalar>::
00129 estimateRigidTransformation (const pcl::PointCloud<PointSource> &cloud_src,
00130                              const pcl::PointCloud<PointTarget> &cloud_tgt,
00131                              const pcl::Correspondences &correspondences,
00132                              Matrix4 &transformation_matrix) const
00133 {
00134   ConstCloudIterator<PointSource> source_it (cloud_src, correspondences, true);
00135   ConstCloudIterator<PointTarget> target_it (cloud_tgt, correspondences, false);
00136   std::vector<Scalar> weights (correspondences.size ());
00137   for (size_t i = 0; i < correspondences.size (); ++i)
00138     weights[i] = correspondences[i].weight;
00139   typename std::vector<Scalar>::const_iterator weights_it = weights.begin ();
00140   estimateRigidTransformation (source_it, target_it, weights_it, transformation_matrix);
00141 }
00142 
00144 template <typename PointSource, typename PointTarget, typename Scalar> inline void
00145 pcl::registration::TransformationEstimationPointToPlaneLLSWeighted<PointSource, PointTarget, Scalar>::
00146 constructTransformationMatrix (const double & alpha, const double & beta, const double & gamma,
00147                                const double & tx,    const double & ty,   const double & tz,
00148                                Matrix4 &transformation_matrix) const
00149 {
00150   // Construct the transformation matrix from rotation and translation 
00151   transformation_matrix = Eigen::Matrix<Scalar, 4, 4>::Zero ();
00152   transformation_matrix (0, 0) = static_cast<Scalar> ( cos (gamma) * cos (beta));
00153   transformation_matrix (0, 1) = static_cast<Scalar> (-sin (gamma) * cos (alpha) + cos (gamma) * sin (beta) * sin (alpha));
00154   transformation_matrix (0, 2) = static_cast<Scalar> ( sin (gamma) * sin (alpha) + cos (gamma) * sin (beta) * cos (alpha));
00155   transformation_matrix (1, 0) = static_cast<Scalar> ( sin (gamma) * cos (beta));
00156   transformation_matrix (1, 1) = static_cast<Scalar> ( cos (gamma) * cos (alpha) + sin (gamma) * sin (beta) * sin (alpha));
00157   transformation_matrix (1, 2) = static_cast<Scalar> (-cos (gamma) * sin (alpha) + sin (gamma) * sin (beta) * cos (alpha));
00158   transformation_matrix (2, 0) = static_cast<Scalar> (-sin (beta));
00159   transformation_matrix (2, 1) = static_cast<Scalar> ( cos (beta) * sin (alpha));
00160   transformation_matrix (2, 2) = static_cast<Scalar> ( cos (beta) * cos (alpha));
00161 
00162   transformation_matrix (0, 3) = static_cast<Scalar> (tx);
00163   transformation_matrix (1, 3) = static_cast<Scalar> (ty);
00164   transformation_matrix (2, 3) = static_cast<Scalar> (tz);
00165   transformation_matrix (3, 3) = static_cast<Scalar> (1);
00166 }
00167 
00169 template <typename PointSource, typename PointTarget, typename Scalar> inline void
00170 pcl::registration::TransformationEstimationPointToPlaneLLSWeighted<PointSource, PointTarget, Scalar>::
00171 estimateRigidTransformation (ConstCloudIterator<PointSource>& source_it,
00172                              ConstCloudIterator<PointTarget>& target_it,
00173                              typename std::vector<Scalar>::const_iterator& weights_it,
00174                              Matrix4 &transformation_matrix) const
00175 {
00176   typedef Eigen::Matrix<double, 6, 1> Vector6d;
00177   typedef Eigen::Matrix<double, 6, 6> Matrix6d;
00178 
00179   Matrix6d ATA;
00180   Vector6d ATb;
00181   ATA.setZero ();
00182   ATb.setZero ();
00183 
00184   while (source_it.isValid () && target_it.isValid ())
00185   {
00186     if (!pcl_isfinite (source_it->x) ||
00187         !pcl_isfinite (source_it->y) ||
00188         !pcl_isfinite (source_it->z) ||
00189         !pcl_isfinite (source_it->normal_x) ||
00190         !pcl_isfinite (source_it->normal_y) ||
00191         !pcl_isfinite (source_it->normal_z) ||
00192         !pcl_isfinite (target_it->x) ||
00193         !pcl_isfinite (target_it->y) ||
00194         !pcl_isfinite (target_it->z) ||
00195         !pcl_isfinite (target_it->normal_x) ||
00196         !pcl_isfinite (target_it->normal_y) ||
00197         !pcl_isfinite (target_it->normal_z))
00198     {
00199       ++ source_it;
00200       ++ target_it;
00201       ++ weights_it;
00202       continue;
00203     }
00204 
00205     const float & sx = source_it->x;
00206     const float & sy = source_it->y;
00207     const float & sz = source_it->z;
00208     const float & dx = target_it->x;
00209     const float & dy = target_it->y;
00210     const float & dz = target_it->z;
00211     const float & nx = target_it->normal[0] * (*weights_it);
00212     const float & ny = target_it->normal[1] * (*weights_it);
00213     const float & nz = target_it->normal[2] * (*weights_it);
00214 
00215     double a = nz*sy - ny*sz;
00216     double b = nx*sz - nz*sx;
00217     double c = ny*sx - nx*sy;
00218 
00219     //    0  1  2  3  4  5
00220     //    6  7  8  9 10 11
00221     //   12 13 14 15 16 17
00222     //   18 19 20 21 22 23
00223     //   24 25 26 27 28 29
00224     //   30 31 32 33 34 35
00225 
00226     ATA.coeffRef (0) += a * a;
00227     ATA.coeffRef (1) += a * b;
00228     ATA.coeffRef (2) += a * c;
00229     ATA.coeffRef (3) += a * nx;
00230     ATA.coeffRef (4) += a * ny;
00231     ATA.coeffRef (5) += a * nz;
00232     ATA.coeffRef (7) += b * b;
00233     ATA.coeffRef (8) += b * c;
00234     ATA.coeffRef (9) += b * nx;
00235     ATA.coeffRef (10) += b * ny;
00236     ATA.coeffRef (11) += b * nz;
00237     ATA.coeffRef (14) += c * c;
00238     ATA.coeffRef (15) += c * nx;
00239     ATA.coeffRef (16) += c * ny;
00240     ATA.coeffRef (17) += c * nz;
00241     ATA.coeffRef (21) += nx * nx;
00242     ATA.coeffRef (22) += nx * ny;
00243     ATA.coeffRef (23) += nx * nz;
00244     ATA.coeffRef (28) += ny * ny;
00245     ATA.coeffRef (29) += ny * nz;
00246     ATA.coeffRef (35) += nz * nz;
00247 
00248     double d = nx*dx + ny*dy + nz*dz - nx*sx - ny*sy - nz*sz;
00249     ATb.coeffRef (0) += a * d;
00250     ATb.coeffRef (1) += b * d;
00251     ATb.coeffRef (2) += c * d;
00252     ATb.coeffRef (3) += nx * d;
00253     ATb.coeffRef (4) += ny * d;
00254     ATb.coeffRef (5) += nz * d;
00255 
00256     ++ source_it;
00257     ++ target_it;
00258     ++ weights_it;
00259   }
00260 
00261   ATA.coeffRef (6) = ATA.coeff (1);
00262   ATA.coeffRef (12) = ATA.coeff (2);
00263   ATA.coeffRef (13) = ATA.coeff (8);
00264   ATA.coeffRef (18) = ATA.coeff (3);
00265   ATA.coeffRef (19) = ATA.coeff (9);
00266   ATA.coeffRef (20) = ATA.coeff (15);
00267   ATA.coeffRef (24) = ATA.coeff (4);
00268   ATA.coeffRef (25) = ATA.coeff (10);
00269   ATA.coeffRef (26) = ATA.coeff (16);
00270   ATA.coeffRef (27) = ATA.coeff (22);
00271   ATA.coeffRef (30) = ATA.coeff (5);
00272   ATA.coeffRef (31) = ATA.coeff (11);
00273   ATA.coeffRef (32) = ATA.coeff (17);
00274   ATA.coeffRef (33) = ATA.coeff (23);
00275   ATA.coeffRef (34) = ATA.coeff (29);
00276 
00277   // Solve A*x = b
00278   Vector6d x = static_cast<Vector6d> (ATA.inverse () * ATb);
00279 
00280   // Construct the transformation matrix from x
00281   constructTransformationMatrix (x (0), x (1), x (2), x (3), x (4), x (5), transformation_matrix);
00282 }
00283 #endif /* PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_POINT_TO_PLANE_LLS_WEIGHTED_HPP_ */


pcl
Author(s): Open Perception
autogenerated on Wed Aug 26 2015 15:36:57