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)