LieVector.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/Lie.h>
#include <gtsam/base/Vector.h>
#include <gtsam/base/DerivedValue.h>

namespace gtsam {

struct LieVector : public Vector, public DerivedValue<LieVector> {

  LieVector() {}

  LieVector(const Vector& v) : Vector(v) {}
  
// Currently TMP constructor causes ICE on MSVS 2013
#if (_MSC_VER < 1800)

  template<int N>
  LieVector(const Eigen::Matrix<double, N, 1>& v) : Vector(v) {}
#endif

  LieVector(double d) : Vector((Vector(1) << d)) {}

  GTSAM_EXPORT LieVector(size_t m, const double* const data);

  Vector vector() const {
    return static_cast<Vector>(*this);
  }

  GTSAM_EXPORT void print(const std::string& name="") const;

  bool equals(const LieVector& expected, double tol=1e-5) const {
    return gtsam::equal(vector(), expected.vector(), tol);
  }

  // Manifold requirements

  size_t dim() const { return this->size(); }

  LieVector retract(const Vector& v) const { return LieVector(vector() + v); }

  Vector localCoordinates(const LieVector& t2) const { return LieVector(t2 - vector()); }

  // Group requirements

  static LieVector identity() {
    throw std::runtime_error("LieVector::identity(): Don't use this function");
    return LieVector();
  }

  // Note: Manually specifying the 'gtsam' namespace for the optional Matrix arguments
  // This is a work-around for linux g++ 4.6.1 that incorrectly selects the Eigen::Matrix class
  // instead of the gtsam::Matrix class. This is related to deriving this class from an Eigen Vector
  // as the other geometry objects (Point3, Rot3, etc.) have this problem
  LieVector compose(const LieVector& p,
      boost::optional<gtsam::Matrix&> H1=boost::none,
      boost::optional<gtsam::Matrix&> H2=boost::none) const {
    if(H1) *H1 = eye(dim());
    if(H2) *H2 = eye(p.dim());

    return LieVector(vector() + p);
  }

  LieVector between(const LieVector& l2,
      boost::optional<gtsam::Matrix&> H1=boost::none,
      boost::optional<gtsam::Matrix&> H2=boost::none) const {
    if(H1) *H1 = -eye(dim());
    if(H2) *H2 = eye(l2.dim());
    return LieVector(l2.vector() - vector());
  }

  LieVector inverse(boost::optional<gtsam::Matrix&> H=boost::none) const {
    if(H) *H = -eye(dim());

    return LieVector(-1.0 * vector());
  }

  // Lie functions

  static LieVector Expmap(const Vector& v) { return LieVector(v); }

  static Vector Logmap(const LieVector& p) { return p; }

private:

  // Serialization function
  friend class boost::serialization::access;
  template<class Archive>
  void serialize(Archive & ar, const unsigned int version) {
    ar & boost::serialization::make_nvp("LieVector",
       boost::serialization::base_object<Value>(*this));
    ar & boost::serialization::make_nvp("Vector",
       boost::serialization::base_object<Vector>(*this));
  }

};
} // \namespace gtsam