Point Cloud Library (PCL) 1.12.0
Loading...
Searching...
No Matches
lum.hpp
1/*
2 * Software License Agreement (BSD License)
3 *
4 * Point Cloud Library (PCL) - www.pointclouds.org
5 * Copyright (c) 2010-2012, Willow Garage, Inc.
6 * Copyright (c) 2012-, Open Perception, Inc.
7 *
8 * All rights reserved.
9 *
10 * Redistribution and use in source and binary forms, with or without
11 * modification, are permitted provided that the following conditions
12 * are met:
13 *
14 * * Redistributions of source code must retain the above copyright
15 * notice, this list of conditions and the following disclaimer.
16 * * Redistributions in binary form must reproduce the above
17 * copyright notice, this list of conditions and the following
18 * disclaimer in the documentation and/or other materials provided
19 * with the distribution.
20 * * Neither the name of the copyright holder(s) nor the names of its
21 * contributors may be used to endorse or promote products derived
22 * from this software without specific prior written permission.
23 *
24 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
25 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
26 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
27 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
28 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
29 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
30 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
31 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
32 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
33 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
34 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
35 * POSSIBILITY OF SUCH DAMAGE.
36 *
37 * $Id: lum.hpp 5663 2012-05-02 13:49:39Z florentinus $
38 *
39 */
40
41#ifndef PCL_REGISTRATION_IMPL_LUM_HPP_
42#define PCL_REGISTRATION_IMPL_LUM_HPP_
43
44#include <tuple>
45
46namespace pcl {
47
48namespace registration {
49
50template <typename PointT>
51inline void
56
57template <typename PointT>
58inline typename LUM<PointT>::SLAMGraphPtr
60{
61 return (slam_graph_);
62}
63
64template <typename PointT>
67{
68 return (num_vertices(*slam_graph_));
69}
70
71template <typename PointT>
72void
77
78template <typename PointT>
79inline int
81{
82 return (max_iterations_);
83}
84
85template <typename PointT>
86void
91
92template <typename PointT>
93inline float
95{
96 return (convergence_threshold_);
97}
98
99template <typename PointT>
100typename LUM<PointT>::Vertex
102{
103 Vertex v = add_vertex(*slam_graph_);
104 (*slam_graph_)[v].cloud_ = cloud;
105 if (v == 0 && pose != Eigen::Vector6f::Zero()) {
106 PCL_WARN(
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();
110 return (v);
111 }
112 (*slam_graph_)[v].pose_ = pose;
113 return (v);
114}
115
116template <typename PointT>
117inline void
119{
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");
123 return;
124 }
125 (*slam_graph_)[vertex].cloud_ = cloud;
126}
127
128template <typename PointT>
129inline typename LUM<PointT>::PointCloudPtr
131{
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");
135 return (PointCloudPtr());
136 }
137 return ((*slam_graph_)[vertex].cloud_);
138}
139
140template <typename PointT>
141inline void
143{
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");
147 return;
148 }
149 if (vertex == 0) {
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");
152 return;
153 }
154 (*slam_graph_)[vertex].pose_ = pose;
155}
156
157template <typename PointT>
158inline Eigen::Vector6f
160{
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());
165 }
166 return ((*slam_graph_)[vertex].pose_);
167}
168
169template <typename PointT>
170inline Eigen::Affine3f
172{
173 Eigen::Vector6f pose = getPose(vertex);
174 return (pcl::getTransformation(pose(0), pose(1), pose(2), pose(3), pose(4), pose(5)));
175}
176
177template <typename PointT>
178void
180 const Vertex& target_vertex,
182{
183 if (source_vertex >= getNumVertices() || target_vertex >= getNumVertices() ||
185 PCL_ERROR(
186 "[pcl::registration::LUM::setCorrespondences] You are attempting to set a set "
187 "of correspondences between non-existing or identical graph vertices.\n");
188 return;
189 }
190 Edge e;
191 bool present;
192 std::tie(e, present) = edge(source_vertex, target_vertex, *slam_graph_);
193 if (!present)
194 std::tie(e, present) = add_edge(source_vertex, target_vertex, *slam_graph_);
195 (*slam_graph_)[e].corrs_ = corrs;
196}
197
198template <typename PointT>
201 const Vertex& target_vertex) const
202{
203 if (source_vertex >= getNumVertices() || target_vertex >= getNumVertices()) {
204 PCL_ERROR("[pcl::registration::LUM::getCorrespondences] You are attempting to get "
205 "a set of correspondences between non-existing graph vertices.\n");
206 return (pcl::CorrespondencesPtr());
207 }
208 Edge e;
209 bool present;
210 std::tie(e, present) = edge(source_vertex, target_vertex, *slam_graph_);
211 if (!present) {
212 PCL_ERROR("[pcl::registration::LUM::getCorrespondences] You are attempting to get "
213 "a set of correspondences from a non-existing graph edge.\n");
214 return (pcl::CorrespondencesPtr());
215 }
216 return ((*slam_graph_)[e].corrs_);
217}
218
219template <typename PointT>
220void
222{
223 int n = static_cast<int>(getNumVertices());
224 if (n < 2) {
225 PCL_ERROR("[pcl::registration::LUM::compute] The slam graph needs at least 2 "
226 "vertices.\n");
227 return;
228 }
229 for (int i = 0; i < max_iterations_; ++i) {
230 // Linearized computation of C^-1 and C^-1*D and convergence checking for all edges
231 // in the graph (results stored in slam_graph_)
232 typename SLAMGraph::edge_iterator e, e_end;
233 for (std::tie(e, e_end) = edges(*slam_graph_); e != e_end; ++e)
234 computeEdge(*e);
235
236 // Declare matrices G and B
237 Eigen::MatrixXf G = Eigen::MatrixXf::Zero(6 * (n - 1), 6 * (n - 1));
238 Eigen::VectorXf B = Eigen::VectorXf::Zero(6 * (n - 1));
239
240 // Start at 1 because 0 is the reference pose
241 for (int vi = 1; vi != n; ++vi) {
242 for (int vj = 0; vj != n; ++vj) {
243 // Attempt to use the forward edge, otherwise use backward edge, otherwise there
244 // was no edge
245 Edge e;
246 bool present1;
247 std::tie(e, present1) = edge(vi, vj, *slam_graph_);
248 if (!present1) {
249 bool present2;
250 std::tie(e, present2) = edge(vj, vi, *slam_graph_);
251 if (!present2)
252 continue;
253 }
254
255 // Fill in elements of G and B
256 if (vj > 0)
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_;
260 }
261 }
262
263 // Computation of the linear equation system: GX = B
264 // TODO investigate accuracy vs. speed tradeoff and find the best solving method for
265 // our type of linear equation (sparse)
266 Eigen::VectorXf X = G.colPivHouseholderQr().solve(B);
267
268 // Update the poses
269 float sum = 0.0;
270 for (int vi = 1; vi != n; ++vi) {
272 -incidenceCorrection(getPose(vi)).inverse() * X.segment(6 * (vi - 1), 6));
273 sum += difference_pose.norm();
274 setPose(vi, getPose(vi) + difference_pose);
275 }
276
277 // Convergence check
278 if (sum <= convergence_threshold_ * static_cast<float>(n - 1))
279 return;
280 }
281}
282
283template <typename PointT>
286{
287 PointCloudPtr out(new PointCloud);
289 return (out);
290}
291
292template <typename PointT>
295{
296 PointCloudPtr out(new PointCloud);
297 typename SLAMGraph::vertex_iterator v, v_end;
298 for (std::tie(v, v_end) = vertices(*slam_graph_); v != v_end; ++v) {
300 pcl::transformPointCloud(*getPointCloud(*v), temp, getTransformation(*v));
301 *out += temp;
302 }
303 return (out);
304}
305
306template <typename PointT>
307void
309{
310 // Get necessary local data from graph
311 PointCloudPtr source_cloud = (*slam_graph_)[source(e, *slam_graph_)].cloud_;
312 PointCloudPtr target_cloud = (*slam_graph_)[target(e, *slam_graph_)].cloud_;
313 Eigen::Vector6f source_pose = (*slam_graph_)[source(e, *slam_graph_)].pose_;
314 Eigen::Vector6f target_pose = (*slam_graph_)[target(e, *slam_graph_)].pose_;
315 pcl::CorrespondencesPtr corrs = (*slam_graph_)[e].corrs_;
316
317 // Build the average and difference vectors for all correspondences
318 std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f>> corrs_aver(
319 corrs->size());
320 std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f>> corrs_diff(
321 corrs->size());
322 int oci = 0; // oci = output correspondence iterator
323 for (const auto& icorr : (*corrs)) // icorr = input correspondence
324 {
325 // Compound the point pair onto the current pose
326 Eigen::Vector3f source_compounded =
328 source_pose(1),
329 source_pose(2),
330 source_pose(3),
331 source_pose(4),
332 source_pose(5)) *
333 (*source_cloud)[icorr.index_query].getVector3fMap();
334 Eigen::Vector3f target_compounded =
336 target_pose(1),
337 target_pose(2),
338 target_pose(3),
339 target_pose(4),
340 target_pose(5)) *
341 (*target_cloud)[icorr.index_match].getVector3fMap();
342
343 // NaN points can not be passed to the remaining computational pipeline
344 if (!std::isfinite(source_compounded(0)) || !std::isfinite(source_compounded(1)) ||
345 !std::isfinite(source_compounded(2)) || !std::isfinite(target_compounded(0)) ||
346 !std::isfinite(target_compounded(1)) || !std::isfinite(target_compounded(2)))
347 continue;
348
349 // Compute the point pair average and difference and store for later use
352 oci++;
353 }
354 corrs_aver.resize(oci);
355 corrs_diff.resize(oci);
356
357 // Need enough valid correspondences to get a good triangulation
358 if (oci < 3) {
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 "
361 "computation.\n",
362 source(e, *slam_graph_),
363 target(e, *slam_graph_));
364 (*slam_graph_)[e].cinv_ = Eigen::Matrix6f::Zero();
365 (*slam_graph_)[e].cinvd_ = Eigen::Vector6f::Zero();
366 return;
367 }
368
369 // Build the matrices M'M and M'Z
370 Eigen::Matrix6f MM = Eigen::Matrix6f::Zero();
371 Eigen::Vector6f MZ = Eigen::Vector6f::Zero();
372 for (int ci = 0; ci != oci; ++ci) // ci = correspondence iterator
373 {
374 // Fast computation of summation elements of M'M
375 MM(0, 4) -= corrs_aver[ci](1);
376 MM(0, 5) += corrs_aver[ci](2);
377 MM(1, 3) -= corrs_aver[ci](2);
378 MM(1, 4) += corrs_aver[ci](0);
379 MM(2, 3) += corrs_aver[ci](1);
380 MM(2, 5) -= corrs_aver[ci](0);
381 MM(3, 4) -= corrs_aver[ci](0) * corrs_aver[ci](2);
382 MM(3, 5) -= corrs_aver[ci](0) * corrs_aver[ci](1);
383 MM(4, 5) -= corrs_aver[ci](1) * corrs_aver[ci](2);
384 MM(3, 3) +=
385 corrs_aver[ci](1) * corrs_aver[ci](1) + corrs_aver[ci](2) * corrs_aver[ci](2);
386 MM(4, 4) +=
387 corrs_aver[ci](0) * corrs_aver[ci](0) + corrs_aver[ci](1) * corrs_aver[ci](1);
388 MM(5, 5) +=
389 corrs_aver[ci](0) * corrs_aver[ci](0) + corrs_aver[ci](2) * corrs_aver[ci](2);
390
391 // Fast computation of M'Z
392 MZ(0) += corrs_diff[ci](0);
393 MZ(1) += corrs_diff[ci](1);
394 MZ(2) += corrs_diff[ci](2);
395 MZ(3) +=
396 corrs_aver[ci](1) * corrs_diff[ci](2) - corrs_aver[ci](2) * corrs_diff[ci](1);
397 MZ(4) +=
398 corrs_aver[ci](0) * corrs_diff[ci](1) - corrs_aver[ci](1) * corrs_diff[ci](0);
399 MZ(5) +=
400 corrs_aver[ci](2) * corrs_diff[ci](0) - corrs_aver[ci](0) * corrs_diff[ci](2);
401 }
402 // Remaining elements of M'M
403 MM(0, 0) = MM(1, 1) = MM(2, 2) = static_cast<float>(oci);
404 MM(4, 0) = MM(0, 4);
405 MM(5, 0) = MM(0, 5);
406 MM(3, 1) = MM(1, 3);
407 MM(4, 1) = MM(1, 4);
408 MM(3, 2) = MM(2, 3);
409 MM(5, 2) = MM(2, 5);
410 MM(4, 3) = MM(3, 4);
411 MM(5, 3) = MM(3, 5);
412 MM(5, 4) = MM(4, 5);
413
414 // Compute pose difference estimation
415 Eigen::Vector6f D = static_cast<Eigen::Vector6f>(MM.inverse() * MZ);
416
417 // Compute s^2
418 float ss = 0.0f;
419 for (int ci = 0; ci != oci; ++ci) // ci = correspondence iterator
420 ss += static_cast<float>(
421 std::pow(corrs_diff[ci](0) -
422 (D(0) + corrs_aver[ci](2) * D(5) - corrs_aver[ci](1) * D(4)),
423 2.0f) +
424 std::pow(corrs_diff[ci](1) -
425 (D(1) + corrs_aver[ci](0) * D(4) - corrs_aver[ci](2) * D(3)),
426 2.0f) +
427 std::pow(corrs_diff[ci](2) -
428 (D(2) + corrs_aver[ci](1) * D(3) - corrs_aver[ci](0) * D(5)),
429 2.0f));
430
431 // When reaching the limitations of computation due to linearization
432 if (ss < 0.0000000000001 || !std::isfinite(ss)) {
433 (*slam_graph_)[e].cinv_ = Eigen::Matrix6f::Zero();
434 (*slam_graph_)[e].cinvd_ = Eigen::Vector6f::Zero();
435 return;
436 }
437
438 // Store the results in the slam graph
439 (*slam_graph_)[e].cinv_ = MM * (1.0f / ss);
440 (*slam_graph_)[e].cinvd_ = MZ * (1.0f / ss);
441}
442
443template <typename PointT>
444inline Eigen::Matrix6f
446{
447 Eigen::Matrix6f out = Eigen::Matrix6f::Identity();
448 float cx = std::cos(pose(3)), sx = sinf(pose(3)), cy = std::cos(pose(4)),
449 sy = sinf(pose(4));
450 out(0, 4) = pose(1) * sx - pose(2) * cx;
451 out(0, 5) = pose(1) * cx * cy + pose(2) * sx * cy;
452 out(1, 3) = pose(2);
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;
458 out(3, 5) = sy;
459 out(4, 4) = sx;
460 out(4, 5) = cx * cy;
461 out(5, 4) = cx;
462 out(5, 5) = -sx * cy;
463 return (out);
464}
465
466} // namespace registration
467} // namespace pcl
468
469#define PCL_INSTANTIATE_LUM(T) template class PCL_EXPORTS pcl::registration::LUM<T>;
470
471#endif // PCL_REGISTRATION_IMPL_LUM_HPP_
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.
Definition lum.h:108
void compute()
Perform LUM's globally consistent scan matching.
Definition lum.hpp:221
float getConvergenceThreshold() const
Get the convergence threshold for the compute() method.
Definition lum.hpp:94
Eigen::Matrix6f incidenceCorrection(const Eigen::Vector6f &pose)
Returns a pose corrected 6DoF incidence matrix.
Definition lum.hpp:445
void computeEdge(const Edge &e)
Linearized computation of C^-1 and C^-1*D (results stored in slam_graph_).
Definition lum.hpp:308
shared_ptr< SLAMGraph > SLAMGraphPtr
Definition lum.h:136
PointCloudPtr getPointCloud(const Vertex &vertex) const
Return a point cloud from one of the SLAM graph's vertices.
Definition lum.hpp:130
SLAMGraphPtr getLoopGraph() const
Get the internal SLAM graph structure.
Definition lum.hpp:59
SLAMGraph::vertices_size_type getNumVertices() const
Get the number of vertices in the SLAM graph.
Definition lum.hpp:66
typename SLAMGraph::vertex_descriptor Vertex
Definition lum.h:137
void setConvergenceThreshold(float convergence_threshold)
Set the convergence threshold for the compute() method.
Definition lum.hpp:87
typename PointCloud::Ptr PointCloudPtr
Definition lum.h:114
void setMaxIterations(int max_iterations)
Set the maximum number of iterations for the compute() method.
Definition lum.hpp:73
PointCloudPtr getTransformedCloud(const Vertex &vertex) const
Return a point cloud from one of the SLAM graph's vertices compounded onto its current pose estimate.
Definition lum.hpp:285
PointCloudPtr getConcatenatedCloud() const
Return a concatenated point cloud of all the SLAM graph's point clouds compounded onto their current ...
Definition lum.hpp:294
int getMaxIterations() const
Get the maximum number of iterations for the compute() method.
Definition lum.hpp:80
void setPose(const Vertex &vertex, const Eigen::Vector6f &pose)
Change a pose estimate on one of the SLAM graph's vertices.
Definition lum.hpp:142
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.
Definition lum.hpp:200
Vertex addPointCloud(const PointCloudPtr &cloud, const Eigen::Vector6f &pose=Eigen::Vector6f::Zero())
Add a new point cloud to the SLAM graph.
Definition lum.hpp:101
void setPointCloud(const Vertex &vertex, const PointCloudPtr &cloud)
Change a point cloud on one of the SLAM graph's vertices.
Definition lum.hpp:118
Eigen::Vector6f getPose(const Vertex &vertex) const
Return a pose estimate from one of the SLAM graph's vertices.
Definition lum.hpp:159
Eigen::Affine3f getTransformation(const Vertex &vertex) const
Return a pose estimate from one of the SLAM graph's vertices as an affine transformation matrix.
Definition lum.hpp:171
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.
Definition lum.hpp:179
void setLoopGraph(const SLAMGraphPtr &slam_graph)
Set the internal SLAM graph structure.
Definition lum.hpp:52
typename SLAMGraph::edge_descriptor Edge
Definition lum.h:138
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)
Definition eigen.hpp:608
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.
@ B
Definition norms.h:54
Eigen::Matrix< float, 6, 1 > Vector6f
Definition lum.h:51
Eigen::Matrix< float, 6, 6 > Matrix6f
Definition lum.h:52
shared_ptr< Correspondences > CorrespondencesPtr