pcl: transformation_estimation_point_to_plane

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