Anchor.h

Go to the documentation of this file.

#pragma once

#include "Node.h"
#include "Factor.h"
#include "Pose2d.h"
#include "Point2d.h"
#include "Pose3d.h"
#include "Slam.h"

namespace isam {

typedef NodeT<Pose2d> Pose2d_Node;
typedef NodeT<Pose3d> Pose3d_Node;

class Anchor2d_Node : public Pose2d_Node {
public:
  Anchor2d_Node(Slam* slam);
  ~Anchor2d_Node();

  void set_prior();

  void add_anchor(Anchor2d_Node* a);

  void merge(Anchor2d_Node* a, Pose2d old_origin);

  Anchor2d_Node* parent() { return _parent; }

private:
  Anchor2d_Node* _parent;
  std::vector<Anchor2d_Node*> _childs;
  Factor* _prior;
  Slam* _slam;
};

class Anchor3d_Node : public Pose3d_Node {
public:
  Anchor3d_Node(Slam* slam);
  ~Anchor3d_Node();

  void set_prior();

  void add_anchor(Anchor3d_Node* a);

  void merge(Anchor3d_Node* a, Pose3d old_origin);

  Anchor3d_Node* parent() { return _parent; }

private:
  Anchor3d_Node* _parent;
  std::vector<Anchor3d_Node*> _childs;
  Factor* _prior;
  Slam* _slam;
};

}