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>;
435 #endif // PCL_REGISTRATION_IMPL_LUM_HPP_
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.
typename PointCloud::Ptr PointCloudPtr
int getMaxIterations() const
Get 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...
Eigen::Matrix< float, 6, 6 > Matrix6f
void compute()
Perform LUM's globally consistent scan matching.
Globally Consistent Scan Matching based on an algorithm by Lu and Milios.
PointCloudPtr getPointCloud(const Vertex &vertex) const
Return a point cloud from one of the SLAM graph's vertices.
typename SLAMGraph::edge_descriptor Edge
void setLoopGraph(const SLAMGraphPtr &slam_graph)
Set the internal SLAM graph structure.
typename SLAMGraph::vertex_descriptor Vertex
Eigen::Matrix6f incidenceCorrection(const Eigen::Vector6f &pose)
Returns a pose corrected 6DoF incidence matrix.
PointCloudPtr getConcatenatedCloud() const
Return a concatenated point cloud of all the SLAM graph's point clouds compounded onto their current ...
Eigen::Matrix< float, 6, 1 > Vector6f
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.
Eigen::Vector6f getPose(const Vertex &vertex) const
Return a pose estimate from one of the SLAM graph's vertices.
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.
Eigen::Affine3f getTransformation(const Vertex &vertex) const
Return a pose estimate from one of the SLAM graph's vertices as an affine transformation matrix...
SLAMGraph::vertices_size_type getNumVertices() const
Get the number of vertices in the SLAM graph.
Vertex addPointCloud(const PointCloudPtr &cloud, const Eigen::Vector6f &pose=Eigen::Vector6f::Zero())
Add a new point cloud to the SLAM graph.
SLAMGraphPtr getLoopGraph() const
Get the internal SLAM graph structure.
void setPointCloud(const Vertex &vertex, const PointCloudPtr &cloud)
Change a point cloud on one of the SLAM graph's vertices.
shared_ptr< SLAMGraph > SLAMGraphPtr
PointCloud represents the base class in PCL for storing collections of 3D points. ...
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 setMaxIterations(int max_iterations)
Set the maximum number of iterations for the compute() method.
void computeEdge(const Edge &e)
Linearized computation of C^-1 and C^-1*D (results stored in slam_graph_).
void setConvergenceThreshold(float convergence_threshold)
Set the convergence threshold for the compute() method.
float getConvergenceThreshold() const
Get the convergence threshold for the compute() method.
shared_ptr< Correspondences > CorrespondencesPtr
void setPose(const Vertex &vertex, const Eigen::Vector6f &pose)
Change a pose estimate on one of the SLAM graph's vertices.