ReferenceFrame.h

Go to the documentation of this file.

#ifndef _GAMS_POSE_REFERENCE_FRAME_H_
#define _GAMS_POSE_REFERENCE_FRAME_H_

#include "gams/GamsExport.h"
#include "gams/CPP11_compat.h"
#include <vector>
#include <string>
#include <cstring>
#include <sstream>
#include <utility>
#include <memory>
#include <stdexcept>
#include <mutex>
#include "ReferenceFrameFwd.h"
#include "CartesianFrame.h"
#include "Pose.h"
#include "Quaternion.h"
#include "madara/knowledge/EvalSettings.h"

namespace gams { namespace pose {

class ReferenceFrameVersion;

class GAMS_EXPORT ReferenceFrameIdentity
{
private:
    std::string id_;

    static std::map<std::string,
        std::weak_ptr<ReferenceFrameIdentity>> idents_;

    static uint64_t default_expiry_;

    static std::mutex idents_lock_;

    mutable std::map<uint64_t, std::weak_ptr<ReferenceFrameVersion>>
      versions_;

    mutable uint64_t expiry_ = -1;

    mutable std::mutex versions_lock_;

public:
    ReferenceFrameIdentity(std::string id, uint64_t expiry)
      : id_(std::move(id)), expiry_(expiry) {}

    static std::shared_ptr<ReferenceFrameIdentity> lookup(std::string id);

    static std::shared_ptr<ReferenceFrameIdentity> find(std::string id);

    static std::shared_ptr<ReferenceFrameIdentity> make_guid();

    void register_version(uint64_t timestamp,
        std::shared_ptr<ReferenceFrameVersion> ver) const
    {
      std::lock_guard<std::mutex> guard(versions_lock_);

      std::weak_ptr<ReferenceFrameVersion> weak(std::move(ver));
      versions_[timestamp] = ver;
    }

    std::shared_ptr<ReferenceFrameVersion> get_version(uint64_t timestamp) const
    {
      std::lock_guard<std::mutex> guard(versions_lock_);

      auto find = versions_.find(timestamp);

      if (find == versions_.end()) {
        return nullptr;
      }

      return find->second.lock();
    }

    const std::string &id() const { return id_; }

    static uint64_t default_expiry(uint64_t age) {
      std::lock_guard<std::mutex> guard(idents_lock_);

      uint64_t ret = default_expiry_;
      default_expiry_ = age;
      return ret;
    }

    static uint64_t default_expiry() {
      std::lock_guard<std::mutex> guard(idents_lock_);
      return default_expiry_;
    }

    uint64_t expiry(uint64_t age) const {
      std::lock_guard<std::mutex> guard(versions_lock_);

      uint64_t ret = expiry_;
      expiry_ = age;
      return ret;
    }

    uint64_t expiry() const {
      std::lock_guard<std::mutex> guard(versions_lock_);
      return expiry_;
    }

    void expire_older_than(madara::knowledge::KnowledgeBase &kb,
        uint64_t time, const FrameEvalSettings &settings = FrameEvalSettings::DEFAULT) const;

    static const std::string &default_prefix() {
      return FrameEvalSettings::default_prefix();
    }

    static void gc();
};

namespace impl {
  inline static std::string &make_kb_key(
      std::string &prefix,
      const std::string &id)
  {
    prefix.reserve(prefix.capacity() + 1 + id.size());

    prefix += ".";
    prefix += id;

    return prefix;
  }

  inline static std::string &make_kb_key(
      std::string &prefix, uint64_t timestamp)
  {
    if (timestamp == (uint64_t)-1) {
      prefix += ".inf";
    } else {
      prefix += ".";

      std::ostringstream oss;
      oss.fill('0');
      oss.width(16);
      oss << std::hex << timestamp;

      prefix += oss.str();
    }

    return prefix;
  }

