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)
260 Eigen::Vector6f difference_pose = static_cast<Eigen::Vector6f> (-incidenceCorrection (getPose (vi)).inverse () * X.segment (6 * (vi - 1), 6));
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, Eigen::aligned_allocator<Eigen::Vector3f> > corrs_aver (corrs->size ());
308 std::vector<Eigen::Vector3f, Eigen::aligned_allocator<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_ 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.
Vertex addPointCloud(const PointCloudPtr &cloud, const Eigen::Vector6f &pose=Eigen::Vector6f::Zero())
Add a new point cloud to the SLAM graph.
Eigen::Matrix< float, 6, 6 > Matrix6f
PointCloud::Ptr PointCloudPtr
Eigen::Matrix< float, 6, 1 > Vector6f
PointCloudPtr getConcatenatedCloud() const
Return a concatenated point cloud of all the SLAM graph's point clouds compounded onto their current ...
void compute()
Perform LUM's globally consistent scan matching.
Globally Consistent Scan Matching based on an algorithm by Lu and Milios.
void setLoopGraph(const SLAMGraphPtr &slam_graph)
Set the internal SLAM graph structure.
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.
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::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=true)
Apply an affine transform defined by an Eigen Transform.
void setPointCloud(const Vertex &vertex, const PointCloudPtr &cloud)
Change a point cloud on one of the SLAM graph's vertices.
SLAMGraph::vertices_size_type getNumVertices() const
Get the number of vertices in the SLAM graph.
boost::shared_ptr< Correspondences > CorrespondencesPtr
PointCloud represents the base class in PCL for storing collections of 3D points.
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.
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::vertex_descriptor Vertex
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.
boost::shared_ptr< SLAMGraph > SLAMGraphPtr
float getConvergenceThreshold() const
Get the convergence threshold for the compute() method.
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.
void setPose(const Vertex &vertex, const Eigen::Vector6f &pose)
Change a pose estimate on one of the SLAM graph's vertices.