Point Cloud Library (PCL)
1.3.1
|
00001 /* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Point Cloud Library (PCL) - www.pointclouds.org 00005 * Copyright (c) 2010, Willow Garage, 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 Willow Garage, Inc. 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 * 00037 */ 00038 #ifndef PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_POINT_TO_PLANE_LLS_HPP_ 00039 #define PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_POINT_TO_PLANE_LLS_HPP_ 00040 00042 template <typename PointSource, typename PointTarget> inline void 00043 pcl::registration::TransformationEstimationPointToPlaneLLS<PointSource, PointTarget>:: 00044 estimateRigidTransformation (const pcl::PointCloud<PointSource> &cloud_src, 00045 const pcl::PointCloud<PointTarget> &cloud_tgt, 00046 Eigen::Matrix4f &transformation_matrix) 00047 { 00048 size_t nr_points = cloud_src.points.size (); 00049 if (cloud_tgt.points.size () != nr_points) 00050 { 00051 PCL_ERROR ("[pcl::TransformationSVD::estimateRigidTransformation] Number or points in source (%lu) differs than target (%lu)!\n", nr_points, (unsigned long)cloud_tgt.points.size ()); 00052 return; 00053 } 00054 00055 // Approximate as a linear least squares problem 00056 Eigen::MatrixXf A (nr_points, 6); 00057 Eigen::MatrixXf b (nr_points, 1); 00058 for (size_t i = 0; i < nr_points; ++i) 00059 { 00060 const float & sx = cloud_src.points[i].x; 00061 const float & sy = cloud_src.points[i].y; 00062 const float & sz = cloud_src.points[i].z; 00063 const float & dx = cloud_tgt.points[i].x; 00064 const float & dy = cloud_tgt.points[i].y; 00065 const float & dz = cloud_tgt.points[i].z; 00066 const float & nx = cloud_tgt.points[i].normal[0]; 00067 const float & ny = cloud_tgt.points[i].normal[1]; 00068 const float & nz = cloud_tgt.points[i].normal[2]; 00069 A (i, 0) = nz*sy - ny*sz; 00070 A (i, 1) = nx*sz - nz*sx; 00071 A (i, 2) = ny*sx - nx*sy; 00072 A (i, 3) = nx; 00073 A (i, 4) = ny; 00074 A (i, 5) = nz; 00075 b (i, 0) = nx*dx + ny*dy + nz*dz - nx*sx - ny*sy - nz*sz; 00076 } 00077 00078 // Solve A*x = b 00079 Eigen::VectorXf x = A.colPivHouseholderQr ().solve (b); 00080 00081 // Construct the transformation matrix from x 00082 constructTransformationMatrix (x (0), x (1), x (2), x (3), x (4), x (5), transformation_matrix); 00083 00084 } 00085 00087 template <typename PointSource, typename PointTarget> void 00088 pcl::registration::TransformationEstimationPointToPlaneLLS<PointSource, PointTarget>:: 00089 estimateRigidTransformation (const pcl::PointCloud<PointSource> &cloud_src, 00090 const std::vector<int> &indices_src, 00091 const pcl::PointCloud<PointTarget> &cloud_tgt, 00092 Eigen::Matrix4f &transformation_matrix) 00093 { 00094 size_t nr_points = indices_src.size (); 00095 if (cloud_tgt.points.size () != nr_points) 00096 { 00097 PCL_ERROR ("[pcl::TransformationSVD::estimateRigidTransformation] Number or points in source (%lu) differs than target (%lu)!\n", (unsigned long)indices_src.size (), (unsigned long)cloud_tgt.points.size ()); 00098 return; 00099 } 00100 00101 // Approximate as a linear least squares problem 00102 Eigen::MatrixXf A (nr_points, 6); 00103 Eigen::MatrixXf b (nr_points, 1); 00104 for (size_t i = 0; i < nr_points; ++i) 00105 { 00106 const float & sx = cloud_src.points[indices_src[i]].x; 00107 const float & sy = cloud_src.points[indices_src[i]].y; 00108 const float & sz = cloud_src.points[indices_src[i]].z; 00109 const float & dx = cloud_tgt.points[i].x; 00110 const float & dy = cloud_tgt.points[i].y; 00111 const float & dz = cloud_tgt.points[i].z; 00112 const float & nx = cloud_tgt.points[i].normal[0]; 00113 const float & ny = cloud_tgt.points[i].normal[1]; 00114 const float & nz = cloud_tgt.points[i].normal[2]; 00115 A (i, 0) = nz*sy - ny*sz; 00116 A (i, 1) = nx*sz - nz*sx; 00117 A (i, 2) = ny*sx - nx*sy; 00118 A (i, 3) = nx; 00119 A (i, 4) = ny; 00120 A (i, 5) = nz; 00121 b (i, 0) = nx*dx + ny*dy + nz*dz - nx*sx - ny*sy - nz*sz; 00122 } 00123 00124 // Solve A*x = b 00125 Eigen::VectorXf x = A.colPivHouseholderQr ().solve (b); 00126 00127 // Construct the transformation matrix from x 00128 constructTransformationMatrix (x (0), x (1), x (2), x (3), x (4), x (5), transformation_matrix); 00129 } 00130 00131 00133 template <typename PointSource, typename PointTarget> inline void 00134 pcl::registration::TransformationEstimationPointToPlaneLLS<PointSource, PointTarget>:: 00135 estimateRigidTransformation (const pcl::PointCloud<PointSource> &cloud_src, 00136 const std::vector<int> &indices_src, 00137 const pcl::PointCloud<PointTarget> &cloud_tgt, 00138 const std::vector<int> &indices_tgt, 00139 Eigen::Matrix4f &transformation_matrix) 00140 { 00141 size_t nr_points = indices_src.size (); 00142 if (indices_tgt.size () != nr_points) 00143 { 00144 PCL_ERROR ("[pcl::TransformationEstimationPointToPlaneLLS::estimateRigidTransformation] Number or points in source (%lu) differs than target (%lu)!\n", (unsigned long)indices_src.size (), (unsigned long)indices_tgt.size ()); 00145 return; 00146 } 00147 00148 // Approximate as a linear least squares problem 00149 Eigen::MatrixXf A (nr_points, 6); 00150 Eigen::MatrixXf b (nr_points, 1); 00151 for (size_t i = 0; i < nr_points; ++i) 00152 { 00153 const float & sx = cloud_src.points[indices_src[i]].x; 00154 const float & sy = cloud_src.points[indices_src[i]].y; 00155 const float & sz = cloud_src.points[indices_src[i]].z; 00156 const float & dx = cloud_tgt.points[indices_tgt[i]].x; 00157 const float & dy = cloud_tgt.points[indices_tgt[i]].y; 00158 const float & dz = cloud_tgt.points[indices_tgt[i]].z; 00159 const float & nx = cloud_tgt.points[indices_tgt[i]].normal[0]; 00160 const float & ny = cloud_tgt.points[indices_tgt[i]].normal[1]; 00161 const float & nz = cloud_tgt.points[indices_tgt[i]].normal[2]; 00162 A (i, 0) = nz*sy - ny*sz; 00163 A (i, 1) = nx*sz - nz*sx; 00164 A (i, 2) = ny*sx - nx*sy; 00165 A (i, 3) = nx; 00166 A (i, 4) = ny; 00167 A (i, 5) = nz; 00168 b (i, 0) = nx*dx + ny*dy + nz*dz - nx*sx - ny*sy - nz*sz; 00169 } 00170 00171 // Solve A*x = b 00172 Eigen::VectorXf x = A.colPivHouseholderQr ().solve (b); 00173 00174 // Construct the transformation matrix from x 00175 constructTransformationMatrix (x (0), x (1), x (2), x (3), x (4), x (5), transformation_matrix); 00176 } 00177 00179 template <typename PointSource, typename PointTarget> inline void 00180 pcl::registration::TransformationEstimationPointToPlaneLLS<PointSource, PointTarget>:: 00181 estimateRigidTransformation (const pcl::PointCloud<PointSource> &cloud_src, 00182 const pcl::PointCloud<PointTarget> &cloud_tgt, 00183 const pcl::Correspondences &correspondences, 00184 Eigen::Matrix4f &transformation_matrix) 00185 { 00186 size_t nr_points = correspondences.size (); 00187 00188 // Approximate as a linear least squares problem 00189 Eigen::MatrixXf A (nr_points, 6); 00190 Eigen::MatrixXf b (nr_points, 1); 00191 for (size_t i = 0; i < nr_points; ++i) 00192 { 00193 const int & src_idx = correspondences[i].index_query; 00194 const int & tgt_idx = correspondences[i].index_match; 00195 const float & sx = cloud_src.points[src_idx].x; 00196 const float & sy = cloud_src.points[src_idx].y; 00197 const float & sz = cloud_src.points[src_idx].z; 00198 const float & dx = cloud_tgt.points[tgt_idx].x; 00199 const float & dy = cloud_tgt.points[tgt_idx].y; 00200 const float & dz = cloud_tgt.points[tgt_idx].z; 00201 const float & nx = cloud_tgt.points[tgt_idx].normal[0]; 00202 const float & ny = cloud_tgt.points[tgt_idx].normal[1]; 00203 const float & nz = cloud_tgt.points[tgt_idx].normal[2]; 00204 A (i, 0) = nz*sy - ny*sz; 00205 A (i, 1) = nx*sz - nz*sx; 00206 A (i, 2) = ny*sx - nx*sy; 00207 A (i, 3) = nx; 00208 A (i, 4) = ny; 00209 A (i, 5) = nz; 00210 b (i, 0) = nx*dx + ny*dy + nz*dz - nx*sx - ny*sy - nz*sz; 00211 } 00212 00213 // Solve A*x = b 00214 Eigen::VectorXf x = A.colPivHouseholderQr ().solve (b); 00215 00216 // Construct the transformation matrix from x 00217 constructTransformationMatrix (x (0), x (1), x (2), x (3), x (4), x (5), transformation_matrix); 00218 } 00219 00220 template <typename PointSource, typename PointTarget> inline void 00221 pcl::registration::TransformationEstimationPointToPlaneLLS<PointSource, PointTarget>:: 00222 constructTransformationMatrix (const float & alpha, const float & beta, const float & gamma, 00223 const float & tx, const float & ty, const float & tz, 00224 Eigen::Matrix4f &transformation_matrix) 00225 { 00226 00227 // Construct the transformation matrix from rotation and translation 00228 transformation_matrix = Eigen::Matrix4f::Zero (); 00229 transformation_matrix (0, 0) = cos(gamma)*cos(beta); 00230 transformation_matrix (0, 1) = -sin(gamma)*cos(alpha) + cos(gamma)*sin(beta)*sin(alpha); 00231 transformation_matrix (0, 2) = sin(gamma)*sin(alpha) + cos(gamma)*sin(beta)*cos(alpha); 00232 transformation_matrix (1, 0) = sin(gamma)*cos(beta); 00233 transformation_matrix (1, 1) = cos(gamma)*cos(alpha) + sin(gamma)*sin(beta)*sin(alpha); 00234 transformation_matrix (1, 2) = -cos(gamma)*sin(alpha) + sin(gamma)*sin(beta)*cos(alpha); 00235 transformation_matrix (2, 0) = -sin(beta); 00236 transformation_matrix (2, 1) = cos(beta)*sin(alpha); 00237 transformation_matrix (2, 2) = cos(beta)*cos(alpha); 00238 00239 transformation_matrix (0, 3) = tx; 00240 transformation_matrix (1, 3) = ty; 00241 transformation_matrix (2, 3) = tz; 00242 transformation_matrix (3, 3) = 1; 00243 } 00244 00245 #endif /* PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_POINT_TO_PLANE_LLS_HPP_ */