  inline static std::string &make_kb_key(
      std::string &prefix,
      const std::string &id, uint64_t timestamp)
  {
    make_kb_key(prefix, id);
    make_kb_key(prefix, timestamp);
    return prefix;
  }
}

MADARA_MAKE_VAL_SUPPORT_TEST(nano_timestamp, x,
    (x.nanos(), x.nanos(0UL)));

inline uint64_t try_get_nano_time(uint64_t def)
{
  return def;
}

template<typename T, typename... Args>
inline auto try_get_nano_time(uint64_t, const T& v, Args&&...) ->
  typename std::enable_if<supports_nano_timestamp<T>::value, uint64_t>::type
{
  return v.nanos();
}

template<typename T, typename... Args>
inline auto try_get_nano_time(uint64_t def, const T&, Args&&... args) ->
  typename std::enable_if<!supports_nano_timestamp<T>::value, uint64_t>::type
{
  return try_get_nano_time(def, std::forward<Args>(args)...);
}

class GAMS_EXPORT ReferenceFrameVersion :
  public std::enable_shared_from_this<ReferenceFrameVersion>
{
private:
  mutable std::shared_ptr<ReferenceFrameIdentity> ident_;
  const ReferenceFrameType *type_;
  uint64_t timestamp_ = -1;
  Pose origin_;
  mutable bool interpolated_ = false;

private:
  template<typename T>
  static uint64_t init_timestamp(uint64_t given, const T &p)
  {
    if (given == (uint64_t)-1) {
      return try_get_nano_time(-1, p);
    } else {
      return given;
    }
  }

public:
  template<typename P,
    typename std::enable_if<
      supports_transform_to<P>::value, void*>::type = nullptr>
  explicit ReferenceFrameVersion(
      P &&origin,
      uint64_t timestamp = -1)
    : ReferenceFrameVersion({}, Cartesian,
        std::forward<P>(origin),
        timestamp) {}

  template<typename P,
    typename std::enable_if<
      supports_transform_to<P>::value, void*>::type = nullptr>
  ReferenceFrameVersion(
      const ReferenceFrameType *type,
      P &&origin,
      uint64_t timestamp = -1)
    : ReferenceFrameVersion({}, type,
        std::forward<P>(origin), timestamp) {}

  template<typename P,
    typename std::enable_if<
      supports_transform_to<P>::value, void*>::type = nullptr>
  ReferenceFrameVersion(
      std::string name,
      P &&origin,
      uint64_t timestamp = -1)
    : ReferenceFrameVersion(
        ReferenceFrameIdentity::lookup(std::move(name)), Cartesian,
        std::forward<P>(origin), timestamp) {}

  template<typename P,
    typename std::enable_if<
      supports_transform_to<P>::value, void*>::type = nullptr>
  ReferenceFrameVersion(
      const ReferenceFrameType *type,
      std::string name,
      P &&origin,
      uint64_t timestamp = -1)
    : ReferenceFrameVersion(
        ReferenceFrameIdentity::lookup(std::move(name)), type,
        std::forward<P>(origin), timestamp) {}

  template<typename P,
    typename std::enable_if<
      supports_transform_to<P>::value, void*>::type = nullptr>
  ReferenceFrameVersion(
      std::shared_ptr<ReferenceFrameIdentity> ident,
      const ReferenceFrameType *type,
      P &&origin,
      uint64_t timestamp = -1)
    : ident_(std::move(ident)),
      type_(type),
      timestamp_(init_timestamp(timestamp, origin)),
      origin_(std::forward<P>(origin)) {}

  const ReferenceFrameIdentity &ident() const {
    if (!ident_) {
      ident_ = ReferenceFrameIdentity::make_guid();
    }
    return *ident_;
  }

  const ReferenceFrameType *type() const { return type_; }

  const Pose &origin() const {
    return origin_;
  }

  Pose &mut_origin() {
    return origin_;
  }

  ReferenceFrame pose(Pose new_origin) const {
    return pose(new_origin, timestamp_);
  }

  ReferenceFrame move(Position new_origin) const {
    return move(new_origin, timestamp_);
  }

  ReferenceFrame orient(Orientation new_origin) const {
    return orient(new_origin, timestamp_);
  }

  ReferenceFrame pose(const Pose &new_origin, uint64_t timestamp) const {
    return ReferenceFrame(std::make_shared<ReferenceFrameVersion>(
          ident_, type(), new_origin, timestamp));
  }

  ReferenceFrame move(const Position &new_origin,
                      uint64_t timestamp) const {
    return pose(Pose(new_origin, Orientation(origin_)), timestamp);
  }

  ReferenceFrame orient(const Orientation &new_origin,
                        uint64_t timestamp) const {
    return pose(Pose(Position(origin_), new_origin), timestamp);
  }

  ReferenceFrame origin_frame() const {
    return origin_.frame();
  }

  bool operator==(const ReferenceFrame &other) const;

  bool operator!=(const ReferenceFrame &other) const;

  bool operator==(const ReferenceFrameVersion &other) const;

  bool operator!=(const ReferenceFrameVersion &other) const;

  const char *name() const {
    return type()->name;
  }

  const std::string &id() const {
    return ident().id();
  }

  bool has_id() const {
    return (bool)ident_;
  }

  const std::string *id_ptr() const {
    return has_id() ? nullptr : &id();
  }

  uint64_t timestamp() const {
    return timestamp_;
  }

  ReferenceFrame timestamp(uint64_t timestamp) const {
    return ReferenceFrame(std::make_shared<ReferenceFrameVersion>(
          ident_, type(), origin(), timestamp));
  }

  bool interpolated() const {
    return interpolated_;
  }

  static const std::string &default_prefix() {
    return ReferenceFrameIdentity::default_prefix();
  }

  std::string key(const FrameEvalSettings &settings = FrameEvalSettings::DEFAULT) const {
    std::string prefix = settings.prefix();
    impl::make_kb_key(prefix, id(), timestamp());
    return prefix;
  }

  void save(madara::knowledge::KnowledgeBase &kb,
            uint64_t expiry,
            const FrameEvalSettings &settings = FrameEvalSettings::DEFAULT) const {
    std::string key = this->key(settings);
    save_as(kb, std::move(key), expiry, settings);
  }

  void save(madara::knowledge::KnowledgeBase &kb,
      const FrameEvalSettings &settings = FrameEvalSettings::DEFAULT) const {
    std::string key = this->key(settings);
    save_as(kb, std::move(key), settings);
  }

  static ReferenceFrame load_exact(
          madara::knowledge::KnowledgeBase &kb,
          const std::string &id,
          uint64_t timestamp = -1,
          uint64_t parent_timestamp = -1,
          const FrameEvalSettings &settings = FrameEvalSettings::DEFAULT);

  static ReferenceFrame load(
          madara::knowledge::KnowledgeBase &kb,
          const std::string &id,
          uint64_t timestamp = -1,
          const FrameEvalSettings &settings = FrameEvalSettings::DEFAULT);

  static uint64_t latest_timestamp(
          madara::knowledge::KnowledgeBase &kb,
          const std::string &id,
          const FrameEvalSettings &settings = FrameEvalSettings::DEFAULT);

  template<typename ForwardIterator>
  static uint64_t latest_common_timestamp(
          madara::knowledge::KnowledgeBase &kb,
          ForwardIterator begin,
          ForwardIterator end,
          const FrameEvalSettings &settings = FrameEvalSettings::DEFAULT)
  {
    madara::knowledge::ContextGuard guard(kb);

    uint64_t timestamp = -1;
    ForwardIterator cur = begin;
    while (cur != end) {
      uint64_t time = latest_timestamp(kb, *cur, settings);
      if (time < timestamp) {
        timestamp = time;
      }
      ++cur;
    }
    return timestamp;
  }

  template<typename Container>
  static uint64_t latest_common_timestamp(
          madara::knowledge::KnowledgeBase &kb,
          const Container &ids,
          const FrameEvalSettings &settings = FrameEvalSettings::DEFAULT)
  {
    madara::knowledge::ContextGuard guard(kb);

    return latest_common_timestamp(kb, ids.cbegin(), ids.cend(), settings);
  }

  template<typename ForwardIterator>
  static std::vector<ReferenceFrame> load_tree(
          madara::knowledge::KnowledgeBase &kb,
          ForwardIterator begin,
          ForwardIterator end,
          uint64_t timestamp = -1,
          const FrameEvalSettings &settings = FrameEvalSettings::DEFAULT)
  {
    std::vector<ReferenceFrame> ret;
    if (std::is_same<typename ForwardIterator::iterator_category,
        std::random_access_iterator_tag>::value) {
      size_t count = std::distance(begin, end);
      ret.reserve(count);
    }

    madara::knowledge::ContextGuard guard(kb);

    if (timestamp == (uint64_t)-1) {
      timestamp = latest_common_timestamp(kb, begin, end, settings);
    }
    while (begin != end) {
      ReferenceFrame frame = load(kb, *begin, timestamp, settings);
      if (!frame.valid()) {
        return {};
      }
      ret.push_back(frame);
      ++begin;
    }
    return ret;
  }

  template<typename Container>
  static std::vector<ReferenceFrame> load_tree(
          madara::knowledge::KnowledgeBase &kb,
          const Container &ids,
          uint64_t timestamp = -1,
          const FrameEvalSettings &settings = FrameEvalSettings::DEFAULT)
  {
    return load_tree(kb, ids.cbegin(), ids.cend(),
                     timestamp, std::move(settings));
  }

  void save_as(madara::knowledge::KnowledgeBase &kb,
               std::string key, uint64_t expiry,
               const FrameEvalSettings &settings = FrameEvalSettings::DEFAULT) const;
  void save_as(madara::knowledge::KnowledgeBase &kb,
               std::string key,
               const FrameEvalSettings &settings = FrameEvalSettings::DEFAULT) const
  {
    save_as(kb, key, ident().expiry(), settings);
  }

  ReferenceFrame interpolate(const ReferenceFrame &other, ReferenceFrame parent, uint64_t time) const
  {
    //std::cerr << "Interpolate has_id: " << has_id() << std::endl;
    if (has_id()) {
      auto ret = ident().get_version(time);
      if (ret) {
        return ret;
      }
    }
    double fraction = (time - timestamp()) / (double)(other.timestamp() - timestamp());

    Pose interp = origin();
    const Pose &opose = other.origin();

    interp.frame(std::move(parent));

    interp.x((fraction * (opose.x() - interp.x())) + interp.x());
    interp.y((fraction * (opose.y() - interp.y())) + interp.y());
    interp.z((fraction * (opose.z() - interp.z())) + interp.z());

    Quaternion iq(origin().as_orientation_vec());
    Quaternion oq(opose.as_orientation_vec());

    iq.slerp_this(oq, fraction);
    iq.to_angular_vector(interp.as_orientation_vec());

    //std::cerr << "Interp " << fraction << " " << origin() << "  " << interp << "  " << opose << std::endl;

    auto ret = std::make_shared<ReferenceFrameVersion>(
        ident_, type(), std::move(interp), time);

    if (has_id()) {
      ident().register_version(time, ret);
    }

    ret->interpolated_ = true;
    return ret;
  }

  template<typename CoordType>
  friend class Coordinate;

private:
  bool check_consistent() const;
};

class GAMS_EXPORT FrameStore {
private:
  /*
   * KnowledgeBase uses const to indicate not changing the underlying
   * KnowledgeBaseImpl, as well as itself. We won't be changing the
   * KnowledgeBase object itself, and will only call thread-safe methods.
   */
  mutable madara::knowledge::KnowledgeBase kb_;

