SO4.h

Go to the documentation of this file.

/* ----------------------------------------------------------------------------
 
 * GTSAM Copyright 2010-2019, 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/geometry/SOn.h>
 
#include <gtsam/base/Group.h>
#include <gtsam/base/Lie.h>
#include <gtsam/base/Manifold.h>
#include <gtsam/base/Matrix.h>
#include <gtsam/dllexport.h>
 
#include <string>
 
namespace gtsam {
 
using SO4 = SO<4>;
 
// /// Random SO(4) element (no big claims about uniformity)
// static SO4 Random(std::mt19937 &rng);
 
// Below are all declarations of SO<4> specializations.
// They are *defined* in SO4.cpp.
 
template <>
GTSAM_EXPORT
Matrix4 SO4::Hat(const TangentVector &xi);
 
template <>
GTSAM_EXPORT
Vector6 SO4::Vee(const Matrix4 &X);
 
template <>
GTSAM_EXPORT
SO4 SO4::Expmap(const Vector6 &xi, ChartJacobian H);
 
template <>
GTSAM_EXPORT
Matrix6 SO4::AdjointMap() const;
 
template <>
GTSAM_EXPORT
SO4::VectorN2 SO4::vec(OptionalJacobian<16, 6> H) const;
 
template <>
GTSAM_EXPORT
SO4 SO4::ChartAtOrigin::Retract(const Vector6 &omega, ChartJacobian H);
 
template <>
GTSAM_EXPORT
Vector6 SO4::ChartAtOrigin::Local(const SO4 &Q, ChartJacobian H);

GTSAM_EXPORT Matrix3 topLeft(const SO4 &Q, OptionalJacobian<9, 6> H = boost::none);

GTSAM_EXPORT Matrix43 stiefel(const SO4 &Q, OptionalJacobian<12, 6> H = boost::none);

template <class Archive>
void serialize(Archive &ar, SO4 &Q, const unsigned int /*version*/) {
  Matrix4 &M = Q.matrix_;
  ar &boost::serialization::make_nvp("Q11", M(0, 0));
  ar &boost::serialization::make_nvp("Q12", M(0, 1));
  ar &boost::serialization::make_nvp("Q13", M(0, 2));
  ar &boost::serialization::make_nvp("Q14", M(0, 3));
 
  ar &boost::serialization::make_nvp("Q21", M(1, 0));
  ar &boost::serialization::make_nvp("Q22", M(1, 1));
  ar &boost::serialization::make_nvp("Q23", M(1, 2));
  ar &boost::serialization::make_nvp("Q24", M(1, 3));
 
  ar &boost::serialization::make_nvp("Q31", M(2, 0));
  ar &boost::serialization::make_nvp("Q32", M(2, 1));
  ar &boost::serialization::make_nvp("Q33", M(2, 2));
  ar &boost::serialization::make_nvp("Q34", M(2, 3));
 
  ar &boost::serialization::make_nvp("Q41", M(3, 0));
  ar &boost::serialization::make_nvp("Q42", M(3, 1));
  ar &boost::serialization::make_nvp("Q43", M(3, 2));
  ar &boost::serialization::make_nvp("Q44", M(3, 3));
}
 
/*
 * Define the traits. internal::LieGroup provides both Lie group and Testable
 */
 
template <>
struct traits<SO4> : public internal::LieGroup<SO4> {};
 
template <>
struct traits<const SO4> : public internal::LieGroup<SO4> {};
 
}  // end namespace gtsam