41 #ifndef PCL_REGISTRATION_IMPL_LUM_HPP_
42 #define PCL_REGISTRATION_IMPL_LUM_HPP_
45 template<
typename Po
intT>
inline void
48 slam_graph_ = slam_graph;
62 return (num_vertices (*slam_graph_));
66 template<
typename Po
intT>
void
69 max_iterations_ = max_iterations;
73 template<
typename Po
intT>
inline int
76 return (max_iterations_);
80 template<
typename Po
intT>
void
83 convergence_threshold_ = convergence_threshold;
87 template<
typename Po
intT>
inline float
90 return (convergence_threshold_);
97 Vertex v = add_vertex (*slam_graph_);
98 (*slam_graph_)[v].cloud_ = cloud;
99 if (v == 0 && pose != Eigen::Vector6f::Zero ())
101 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");
102 (*slam_graph_)[v].pose_ = Eigen::Vector6f::Zero ();
105 (*slam_graph_)[v].pose_ = pose;
110 template<
typename Po
intT>
inline void
113 if (vertex >= getNumVertices ())
115 PCL_ERROR(
"[pcl::registration::LUM::setPointCloud] You are attempting to set a point cloud to a non-existing graph vertex.\n");
118 (*slam_graph_)[vertex].cloud_ = cloud;
125 if (vertex >= getNumVertices ())
127 PCL_ERROR(
"[pcl::registration::LUM::getPointCloud] You are attempting to get a point cloud from a non-existing graph vertex.\n");
130 return ((*slam_graph_)[vertex].cloud_);
134 template<
typename Po
intT>
inline void
137 if (vertex >= getNumVertices ())
139 PCL_ERROR(
"[pcl::registration::LUM::setPose] You are attempting to set a pose estimate to a non-existing graph vertex.\n");
144 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");
147 (*slam_graph_)[vertex].pose_ = pose;
154 if (vertex >= getNumVertices ())
156 PCL_ERROR(
"[pcl::registration::LUM::getPose] You are attempting to get a pose estimate from a non-existing graph vertex.\n");
157 return (Eigen::Vector6f::Zero ());
159 return ((*slam_graph_)[vertex].pose_);
163 template<
typename Po
intT>
inline Eigen::Affine3f
171 template<
typename Po
intT>
void
174 if (source_vertex >= getNumVertices () || target_vertex >= getNumVertices () || source_vertex == target_vertex)
176 PCL_ERROR(
"[pcl::registration::LUM::setCorrespondences] You are attempting to set a set of correspondences between non-existing or identical graph vertices.\n");
181 boost::tuples::tie (e, present) = edge (source_vertex, target_vertex, *slam_graph_);
183 boost::tuples::tie (e, present) = add_edge (source_vertex, target_vertex, *slam_graph_);
184 (*slam_graph_)[e].corrs_ = corrs;
191 if (source_vertex >= getNumVertices () || target_vertex >= getNumVertices ())
193 PCL_ERROR(
"[pcl::registration::LUM::getCorrespondences] You are attempting to get a set of correspondences between non-existing graph vertices.\n");
198 boost::tuples::tie (e, present) = edge (source_vertex, target_vertex, *slam_graph_);
201 PCL_ERROR(
"[pcl::registration::LUM::getCorrespondences] You are attempting to get a set of correspondences from a non-existing graph edge.\n");
204 return ((*slam_graph_)[e].corrs_);
208 template<
typename Po
intT>
void
211 int n =
static_cast<int> (getNumVertices ());
214 PCL_ERROR(
"[pcl::registration::LUM::compute] The slam graph needs at least 2 vertices.\n");
217 for (
int i = 0; i < max_iterations_; ++i)
220 typename SLAMGraph::edge_iterator e, e_end;
221 for (boost::tuples::tie (e, e_end) = edges (*slam_graph_); e != e_end; ++e)
225 Eigen::MatrixXf G = Eigen::MatrixXf::Zero (6 * (n - 1), 6 * (n - 1));
226 Eigen::VectorXf
B = Eigen::VectorXf::Zero (6 * (n - 1));
229 for (
int vi = 1; vi != n; ++vi)
231 for (
int vj = 0; vj != n; ++vj)
235 bool present1, present2;
236 boost::tuples::tie (e, present1) = edge (vi, vj, *slam_graph_);
239 boost::tuples::tie (e, present2) = edge (vj, vi, *slam_graph_);
246 G.block (6 * (vi - 1), 6 * (vj - 1), 6, 6) = -(*slam_graph_)[e].cinv_;
247 G.block (6 * (vi - 1), 6 * (vi - 1), 6, 6) += (*slam_graph_)[e].cinv_;
248 B.segment (6 * (vi - 1), 6) += (present1 ? 1 : -1) * (*slam_graph_)[e].cinvd_;
254 Eigen::VectorXf X = G.colPivHouseholderQr ().solve (B);
258 for (
int vi = 1; vi != n; ++vi)
261 sum += difference_pose.norm ();
262 setPose (vi, getPose (vi) + difference_pose);
266 if (sum <= convergence_threshold_ * static_cast<float> (n - 1))
285 typename SLAMGraph::vertex_iterator v, v_end;
286 for (boost::tuples::tie (v, v_end) = vertices (*slam_graph_); v != v_end; ++v)
296 template<
typename Po
intT>
void
300 PointCloudPtr source_cloud = (*slam_graph_)[source (e, *slam_graph_)].cloud_;
301 PointCloudPtr target_cloud = (*slam_graph_)[target (e, *slam_graph_)].cloud_;
302 Eigen::Vector6f source_pose = (*slam_graph_)[source (e, *slam_graph_)].pose_;
303 Eigen::Vector6f target_pose = (*slam_graph_)[target (e, *slam_graph_)].pose_;
307 std::vector <Eigen::Vector3f> corrs_aver (corrs->size ());
308 std::vector <Eigen::Vector3f> corrs_diff (corrs->size ());
310 for (
int ici = 0; ici !=
static_cast<int> (corrs->size ()); ++ici)
313 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 ();
314 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 ();
317 if (!pcl_isfinite (source_compounded (0)) || !pcl_isfinite (source_compounded (1)) || !pcl_isfinite (source_compounded (2)) || !pcl_isfinite (target_compounded (0)) || !pcl_isfinite (target_compounded (1)) || !pcl_isfinite (target_compounded (2)))
321 corrs_aver[oci] = 0.5 * (source_compounded + target_compounded);
322 corrs_diff[oci] = source_compounded - target_compounded;
325 corrs_aver.resize (oci);
326 corrs_diff.resize (oci);
331 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_));
332 (*slam_graph_)[e].cinv_ = Eigen::Matrix6f::Zero ();
333 (*slam_graph_)[e].cinvd_ = Eigen::Vector6f::Zero ();
340 for (
int ci = 0; ci != oci; ++ci)
343 MM (0, 4) -= corrs_aver[ci] (1);
344 MM (0, 5) += corrs_aver[ci] (2);
345 MM (1, 3) -= corrs_aver[ci] (2);
346 MM (1, 4) += corrs_aver[ci] (0);
347 MM (2, 3) += corrs_aver[ci] (1);
348 MM (2, 5) -= corrs_aver[ci] (0);
349 MM (3, 4) -= corrs_aver[ci] (0) * corrs_aver[ci] (2);
350 MM (3, 5) -= corrs_aver[ci] (0) * corrs_aver[ci] (1);
351 MM (4, 5) -= corrs_aver[ci] (1) * corrs_aver[ci] (2);
352 MM (3, 3) += corrs_aver[ci] (1) * corrs_aver[ci] (1) + corrs_aver[ci] (2) * corrs_aver[ci] (2);
353 MM (4, 4) += corrs_aver[ci] (0) * corrs_aver[ci] (0) + corrs_aver[ci] (1) * corrs_aver[ci] (1);
354 MM (5, 5) += corrs_aver[ci] (0) * corrs_aver[ci] (0) + corrs_aver[ci] (2) * corrs_aver[ci] (2);
357 MZ (0) += corrs_diff[ci] (0);
358 MZ (1) += corrs_diff[ci] (1);
359 MZ (2) += corrs_diff[ci] (2);
360 MZ (3) += corrs_aver[ci] (1) * corrs_diff[ci] (2) - corrs_aver[ci] (2) * corrs_diff[ci] (1);
361 MZ (4) += corrs_aver[ci] (0) * corrs_diff[ci] (1) - corrs_aver[ci] (1) * corrs_diff[ci] (0);
362 MZ (5) += corrs_aver[ci] (2) * corrs_diff[ci] (0) - corrs_aver[ci] (0) * corrs_diff[ci] (2);
365 MM (0, 0) = MM (1, 1) = MM (2, 2) =
static_cast<float> (oci);
366 MM (4, 0) = MM (0, 4);
367 MM (5, 0) = MM (0, 5);
368 MM (3, 1) = MM (1, 3);
369 MM (4, 1) = MM (1, 4);
370 MM (3, 2) = MM (2, 3);
371 MM (5, 2) = MM (2, 5);
372 MM (4, 3) = MM (3, 4);
373 MM (5, 3) = MM (3, 5);
374 MM (5, 4) = MM (4, 5);
381 for (
int ci = 0; ci != oci; ++ci)
382 ss += static_cast<float> (pow (corrs_diff[ci] (0) - (D (0) + corrs_aver[ci] (2) * D (5) - corrs_aver[ci] (1) * D (4)), 2.0f)
383 + pow (corrs_diff[ci] (1) - (D (1) + corrs_aver[ci] (0) * D (4) - corrs_aver[ci] (2) * D (3)), 2.0f)
384 + pow (corrs_diff[ci] (2) - (D (2) + corrs_aver[ci] (1) * D (3) - corrs_aver[ci] (0) * D (5)), 2.0f));
387 if (ss < 0.0000000000001 || !pcl_isfinite (ss))
389 (*slam_graph_)[e].cinv_ = Eigen::Matrix6f::Zero ();
390 (*slam_graph_)[e].cinvd_ = Eigen::Vector6f::Zero ();
395 (*slam_graph_)[e].cinv_ = MM * (1.0f / ss);
396 (*slam_graph_)[e].cinvd_ = MZ * (1.0f / ss);
404 float cx = cosf (pose (3)), sx = sinf (pose (3)), cy = cosf (pose (4)), sy = sinf (pose (4));
405 out (0, 4) = pose (1) * sx - pose (2) * cx;
406 out (0, 5) = pose (1) * cx * cy + pose (2) * sx * cy;
407 out (1, 3) = pose (2);
408 out (1, 4) = -pose (0) * sx;
409 out (1, 5) = -pose (0) * cx * cy + pose (2) * sy;
410 out (2, 3) = -pose (1);
411 out (2, 4) = pose (0) * cx;
412 out (2, 5) = -pose (0) * sx * cy - pose (1) * sy;
415 out (4, 5) = cx * cy;
417 out (5, 5) = -sx * cy;
421 #define PCL_INSTANTIATE_LUM(T) template class PCL_EXPORTS pcl::registration::LUM<T>;
423 #endif // PCL_REGISTRATION_IMPL_LUM_HPP_
Vertex addPointCloud(const PointCloudPtr &cloud, const Eigen::Vector6f &pose=Eigen::Vector6f::Zero())
Add a new point cloud to the SLAM graph.
boost::shared_ptr< Correspondences > CorrespondencesPtr
void setConvergenceThreshold(float convergence_threshold)
Set the convergence threshold for the compute() method.
SLAMGraph::edge_descriptor Edge
void setPointCloud(const Vertex &vertex, const PointCloudPtr &cloud)
Change a point cloud on one of the SLAM graph's vertices.
Globally Consistent Scan Matching based on an algorithm by Lu and Milios.
SLAMGraph::vertex_descriptor Vertex
SLAMGraph::vertices_size_type getNumVertices() const
Get the number of vertices in the SLAM graph.
void transformPointCloud(const pcl::PointCloud< PointT > &cloud_in, pcl::PointCloud< PointT > &cloud_out, const Eigen::Transform< Scalar, 3, Eigen::Affine > &transform)
Apply an affine transform defined by an Eigen Transform.
Eigen::Vector6f getPose(const Vertex &vertex) const
Return a pose estimate from one of the SLAM graph's vertices.
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 ...
float getConvergenceThreshold() const
Get the convergence threshold for the compute() method.
SLAMGraphPtr getLoopGraph() const
Get the internal SLAM graph structure.
Eigen::Matrix< float, 6, 1 > Vector6f
PointCloud::Ptr PointCloudPtr
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) ...
PointCloudPtr getTransformedCloud(const Vertex &vertex) const
Return a point cloud from one of the SLAM graph's vertices compounded onto its current pose estimate...
void setLoopGraph(const SLAMGraphPtr &slam_graph)
Set the internal SLAM graph structure.
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::Matrix< float, 6, 6 > Matrix6f
void setMaxIterations(int max_iterations)
Set 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.
void compute()
Perform LUM's globally consistent scan matching.
boost::shared_ptr< SLAMGraph > SLAMGraphPtr
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.
int getMaxIterations() const
Get 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_).
Eigen::Affine3f getTransformation(const Vertex &vertex) const
Return a pose estimate from one of the SLAM graph's vertices as an affine transformation matrix...
PointCloudPtr getPointCloud(const Vertex &vertex) const
Return a point cloud from one of the SLAM graph's vertices.