  FrameEvalSettings settings_ = FrameEvalSettings::DEFAULT;
  uint64_t expiry_;

public:
  FrameStore(madara::knowledge::KnowledgeBase kb,
             FrameEvalSettings settings, uint64_t expiry)
    : kb_(std::move(kb)), settings_(std::move(settings)), expiry_(expiry) {}

  FrameStore(madara::knowledge::KnowledgeBase kb, FrameEvalSettings settings)
    : FrameStore(std::move(kb), std::move(settings),
        ReferenceFrame::default_expiry()) {}

  FrameStore(madara::knowledge::KnowledgeBase kb, uint64_t expiry)
    : FrameStore(std::move(kb), FrameEvalSettings::DEFAULT, expiry) {}


  FrameStore(madara::knowledge::KnowledgeBase kb)
    : FrameStore(std::move(kb), FrameEvalSettings::DEFAULT,
        ReferenceFrame::default_expiry()) {}

  uint64_t expiry() const { return expiry_; }

  const FrameEvalSettings &settings() const { return settings_; }

  const madara::knowledge::KnowledgeBase &kb() const { return kb_; }

  void save(const ReferenceFrame &frame) const {
    return frame.save(kb_, expiry_, settings_);
  }

   ReferenceFrame load(const std::string &id, uint64_t timestamp = -1) {
     return ReferenceFrame::load(kb_, id, timestamp, settings_);
   }

