1 /*elch.h 2 * Software License Agreement (BSD License) 3 * 4 * Point Cloud Library (PCL) - www.pointclouds.org 5 * Copyright (c) 2011, Willow Garage, Inc. 6 * Copyright (c) 2012-, Open Perception, Inc. 7 * 8 * All rights reserved. 9 * 10 * Redistribution and use in source and binary forms, with or without 11 * modification, are permitted provided that the following conditions 12 * are met: 13 * 14 * * Redistributions of source code must retain the above copyright 15 * notice, this list of conditions and the following disclaimer. 16 * * Redistributions in binary form must reproduce the above 17 * copyright notice, this list of conditions and the following 18 * disclaimer in the documentation and/or other materials provided 19 * with the distribution. 20 * * Neither the name of the copyright holder(s) nor the names of its 21 * contributors may be used to endorse or promote products derived 22 * from this software without specific prior written permission. 23 * 24 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 25 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 26 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 27 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 28 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 29 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 30 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 31 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 32 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 33 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 34 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 35 * POSSIBILITY OF SUCH DAMAGE. 36 * 37 * $Id$ 38 * 39 */ #ifndef PCL_ELCH_H_ #define PCL_ELCH_H_ #include <pcl/pcl_base.h> #include <pcl/point_types.h> #include <pcl/point_cloud.h> #include <pcl/registration/registration.h> #include <pcl/registration/boost.h> #include <pcl/registration/eigen.h> #include <pcl/registration/icp.h> #include <pcl/registration/boost_graph.h> namespace pcl { namespace registration { /** rief @b ELCH (Explicit Loop Closing Heuristic) class * author Jochen Sprickerhof * ingroup registration */ template <typename PointT> class ELCH : public PCLBase<PointT> { public: typedef boost::shared_ptr< ELCH<PointT> > Ptr; typedef boost::shared_ptr< const ELCH<PointT> > ConstPtr; typedef pcl::PointCloud<PointT> PointCloud; typedef typename PointCloud::Ptr PointCloudPtr; typedef typename PointCloud::ConstPtr PointCloudConstPtr; struct Vertex { Vertex () : cloud () {} PointCloudPtr cloud; }; /** rief graph structure to hold the SLAM graph */ typedef boost::adjacency_list< boost::listS, boost::eigen_vecS, boost::undirectedS, Vertex, boost::no_property> LoopGraph; typedef boost::shared_ptr< LoopGraph > LoopGraphPtr; typedef typename pcl::Registration<PointT, PointT> Registration; typedef typename Registration::Ptr RegistrationPtr; typedef typename Registration::ConstPtr RegistrationConstPtr; /** rief Empty constructor. */ ELCH () : loop_graph_ (new LoopGraph), loop_start_ (0), loop_end_ (0), reg_ (new pcl::IterativeClosestPoint<PointT, PointT>), loop_transform_ (), compute_loop_ (true), vd_ () {}; /** rief Empty destructor */ virtual ~ELCH () {} /** rief Add a new point cloud to the internal graph. * param[in] cloud the new point cloud */ inline void addPointCloud (PointCloudPtr cloud) { typename boost::graph_traits<LoopGraph>::vertex_descriptor vd = add_vertex (*loop_graph_); (*loop_graph_)[vd].cloud = cloud; if (num_vertices (*loop_graph_) > 1) add_edge (vd_, vd, *loop_graph_); vd_ = vd; } /** rief Getter for the internal graph. */ inline LoopGraphPtr getLoopGraph () { return (loop_graph_); } /** rief Setter for a new internal graph. * param[in] loop_graph the new graph */ inline void setLoopGraph (LoopGraphPtr loop_graph) { loop_graph_ = loop_graph; } /** rief Getter for the first scan of a loop. */ inline typename boost::graph_traits<LoopGraph>::vertex_descriptor getLoopStart () { return (loop_start_); } /** rief Setter for the first scan of a loop. * param[in] loop_start the scan that starts the loop */ inline void setLoopStart (const typename boost::graph_traits<LoopGraph>::vertex_descriptor &loop_start) { loop_start_ = loop_start; } /** rief Getter for the last scan of a loop. */ inline typename boost::graph_traits<LoopGraph>::vertex_descriptor getLoopEnd () { return (loop_end_); } /** rief Setter for the last scan of a loop. * param[in] loop_end the scan that ends the loop */ inline void setLoopEnd (const typename boost::graph_traits<LoopGraph>::vertex_descriptor &loop_end) { loop_end_ = loop_end; } /** rief Getter for the registration algorithm. */ inline RegistrationPtr getReg () { return (reg_); } /** rief Setter for the registration algorithm. * param[in] reg the registration algorithm used to compute the transformation between the start and the end of the loop */ inline void setReg (RegistrationPtr reg) { reg_ = reg; } /** rief Getter for the transformation between the first and the last scan. */ inline Eigen::Matrix4f getLoopTransform () { return (loop_transform_); } /** rief Setter for the transformation between the first and the last scan. * param[in] loop_transform the transformation between the first and the last scan */ inline void setLoopTransform (const Eigen::Matrix4f &loop_transform) { loop_transform_ = loop_transform; compute_loop_ = false; } /** rief Computes now poses for all point clouds by closing the loop * between start and end point cloud. This will transform all given point * clouds for now! */ void compute (); protected: using PCLBase<PointT>::deinitCompute; /** rief This method should get called before starting the actual computation. */ virtual bool initCompute (); private: /** rief graph structure for the internal optimization graph */ typedef boost::adjacency_list< boost::listS, boost::vecS, boost::undirectedS, boost::no_property, boost::property< boost::edge_weight_t, double > > LOAGraph; /** * graph balancer algorithm computes the weights * @param[in] g the graph * @param[in] f index of the first node * @param[in] l index of the last node * @param[out] weights array for the weights */ void loopOptimizerAlgorithm (LOAGraph &g, double *weights); /** rief The internal loop graph. */ LoopGraphPtr loop_graph_; /** rief The first scan of the loop. */ typename boost::graph_traits<LoopGraph>::vertex_descriptor loop_start_; /** rief The last scan of the loop. */ typename boost::graph_traits<LoopGraph>::vertex_descriptor loop_end_; /** rief The registration object used to close the loop. */ RegistrationPtr reg_; /** rief The transformation between that start and end of the loop. */ Eigen::Matrix4f loop_transform_; bool compute_loop_; /** rief previously added node in the loop_graph_. */ typename boost::graph_traits<LoopGraph>::vertex_descriptor vd_; public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; } } #include <pcl/registration/impl/elch.hpp> #endif // PCL_ELCH_H_
/* * Software License Agreement (BSD License) * * Point Cloud Library (PCL) - www.pointclouds.org * Copyright (c) 2011, Willow Garage, Inc. * Copyright (c) 2012-, Open Perception, Inc. * * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of the copyright holder(s) nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. * * $Id$ * */ #ifndef PCL_REGISTRATION_IMPL_ELCH_H_ #define PCL_REGISTRATION_IMPL_ELCH_H_ #include <list> #include <algorithm> #include <pcl/common/transforms.h> #include <pcl/registration/eigen.h> #include <pcl/registration/boost.h> #include <pcl/registration/registration.h> ////////////////////////////////////////////////////////////////////////////////////////////// template <typename PointT> void pcl::registration::ELCH<PointT>::loopOptimizerAlgorithm (LOAGraph &g, double *weights) { std::list<int> crossings, branches; crossings.push_back (static_cast<int> (loop_start_)); crossings.push_back (static_cast<int> (loop_end_)); weights[loop_start_] = 0; weights[loop_end_] = 1; int *p = new int[num_vertices (g)]; int *p_min = new int[num_vertices (g)]; double *d = new double[num_vertices (g)]; double *d_min = new double[num_vertices (g)]; double dist; bool do_swap = false; std::list<int>::iterator crossings_it, end_it, start_min, end_min; // process all junctions while (!crossings.empty ()) { dist = -1; // find shortest crossing for all vertices on the loop for (crossings_it = crossings.begin (); crossings_it != crossings.end (); ) { dijkstra_shortest_paths (g, *crossings_it, predecessor_map(boost::make_iterator_property_map(p, get(boost::vertex_index, g))). distance_map(boost::make_iterator_property_map(d, get(boost::vertex_index, g)))); end_it = crossings_it; end_it++; // find shortest crossing for one vertex for (; end_it != crossings.end (); end_it++) { if (*end_it != p[*end_it] && (dist < 0 || d[*end_it] < dist)) { dist = d[*end_it]; start_min = crossings_it; end_min = end_it; do_swap = true; } } if (do_swap) { std::swap (p, p_min); std::swap (d, d_min); do_swap = false; } // vertex starts a branch if (dist < 0) { branches.push_back (static_cast<int> (*crossings_it)); crossings_it = crossings.erase (crossings_it); } else crossings_it++; } if (dist > -1) { remove_edge (*end_min, p_min[*end_min], g); for (int i = p_min[*end_min]; i != *start_min; i = p_min[i]) { //even right with weights[*start_min] > weights[*end_min]! (math works) weights[i] = weights[*start_min] + (weights[*end_min] - weights[*start_min]) * d_min[i] / d_min[*end_min]; remove_edge (i, p_min[i], g); if (degree (i, g) > 0) { crossings.push_back (i); } } if (degree (*start_min, g) == 0) crossings.erase (start_min); if (degree (*end_min, g) == 0) crossings.erase (end_min); } } delete[] p; delete[] p_min; delete[] d; delete[] d_min; boost::graph_traits<LOAGraph>::adjacency_iterator adjacent_it, adjacent_it_end; int s; // error propagation while (!branches.empty ()) { s = branches.front (); branches.pop_front (); for (boost::tuples::tie (adjacent_it, adjacent_it_end) = adjacent_vertices (s, g); adjacent_it != adjacent_it_end; ++adjacent_it) { weights[*adjacent_it] = weights[s]; if (degree (*adjacent_it, g) > 1) branches.push_back (static_cast<int> (*adjacent_it)); } clear_vertex (s, g); } } ////////////////////////////////////////////////////////////////////////////////////////////// template <typename PointT> bool pcl::registration::ELCH<PointT>::initCompute () { /*if (!PCLBase<PointT>::initCompute ()) { PCL_ERROR ("[pcl::registration:ELCH::initCompute] Init failed. "); return (false); }*/ //TODO if (loop_end_ == 0) { PCL_ERROR ("[pcl::registration::ELCH::initCompute] no end of loop defined! "); deinitCompute (); return (false); } //compute transformation if it's not given if (compute_loop_) { PointCloudPtr meta_start (new PointCloud); PointCloudPtr meta_end (new PointCloud); *meta_start = *(*loop_graph_)[loop_start_].cloud; *meta_end = *(*loop_graph_)[loop_end_].cloud; typename boost::graph_traits<LoopGraph>::adjacency_iterator si, si_end; for (boost::tuples::tie (si, si_end) = adjacent_vertices (loop_start_, *loop_graph_); si != si_end; si++) *meta_start += *(*loop_graph_)[*si].cloud; for (boost::tuples::tie (si, si_end) = adjacent_vertices (loop_end_, *loop_graph_); si != si_end; si++) *meta_end += *(*loop_graph_)[*si].cloud; //TODO use real pose instead of centroid //Eigen::Vector4f pose_start; //pcl::compute3DCentroid (*(*loop_graph_)[loop_start_].cloud, pose_start); //Eigen::Vector4f pose_end; //pcl::compute3DCentroid (*(*loop_graph_)[loop_end_].cloud, pose_end); PointCloudPtr tmp (new PointCloud); //Eigen::Vector4f diff = pose_start - pose_end; //Eigen::Translation3f translation (diff.head (3)); //Eigen::Affine3f trans = translation * Eigen::Quaternionf::Identity (); //pcl::transformPointCloud (*(*loop_graph_)[loop_end_].cloud, *tmp, trans); reg_->setInputTarget (meta_start); reg_->setInputSource (meta_end); reg_->align (*tmp); loop_transform_ = reg_->getFinalTransformation (); //TODO hack //loop_transform_ *= trans.matrix (); } return (true); } ////////////////////////////////////////////////////////////////////////////////////////////// template <typename PointT> void pcl::registration::ELCH<PointT>::compute () { if (!initCompute ()) { return; } LOAGraph grb[4]; typename boost::graph_traits<LoopGraph>::edge_iterator edge_it, edge_it_end; for (boost::tuples::tie (edge_it, edge_it_end) = edges (*loop_graph_); edge_it != edge_it_end; edge_it++) { for (int j = 0; j < 4; j++) add_edge (source (*edge_it, *loop_graph_), target (*edge_it, *loop_graph_), 1, grb[j]); //TODO add variance } double *weights[4]; for (int i = 0; i < 4; i++) { weights[i] = new double[num_vertices (*loop_graph_)]; loopOptimizerAlgorithm (grb[i], weights[i]); } //TODO use pose //Eigen::Vector4f cend; //pcl::compute3DCentroid (*((*loop_graph_)[loop_end_].cloud), cend); //Eigen::Translation3f tend (cend.head (3)); //Eigen::Affine3f aend (tend); //Eigen::Affine3f aendI = aend.inverse (); //TODO iterate ovr loop_graph_ //typename boost::graph_traits<LoopGraph>::vertex_iterator vertex_it, vertex_it_end; //for (boost::tuples::tie (vertex_it, vertex_it_end) = vertices (*loop_graph_); vertex_it != vertex_it_end; vertex_it++) for (size_t i = 0; i < num_vertices (*loop_graph_); i++) { Eigen::Vector3f t2; t2[0] = loop_transform_ (0, 3) * static_cast<float> (weights[0][i]); t2[1] = loop_transform_ (1, 3) * static_cast<float> (weights[1][i]); t2[2] = loop_transform_ (2, 3) * static_cast<float> (weights[2][i]); Eigen::Affine3f bl (loop_transform_); Eigen::Quaternionf q (bl.rotation ()); Eigen::Quaternionf q2; q2 = Eigen::Quaternionf::Identity ().slerp (static_cast<float> (weights[3][i]), q); //TODO use rotation from branch start Eigen::Translation3f t3 (t2); Eigen::Affine3f a (t3 * q2); //a = aend * a * aendI; pcl::transformPointCloud (*(*loop_graph_)[i].cloud, *(*loop_graph_)[i].cloud, a); (*loop_graph_)[i].transform = a; } add_edge (loop_start_, loop_end_, *loop_graph_); deinitCompute ();//just return true } #endif // PCL_REGISTRATION_IMPL_ELCH_H_
/*
* Software License Agreement (BSD License)
*
* Point Cloud Library (PCL) - www.pointclouds.org
* Copyright (c) 2011, Willow Garage, Inc.
*
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the copyright holder(s) nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* $Id$
*
*/
#include <pcl/console/parse.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/common/transforms.h>
#include <pcl/registration/icp.h>
#include <pcl/registration/elch.h>
#include <iostream>
#include <string>
#include <vector>
typedef pcl::PointXYZ PointType;
typedef pcl::PointCloud<PointType> Cloud;
typedef Cloud::ConstPtr CloudConstPtr;
typedef Cloud::Ptr CloudPtr;
typedef std::pair<std::string, CloudPtr> CloudPair;
typedef std::vector<CloudPair> CloudVector;
bool
loopDetection (int end, const CloudVector &clouds, double dist, int &first, int &last)
{
static double min_dist = -1;
int state = 0;
for (int i = end-1; i > 0; i--)
{
Eigen::Vector4f cstart, cend;
//TODO use pose of scan
pcl::compute3DCentroid (*(clouds[i].second), cstart);
pcl::compute3DCentroid (*(clouds[end].second), cend);
Eigen::Vector4f diff = cend - cstart;
double norm = diff.norm ();
//std::cout << "distance between " << i << " and " << end << " is " << norm << " state is " << state << std::endl;
if (state == 0 && norm > dist)
{
state = 1;
//std::cout << "state 1" << std::endl;
}
if (state > 0 && norm < dist)
{
state = 2;
//std::cout << "loop detected between scan " << i << " (" << clouds[i].first << ") and scan " << end << " (" << clouds[end].first << ")" << std::endl;
if (min_dist < 0 || norm < min_dist)
{
min_dist = norm;
first = i;
last = end;
}
}
}
//std::cout << "min_dist: " << min_dist << " state: " << state << " first: " << first << " end: " << end << std::endl;
if (min_dist > 0 && (state < 2 || end == int (clouds.size ()) - 1)) //TODO
{
min_dist = -1;
return true;
}
return false;
}
int
main (int argc, char **argv)
{
double dist = 0.1;
pcl::console::parse_argument (argc, argv, "-d", dist);
double rans = 0.1;
pcl::console::parse_argument (argc, argv, "-r", rans);
int iter = 100;
pcl::console::parse_argument (argc, argv, "-i", iter);
pcl::registration::ELCH<PointType> elch;
pcl::IterativeClosestPoint<PointType, PointType>::Ptr icp (new pcl::IterativeClosestPoint<PointType, PointType>);
icp->setMaximumIterations (iter);
icp->setMaxCorrespondenceDistance (dist);
icp->setRANSACOutlierRejectionThreshold (rans);
elch.setReg (icp);
std::vector<int> pcd_indices;
pcd_indices = pcl::console::parse_file_extension_argument (argc, argv, ".pcd");
CloudVector clouds;
for (size_t i = 0; i < pcd_indices.size (); i++)
{
CloudPtr pc (new Cloud);
pcl::io::loadPCDFile (argv[pcd_indices[i]], *pc);
clouds.push_back (CloudPair (argv[pcd_indices[i]], pc));
std::cout << "loading file: " << argv[pcd_indices[i]] << " size: " << pc->size () << std::endl;
elch.addPointCloud (clouds[i].second);
}
int first = 0, last = 0;
for (size_t i = 0; i < clouds.size (); i++)
{
if (loopDetection (int (i), clouds, 3.0, first, last))
{
std::cout << "Loop between " << first << " (" << clouds[first].first << ") and " << last << " (" << clouds[last].first << ")" << std::endl;
elch.setLoopStart (first);
elch.setLoopEnd (last);
elch.compute ();
}
}
for (size_t i = 0; i < clouds.size (); i++)
{
std::string result_filename (clouds[i].first);
result_filename = result_filename.substr (result_filename.rfind ("/") + 1);
pcl::io::savePCDFileBinary (result_filename.c_str (), *(clouds[i].second));
std::cout << "saving result to " << result_filename << std::endl;
}
return 0;
}
上面这些是elch的基本代码，核心代码在hpp中，若要用ELCH进行优化，首先要将点云信息加入到节点中，（要注册号的点云），然后发现回环的时候，得到发现会换的点云序号，ELCH会进行回环的两端点云的ICP计算，通过插值的方法将点云进行调整。若要显示路径，则要记录注册时候的点云*改变后的点云。