Point Cloud Library (PCL)
1.3.1
|
00001 /* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2010, Willow Garage, Inc. 00005 * All rights reserved. 00006 * 00007 * Redistribution and use in source and binary forms, with or without 00008 * modification, are permitted provided that the following conditions 00009 * are met: 00010 * 00011 * * Redistributions of source code must retain the above copyright 00012 * notice, this list of conditions and the following disclaimer. 00013 * * Redistributions in binary form must reproduce the above 00014 * copyright notice, this list of conditions and the following 00015 * disclaimer in the documentation and/or other materials provided 00016 * with the distribution. 00017 * * Neither the name of Willow Garage, Inc. nor the names of its 00018 * contributors may be used to endorse or promote products derived 00019 * from this software without specific prior written permission. 00020 * 00021 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00022 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00023 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00024 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00025 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00026 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00027 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00028 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00029 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00030 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00031 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00032 * POSSIBILITY OF SUCH DAMAGE. 00033 * 00034 */ 00035 00037 00042 template <typename PointSource, typename PointTarget> void 00043 pcl::estimateRigidTransformationGICP (const pcl::PointCloud<PointSource> &cloud_src, 00044 const pcl::PointCloud<PointTarget> &cloud_tgt, 00045 Eigen::Matrix4f &transformation_matrix) 00046 { 00047 ROS_ASSERT (cloud_src.points.size () == cloud_tgt.points.size ()); 00048 00049 // <cloud_src,cloud_src> is the source dataset 00050 transformation_matrix.setIdentity (); 00051 } 00052 00054 00061 template <typename PointSource, typename PointTarget> void 00062 pcl::estimateRigidTransformationGICP (const pcl::PointCloud<PointSource> &cloud_src, 00063 const std::vector<int> &indices_src, 00064 const pcl::PointCloud<PointTarget> &cloud_tgt, 00065 const std::vector<int> &indices_tgt, 00066 Eigen::Matrix4f &transformation_matrix) 00067 { 00068 assert (indices_src.size () == indices_tgt.size ()); 00069 00070 // <cloud_src,cloud_src> is the source dataset 00071 transformation_matrix.setIdentity (); 00072 00073 } 00074 00075 00077 00083 template <typename PointSource, typename PointTarget> void 00084 pcl::estimateRigidTransformationGICP (const pcl::PointCloud<PointSource> &cloud_src, 00085 const std::vector<int> &indices_src, 00086 const pcl::PointCloud<PointTarget> &cloud_tgt, 00087 Eigen::Matrix4f &transformation_matrix) 00088 { 00089 ROS_ASSERT (indices_src.size () == cloud_tgt.points.size ()); 00090 00091 // <cloud_src,cloud_src> is the source dataset 00092 transformation_matrix.setIdentity (); 00093 00094 } 00095 00096 00097 00099 00102 template <typename PointSource, typename PointTarget> void 00103 pcl::GeneralizedIterativeClosestPoint<PointSource, PointTarget>::computeTransformation (PointCloudSource &output) 00104 { 00105 dgc::gicp::GICPPointSet p1, p2; 00106 dgc_transform_t t_base, t0, t1; 00107 double gicp_epsilon = 1e-3; 00108 00109 p1.SetDebug(false); 00110 p2.SetDebug(false); 00111 00112 // set up the transformations 00113 dgc_transform_identity(t_base); 00114 dgc_transform_identity(t0); 00115 dgc_transform_identity(t1); 00116 00117 // load points 00118 dgc::gicp::GICPPoint pt; 00119 pt.range = -1; 00120 for(int k = 0; k < 3; k++) { 00121 for(int l = 0; l < 3; l++) { 00122 pt.C[k][l] = (k == l)?1:0; 00123 } 00124 } 00125 p1.point_.resize(input_->points.size()); 00126 p2.point_.resize(target_->points.size()); 00127 for(unsigned int i = 0; i < input_->points.size(); ++i) { 00128 pt.x = input_->points[i].x; 00129 pt.y = input_->points[i].y; 00130 pt.z = input_->points[i].z; 00131 p1.point_[i] = pt; 00132 } 00133 for(unsigned int i = 0; i < target_->points.size(); ++i) { 00134 pt.x = target_->points[i].x; 00135 pt.y = target_->points[i].y; 00136 pt.z = target_->points[i].z; 00137 p2.point_[i] = pt; 00138 } 00139 00140 // setting up matrices and trees 00141 p1.SetGICPEpsilon(gicp_epsilon); 00142 p2.SetGICPEpsilon(gicp_epsilon); 00143 p1.BuildKDTree(); 00144 p1.ComputeMatrices(); 00145 p2.BuildKDTree(); 00146 p2.ComputeMatrices(); 00147 00148 // register 00149 //int iterations = p2.AlignScan(&p1, t_base, t1, max_distance); 00150 p2.AlignScan(&p1, t_base, t1, max_distance_); 00151 00152 // transform point cloud 00153 for (unsigned int i = 0; i < p1.point_.size(); ++i) 00154 dgc_transform_point(&p1[i].x, &p1[i].y, &p1[i].z, t1); 00155 00156 // write output cloud 00157 output.points.resize(p1.point_.size()); 00158 for(unsigned int i = 0; i < p1.point_.size(); ++i) { 00159 output.points[i].x = p1.point_[i].x; 00160 output.points[i].y = p1.point_[i].y; 00161 output.points[i].z = p1.point_[i].z; 00162 } 00163 00164 Eigen::Affine3f transformation_gicp; 00165 double tx, ty, tz, rx, ry, rz; 00166 dgc_transform_get_translation(t1, &tx, &ty, &tz); 00167 dgc_transform_get_rotation(t1, &rx, &ry, &rz); 00168 00169 00170 // compute final transform (hehe) 00171 00172 float A=cosf(rz), B=sinf(rz), C=cosf(ry), D=sinf(ry), 00173 E=cosf(rx), F=sinf(rx), DE=D*E, DF=D*F; 00174 final_transformation_(0,0) = A*C; final_transformation_(0,1) = A*DF - B*E; final_transformation_(0,2) = B*F + A*DE; final_transformation_(0,3) = tx; 00175 final_transformation_(1,0) = B*C; final_transformation_(1,1) = A*E + B*DF; final_transformation_(1,2) = B*DE - A*F; final_transformation_(1,3) = ty; 00176 final_transformation_(2,0) = -D; final_transformation_(2,1) = C*F; final_transformation_(2,2) = C*E; final_transformation_(2,3) = tz; 00177 final_transformation_(3,0) = 0; final_transformation_(3,1) = 0; final_transformation_(3,2) = 0; final_transformation_(3,3) = 1; 00178 00179 // transformation_gicp = pcl::getTransformation( (float)(tx), (float)(ty), (float)(tz), (float)(rx), (float)(ry), (float)(rz)); 00180 // final_transformation_ = transformation_gicp; //transformation_ * final_transformation_; 00181 00182 } 00183