SO3.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 <gtsam/base/Lie.h>

#include <cmath>

namespace gtsam {

class GTSAM_EXPORT SO3: public Matrix3, public LieGroup<SO3, 3> {

protected:

public:
  enum {
    dimension = 3
  };


  SO3() :
      Matrix3(I_3x3) {
  }

  template<typename Derived>
  SO3(const MatrixBase<Derived>& R) :
      Matrix3(R.eval()) {
  }

  SO3(const Eigen::AngleAxisd& angleAxis) :
      Matrix3(angleAxis) {
  }

  static SO3 AxisAngle(const Vector3& axis, double theta);


  void print(const std::string& s) const {
    std::cout << s << *this << std::endl;
  }

  bool equals(const SO3 & R, double tol) const {
    return equal_with_abs_tol(*this, R, tol);
  }


  static SO3 identity() {
    return I_3x3;
  }

  SO3 inverse() const {
    return this->Matrix3::inverse();
  }


  static SO3 Expmap(const Vector3& omega, ChartJacobian H = boost::none);

  static Matrix3 ExpmapDerivative(const Vector3& omega);

  static Vector3 Logmap(const SO3& R, ChartJacobian H = boost::none);

  static Matrix3 LogmapDerivative(const Vector3& omega);

  Matrix3 AdjointMap() const {
    return *this;
  }

  // Chart at origin
  struct ChartAtOrigin {
    static SO3 Retract(const Vector3& omega, ChartJacobian H = boost::none) {
      return Expmap(omega, H);
    }
    static Vector3 Local(const SO3& R, ChartJacobian H = boost::none) {
      return Logmap(R, H);
    }
  };

  using LieGroup<SO3, 3>::inverse;

};

// This namespace exposes two functors that allow for saving computation when
// exponential map and its derivatives are needed at the same location in so<3>
// The second functor also implements dedicated methods to apply dexp and/or inv(dexp)
namespace so3 {

class ExpmapFunctor {
 protected:
  const double theta2;
  Matrix3 W, K, KK;
  bool nearZero;
  double theta, sin_theta, one_minus_cos;  // only defined if !nearZero

  void init(bool nearZeroApprox = false);

 public:
  ExpmapFunctor(const Vector3& omega, bool nearZeroApprox = false);

  ExpmapFunctor(const Vector3& axis, double angle, bool nearZeroApprox = false);

  SO3 expmap() const;
};

class DexpFunctor : public ExpmapFunctor {
  const Vector3 omega;
  double a, b;
  Matrix3 dexp_;

 public:
  DexpFunctor(const Vector3& omega, bool nearZeroApprox = false);

  // NOTE(luca): Right Jacobian for Exponential map in SO(3) - equation
  // (10.86) and following equations in G.S. Chirikjian, "Stochastic Models,
  // Information Theory, and Lie Groups", Volume 2, 2008.
  //   expmap(omega + v) \approx expmap(omega) * expmap(dexp * v)
  // This maps a perturbation v in the tangent space to
  // a perturbation on the manifold Expmap(dexp * v) */
  const Matrix3& dexp() const { return dexp_; }

  Vector3 applyDexp(const Vector3& v, OptionalJacobian<3, 3> H1 = boost::none,
                    OptionalJacobian<3, 3> H2 = boost::none) const;

  Vector3 applyInvDexp(const Vector3& v,
                       OptionalJacobian<3, 3> H1 = boost::none,
                       OptionalJacobian<3, 3> H2 = boost::none) const;
};
}  //  namespace so3

template<>
struct traits<SO3> : public internal::LieGroup<SO3> {
};

template<>
struct traits<const SO3> : public internal::LieGroup<SO3> {
};
} // end namespace gtsam