PCL/registration
Participants
Michael Dixon
Radu Rusu
Nicola Fioraio
Jochen Sprickerhof
Existing Frameworks
SLAM6D
Toro
Hogman
G2O
MegaSLAM/MegaICP
Mission
Provide a common interface/architecture for all of these and future SLAM ideas.
Ideas
Separate algorithms from data structures.
strip down everything to it’s basics and define an interface.
modify data structure in algorithms (you can copy them before if you need to).
point clouds are not transformed, only the translation and rotation is updated.
Data structures
Note
These ideas are independent of actual data structures in the PCL for now. We can see later how to integrate them best.
Pose
struct Pose
{
Eigen::Vector3 translation;
Eigen::Quaternion rotation;
}
PointCloud
typedef vector<vector <float> > Points;
PosedPointCloud
typedef pair<Pose*, PointCloud*> PosedPointCloud;
PointCloud* can be 0.
Graph
This should hold the SLAM graph. I would propose to use Boost::Graph for it, as it allows us to access a lot of algorithms.
Note
define abstract structure.
CovarianceMatrix
typedef Eigen::Matrix4f CovarianceMatrix;
Measurement
struct Measurement
{
Pose pose;
CovarianceMatrix covariance;
}
Idea: change the CovarianceMatrix into a function pointer.
Interfaces
GlobalRegistration
class GlobalRegistration
{
public:
/**
* \param history how many poses should be cached (0 means all)
*/
GlobalRegistration (int history = 0) : history_(history) {}
/**
* \param pc a new point cloud for GlobalRegistration
* \param pose the initial pose of the pc, could be 0 (unknown)
*/
void addPointCloud (PointCloud &pc, Pose &pose = 0)
{
new_clouds_.push_back (std::make_pair (pc, pose));
}
/**
* returns the current estimate of the transformation from point cloud from to point cloud to
throws an exception if the transformation is unknown
*/
Pose getTF (PointCloud &from, PointCloud &to);
/**
* run the optimization process
* \param lod the level of detail (optional). Roughly how long it should run (TODO: better name/parametrization?)
*/
virtual void compute (int lod = 0) {}
private:
int history_;
map<PointCloud*, Pose*> poses_;
PosedPointCloud new_clouds_;
};
This will be the base class interface for every SLAM algorithm. At any point you can add point clouds and they will be processed. The poses can be either in a global or in a local coordinate system (meaning that they are incremental regarding the last one). Idea: Do we need the compute? Could it be included into the addPointCloud or getTF?
GraphOptimizer
class GraphOptimizer
{
public:
virtual void optimize (Graph &gr) = 0;
}
LoopDetection
class LoopDetection
{
public:
virtual ~LoopDetection () {}
virtual list<std::pair<PointCloud*, PointCloud*> > detectLoop(list<PosedPointCloud*> poses, list<PosedPointCloud*> query) {} = 0;
}
GraphHandler
class GraphHandler
{
void addPose (Graph &gr, PointCloud &pc);
void addConstraint (Graph &gr, PointCloud &from, PointCloud &to, Pose &pose);
}
Note
I’m not sure about this one.
Example Implementations
PairwiseGlobalRegistration
class PairwiseGlobalRegistration : public GlobalRegistration
{
public:
PairwiseGlobalRegistration(Registration ®) : reg_(reg) {}
virtual void compute (int lod = 0) {}
{
list<PosedPointCloud >::iterator cloud_it;
for (cloud_it = new_clouds_.begin(); cloud_it != new_clouds_.end(); cloud_it++)
{
if(!old_) {
old = *cloud_it;
continue;
}
reg_.align(old_, *cloud_it, transformation);
poses[*cloud_it] = transformation;
old_ = *cloud_it;
}
new_clouds_.clear();
}
private:
Registration ®_;
PointCloud &old_;
}
DistanceLoopDetection
class DistanceLoopDetection : LoopDetection
{
public:
virtual list<std::pair<PointCloud*, PointCloud*> > detectLoop(list<PosedPointCloud*> poses, list<PosedPointCloud*> query)
{
//I want a map reduce here ;)
list<PosedPointCloud >::iterator poses_it;
for (poses_it = poses.begin(); poses_it != poses.end(); poses_it++)
{
list<PosedPointCloud >::iterator query_it;
for (query_it = query.begin(); query_it != query.end(); query_it++)
{
if (dist (*poses_it, *query_it) < min_dist_)
{
//..
}
}
}
}
ELCH
class ELCH : public GlobalRegistration
{
public:
ELCH(GlobalRegistration &initial_optimizer = PairwiseGlobalRegistration(), LoopDetection &loop_detection, GraphOptimizer &loop_optimizer, GraphOptimizer &graph_optimizer = LUM())
}
LUM
class ELCH : public GlobalRegistration
{
public:
ELCH(GlobalRegistration &initial_optimizer = PairwiseGlobalRegistration(), LoopDetection &loop_detection, GraphOptimizer &loop_optimizer, GraphOptimizer &graph_optimizer)
}
Lu and Milios style scan matching (as in SLAM6D)