117 lines
3.1 KiB
C++
117 lines
3.1 KiB
C++
/* ----------------------------------------------------------------------------
|
|
|
|
* 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
|
|
|
|
* -------------------------------------------------------------------------- */
|
|
|
|
/**
|
|
* @file SO4.h
|
|
* @brief 4*4 matrix representation of SO(4)
|
|
* @author Frank Dellaert
|
|
* @author Luca Carlone
|
|
* @date March 2019
|
|
*/
|
|
|
|
#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);
|
|
|
|
/**
|
|
* Project to top-left 3*3 matrix. Note this is *not* in general \in SO(3).
|
|
*/
|
|
GTSAM_EXPORT Matrix3 topLeft(const SO4 &Q, OptionalJacobian<9, 6> H = boost::none);
|
|
|
|
/**
|
|
* Project to Stiefel manifold of 4*3 orthonormal 3-frames in R^4, i.e., pi(Q)
|
|
* -> \f$ S \in St(3,4) \f$.
|
|
*/
|
|
GTSAM_EXPORT Matrix43 stiefel(const SO4 &Q, OptionalJacobian<12, 6> H = boost::none);
|
|
|
|
/** Serialization function */
|
|
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
|