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) 2011, 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 00041 #ifndef PCL_ELCH_H_ 00042 #define PCL_ELCH_H_ 00043 00044 #include <pcl/pcl_base.h> 00045 #include <pcl/point_types.h> 00046 #include <pcl/point_cloud.h> 00047 #include <pcl/registration/registration.h> 00048 #include <pcl/registration/boost.h> 00049 #include <pcl/registration/eigen.h> 00050 #include <pcl/registration/icp.h> 00051 #include <pcl/registration/boost_graph.h> 00052 00053 namespace pcl 00054 { 00055 namespace registration 00056 { 00057 /** \brief @b ELCH (Explicit Loop Closing Heuristic) class 00058 * \author Jochen Sprickerhof 00059 * \ingroup registration 00060 */ 00061 template <typename PointT> 00062 class ELCH : public PCLBase<PointT> 00063 { 00064 public: 00065 typedef boost::shared_ptr< ELCH<PointT> > Ptr; 00066 typedef boost::shared_ptr< const ELCH<PointT> > ConstPtr; 00067 00068 typedef pcl::PointCloud<PointT> PointCloud; 00069 typedef typename PointCloud::Ptr PointCloudPtr; 00070 typedef typename PointCloud::ConstPtr PointCloudConstPtr; 00071 00072 struct Vertex 00073 { 00074 Vertex () : cloud () {} 00075 PointCloudPtr cloud; 00076 }; 00077 00078 /** \brief graph structure to hold the SLAM graph */ 00079 typedef boost::adjacency_list< 00080 boost::listS, boost::eigen_vecS, boost::undirectedS, 00081 Vertex, 00082 boost::no_property> 00083 LoopGraph; 00084 00085 typedef boost::shared_ptr< LoopGraph > LoopGraphPtr; 00086 00087 typedef typename pcl::Registration<PointT, PointT> Registration; 00088 typedef typename Registration::Ptr RegistrationPtr; 00089 typedef typename Registration::ConstPtr RegistrationConstPtr; 00090 00091 /** \brief Empty constructor. */ 00092 ELCH () : 00093 loop_graph_ (new LoopGraph), 00094 loop_start_ (0), 00095 loop_end_ (0), 00096 reg_ (new pcl::IterativeClosestPoint<PointT, PointT>), 00097 loop_transform_ (), 00098 compute_loop_ (true), 00099 vd_ () 00100 {}; 00101 00102 /** \brief Empty destructor */ 00103 virtual ~ELCH () {} 00104 00105 /** \brief Add a new point cloud to the internal graph. 00106 * \param[in] cloud the new point cloud 00107 */ 00108 inline void 00109 addPointCloud (PointCloudPtr cloud) 00110 { 00111 typename boost::graph_traits<LoopGraph>::vertex_descriptor vd = add_vertex (*loop_graph_); 00112 (*loop_graph_)[vd].cloud = cloud; 00113 if (num_vertices (*loop_graph_) > 1) 00114 add_edge (vd_, vd, *loop_graph_); 00115 vd_ = vd; 00116 } 00117 00118 /** \brief Getter for the internal graph. */ 00119 inline LoopGraphPtr 00120 getLoopGraph () 00121 { 00122 return (loop_graph_); 00123 } 00124 00125 /** \brief Setter for a new internal graph. 00126 * \param[in] loop_graph the new graph 00127 */ 00128 inline void 00129 setLoopGraph (LoopGraphPtr loop_graph) 00130 { 00131 loop_graph_ = loop_graph; 00132 } 00133 00134 /** \brief Getter for the first scan of a loop. */ 00135 inline typename boost::graph_traits<LoopGraph>::vertex_descriptor 00136 getLoopStart () 00137 { 00138 return (loop_start_); 00139 } 00140 00141 /** \brief Setter for the first scan of a loop. 00142 * \param[in] loop_start the scan that starts the loop 00143 */ 00144 inline void 00145 setLoopStart (const typename boost::graph_traits<LoopGraph>::vertex_descriptor &loop_start) 00146 { 00147 loop_start_ = loop_start; 00148 } 00149 00150 /** \brief Getter for the last scan of a loop. */ 00151 inline typename boost::graph_traits<LoopGraph>::vertex_descriptor 00152 getLoopEnd () 00153 { 00154 return (loop_end_); 00155 } 00156 00157 /** \brief Setter for the last scan of a loop. 00158 * \param[in] loop_end the scan that ends the loop 00159 */ 00160 inline void 00161 setLoopEnd (const typename boost::graph_traits<LoopGraph>::vertex_descriptor &loop_end) 00162 { 00163 loop_end_ = loop_end; 00164 } 00165 00166 /** \brief Getter for the registration algorithm. */ 00167 inline RegistrationPtr 00168 getReg () 00169 { 00170 return (reg_); 00171 } 00172 00173 /** \brief Setter for the registration algorithm. 00174 * \param[in] reg the registration algorithm used to compute the transformation between the start and the end of the loop 00175 */ 00176 inline void 00177 setReg (RegistrationPtr reg) 00178 { 00179 reg_ = reg; 00180 } 00181 00182 /** \brief Getter for the transformation between the first and the last scan. */ 00183 inline Eigen::Matrix4f 00184 getLoopTransform () 00185 { 00186 return (loop_transform_); 00187 } 00188 00189 /** \brief Setter for the transformation between the first and the last scan. 00190 * \param[in] loop_transform the transformation between the first and the last scan 00191 */ 00192 inline void 00193 setLoopTransform (const Eigen::Matrix4f &loop_transform) 00194 { 00195 loop_transform_ = loop_transform; 00196 compute_loop_ = false; 00197 } 00198 00199 /** \brief Computes now poses for all point clouds by closing the loop 00200 * between start and end point cloud. This will transform all given point 00201 * clouds for now! 00202 */ 00203 void 00204 compute (); 00205 00206 protected: 00207 using PCLBase<PointT>::deinitCompute; 00208 00209 /** \brief This method should get called before starting the actual computation. */ 00210 virtual bool 00211 initCompute (); 00212 00213 private: 00214 /** \brief graph structure for the internal optimization graph */ 00215 typedef boost::adjacency_list< 00216 boost::listS, boost::vecS, boost::undirectedS, 00217 boost::no_property, 00218 boost::property< boost::edge_weight_t, double > > 00219 LOAGraph; 00220 00221 /** 00222 * graph balancer algorithm computes the weights 00223 * @param[in] g the graph 00224 * @param[in] f index of the first node 00225 * @param[in] l index of the last node 00226 * @param[out] weights array for the weights 00227 */ 00228 void 00229 loopOptimizerAlgorithm (LOAGraph &g, double *weights); 00230 00231 /** \brief The internal loop graph. */ 00232 LoopGraphPtr loop_graph_; 00233 00234 /** \brief The first scan of the loop. */ 00235 typename boost::graph_traits<LoopGraph>::vertex_descriptor loop_start_; 00236 00237 /** \brief The last scan of the loop. */ 00238 typename boost::graph_traits<LoopGraph>::vertex_descriptor loop_end_; 00239 00240 /** \brief The registration object used to close the loop. */ 00241 RegistrationPtr reg_; 00242 00243 /** \brief The transformation between that start and end of the loop. */ 00244 Eigen::Matrix4f loop_transform_; 00245 bool compute_loop_; 00246 00247 /** \brief previously added node in the loop_graph_. */ 00248 typename boost::graph_traits<LoopGraph>::vertex_descriptor vd_; 00249 00250 public: 00251 EIGEN_MAKE_ALIGNED_OPERATOR_NEW 00252 }; 00253 } 00254 } 00255 00256 #include <pcl/registration/impl/elch.hpp> 00257 00258 #endif // PCL_ELCH_H_