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) 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 00043 ////////////////////////////////////////////////////////////////////////////////////////////// 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 00069 ////////////////////////////////////////////////////////////////////////////////////////////// 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 00098 ////////////////////////////////////////////////////////////////////////////////////////////// 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 00126 ////////////////////////////////////////////////////////////////////////////////////////////// 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 00143 ////////////////////////////////////////////////////////////////////////////////////////////// 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 00168 ////////////////////////////////////////////////////////////////////////////////////////////// 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_ */