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_(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 &reg_;
    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)