43 #include <pcl/registration/boost_graph.h>
44 #include <pcl/registration/icp.h>
45 #include <pcl/registration/registration.h>
46 #include <pcl/memory.h>
47 #include <pcl/pcl_base.h>
48 #include <pcl/pcl_macros.h>
49 #include <pcl/point_cloud.h>
50 #include <pcl/point_types.h>
53 namespace registration {
58 template <
typename Po
intT>
61 using Ptr = shared_ptr<ELCH<PointT>>;
62 using ConstPtr = shared_ptr<const ELCH<PointT>>;
75 using LoopGraph = boost::adjacency_list<boost::listS,
96 ~ELCH()
override =
default;
104 typename boost::graph_traits<LoopGraph>::vertex_descriptor vd =
105 add_vertex(*loop_graph_);
106 (*loop_graph_)[vd].cloud = cloud;
107 if (num_vertices(*loop_graph_) > 1)
108 add_edge(vd_, vd, *loop_graph_);
116 return (loop_graph_);
125 loop_graph_ = loop_graph;
129 inline typename boost::graph_traits<LoopGraph>::vertex_descriptor
132 return (loop_start_);
140 const typename boost::graph_traits<LoopGraph>::vertex_descriptor& loop_start)
142 loop_start_ = loop_start;
146 inline typename boost::graph_traits<LoopGraph>::vertex_descriptor
156 setLoopEnd(
const typename boost::graph_traits<LoopGraph>::vertex_descriptor& loop_end)
158 loop_end_ = loop_end;
179 inline Eigen::Matrix4f
182 return (loop_transform_);
191 loop_transform_ = loop_transform;
192 compute_loop_ =
false;
211 using LOAGraph = boost::adjacency_list<boost::listS,
215 boost::property<boost::edge_weight_t, double>>;
225 loopOptimizerAlgorithm(LOAGraph& g,
double* weights);
231 typename boost::graph_traits<LoopGraph>::vertex_descriptor loop_start_;
234 typename boost::graph_traits<LoopGraph>::vertex_descriptor loop_end_;
240 Eigen::Matrix4f loop_transform_;
241 bool compute_loop_{
true};
244 typename boost::graph_traits<LoopGraph>::vertex_descriptor vd_;
252 #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
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.
boost::adjacency_list< boost::listS, boost::eigen_vecS, boost::undirectedS, Vertex, boost::no_property > LoopGraph
graph structure to hold the SLAM 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< Registration< PointT, PointT, float >> 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
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.
~ELCH() override=default
Empty destructor.
void addPointCloud(PointCloudPtr cloud)
Add a new point cloud to the internal graph.
LoopGraphPtr getLoopGraph()
Getter for the internal graph.
shared_ptr< const Registration< PointT, PointT, float >> ConstPtr
typename PointCloud::ConstPtr PointCloudConstPtr
shared_ptr< const ELCH< PointT >> ConstPtr
shared_ptr< ELCH< PointT >> Ptr