44 #include <pcl/pcl_base.h>
45 #include <pcl/point_types.h>
46 #include <pcl/point_cloud.h>
47 #include <pcl/registration/registration.h>
48 #include <pcl/registration/boost.h>
49 #include <pcl/registration/eigen.h>
50 #include <pcl/registration/icp.h>
51 #include <pcl/registration/boost_graph.h>
55 namespace registration
61 template <
typename Po
intT>
65 typedef boost::shared_ptr< ELCH<PointT> >
Ptr;
66 typedef boost::shared_ptr< const ELCH<PointT> >
ConstPtr;
80 typedef boost::adjacency_list<
112 typename boost::graph_traits<LoopGraph>::vertex_descriptor vd = add_vertex (*loop_graph_);
113 (*loop_graph_)[vd].cloud = cloud;
114 if (num_vertices (*loop_graph_) > 1)
115 add_edge (vd_, vd, *loop_graph_);
123 return (loop_graph_);
132 loop_graph_ = loop_graph;
136 inline typename boost::graph_traits<LoopGraph>::vertex_descriptor
139 return (loop_start_);
146 setLoopStart (
const typename boost::graph_traits<LoopGraph>::vertex_descriptor &loop_start)
148 loop_start_ = loop_start;
152 inline typename boost::graph_traits<LoopGraph>::vertex_descriptor
162 setLoopEnd (
const typename boost::graph_traits<LoopGraph>::vertex_descriptor &loop_end)
164 loop_end_ = loop_end;
184 inline Eigen::Matrix4f
187 return (loop_transform_);
196 loop_transform_ = loop_transform;
197 compute_loop_ =
false;
216 typedef boost::adjacency_list<
217 boost::listS, boost::vecS, boost::undirectedS,
219 boost::property< boost::edge_weight_t, double > >
230 loopOptimizerAlgorithm (LOAGraph &g,
double *weights);
236 typename boost::graph_traits<LoopGraph>::vertex_descriptor loop_start_;
239 typename boost::graph_traits<LoopGraph>::vertex_descriptor loop_end_;
245 Eigen::Matrix4f loop_transform_;
249 typename boost::graph_traits<LoopGraph>::vertex_descriptor vd_;
252 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
257 #include <pcl/registration/impl/elch.hpp>
259 #endif // PCL_ELCH_H_
boost::adjacency_list< boost::listS, boost::eigen_vecS, boost::undirectedS, Vertex, boost::no_property > LoopGraph
graph structure to hold the SLAM graph
void setLoopEnd(const typename boost::graph_traits< LoopGraph >::vertex_descriptor &loop_end)
Setter for the last scan of a loop.
boost::shared_ptr< const PointCloud< PointT > > ConstPtr
virtual ~ELCH()
Empty destructor.
boost::shared_ptr< const Registration< PointT, PointT, float > > ConstPtr
boost::shared_ptr< PointCloud< PointT > > Ptr
pcl::PointCloud< PointT > PointCloud
Eigen::Matrix4f getLoopTransform()
Getter for the transformation between the first and the last scan.
PointCloud::ConstPtr PointCloudConstPtr
void compute()
Computes new poses for all point clouds by closing the loop between start and end point cloud...
void setLoopGraph(LoopGraphPtr loop_graph)
Setter for a new internal graph.
boost::shared_ptr< Registration< PointSource, PointTarget, Scalar > > Ptr
RegistrationPtr getReg()
Getter for the registration algorithm.
void setLoopStart(const typename boost::graph_traits< LoopGraph >::vertex_descriptor &loop_start)
Setter for the first scan of a loop.
ELCH (Explicit Loop Closing Heuristic) class
PointCloud::Ptr PointCloudPtr
void setReg(RegistrationPtr reg)
Setter for the registration algorithm.
boost::shared_ptr< LoopGraph > LoopGraphPtr
pcl::Registration< PointT, PointT > Registration
boost::graph_traits< LoopGraph >::vertex_descriptor getLoopEnd()
Getter for the last scan of a loop.
boost::graph_traits< LoopGraph >::vertex_descriptor getLoopStart()
Getter for the first scan of a loop.
IterativeClosestPoint provides a base implementation of the Iterative Closest Point algorithm...
boost::shared_ptr< const ELCH< PointT > > ConstPtr
virtual bool initCompute()
This method should get called before starting the actual computation.
Registration::ConstPtr RegistrationConstPtr
PointCloud::Ptr PointCloudPtr
boost::shared_ptr< ELCH< PointT > > Ptr
Eigen::Affine3f transform
void setLoopTransform(const Eigen::Matrix4f &loop_transform)
Setter for the transformation between the first and the last scan.
A point structure representing Euclidean xyz coordinates, and the RGB color.
void addPointCloud(PointCloudPtr cloud)
Add a new point cloud to the internal graph.
LoopGraphPtr getLoopGraph()
Getter for the internal graph.
Registration::Ptr RegistrationPtr