initialize('web'); $snip = $modx->runSnippet("getSiteNavigation", array('id'=>5, 'phLevels'=>'sitenav.level0,sitenav.level1', 'showPageNav'=>'n')); $chunkOutput = $modx->getChunk("site-header", array('sitenav'=>$snip)); $bodytag = str_replace("[[+showSubmenus:notempty=`", "", $chunkOutput); $bodytag = str_replace("`]]", "", $bodytag); echo $bodytag; echo "\n"; ?>
Provide a common interface/architecture for all of these and future SLAM ideas.
Note
These ideas are independent of actual data structures in the PCL for now. We can see later how to integrate them best.
struct Pose
{
Eigen::Vector3 translation;
Eigen::Quaternion rotation;
}
typedef vector<vector <float> > Points;
typedef pair<Pose*, PointCloud*> PosedPointCloud;
PointCloud* can be 0.
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.
typedef Eigen::Matrix4f CovarianceMatrix;
struct Measurement
{
Pose pose;
CovarianceMatrix covariance;
}
Idea: change the CovarianceMatrix into a function pointer.
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 (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?
class GraphOptimizer
{
public:
virtual void optimize (Graph &gr) = 0;
}
class LoopDetection
{
public:
virtual ~LoopDetection () {}
virtual list<pair<PointCloud*, PointCloud*> > detectLoop(list<PosedPointCloud*> poses, list<PosedPointCloud*> query) {} = 0;
}
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.
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_;
}
class DistanceLoopDetection : LoopDetection
{
public:
virtual list<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_)
{
//..
}
}
}
}
class ELCH : public GlobalRegistration
{
public:
ELCH(GlobalRegistration &initial_optimizer = PairwiseGlobalRegistration(), LoopDetection &loop_detection, GraphOptimizer &loop_optimizer, GraphOptimizer &graph_optimizer = 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)