Point Cloud Library (PCL)  1.3.1
stanford_gicp.hpp
Go to the documentation of this file.
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 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines