Point Cloud Library (PCL)
1.7.0
|
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 00044 ////////////////////////////////////////////////////////////////////////////////////////////// 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 00063 ////////////////////////////////////////////////////////////////////////////////////////////// 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 00084 ////////////////////////////////////////////////////////////////////////////////////////////// 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 00105 ////////////////////////////////////////////////////////////////////////////////////////////// 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 00118 ////////////////////////////////////////////////////////////////////////////////////////////// 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 00143 ////////////////////////////////////////////////////////////////////////////////////////////// 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_ */