43 #include <pcl/memory.h>
44 #include <pcl/pcl_macros.h>
45 #include <pcl/pcl_base.h>
46 #include <pcl/point_types.h>
47 #include <pcl/point_cloud.h>
48 #include <pcl/registration/registration.h>
49 #include <pcl/registration/boost.h>
50 #include <pcl/registration/eigen.h>
51 #include <pcl/registration/icp.h>
52 #include <pcl/registration/boost_graph.h>
56 namespace registration
62 template <
typename Po
intT>
66 using Ptr = shared_ptr<ELCH<PointT> >;
67 using ConstPtr = shared_ptr<const ELCH<PointT> >;
111 typename boost::graph_traits<LoopGraph>::vertex_descriptor vd = add_vertex (*loop_graph_);
112 (*loop_graph_)[vd].cloud = cloud;
113 if (num_vertices (*loop_graph_) > 1)
114 add_edge (vd_, vd, *loop_graph_);
122 return (loop_graph_);
131 loop_graph_ = loop_graph;
135 inline typename boost::graph_traits<LoopGraph>::vertex_descriptor
138 return (loop_start_);
145 setLoopStart (
const typename boost::graph_traits<LoopGraph>::vertex_descriptor &loop_start)
147 loop_start_ = loop_start;
151 inline typename boost::graph_traits<LoopGraph>::vertex_descriptor
161 setLoopEnd (
const typename boost::graph_traits<LoopGraph>::vertex_descriptor &loop_end)
163 loop_end_ = loop_end;
183 inline Eigen::Matrix4f
186 return (loop_transform_);
195 loop_transform_ = loop_transform;
196 compute_loop_ =
false;
215 using LOAGraph = boost::adjacency_list<
216 boost::listS, boost::vecS, boost::undirectedS,
218 boost::property< boost::edge_weight_t, double > >;
228 loopOptimizerAlgorithm (LOAGraph &g,
double *weights);
234 typename boost::graph_traits<LoopGraph>::vertex_descriptor loop_start_;
237 typename boost::graph_traits<LoopGraph>::vertex_descriptor loop_end_;
243 Eigen::Matrix4f loop_transform_;
247 typename boost::graph_traits<LoopGraph>::vertex_descriptor vd_;
255 #include <pcl/registration/impl/elch.hpp>
shared_ptr< LoopGraph > LoopGraphPtr
void setLoopEnd(const typename boost::graph_traits< LoopGraph >::vertex_descriptor &loop_end)
Setter for the last scan of a loop.
shared_ptr< PointCloud< PointT > > Ptr
shared_ptr< const ELCH< PointT > > ConstPtr
Eigen::Matrix4f getLoopTransform()
Getter for the transformation between the first and the last scan.
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.
typename Registration::ConstPtr RegistrationConstPtr
RegistrationPtr getReg()
Getter for the registration algorithm.
#define PCL_MAKE_ALIGNED_OPERATOR_NEW
Macro to signal a class requires a custom allocator.
shared_ptr< ELCH< PointT > > Ptr
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
shared_ptr< Registration< PointT, PointT, float > > Ptr
void setReg(RegistrationPtr reg)
Setter for the registration algorithm.
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.
typename PointCloud::Ptr PointCloudPtr
IterativeClosestPoint provides a base implementation of the Iterative Closest Point algorithm...
virtual bool initCompute()
This method should get called before starting the actual computation.
PointCloud represents the base class in PCL for storing collections of 3D points. ...
typename Registration::Ptr RegistrationPtr
Eigen::Affine3f transform
shared_ptr< const PointCloud< PointT > > ConstPtr
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.
boost::adjacency_list< boost::listS, boost::eigen_vecS, boost::undirectedS, Vertex, boost::no_property > LoopGraph
graph structure to hold the SLAM graph
shared_ptr< const Registration< PointT, PointT, float > > ConstPtr
void addPointCloud(PointCloudPtr cloud)
Add a new point cloud to the internal graph.
LoopGraphPtr getLoopGraph()
Getter for the internal graph.
typename PointCloud::ConstPtr PointCloudConstPtr