  template<typename InputIterator>
  std::vector<ReferenceFrame> load_tree(
        InputIterator begin,
        InputIterator end,
        uint64_t timestamp = -1) const {
   return ReferenceFrame::load_tree(kb_, begin, end, timestamp, settings_);
  }

  template<typename Container>
  std::vector<ReferenceFrame> load_tree(
        const Container &ids,
        uint64_t timestamp = -1) const {
   return ReferenceFrame::load_tree(kb_, ids, timestamp, settings_);
  }
};

GAMS_EXPORT const ReferenceFrame *find_common_frame(
    const ReferenceFrame *from,
    const ReferenceFrame *to,
    std::vector<const ReferenceFrame *> *to_stack = nullptr);

class unrelated_frames : public std::runtime_error
{
public:
  unrelated_frames(ReferenceFrame from_frame, ReferenceFrame to_frame);
  ~unrelated_frames() throw();

  ReferenceFrame from_frame;
  ReferenceFrame to_frame;
};

class undefined_transform : public std::runtime_error
{
public:
  undefined_transform(
    const ReferenceFrameType *parent_frame,
    const ReferenceFrameType *child_frame,
    bool is_child_to_parent,
    bool unsupported_angular = false);
  ~undefined_transform() throw();

  const ReferenceFrameType *parent_frame;
  const ReferenceFrameType *child_frame;
  bool is_child_to_parent;
  bool unsupported_angular;
};

namespace simple_rotate {
  void orient_linear_vec(
      double &x, double &y, double &z,
      double rx, double ry, double rz,
      bool reverse = false);

