41#ifndef PCL_REGISTRATION_IMPL_LUM_HPP_
42#define PCL_REGISTRATION_IMPL_LUM_HPP_
48namespace registration {
50template <
typename Po
intT>
57template <
typename Po
intT>
64template <
typename Po
intT>
71template <
typename Po
intT>
78template <
typename Po
intT>
82 return (max_iterations_);
85template <
typename Po
intT>
92template <
typename Po
intT>
96 return (convergence_threshold_);
99template <
typename Po
intT>
104 (*slam_graph_)[v].cloud_ = cloud;
105 if (v == 0 && pose != Eigen::Vector6f::Zero()) {
107 "[pcl::registration::LUM::addPointCloud] The pose estimate is ignored for the "
108 "first cloud in the graph since that will become the reference pose.\n");
109 (*slam_graph_)[v].pose_ = Eigen::Vector6f::Zero();
112 (*slam_graph_)[v].pose_ = pose;
116template <
typename Po
intT>
120 if (
vertex >= getNumVertices()) {
121 PCL_ERROR(
"[pcl::registration::LUM::setPointCloud] You are attempting to set a "
122 "point cloud to a non-existing graph vertex.\n");
125 (*slam_graph_)[
vertex].cloud_ = cloud;
128template <
typename Po
intT>
132 if (
vertex >= getNumVertices()) {
133 PCL_ERROR(
"[pcl::registration::LUM::getPointCloud] You are attempting to get a "
134 "point cloud from a non-existing graph vertex.\n");
137 return ((*slam_graph_)[
vertex].cloud_);
140template <
typename Po
intT>
144 if (
vertex >= getNumVertices()) {
145 PCL_ERROR(
"[pcl::registration::LUM::setPose] You are attempting to set a pose "
146 "estimate to a non-existing graph vertex.\n");
150 PCL_ERROR(
"[pcl::registration::LUM::setPose] The pose estimate is ignored for the "
151 "first cloud in the graph since that will become the reference pose.\n");
154 (*slam_graph_)[
vertex].pose_ = pose;
157template <
typename Po
intT>
161 if (
vertex >= getNumVertices()) {
162 PCL_ERROR(
"[pcl::registration::LUM::getPose] You are attempting to get a pose "
163 "estimate from a non-existing graph vertex.\n");
164 return (Eigen::Vector6f::Zero());
166 return ((*slam_graph_)[
vertex].pose_);
169template <
typename Po
intT>
170inline Eigen::Affine3f
177template <
typename Po
intT>
186 "[pcl::registration::LUM::setCorrespondences] You are attempting to set a set "
187 "of correspondences between non-existing or identical graph vertices.\n");
195 (*slam_graph_)[
e].corrs_ =
corrs;
198template <
typename Po
intT>
204 PCL_ERROR(
"[pcl::registration::LUM::getCorrespondences] You are attempting to get "
205 "a set of correspondences between non-existing graph vertices.\n");
212 PCL_ERROR(
"[pcl::registration::LUM::getCorrespondences] You are attempting to get "
213 "a set of correspondences from a non-existing graph edge.\n");
216 return ((*slam_graph_)[
e].corrs_);
219template <
typename Po
intT>
223 int n =
static_cast<int>(getNumVertices());
225 PCL_ERROR(
"[pcl::registration::LUM::compute] The slam graph needs at least 2 "
229 for (
int i = 0; i < max_iterations_; ++i) {
232 typename SLAMGraph::edge_iterator
e,
e_end;
233 for (std::tie(
e,
e_end) = edges(*slam_graph_);
e !=
e_end; ++
e)
237 Eigen::MatrixXf
G = Eigen::MatrixXf::Zero(6 * (n - 1), 6 * (n - 1));
238 Eigen::VectorXf
B = Eigen::VectorXf::Zero(6 * (n - 1));
241 for (
int vi = 1;
vi != n; ++
vi) {
242 for (
int vj = 0;
vj != n; ++
vj) {
257 G.block(6 * (
vi - 1), 6 * (
vj - 1), 6, 6) = -(*slam_graph_)[
e].cinv_;
258 G.block(6 * (
vi - 1), 6 * (
vi - 1), 6, 6) += (*slam_graph_)[
e].cinv_;
259 B.segment(6 * (
vi - 1), 6) += (
present1 ? 1 : -1) * (*slam_graph_)[
e].cinvd_;
266 Eigen::VectorXf X =
G.colPivHouseholderQr().solve(
B);
270 for (
int vi = 1;
vi != n; ++
vi) {
272 -incidenceCorrection(getPose(
vi)).inverse() * X.segment(6 * (
vi - 1), 6));
278 if (
sum <= convergence_threshold_ *
static_cast<float>(n - 1))
283template <
typename Po
intT>
292template <
typename Po
intT>
297 typename SLAMGraph::vertex_iterator v, v_end;
298 for (std::tie(v, v_end) = vertices(*slam_graph_); v != v_end; ++v) {
306template <
typename Po
intT>
318 std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f>>
corrs_aver(
320 std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f>>
corrs_diff(
323 for (
const auto&
icorr : (*corrs))
333 (*source_cloud)[
icorr.index_query].getVector3fMap();
341 (*target_cloud)[
icorr.index_match].getVector3fMap();
359 PCL_WARN(
"[pcl::registration::LUM::compute] The correspondences between vertex %d "
360 "and %d do not contain enough valid correspondences to be considered for "
364 (*slam_graph_)[
e].cinv_ = Eigen::Matrix6f::Zero();
365 (*slam_graph_)[
e].cinvd_ = Eigen::Vector6f::Zero();
403 MM(0, 0) =
MM(1, 1) =
MM(2, 2) =
static_cast<float>(
oci);
420 ss +=
static_cast<float>(
432 if (
ss < 0.0000000000001 || !std::isfinite(
ss)) {
433 (*slam_graph_)[
e].cinv_ = Eigen::Matrix6f::Zero();
434 (*slam_graph_)[
e].cinvd_ = Eigen::Vector6f::Zero();
439 (*slam_graph_)[
e].cinv_ =
MM * (1.0f /
ss);
440 (*slam_graph_)[
e].cinvd_ =
MZ * (1.0f /
ss);
443template <
typename Po
intT>
448 float cx = std::cos(pose(3)),
sx =
sinf(pose(3)), cy = std::cos(pose(4)),
450 out(0, 4) = pose(1) *
sx - pose(2) * cx;
451 out(0, 5) = pose(1) * cx * cy + pose(2) *
sx * cy;
453 out(1, 4) = -pose(0) *
sx;
454 out(1, 5) = -pose(0) * cx * cy + pose(2) *
sy;
455 out(2, 3) = -pose(1);
456 out(2, 4) = pose(0) * cx;
457 out(2, 5) = -pose(0) *
sx * cy - pose(1) *
sy;
462 out(5, 5) = -
sx * cy;
469#define PCL_INSTANTIATE_LUM(T) template class PCL_EXPORTS pcl::registration::LUM<T>;
Iterator class for point clouds with or without given indices.
std::size_t size() const
Size of the range the iterator is going through.
PointCloud represents the base class in PCL for storing collections of 3D points.
Globally Consistent Scan Matching based on an algorithm by Lu and Milios.
void compute()
Perform LUM's globally consistent scan matching.
float getConvergenceThreshold() const
Get the convergence threshold for the compute() method.
Eigen::Matrix6f incidenceCorrection(const Eigen::Vector6f &pose)
Returns a pose corrected 6DoF incidence matrix.
void computeEdge(const Edge &e)
Linearized computation of C^-1 and C^-1*D (results stored in slam_graph_).
shared_ptr< SLAMGraph > SLAMGraphPtr
PointCloudPtr getPointCloud(const Vertex &vertex) const
Return a point cloud from one of the SLAM graph's vertices.
SLAMGraphPtr getLoopGraph() const
Get the internal SLAM graph structure.
SLAMGraph::vertices_size_type getNumVertices() const
Get the number of vertices in the SLAM graph.
typename SLAMGraph::vertex_descriptor Vertex
void setConvergenceThreshold(float convergence_threshold)
Set the convergence threshold for the compute() method.
typename PointCloud::Ptr PointCloudPtr
void setMaxIterations(int max_iterations)
Set the maximum number of iterations for the compute() method.
PointCloudPtr getTransformedCloud(const Vertex &vertex) const
Return a point cloud from one of the SLAM graph's vertices compounded onto its current pose estimate.
PointCloudPtr getConcatenatedCloud() const
Return a concatenated point cloud of all the SLAM graph's point clouds compounded onto their current ...
int getMaxIterations() const
Get the maximum number of iterations for the compute() method.
void setPose(const Vertex &vertex, const Eigen::Vector6f &pose)
Change a pose estimate on one of the SLAM graph's vertices.
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.
Vertex addPointCloud(const PointCloudPtr &cloud, const Eigen::Vector6f &pose=Eigen::Vector6f::Zero())
Add a new point cloud to the SLAM graph.
void setPointCloud(const Vertex &vertex, const PointCloudPtr &cloud)
Change a point cloud on 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.
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.
typename SLAMGraph::edge_descriptor Edge
void getTransformation(Scalar x, Scalar y, Scalar z, Scalar roll, Scalar pitch, Scalar yaw, Eigen::Transform< Scalar, 3, Eigen::Affine > &t)
Create a transformation from the given translation and Euler angles (XYZ-convention)
void transformPointCloud(const pcl::PointCloud< PointT > &cloud_in, pcl::PointCloud< PointT > &cloud_out, const Eigen::Matrix< Scalar, 4, 4 > &transform, bool copy_all_fields)
Apply a rigid transform defined by a 4x4 matrix.
Eigen::Matrix< float, 6, 1 > Vector6f
Eigen::Matrix< float, 6, 6 > Matrix6f
shared_ptr< Correspondences > CorrespondencesPtr