41 #ifndef PCL_REGISTRATION_IMPL_LUM_HPP_
42 #define PCL_REGISTRATION_IMPL_LUM_HPP_
48 namespace registration {
50 template <
typename Po
intT>
54 slam_graph_ = slam_graph;
57 template <
typename Po
intT>
64 template <
typename Po
intT>
68 return (num_vertices(*slam_graph_));
71 template <
typename Po
intT>
75 max_iterations_ = max_iterations;
78 template <
typename Po
intT>
82 return (max_iterations_);
85 template <
typename Po
intT>
89 convergence_threshold_ = convergence_threshold;
92 template <
typename Po
intT>
96 return (convergence_threshold_);
99 template <
typename Po
intT>
103 Vertex v = add_vertex(*slam_graph_);
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;
116 template <
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;
128 template <
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_);
140 template <
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;
157 template <
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_);
169 template <
typename Po
intT>
170 inline Eigen::Affine3f
177 template <
typename Po
intT>
180 const Vertex& target_vertex,
183 if (source_vertex >= getNumVertices() || target_vertex >= getNumVertices() ||
184 source_vertex == target_vertex) {
186 "[pcl::registration::LUM::setCorrespondences] You are attempting to set a set "
187 "of correspondences between non-existing or identical graph vertices.\n");
192 std::tie(e, present) = edge(source_vertex, target_vertex, *slam_graph_);
194 std::tie(e, present) = add_edge(source_vertex, target_vertex, *slam_graph_);
195 (*slam_graph_)[e].corrs_ = corrs;
198 template <
typename Po
intT>
201 const Vertex& target_vertex)
const
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");
210 std::tie(e, present) = edge(source_vertex, target_vertex, *slam_graph_);
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_);
219 template <
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) {
247 std::tie(e, present1) = edge(vi, vj, *slam_graph_);
250 std::tie(e, present2) = edge(vj, vi, *slam_graph_);
257 G.block<6, 6>(6 * (vi - 1), 6 * (vj - 1)) = -(*slam_graph_)[e].cinv_;
258 G.block<6, 6>(6 * (vi - 1), 6 * (vi - 1)) += (*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));
273 sum += difference_pose.norm();
274 setPose(vi, getPose(vi) + difference_pose);
278 if (sum <= convergence_threshold_ * static_cast<float>(n - 1))
283 template <
typename Po
intT>
292 template <
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) {
306 template <
typename Po
intT>
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_;
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))
326 Eigen::Vector3f source_compounded =
333 (*source_cloud)[icorr.index_query].getVector3fMap();
334 Eigen::Vector3f target_compounded =
341 (*target_cloud)[icorr.index_match].getVector3fMap();
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)))
350 corrs_aver[oci] = 0.5 * (source_compounded + target_compounded);
351 corrs_diff[oci] = source_compounded - target_compounded;
354 corrs_aver.resize(oci);
355 corrs_diff.resize(oci);
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 "
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();
372 for (
int ci = 0; ci != oci; ++ci)
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);
385 corrs_aver[ci](1) * corrs_aver[ci](1) + corrs_aver[ci](2) * corrs_aver[ci](2);
387 corrs_aver[ci](0) * corrs_aver[ci](0) + corrs_aver[ci](1) * corrs_aver[ci](1);
389 corrs_aver[ci](0) * corrs_aver[ci](0) + corrs_aver[ci](2) * corrs_aver[ci](2);
392 MZ(0) += corrs_diff[ci](0);
393 MZ(1) += corrs_diff[ci](1);
394 MZ(2) += corrs_diff[ci](2);
396 corrs_aver[ci](1) * corrs_diff[ci](2) - corrs_aver[ci](2) * corrs_diff[ci](1);
398 corrs_aver[ci](0) * corrs_diff[ci](1) - corrs_aver[ci](1) * corrs_diff[ci](0);
400 corrs_aver[ci](2) * corrs_diff[ci](0) - corrs_aver[ci](0) * corrs_diff[ci](2);
403 MM(0, 0) = MM(1, 1) = MM(2, 2) =
static_cast<float>(oci);
419 for (
int ci = 0; ci != oci; ++ci)
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)),
424 std::pow(corrs_diff[ci](1) -
425 (D(1) + corrs_aver[ci](0) * D(4) - corrs_aver[ci](2) * D(3)),
427 std::pow(corrs_diff[ci](2) -
428 (D(2) + corrs_aver[ci](1) * D(3) - corrs_aver[ci](0) * D(5)),
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);
443 template <
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>;
471 #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.
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.
PointCloud represents the base class in PCL for storing collections of 3D points. ...
shared_ptr< SLAMGraph > SLAMGraphPtr
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 (intrinsic rotations, ZYX-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 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.
void setPose(const Vertex &vertex, const Eigen::Vector6f &pose)
Change a pose estimate on one of the SLAM graph's vertices.