  void transform_angular_to_origin(
                  const ReferenceFrameType *origin,
                  const ReferenceFrameType *self,
                  double orx, double ory, double orz,
                  double &rx, double &ry, double &rz);

  void transform_angular_from_origin(
                  const ReferenceFrameType *origin,
                  const ReferenceFrameType *self,
                  double orx, double ory, double orz,
                  double &rx, double &ry, double &rz);

  void transform_pose_to_origin(
                  const ReferenceFrameType *origin,
                  const ReferenceFrameType *self,
                  double ox, double oy, double oz,
                  double orx, double ory, double orz,
                  double &x, double &y, double &z,
                  double &rx, double &ry, double &rz,
                  bool fixed);

  void transform_pose_from_origin(
                  const ReferenceFrameType *origin,
                  const ReferenceFrameType *self,
                  double ox, double oy, double oz,
                  double orx, double ory, double orz,
                  double &x, double &y, double &z,
                  double &rx, double &ry, double &rz,
                  bool fixed);

  double calc_angle(
                  const ReferenceFrameType *self,
                  double rx1, double ry1, double rz1,
                  double rx2, double ry2, double rz2);
}

inline void default_normalize_linear(
            const ReferenceFrameType *,
            double &, double &, double &) {}

inline void default_normalize_angular(
            const ReferenceFrameType *,
            double &, double &, double &) {}

inline void default_normalize_pose(
                const ReferenceFrameType *self,
                double &x, double &y, double &z,
                double &rx, double &ry, double &rz)
{
  self->normalize_linear(self, x, y, z);
  self->normalize_angular(self, rx, ry, rz);
}

} }

#include "ReferenceFrame.inl"

#endif