ReferenceFrameFactor.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
 
 * -------------------------------------------------------------------------- */

 
/*
 * @file ReferenceFrameFactor.h
 * @brief A constraint for combining graphs by common landmarks and a transform node
 * @author Alex Cunningham
 */
 
#pragma once
 
#include <gtsam/nonlinear/NonlinearFactor.h>
 
namespace gtsam {

template<class T, class P>
P transform_point(
    const T& trans, const P& global,
    OptionalMatrixType Dtrans,
    OptionalMatrixType Dglobal) {
  return trans.transformFrom(global, Dtrans, Dglobal);
}

template<class POINT, class TRANSFORM>
class ReferenceFrameFactor
    : public NoiseModelFactorT<typename traits<POINT>::TangentVector, POINT,
                               TRANSFORM, POINT> {
protected:
  ReferenceFrameFactor() {}
 
public:
  typedef NoiseModelFactorT<typename traits<POINT>::TangentVector, POINT,
                            TRANSFORM, POINT>
      Base;
  using ErrorVector = typename traits<POINT>::TangentVector;
  typedef ReferenceFrameFactor<POINT, TRANSFORM> This;
 
  // Provide access to the Matrix& version of evaluateError:
  using Base::evaluateError;
 
  typedef POINT Point;
  typedef TRANSFORM Transform;

  ReferenceFrameFactor(Key globalKey, Key transKey, Key localKey, const noiseModel::Base::shared_ptr& model)
  : Base(model,globalKey, transKey, localKey) {}

  ReferenceFrameFactor(double mu, Key globalKey, Key transKey, Key localKey)
      : Base(noiseModel::Constrained::All(Point().dim(), mu), globalKey,
             transKey, localKey) {}

  ReferenceFrameFactor(Key globalKey, Key transKey, Key localKey, double sigma = 1e-2)
  : Base(noiseModel::Isotropic::Sigma(traits<POINT>::dimension, sigma),
      globalKey, transKey, localKey) {}
 
  ~ReferenceFrameFactor() override{}
 
  NonlinearFactor::shared_ptr clone() const override {
    return std::static_pointer_cast<NonlinearFactor>(
        NonlinearFactor::shared_ptr(new This(*this))); }

  ErrorVector evaluateError(const Point& _global, const Transform& trans,
                            const Point& local, OptionalMatrixType Dforeign,
                            OptionalMatrixType Dtrans,
                            OptionalMatrixType Dlocal) const override {
    Point newlocal = transform_point<Transform,Point>(trans, _global, Dtrans, Dforeign);
    if (Dlocal) {
      *Dlocal = -1* Matrix::Identity(traits<Point>::dimension, traits<Point>::dimension);
    }
    return traits<Point>::Local(local,newlocal);
  }
 
  void print(const std::string& s="",
      const gtsam::KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
    std::cout << s << ": ReferenceFrameFactor("
        << "Global: " << keyFormatter(this->key1()) << ","
        << " Transform: " << keyFormatter(this->key2()) << ","
        << " Local: " << keyFormatter(this->key3()) << ")\n";
    this->noiseModel_->print("  noise model");
  }
 
  // access - convenience functions
  Key global_key() const { return this->key1(); }
  Key transform_key() const { return this->key2(); }
  Key local_key() const { return this->key3(); }
 
private:
#if GTSAM_ENABLE_BOOST_SERIALIZATION
  friend class boost::serialization::access;
  template<class ARCHIVE>
  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
    ar & boost::serialization::make_nvp("NonlinearFactor3",
        boost::serialization::base_object<Base>(*this));
  }
#endif
};

template<class T1, class T2>
struct traits<ReferenceFrameFactor<T1, T2> > : public Testable<ReferenceFrameFactor<T1, T2> > {};
 
} // \namespace gtsam