Globally Consistent Scan Matching based on an algorithm by Lu and Milios. More...
#include <lum.h>
Classes | |
| struct | EdgeProperties |
| struct | VertexProperties |
Public Types | |
| typedef boost::shared_ptr < const LUM< PointT > > | ConstPtr |
| typedef SLAMGraph::edge_descriptor | Edge |
| typedef pcl::PointCloud< PointT > | PointCloud |
| typedef PointCloud::ConstPtr | PointCloudConstPtr |
| typedef PointCloud::Ptr | PointCloudPtr |
| typedef boost::shared_ptr< LUM < PointT > > | Ptr |
| typedef boost::adjacency_list < boost::eigen_vecS, boost::eigen_vecS, boost::bidirectionalS, VertexProperties, EdgeProperties, boost::no_property, boost::eigen_listS > | SLAMGraph |
| typedef boost::shared_ptr < SLAMGraph > | SLAMGraphPtr |
| typedef SLAMGraph::vertex_descriptor | Vertex |
Public Member Functions | |
| Vertex | addPointCloud (const PointCloudPtr &cloud, const Eigen::Vector6f &pose=Eigen::Vector6f::Zero()) |
| Add a new point cloud to the SLAM graph. | |
| void | compute () |
| Perform LUM's globally consistent scan matching. | |
| PointCloudPtr | getConcatenatedCloud () const |
| Return a concatenated point cloud of all the SLAM graph's point clouds compounded onto their current pose estimates. | |
| float | getConvergenceThreshold () const |
| Get the convergence threshold for the compute() method. | |
| pcl::CorrespondencesPtr | getCorrespondences (const Vertex &source_vertex, const Vertex &target_vertex) const |
| Return a set of correspondences from one of the SLAM graph's edges. | |
| SLAMGraphPtr | getLoopGraph () const |
| Get the internal SLAM graph structure. | |
| int | getMaxIterations () const |
| Get the maximum number of iterations for the compute() method. | |
| SLAMGraph::vertices_size_type | getNumVertices () const |
| Get the number of vertices in the SLAM graph. | |
| PointCloudPtr | getPointCloud (const Vertex &vertex) const |
| Return a point cloud from one of the SLAM graph's vertices. | |
| Eigen::Vector6f | getPose (const Vertex &vertex) const |
| Return a pose estimate from one of the SLAM graph's vertices. | |
| Eigen::Affine3f | getTransformation (const Vertex &vertex) const |
| Return a pose estimate from one of the SLAM graph's vertices as an affine transformation matrix. | |
| PointCloudPtr | getTransformedCloud (const Vertex &vertex) const |
| Return a point cloud from one of the SLAM graph's vertices compounded onto its current pose estimate. | |
| LUM () | |
| Empty constructor. | |
| void | setConvergenceThreshold (float convergence_threshold) |
| Set the convergence threshold for the compute() method. | |
| void | setCorrespondences (const Vertex &source_vertex, const Vertex &target_vertex, const pcl::CorrespondencesPtr &corrs) |
| Add/change a set of correspondences for one of the SLAM graph's edges. | |
| void | setLoopGraph (const SLAMGraphPtr &slam_graph) |
| Set the internal SLAM graph structure. | |
| void | setMaxIterations (int max_iterations) |
| Set the maximum number of iterations for the compute() method. | |
| void | setPointCloud (const Vertex &vertex, const PointCloudPtr &cloud) |
| Change a point cloud on one of the SLAM graph's vertices. | |
| void | setPose (const Vertex &vertex, const Eigen::Vector6f &pose) |
| Change a pose estimate on one of the SLAM graph's vertices. | |
Protected Member Functions | |
| void | computeEdge (const Edge &e) |
| Linearized computation of C^-1 and C^-1*D (results stored in slam_graph_). | |
| Eigen::Matrix6f | incidenceCorrection (const Eigen::Vector6f &pose) |
| Returns a pose corrected 6DoF incidence matrix. | |
Private Attributes | |
| float | convergence_threshold_ |
| The convergence threshold for the summed vector lengths of all poses. | |
| int | max_iterations_ |
| The maximum number of iterations for the compute() method. | |
| SLAMGraphPtr | slam_graph_ |
| The internal SLAM graph structure. | |
Globally Consistent Scan Matching based on an algorithm by Lu and Milios.
A GraphSLAM algorithm where registration data is managed in a graph:
Computation uses the first point cloud in the SLAM graph as a reference pose and attempts to align all other point clouds to it simultaneously. For more information:
Usage example:
pcl::registration::LUM<pcl::PointXYZ> lum; // Add point clouds as vertices to the SLAM graph lum.addPointCloud (cloud_0); lum.addPointCloud (cloud_1); lum.addPointCloud (cloud_2); // Use your favorite pairwise correspondence estimation algorithm(s) corrs_0_to_1 = someAlgo (cloud_0, cloud_1); corrs_1_to_2 = someAlgo (cloud_1, cloud_2); corrs_2_to_0 = someAlgo (lum.getPointCloud (2), lum.getPointCloud (0)); // Add the correspondence results as edges to the SLAM graph lum.setCorrespondences (0, 1, corrs_0_to_1); lum.setCorrespondences (1, 2, corrs_1_to_2); lum.setCorrespondences (2, 0, corrs_2_to_0); // Change the computation parameters lum.setMaxIterations (5); lum.setConvergenceThreshold (0.0); // Perform the actual LUM computation lum.compute (); // Return the concatenated point cloud result cloud_out = lum.getConcatenatedCloud (); // Return the separate point cloud transformations for(int i = 0; i < lum.getNumVertices (); i++) { transforms_out[i] = lum.getTransformation (i); }
| typedef boost::shared_ptr<const LUM<PointT> > pcl::registration::LUM< PointT >::ConstPtr |
| typedef SLAMGraph::edge_descriptor pcl::registration::LUM< PointT >::Edge |
| typedef pcl::PointCloud<PointT> pcl::registration::LUM< PointT >::PointCloud |
| typedef PointCloud::ConstPtr pcl::registration::LUM< PointT >::PointCloudConstPtr |
| typedef PointCloud::Ptr pcl::registration::LUM< PointT >::PointCloudPtr |
| typedef boost::shared_ptr<LUM<PointT> > pcl::registration::LUM< PointT >::Ptr |
| typedef boost::adjacency_list<boost::eigen_vecS, boost::eigen_vecS, boost::bidirectionalS, VertexProperties, EdgeProperties, boost::no_property, boost::eigen_listS> pcl::registration::LUM< PointT >::SLAMGraph |
| typedef boost::shared_ptr<SLAMGraph> pcl::registration::LUM< PointT >::SLAMGraphPtr |
| typedef SLAMGraph::vertex_descriptor pcl::registration::LUM< PointT >::Vertex |
| pcl::registration::LUM< PointT >::LUM | ( | ) | [inline] |
| pcl::registration::LUM< PointT >::Vertex pcl::registration::LUM< PointT >::addPointCloud | ( | const PointCloudPtr & | cloud, |
| const Eigen::Vector6f & | pose = Eigen::Vector6f::Zero () |
||
| ) |
Add a new point cloud to the SLAM graph.
This method will add a new vertex to the SLAM graph and attach a point cloud to that vertex. Optionally you can specify a pose estimate for this point cloud. A vertex' pose is always relative to the first vertex in the SLAM graph, i.e. the first point cloud that was added. Because this first vertex is the reference, you can not set a pose estimate for this vertex. Providing pose estimates to the vertices in the SLAM graph will reduce overall computation time of LUM.
| [in] | cloud | The new point cloud. |
| [in] | pose | (optional) The pose estimate relative to the reference pose (first point cloud that was added). |
| void pcl::registration::LUM< PointT >::compute | ( | ) |
Perform LUM's globally consistent scan matching.
Computation uses the first point cloud in the SLAM graph as a reference pose and attempts to align all other point clouds to it simultaneously.
Things to keep in mind:
Computation ends when either of the following conditions hold:
Computation will change the pose estimates for the vertices of the SLAM graph, not the point clouds attached to them. The results can be retrieved with getPose(), getTransformation(), getTransformedCloud() or getConcatenatedCloud().
| void pcl::registration::LUM< PointT >::computeEdge | ( | const Edge & | e | ) | [protected] |
| pcl::registration::LUM< PointT >::PointCloudPtr pcl::registration::LUM< PointT >::getConcatenatedCloud | ( | ) | const |
| float pcl::registration::LUM< PointT >::getConvergenceThreshold | ( | ) | const [inline] |
Get the convergence threshold for the compute() method.
When the compute() method computes the new poses relative to the old poses, it will determine the length of the difference vector. When the average length of all difference vectors becomes less than the convergence_threshold the convergence is assumed to be met.
| pcl::CorrespondencesPtr pcl::registration::LUM< PointT >::getCorrespondences | ( | const Vertex & | source_vertex, |
| const Vertex & | target_vertex | ||
| ) | const [inline] |
Return a set of correspondences from one of the SLAM graph's edges.
| [in] | source_vertex | The vertex descriptor of the correspondences' source point cloud. |
| [in] | target_vertex | The vertex descriptor of the correspondences' target point cloud. |
| pcl::registration::LUM< PointT >::SLAMGraphPtr pcl::registration::LUM< PointT >::getLoopGraph | ( | ) | const [inline] |
Get the internal SLAM graph structure.
All data used and produced by LUM is stored in this boost::adjacency_list. It is recommended to use the LUM class itself to build the graph. This method could otherwise be useful for managing several SLAM graphs in one instance of LUM.
| int pcl::registration::LUM< PointT >::getMaxIterations | ( | ) | const [inline] |
| pcl::registration::LUM< PointT >::SLAMGraph::vertices_size_type pcl::registration::LUM< PointT >::getNumVertices | ( | ) | const |
| pcl::registration::LUM< PointT >::PointCloudPtr pcl::registration::LUM< PointT >::getPointCloud | ( | const Vertex & | vertex | ) | const [inline] |
| Eigen::Vector6f pcl::registration::LUM< PointT >::getPose | ( | const Vertex & | vertex | ) | const [inline] |
| Eigen::Affine3f pcl::registration::LUM< PointT >::getTransformation | ( | const Vertex & | vertex | ) | const [inline] |
Return a pose estimate from one of the SLAM graph's vertices as an affine transformation matrix.
| [in] | vertex | The vertex descriptor of which to return the transformation matrix. |
| pcl::registration::LUM< PointT >::PointCloudPtr pcl::registration::LUM< PointT >::getTransformedCloud | ( | const Vertex & | vertex | ) | const |
Return a point cloud from one of the SLAM graph's vertices compounded onto its current pose estimate.
| [in] | vertex | The vertex descriptor of which to return the transformed point cloud. |
| Eigen::Matrix6f pcl::registration::LUM< PointT >::incidenceCorrection | ( | const Eigen::Vector6f & | pose | ) | [inline, protected] |
| void pcl::registration::LUM< PointT >::setConvergenceThreshold | ( | float | convergence_threshold | ) |
Set the convergence threshold for the compute() method.
When the compute() method computes the new poses relative to the old poses, it will determine the length of the difference vector. When the average length of all difference vectors becomes less than the convergence_threshold the convergence is assumed to be met.
| [in] | convergence_threshold | The new convergence threshold (default = 0.0). |
| void pcl::registration::LUM< PointT >::setCorrespondences | ( | const Vertex & | source_vertex, |
| const Vertex & | target_vertex, | ||
| const pcl::CorrespondencesPtr & | corrs | ||
| ) |
Add/change a set of correspondences for one of the SLAM graph's edges.
The edges in the SLAM graph are directional and point from source vertex to target vertex. The query indices of the correspondences, index the points at the source vertex' point cloud. The matching indices of the correspondences, index the points at the target vertex' point cloud. If no edge was present at the specified location, this method will add a new edge to the SLAM graph and attach the correspondences to that edge. If the edge was already present, this method will overwrite the correspondence information of that edge and will not alter the SLAM graph structure.
| [in] | source_vertex | The vertex descriptor of the correspondences' source point cloud. |
| [in] | target_vertex | The vertex descriptor of the correspondences' target point cloud. |
| [in] | corrs | The new set of correspondences for that edge. |
| void pcl::registration::LUM< PointT >::setLoopGraph | ( | const SLAMGraphPtr & | slam_graph | ) | [inline] |
Set the internal SLAM graph structure.
All data used and produced by LUM is stored in this boost::adjacency_list. It is recommended to use the LUM class itself to build the graph. This method could otherwise be useful for managing several SLAM graphs in one instance of LUM.
| [in] | slam_graph | The new SLAM graph. |
| void pcl::registration::LUM< PointT >::setMaxIterations | ( | int | max_iterations | ) |
| void pcl::registration::LUM< PointT >::setPointCloud | ( | const Vertex & | vertex, |
| const PointCloudPtr & | cloud | ||
| ) | [inline] |
Change a point cloud on one of the SLAM graph's vertices.
This method will change the point cloud attached to an existing vertex and will not alter the SLAM graph structure. Note that the correspondences attached to this vertex will not change and may need to be updated manually.
| [in] | vertex | The vertex descriptor of which to change the point cloud. |
| [in] | cloud | The new point cloud for that vertex. |
| void pcl::registration::LUM< PointT >::setPose | ( | const Vertex & | vertex, |
| const Eigen::Vector6f & | pose | ||
| ) | [inline] |
Change a pose estimate on one of the SLAM graph's vertices.
A vertex' pose is always relative to the first vertex in the SLAM graph, i.e. the first point cloud that was added. Because this first vertex is the reference, you can not set a pose estimate for this vertex. Providing pose estimates to the vertices in the SLAM graph will reduce overall computation time of LUM.
| [in] | vertex | The vertex descriptor of which to set the pose estimate. |
| [in] | pose | The new pose estimate for that vertex. |
float pcl::registration::LUM< PointT >::convergence_threshold_ [private] |
int pcl::registration::LUM< PointT >::max_iterations_ [private] |
SLAMGraphPtr pcl::registration::LUM< PointT >::slam_graph_ [private] |