deprecated/LieScalar.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/dllexport.h>
#include <gtsam/base/VectorSpace.h>
#include <iostream>

namespace gtsam {

  struct LieScalar {

    enum { dimension = 1 };

    LieScalar() : d_(0.0) {}

    /*explicit*/ LieScalar(double d) : d_(d) {}

    double value() const { return d_; }

    operator double() const { return d_; }

    Vector1 vector() const { Vector1 v; v<<d_; return v; }

    void print(const std::string& name = "") const {
      std::cout << name << ": " << d_ << std::endl;
    }
    bool equals(const LieScalar& expected, double tol = 1e-5) const {
      return std::abs(expected.d_ - d_) <= tol;
    }

    static LieScalar identity() { return LieScalar(0);}
    LieScalar compose(const LieScalar& q) { return (*this)+q;}
    LieScalar between(const LieScalar& q) { return q-(*this);}
    LieScalar inverse() { return -(*this);}

    size_t dim() const { return 1; }
    Vector1 localCoordinates(const LieScalar& q) { return between(q).vector();}
    LieScalar retract(const Vector1& v) {return compose(LieScalar(v[0]));}

    static Vector1 Logmap(const LieScalar& p) { return p.vector();}
    static LieScalar Expmap(const Vector1& v) { return LieScalar(v[0]);}

  private:
      double d_;
  };

  template<>
  struct traits<LieScalar> : public internal::ScalarTraits<LieScalar> {};

} // \namespace gtsam