concepts.h

Go to the documentation of this file.

/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

#pragma once

#include <gtsam/base/Matrix.h>
#include <boost/optional.hpp>

namespace gtsam {

template<class POSE>
class PoseConcept {
public:
  typedef typename POSE::Translation Translation;
  typedef typename POSE::Rotation Rotation;

private:
  static Rotation checkRotationMemberAccess(const POSE& p) {
    return p.rotation();
  }

  static Translation checkTranslationMemberAccess(const POSE& p) {
    return p.translation();
  }

  static std::pair<size_t, size_t> checkTranslationInterval() {
    return POSE::translationInterval();
  }

  static std::pair<size_t, size_t> checkRotationInterval() {
    return POSE::rotationInterval();
  }
};

template<class V1, class V2>
class RangeMeasurementConcept {
private:
  static double checkRangeMeasurement(const V1& x, const V2& p) {
    return x.range(p);
  }

  static double checkRangeMeasurementDerivatives(const V1& x, const V2& p) {
    boost::optional<Matrix&> H1, H2;
    return x.range(p, H1, H2);
  }
};

} // \namespace gtsam

#define GTSAM_CONCEPT_POSE_INST(T) template class gtsam::PoseConcept<T>;
#define GTSAM_CONCEPT_POSE_TYPE(T) typedef gtsam::PoseConcept<T> _gtsam_PoseConcept##T;

#define GTSAM_CONCEPT_RANGE_MEASUREMENT_INST(V1,V2) template class gtsam::RangeMeasurementConcept<V1,V2>;
#define GTSAM_CONCEPT_RANGE_MEASUREMENT_TYPE(V1,V2) typedef gtsam::RangeMeasurementConcept<V1,V2> _gtsam_RangeMeasurementConcept##V1##V2;