41 #ifndef PCL_REGISTRATION_IMPL_LUM_HPP_
42 #define PCL_REGISTRATION_IMPL_LUM_HPP_
50 namespace registration
53 template<
typename Po
intT>
inline void
56 slam_graph_ = slam_graph;
70 return (num_vertices (*slam_graph_));
74 template<
typename Po
intT>
void
77 max_iterations_ = max_iterations;
81 template<
typename Po
intT>
inline int
84 return (max_iterations_);
88 template<
typename Po
intT>
void
91 convergence_threshold_ = convergence_threshold;
95 template<
typename Po
intT>
inline float
98 return (convergence_threshold_);
105 Vertex v = add_vertex (*slam_graph_);
106 (*slam_graph_)[v].cloud_ = cloud;
107 if (v == 0 && pose != Eigen::Vector6f::Zero ())
109 PCL_WARN(
"[pcl::registration::LUM::addPointCloud] The pose estimate is ignored for the first cloud in the graph since that will become the reference pose.\n");
110 (*slam_graph_)[v].pose_ = Eigen::Vector6f::Zero ();
113 (*slam_graph_)[v].pose_ = pose;
118 template<
typename Po
intT>
inline void
121 if (vertex >= getNumVertices ())
123 PCL_ERROR(
"[pcl::registration::LUM::setPointCloud] You are attempting to set a point cloud to a non-existing graph vertex.\n");
126 (*slam_graph_)[vertex].cloud_ = cloud;
133 if (vertex >= getNumVertices ())
135 PCL_ERROR(
"[pcl::registration::LUM::getPointCloud] You are attempting to get a point cloud from a non-existing graph vertex.\n");
138 return ((*slam_graph_)[vertex].cloud_);
142 template<
typename Po
intT>
inline void
145 if (vertex >= getNumVertices ())
147 PCL_ERROR(
"[pcl::registration::LUM::setPose] You are attempting to set a pose estimate to a non-existing graph vertex.\n");
152 PCL_ERROR(
"[pcl::registration::LUM::setPose] The pose estimate is ignored for the first cloud in the graph since that will become the reference pose.\n");
155 (*slam_graph_)[vertex].pose_ = pose;
162 if (vertex >= getNumVertices ())
164 PCL_ERROR(
"[pcl::registration::LUM::getPose] You are attempting to get a pose estimate from a non-existing graph vertex.\n");
165 return (Eigen::Vector6f::Zero ());
167 return ((*slam_graph_)[vertex].pose_);
171 template<
typename Po
intT>
inline Eigen::Affine3f
179 template<
typename Po
intT>
void
182 if (source_vertex >= getNumVertices () || target_vertex >= getNumVertices () || source_vertex == target_vertex)
184 PCL_ERROR(
"[pcl::registration::LUM::setCorrespondences] You are attempting to set a set of correspondences between non-existing or identical graph vertices.\n");
189 std::tie (e, present) = edge (source_vertex, target_vertex, *slam_graph_);
191 std::tie (e, present) = add_edge (source_vertex, target_vertex, *slam_graph_);
192 (*slam_graph_)[e].corrs_ = corrs;
199 if (source_vertex >= getNumVertices () || target_vertex >= getNumVertices ())
201 PCL_ERROR(
"[pcl::registration::LUM::getCorrespondences] You are attempting to get a set of correspondences between non-existing graph vertices.\n");
206 std::tie (e, present) = edge (source_vertex, target_vertex, *slam_graph_);
209 PCL_ERROR(
"[pcl::registration::LUM::getCorrespondences] You are attempting to get a set of correspondences from a non-existing graph edge.\n");
212 return ((*slam_graph_)[e].corrs_);
216 template<
typename Po
intT>
void
219 int n =
static_cast<int> (getNumVertices ());
222 PCL_ERROR(
"[pcl::registration::LUM::compute] The slam graph needs at least 2 vertices.\n");
225 for (
int i = 0; i < max_iterations_; ++i)
228 typename SLAMGraph::edge_iterator e, e_end;
229 for (std::tie (e, e_end) = edges (*slam_graph_); e != e_end; ++e)
233 Eigen::MatrixXf G = Eigen::MatrixXf::Zero (6 * (n - 1), 6 * (n - 1));
234 Eigen::VectorXf
B = Eigen::VectorXf::Zero (6 * (n - 1));
237 for (
int vi = 1; vi != n; ++vi)
239 for (
int vj = 0; vj != n; ++vj)
244 std::tie (e, present1) = edge (vi, vj, *slam_graph_);
248 std::tie (e, present2) = edge (vj, vi, *slam_graph_);
255 G.block (6 * (vi - 1), 6 * (vj - 1), 6, 6) = -(*slam_graph_)[e].cinv_;
256 G.block (6 * (vi - 1), 6 * (vi - 1), 6, 6) += (*slam_graph_)[e].cinv_;
257 B.segment (6 * (vi - 1), 6) += (present1 ? 1 : -1) * (*slam_graph_)[e].cinvd_;
263 Eigen::VectorXf X = G.colPivHouseholderQr ().solve (
B);
267 for (
int vi = 1; vi != n; ++vi)
270 sum += difference_pose.norm ();
271 setPose (vi, getPose (vi) + difference_pose);
275 if (sum <= convergence_threshold_ *
static_cast<float> (n - 1))
294 typename SLAMGraph::vertex_iterator v, v_end;
295 for (std::tie (v, v_end) = vertices (*slam_graph_); v != v_end; ++v)
305 template<
typename Po
intT>
void
309 PointCloudPtr source_cloud = (*slam_graph_)[source (e, *slam_graph_)].cloud_;
310 PointCloudPtr target_cloud = (*slam_graph_)[target (e, *slam_graph_)].cloud_;
311 Eigen::Vector6f source_pose = (*slam_graph_)[source (e, *slam_graph_)].pose_;
312 Eigen::Vector6f target_pose = (*slam_graph_)[target (e, *slam_graph_)].pose_;
316 std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > corrs_aver (corrs->size ());
317 std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > corrs_diff (corrs->size ());
319 for (
int ici = 0; ici !=
static_cast<int> (corrs->size ()); ++ici)
322 Eigen::Vector3f source_compounded =
pcl::getTransformation (source_pose (0), source_pose (1), source_pose (2), source_pose (3), source_pose (4), source_pose (5)) * source_cloud->points[(*corrs)[ici].index_query].getVector3fMap ();
323 Eigen::Vector3f target_compounded =
pcl::getTransformation (target_pose (0), target_pose (1), target_pose (2), target_pose (3), target_pose (4), target_pose (5)) * target_cloud->points[(*corrs)[ici].index_match].getVector3fMap ();
326 if (!std::isfinite (source_compounded (0)) || !std::isfinite (source_compounded (1)) || !std::isfinite (source_compounded (2)) || !std::isfinite (target_compounded (0)) || !std::isfinite (target_compounded (1)) || !std::isfinite (target_compounded (2)))
330 corrs_aver[oci] = 0.5 * (source_compounded + target_compounded);
331 corrs_diff[oci] = source_compounded - target_compounded;
334 corrs_aver.resize (oci);
335 corrs_diff.resize (oci);
340 PCL_WARN(
"[pcl::registration::LUM::compute] The correspondences between vertex %d and %d do not contain enough valid correspondences to be considered for computation.\n", source (e, *slam_graph_), target (e, *slam_graph_));
341 (*slam_graph_)[e].cinv_ = Eigen::Matrix6f::Zero ();
342 (*slam_graph_)[e].cinvd_ = Eigen::Vector6f::Zero ();
349 for (
int ci = 0; ci != oci; ++ci)
352 MM (0, 4) -= corrs_aver[ci] (1);
353 MM (0, 5) += corrs_aver[ci] (2);
354 MM (1, 3) -= corrs_aver[ci] (2);
355 MM (1, 4) += corrs_aver[ci] (0);
356 MM (2, 3) += corrs_aver[ci] (1);
357 MM (2, 5) -= corrs_aver[ci] (0);
358 MM (3, 4) -= corrs_aver[ci] (0) * corrs_aver[ci] (2);
359 MM (3, 5) -= corrs_aver[ci] (0) * corrs_aver[ci] (1);
360 MM (4, 5) -= corrs_aver[ci] (1) * corrs_aver[ci] (2);
361 MM (3, 3) += corrs_aver[ci] (1) * corrs_aver[ci] (1) + corrs_aver[ci] (2) * corrs_aver[ci] (2);
362 MM (4, 4) += corrs_aver[ci] (0) * corrs_aver[ci] (0) + corrs_aver[ci] (1) * corrs_aver[ci] (1);
363 MM (5, 5) += corrs_aver[ci] (0) * corrs_aver[ci] (0) + corrs_aver[ci] (2) * corrs_aver[ci] (2);
366 MZ (0) += corrs_diff[ci] (0);
367 MZ (1) += corrs_diff[ci] (1);
368 MZ (2) += corrs_diff[ci] (2);
369 MZ (3) += corrs_aver[ci] (1) * corrs_diff[ci] (2) - corrs_aver[ci] (2) * corrs_diff[ci] (1);
370 MZ (4) += corrs_aver[ci] (0) * corrs_diff[ci] (1) - corrs_aver[ci] (1) * corrs_diff[ci] (0);
371 MZ (5) += corrs_aver[ci] (2) * corrs_diff[ci] (0) - corrs_aver[ci] (0) * corrs_diff[ci] (2);
374 MM (0, 0) = MM (1, 1) = MM (2, 2) =
static_cast<float> (oci);
375 MM (4, 0) = MM (0, 4);
376 MM (5, 0) = MM (0, 5);
377 MM (3, 1) = MM (1, 3);
378 MM (4, 1) = MM (1, 4);
379 MM (3, 2) = MM (2, 3);
380 MM (5, 2) = MM (2, 5);
381 MM (4, 3) = MM (3, 4);
382 MM (5, 3) = MM (3, 5);
383 MM (5, 4) = MM (4, 5);
390 for (
int ci = 0; ci != oci; ++ci)
391 ss +=
static_cast<float> (std::pow (corrs_diff[ci] (0) - (D (0) + corrs_aver[ci] (2) * D (5) - corrs_aver[ci] (1) * D (4)), 2.0f)
392 + std::pow (corrs_diff[ci] (1) - (D (1) + corrs_aver[ci] (0) * D (4) - corrs_aver[ci] (2) * D (3)), 2.0f)
393 + std::pow (corrs_diff[ci] (2) - (D (2) + corrs_aver[ci] (1) * D (3) - corrs_aver[ci] (0) * D (5)), 2.0f));
396 if (ss < 0.0000000000001 || !std::isfinite (ss))
398 (*slam_graph_)[e].cinv_ = Eigen::Matrix6f::Zero ();
399 (*slam_graph_)[e].cinvd_ = Eigen::Vector6f::Zero ();
404 (*slam_graph_)[e].cinv_ = MM * (1.0f / ss);
405 (*slam_graph_)[e].cinvd_ = MZ * (1.0f / ss);
413 float cx = std::cos (pose (3)), sx = sinf (pose (3)), cy = std::cos (pose (4)), sy = sinf (pose (4));
414 out (0, 4) = pose (1) * sx - pose (2) * cx;
415 out (0, 5) = pose (1) * cx * cy + pose (2) * sx * cy;
416 out (1, 3) = pose (2);
417 out (1, 4) = -pose (0) * sx;
418 out (1, 5) = -pose (0) * cx * cy + pose (2) * sy;
419 out (2, 3) = -pose (1);
420 out (2, 4) = pose (0) * cx;
421 out (2, 5) = -pose (0) * sx * cy - pose (1) * sy;
424 out (4, 5) = cx * cy;
426 out (5, 5) = -sx * cy;
433 #define PCL_INSTANTIATE_LUM(T) template class PCL_EXPORTS pcl::registration::LUM<T>;
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 transformPointCloud(const pcl::PointCloud< PointT > &cloud_in, pcl::PointCloud< PointT > &cloud_out, const Eigen::Transform< Scalar, 3, Eigen::Affine > &transform, bool copy_all_fields)
Apply an affine transform defined by an Eigen Transform.
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)
Eigen::Matrix< float, 6, 1 > Vector6f
Eigen::Matrix< float, 6, 6 > Matrix6f
shared_ptr< Correspondences > CorrespondencesPtr