12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Full enchilada: SO3 is now SO<3>

release/4.3a0
Frank Dellaert 2019-05-07 10:48:55 -04:00 committed by Fan Jiang
parent 8eacdcbe58
commit 06952cd83b
12 changed files with 276 additions and 769 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

72
gtsam/geometry/Rot3.h
View File

@ -61,7 +61,7 @@ namespace gtsam {
/** Internal Eigen Quaternion */
gtsam::Quaternion quaternion_;
#else
Matrix3 rot_;
SO3 rot_;
#endif
public:
@ -92,26 +92,33 @@ namespace gtsam {
* allow assignment/construction from a generic matrix.
* See: http://stackoverflow.com/questions/27094132/cannot-understand-if-this-is-circular-dependency-or-clang#tab-top
*/
template<typename Derived>
inline explicit Rot3(const Eigen::MatrixBase<Derived>& R) {
#ifdef GTSAM_USE_QUATERNIONS
quaternion_=Matrix3(R);
#else
rot_ = R;
#endif
template <typename Derived>
#ifdef GTSAM_USE_QUATERNIONS
explicit Rot3(const Eigen::MatrixBase<Derived>& R) : quaternion_(R) {
}
#else
explicit Rot3(const Eigen::MatrixBase<Derived>& R) : rot_(R) {
}
#endif
/**
* Constructor from a rotation matrix
* Overload version for Matrix3 to avoid casting in quaternion mode.
*/
inline explicit Rot3(const Matrix3& R) {
#ifdef GTSAM_USE_QUATERNIONS
quaternion_=R;
#else
rot_ = R;
#endif
}
#ifdef GTSAM_USE_QUATERNIONS
explicit Rot3(const Matrix3& R) : quaternion_(R) {}
#else
explicit Rot3(const Matrix3& R) : rot_(R) {}
#endif
/**
* Constructor from an SO3 instance
*/
#ifdef GTSAM_USE_QUATERNIONS
explicit Rot3(const SO3& R) : quaternion_(R.matrix()) {}
#else
explicit Rot3(const SO3& R) : rot_(R) {}
#endif
/** Constructor from a quaternion. This can also be called using a plain
* Vector, due to implicit conversion from Vector to Quaternion
@ -486,28 +493,27 @@ namespace gtsam {
/// @}
#endif
private:
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/)
{
template <class ARCHIVE>
void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
#ifndef GTSAM_USE_QUATERNIONS
ar & boost::serialization::make_nvp("rot11", rot_(0,0));
ar & boost::serialization::make_nvp("rot12", rot_(0,1));
ar & boost::serialization::make_nvp("rot13", rot_(0,2));
ar & boost::serialization::make_nvp("rot21", rot_(1,0));
ar & boost::serialization::make_nvp("rot22", rot_(1,1));
ar & boost::serialization::make_nvp("rot23", rot_(1,2));
ar & boost::serialization::make_nvp("rot31", rot_(2,0));
ar & boost::serialization::make_nvp("rot32", rot_(2,1));
ar & boost::serialization::make_nvp("rot33", rot_(2,2));
Matrix3& M = rot_.matrix_;
ar& boost::serialization::make_nvp("rot11", M(0, 0));
ar& boost::serialization::make_nvp("rot12", M(0, 1));
ar& boost::serialization::make_nvp("rot13", M(0, 2));
ar& boost::serialization::make_nvp("rot21", M(1, 0));
ar& boost::serialization::make_nvp("rot22", M(1, 1));
ar& boost::serialization::make_nvp("rot23", M(1, 2));
ar& boost::serialization::make_nvp("rot31", M(2, 0));
ar& boost::serialization::make_nvp("rot32", M(2, 1));
ar& boost::serialization::make_nvp("rot33", M(2, 2));
#else
ar & boost::serialization::make_nvp("w", quaternion_.w());
ar & boost::serialization::make_nvp("x", quaternion_.x());
ar & boost::serialization::make_nvp("y", quaternion_.y());
ar & boost::serialization::make_nvp("z", quaternion_.z());
ar& boost::serialization::make_nvp("w", quaternion_.w());
ar& boost::serialization::make_nvp("x", quaternion_.x());
ar& boost::serialization::make_nvp("y", quaternion_.y());
ar& boost::serialization::make_nvp("z", quaternion_.z());
#endif
}

37
gtsam/geometry/Rot3M.cpp
View File

@ -36,18 +36,17 @@ Rot3::Rot3() : rot_(I_3x3) {}
/* ************************************************************************* */
Rot3::Rot3(const Point3& col1, const Point3& col2, const Point3& col3) {
rot_.col(0) = col1;
rot_.col(1) = col2;
rot_.col(2) = col3;
Matrix3 R;
R << col1, col2, col3;
rot_ = SO3(R);
}
/* ************************************************************************* */
Rot3::Rot3(double R11, double R12, double R13,
double R21, double R22, double R23,
double R31, double R32, double R33) {
rot_ << R11, R12, R13,
R21, R22, R23,
R31, R32, R33;
Rot3::Rot3(double R11, double R12, double R13, double R21, double R22,
double R23, double R31, double R32, double R33) {
Matrix3 R;
R << R11, R12, R13, R21, R22, R23, R31, R32, R33;
rot_ = SO3(R);
}
/* ************************************************************************* */
@ -107,21 +106,21 @@ Rot3 Rot3::RzRyRx(double x, double y, double z) {
/* ************************************************************************* */
Rot3 Rot3::operator*(const Rot3& R2) const {
return Rot3(Matrix3(rot_*R2.rot_));
return Rot3(rot_*R2.rot_);
}
/* ************************************************************************* */
// TODO const Eigen::Transpose<const Matrix3> Rot3::transpose() const {
Matrix3 Rot3::transpose() const {
return rot_.transpose();
return rot_.matrix().transpose();
}
/* ************************************************************************* */
Point3 Rot3::rotate(const Point3& p,
OptionalJacobian<3,3> H1, OptionalJacobian<3,3> H2) const {
if (H1) *H1 = rot_ * skewSymmetric(-p.x(), -p.y(), -p.z());
if (H2) *H2 = rot_;
return rot_ * p;
if (H1) *H1 = rot_.matrix() * skewSymmetric(-p.x(), -p.y(), -p.z());
if (H2) *H2 = rot_.matrix();
return rot_.matrix() * p;
}
/* ************************************************************************* */
@ -178,21 +177,21 @@ Vector3 Rot3::ChartAtOrigin::Local(const Rot3& R, ChartJacobian H) {
/* ************************************************************************* */
Matrix3 Rot3::matrix() const {
return rot_;
return rot_.matrix();
}
/* ************************************************************************* */
Point3 Rot3::r1() const { return Point3(rot_.col(0)); }
Point3 Rot3::r1() const { return Point3(rot_.matrix().col(0)); }
/* ************************************************************************* */
Point3 Rot3::r2() const { return Point3(rot_.col(1)); }
Point3 Rot3::r2() const { return Point3(rot_.matrix().col(1)); }
/* ************************************************************************* */
Point3 Rot3::r3() const { return Point3(rot_.col(2)); }
Point3 Rot3::r3() const { return Point3(rot_.matrix().col(2)); }
/* ************************************************************************* */
gtsam::Quaternion Rot3::toQuaternion() const {
return gtsam::Quaternion(rot_);
return gtsam::Quaternion(rot_.matrix());
}
/* ************************************************************************* */

195
gtsam/geometry/SO3.cpp
View File

@ -18,22 +18,23 @@
* @date December 2014
*/
#include <gtsam/geometry/SO3.h>
#include <gtsam/base/concepts.h>
#include <gtsam/geometry/SO3.h>
#include <Eigen/SVD>
#include <cmath>
#include <limits>
#include <iostream>
#include <limits>
namespace gtsam {
/* ************************************************************************* */
//******************************************************************************
namespace so3 {
Matrix99 Dcompose(const SO3& R) {
Matrix99 Dcompose(const SO3& Q) {
Matrix99 H;
auto R = Q.matrix();
H << I_3x3 * R(0, 0), I_3x3 * R(1, 0), I_3x3 * R(2, 0), //
I_3x3 * R(0, 1), I_3x3 * R(1, 1), I_3x3 * R(2, 1), //
I_3x3 * R(0, 2), I_3x3 * R(1, 2), I_3x3 * R(2, 2);
@ -47,7 +48,8 @@ Matrix3 compose(const Matrix3& M, const SO3& R, OptionalJacobian<9, 9> H) {
}
void ExpmapFunctor::init(bool nearZeroApprox) {
nearZero = nearZeroApprox || (theta2 <= std::numeric_limits<double>::epsilon());
nearZero =
nearZeroApprox || (theta2 <= std::numeric_limits<double>::epsilon());
if (!nearZero) {
sin_theta = std::sin(theta);
const double s2 = std::sin(theta / 2.0);
@ -79,9 +81,9 @@ ExpmapFunctor::ExpmapFunctor(const Vector3& axis, double angle, bool nearZeroApp
SO3 ExpmapFunctor::expmap() const {
if (nearZero)
return I_3x3 + W;
return SO3(I_3x3 + W);
else
return I_3x3 + sin_theta * K + one_minus_cos * KK;
return SO3(I_3x3 + sin_theta * K + one_minus_cos * KK);
}
DexpFunctor::DexpFunctor(const Vector3& omega, bool nearZeroApprox)
@ -121,7 +123,7 @@ Vector3 DexpFunctor::applyInvDexp(const Vector3& v, OptionalJacobian<3, 3> H1,
if (H1) {
Matrix3 D_dexpv_omega;
applyDexp(c, D_dexpv_omega); // get derivative H of forward mapping
*H1 = -invDexp* D_dexpv_omega;
*H1 = -invDexp * D_dexpv_omega;
}
if (H2) *H2 = invDexp;
return c;
@ -129,72 +131,117 @@ Vector3 DexpFunctor::applyInvDexp(const Vector3& v, OptionalJacobian<3, 3> H1,
} // namespace so3
/* ************************************************************************* */
//******************************************************************************
template <>
SO3 SO3::AxisAngle(const Vector3& axis, double theta) {
return so3::ExpmapFunctor(axis, theta).expmap();
}
/* ************************************************************************* */
//******************************************************************************
template <>
SO3 SO3::ClosestTo(const Matrix3& M) {
Eigen::JacobiSVD<Matrix3> svd(M, Eigen::ComputeFullU | Eigen::ComputeFullV);
const auto& U = svd.matrixU();
const auto& V = svd.matrixV();
const double det = (U * V.transpose()).determinant();
return U * Vector3(1, 1, det).asDiagonal() * V.transpose();
return SO3(U * Vector3(1, 1, det).asDiagonal() * V.transpose());
}
/* ************************************************************************* */
//******************************************************************************
template <>
SO3 SO3::ChordalMean(const std::vector<SO3>& rotations) {
// See Hartley13ijcv:
// Cost function C(R) = \sum sqr(|R-R_i|_F)
// See Hartley13ijcv:
// Cost function C(R) = \sum sqr(|R-R_i|_F)
// Closed form solution = ClosestTo(C_e), where C_e = \sum R_i !!!!
Matrix3 C_e {Z_3x3};
Matrix3 C_e{Z_3x3};
for (const auto& R_i : rotations) {
C_e += R_i;
C_e += R_i.matrix();
}
return ClosestTo(C_e);
}
/* ************************************************************************* */
void SO3::print(const std::string& s) const {
std::cout << s << *this << std::endl;
}
//******************************************************************************
Matrix3 SO3::Hat(const Vector3& xi) { return skewSymmetric(xi); }
/* ************************************************************************* */
Vector3 SO3::Vee(const Matrix3& X) {
Vector3 xi;
xi(0) = -X(1, 2);
xi(1) = X(0, 2);
xi(2) = -X(0, 1);
return xi;
template <>
Matrix3 SO3::Hat(const Vector3& xi) {
// skew symmetric matrix X = xi^
Matrix3 Y = Z_3x3;
Y(0, 1) = -xi(2);
Y(0, 2) = +xi(1);
Y(1, 2) = -xi(0);
return Y - Y.transpose();
}
/* ************************************************************************* */
//******************************************************************************
template <>
Vector3 SO3::Vee(const Matrix3& X) {
Vector3 xi;
xi(0) = -X(1, 2);
xi(1) = +X(0, 2);
xi(2) = -X(0, 1);
return xi;
}
//******************************************************************************
template <>
Matrix3 SO3::AdjointMap() const {
return matrix_;
}
//******************************************************************************
template <>
SO3 SO3::Expmap(const Vector3& omega, ChartJacobian H) {
if (H) {
so3::DexpFunctor impl(omega);
*H = impl.dexp();
return impl.expmap();
} else
} else {
return so3::ExpmapFunctor(omega).expmap();
}
}
template <>
Matrix3 SO3::ExpmapDerivative(const Vector3& omega) {
return so3::DexpFunctor(omega).dexp();
}
/* ************************************************************************* */
Vector3 SO3::Logmap(const SO3& R, ChartJacobian H) {
using std::sqrt;
//******************************************************************************
/* Right Jacobian for Log map in SO(3) - equation (10.86) and following
equations in G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie
Groups", Volume 2, 2008.
logmap( Rhat * expmap(omega) ) \approx logmap(Rhat) + Jrinv * omega
where Jrinv = LogmapDerivative(omega). This maps a perturbation on the
manifold (expmap(omega)) to a perturbation in the tangent space (Jrinv *
omega)
*/
template <>
Matrix3 SO3::LogmapDerivative(const Vector3& omega) {
using std::cos;
using std::sin;
double theta2 = omega.dot(omega);
if (theta2 <= std::numeric_limits<double>::epsilon()) return I_3x3;
double theta = std::sqrt(theta2); // rotation angle
// element of Lie algebra so(3): W = omega^
const Matrix3 W = Hat(omega);
return I_3x3 + 0.5 * W +
(1 / (theta * theta) - (1 + cos(theta)) / (2 * theta * sin(theta))) *
W * W;
}
//******************************************************************************
template <>
Vector3 SO3::Logmap(const SO3& Q, ChartJacobian H) {
using std::sin;
using std::sqrt;
// note switch to base 1
const double& R11 = R(0, 0), R12 = R(0, 1), R13 = R(0, 2);
const double& R21 = R(1, 0), R22 = R(1, 1), R23 = R(1, 2);
const double& R31 = R(2, 0), R32 = R(2, 1), R33 = R(2, 2);
const Matrix3& R = Q.matrix();
const double &R11 = R(0, 0), R12 = R(0, 1), R13 = R(0, 2);
const double &R21 = R(1, 0), R22 = R(1, 1), R23 = R(1, 2);
const double &R31 = R(2, 0), R32 = R(2, 1), R33 = R(2, 2);
// Get trace(R)
const double tr = R.trace();
@ -213,7 +260,7 @@ Vector3 SO3::Logmap(const SO3& R, ChartJacobian H) {
omega = (M_PI / sqrt(2.0 + 2.0 * R11)) * Vector3(1.0 + R11, R21, R31);
} else {
double magnitude;
const double tr_3 = tr - 3.0; // always negative
const double tr_3 = tr - 3.0; // always negative
if (tr_3 < -1e-7) {
double theta = acos((tr - 1.0) / 2.0);
magnitude = theta / (2.0 * sin(theta));
@ -225,53 +272,49 @@ Vector3 SO3::Logmap(const SO3& R, ChartJacobian H) {
omega = magnitude * Vector3(R32 - R23, R13 - R31, R21 - R12);
}
if(H) *H = LogmapDerivative(omega);
if (H) *H = LogmapDerivative(omega);
return omega;
}
/* ************************************************************************* */
Matrix3 SO3::LogmapDerivative(const Vector3& omega) {
using std::cos;
using std::sin;
//******************************************************************************
// Chart at origin for SO3 is *not* Cayley but actual Expmap/Logmap
double theta2 = omega.dot(omega);
if (theta2 <= std::numeric_limits<double>::epsilon()) return I_3x3;
double theta = std::sqrt(theta2); // rotation angle
/** Right Jacobian for Log map in SO(3) - equation (10.86) and following equations in
* G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", Volume 2, 2008.
* logmap( Rhat * expmap(omega) ) \approx logmap( Rhat ) + Jrinv * omega
* where Jrinv = LogmapDerivative(omega);
* This maps a perturbation on the manifold (expmap(omega))
* to a perturbation in the tangent space (Jrinv * omega)
*/
const Matrix3 W = skewSymmetric(omega); // element of Lie algebra so(3): W = omega^
return I_3x3 + 0.5 * W +
(1 / (theta * theta) - (1 + cos(theta)) / (2 * theta * sin(theta))) *
W * W;
template <>
SO3 SO3::ChartAtOrigin::Retract(const Vector3& omega, ChartJacobian H) {
return Expmap(omega, H);
}
/* ************************************************************************* */
static Vector9 vec(const SO3& R) { return Eigen::Map<const Vector9>(R.data()); }
template <>
Vector3 SO3::ChartAtOrigin::Local(const SO3& R, ChartJacobian H) {
return Logmap(R, H);
}
static const std::vector<const Matrix3> G({SO3::Hat(Vector3::Unit(0)),
SO3::Hat(Vector3::Unit(1)),
SO3::Hat(Vector3::Unit(2))});
//******************************************************************************
// local vectorize
static Vector9 vec3(const Matrix3& R) {
return Eigen::Map<const Vector9>(R.data());
}
static const Matrix93 P =
(Matrix93() << vec(G[0]), vec(G[1]), vec(G[2])).finished();
// so<3> generators
static const std::vector<const Matrix3> G3({SO3::Hat(Vector3::Unit(0)),
SO3::Hat(Vector3::Unit(1)),
SO3::Hat(Vector3::Unit(2))});
/* ************************************************************************* */
// vectorized generators
static const Matrix93 P3 =
(Matrix93() << vec3(G3[0]), vec3(G3[1]), vec3(G3[2])).finished();
//******************************************************************************
template <>
Vector9 SO3::vec(OptionalJacobian<9, 3> H) const {
const SO3& R = *this;
const Matrix3& R = matrix_;
if (H) {
// As Luca calculated (for SO4), this is (I3 \oplus R) * P
*H << R * P.block<3, 3>(0, 0), R * P.block<3, 3>(3, 0),
R * P.block<3, 3>(6, 0);
// As Luca calculated (for SO4), this is (I3 \oplus R) * P3
*H << R * P3.block<3, 3>(0, 0), R * P3.block<3, 3>(3, 0),
R * P3.block<3, 3>(6, 0);
}
return gtsam::vec(R);
};
/* ************************************************************************* */
} // end namespace gtsam
return gtsam::vec3(R);
}
//******************************************************************************
} // end namespace gtsam

206
gtsam/geometry/SO3.h
View File

@ -20,146 +20,92 @@
#pragma once
#include <gtsam/geometry/SOn.h>
#include <gtsam/base/Lie.h>
#include <gtsam/base/Matrix.h>
#include <cmath>
#include <iosfwd>
#include <vector>
namespace gtsam {
using SO3 = SO<3>;
// Below are all declarations of SO<3> specializations.
// They are *defined* in SO3.cpp.
template <>
SO3 SO3::AxisAngle(const Vector3& axis, double theta);
template <>
SO3 SO3::ClosestTo(const Matrix3& M);
template <>
SO3 SO3::ChordalMean(const std::vector<SO3>& rotations);
template <>
Matrix3 SO3::Hat(const Vector3& xi); ///< make skew symmetric matrix
template <>
Vector3 SO3::Vee(const Matrix3& X); ///< inverse of Hat
/// Adjoint map
template <>
Matrix3 SO3::AdjointMap() const;
/**
* True SO(3), i.e., 3*3 matrix subgroup
* We guarantee (all but first) constructors only generate from sub-manifold.
* However, round-off errors in repeated composition could move off it...
* Exponential map at identity - create a rotation from canonical coordinates
* \f$ [R_x,R_y,R_z] \f$ using Rodrigues' formula
*/
class SO3 : public Matrix3, public LieGroup<SO3, 3> {
public:
enum { N = 3 };
enum { dimension = 3 };
template <>
SO3 SO3::Expmap(const Vector3& omega, ChartJacobian H);
/// @name Constructors
/// @{
/// Derivative of Expmap
template <>
Matrix3 SO3::ExpmapDerivative(const Vector3& omega);
/// Default constructor creates identity
SO3() : Matrix3(I_3x3) {}
/**
* Log map at identity - returns the canonical coordinates
* \f$ [R_x,R_y,R_z] \f$ of this rotation
*/
template <>
Vector3 SO3::Logmap(const SO3& R, ChartJacobian H);
/// Constructor from Eigen Matrix
template <typename Derived>
SO3(const MatrixBase<Derived>& R) : Matrix3(R.eval()) {}
/// Derivative of Logmap
template <>
Matrix3 SO3::LogmapDerivative(const Vector3& omega);
/// Constructor from AngleAxisd
SO3(const Eigen::AngleAxisd& angleAxis) : Matrix3(angleAxis) {}
// Chart at origin for SO3 is *not* Cayley but actual Expmap/Logmap
template <>
SO3 SO3::ChartAtOrigin::Retract(const Vector3& omega, ChartJacobian H);
/// Static, named constructor. TODO(dellaert): think about relation with above
GTSAM_EXPORT static SO3 AxisAngle(const Vector3& axis, double theta);
template <>
Vector3 SO3::ChartAtOrigin::Local(const SO3& R, ChartJacobian H);
/// Static, named constructor that finds SO(3) matrix closest to M in Frobenius norm.
static SO3 ClosestTo(const Matrix3& M);
template <>
Vector9 SO3::vec(OptionalJacobian<9, 3> H) const;
/// Static, named constructor that finds chordal mean = argmin_R \sum sqr(|R-R_i|_F).
static SO3 ChordalMean(const std::vector<SO3>& rotations);
/// @}
/// @name Testable
/// @{
GTSAM_EXPORT void print(const std::string& s) const;
bool equals(const SO3 & R, double tol) const {
return equal_with_abs_tol(*this, R, tol);
}
/// @}
/// @name Group
/// @{
/// identity rotation for group operation
static SO3 identity() {
return I_3x3;
}
/// inverse of a rotation = transpose
SO3 inverse() const {
return this->transpose();
}
/// @}
/// @name Lie Group
/// @{
static Matrix3 Hat(const Vector3 &xi); ///< make skew symmetric matrix
static Vector3 Vee(const Matrix3 &X); ///< inverse of Hat
/**
* Exponential map at identity - create a rotation from canonical coordinates
* \f$ [R_x,R_y,R_z] \f$ using Rodrigues' formula
*/
GTSAM_EXPORT static SO3 Expmap(const Vector3& omega, ChartJacobian H = boost::none);
/// Derivative of Expmap
GTSAM_EXPORT static Matrix3 ExpmapDerivative(const Vector3& omega);
/**
* Log map at identity - returns the canonical coordinates
* \f$ [R_x,R_y,R_z] \f$ of this rotation
*/
GTSAM_EXPORT static Vector3 Logmap(const SO3& R, ChartJacobian H = boost::none);
/// Derivative of Logmap
GTSAM_EXPORT 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;
/// @}
/// @name Other methods
/// @{
/// Vectorize
Vector9 vec(OptionalJacobian<9, 3> H = boost::none) const;
/// Return matrix (for wrapper)
const Matrix3& matrix() const { return *this;}
/// @
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/)
{
ar & boost::serialization::make_nvp("R11", (*this)(0,0));
ar & boost::serialization::make_nvp("R12", (*this)(0,1));
ar & boost::serialization::make_nvp("R13", (*this)(0,2));
ar & boost::serialization::make_nvp("R21", (*this)(1,0));
ar & boost::serialization::make_nvp("R22", (*this)(1,1));
ar & boost::serialization::make_nvp("R23", (*this)(1,2));
ar & boost::serialization::make_nvp("R31", (*this)(2,0));
ar & boost::serialization::make_nvp("R32", (*this)(2,1));
ar & boost::serialization::make_nvp("R33", (*this)(2,2));
}
};
/** Serialization function */
template <class Archive>
void serialize(Archive& ar, SO3& R, const unsigned int /*version*/) {
Matrix3& M = R.matrix_;
ar& boost::serialization::make_nvp("R11", M(0, 0));
ar& boost::serialization::make_nvp("R12", M(0, 1));
ar& boost::serialization::make_nvp("R13", M(0, 2));
ar& boost::serialization::make_nvp("R21", M(1, 0));
ar& boost::serialization::make_nvp("R22", M(1, 1));
ar& boost::serialization::make_nvp("R23", M(1, 2));
ar& boost::serialization::make_nvp("R31", M(2, 0));
ar& boost::serialization::make_nvp("R32", M(2, 1));
ar& boost::serialization::make_nvp("R33", M(2, 2));
}
namespace so3 {
/**
* Compose general matrix with an SO(3) element.
/**
* Compose general matrix with an SO(3) element.
* We only provide the 9*9 derivative in the first argument M.
*/
Matrix3 compose(const Matrix3& M, const SO3& R,
@ -184,7 +130,7 @@ class GTSAM_EXPORT ExpmapFunctor {
public:
/// Constructor with element of Lie algebra so(3)
ExpmapFunctor(const Vector3& omega, bool nearZeroApprox = false);
explicit ExpmapFunctor(const Vector3& omega, bool nearZeroApprox = false);
/// Constructor with axis-angle
ExpmapFunctor(const Vector3& axis, double angle, bool nearZeroApprox = false);
@ -201,7 +147,7 @@ class DexpFunctor : public ExpmapFunctor {
public:
/// Constructor with element of Lie algebra so(3)
GTSAM_EXPORT DexpFunctor(const Vector3& omega, bool nearZeroApprox = false);
GTSAM_EXPORT explicit 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,
@ -222,12 +168,14 @@ class DexpFunctor : public ExpmapFunctor {
};
} // namespace so3
template<>
struct traits<SO3> : public internal::LieGroup<SO3> {
};
/*
* Define the traits. internal::LieGroup provides both Lie group and Testable
*/
template<>
struct traits<const SO3> : public internal::LieGroup<SO3> {
};
} // end namespace gtsam
template <>
struct traits<SO3> : public internal::LieGroup<SO3> {};
template <>
struct traits<const SO3> : public internal::LieGroup<SO3> {};
} // end namespace gtsam

3
gtsam/geometry/SOn.h
View File

@ -272,8 +272,9 @@ class SO : public LieGroup<SO<N>, internal::DimensionSO(N)> {
/// @}
template <class Archive>
friend void serialize(Archive& ar, SO& R, const unsigned int /*version*/);
friend void serialize(Archive&, SO&, const unsigned int);
friend class boost::serialization::access;
friend class Rot3; // for serialize
};
using SOn = SO<Eigen::Dynamic>;

303
gtsam/geometry/SOt.cpp
View File

@ -1,303 +0,0 @@
/* ----------------------------------------------------------------------------
* 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
* -------------------------------------------------------------------------- */
/**
* @file SO3.cpp
* @brief 3*3 matrix representation of SO(3)
* @author Frank Dellaert
* @author Luca Carlone
* @author Duy Nguyen Ta
* @date December 2014
*/
#include <gtsam/base/concepts.h>
#include <gtsam/geometry/SOt.h>
#include <Eigen/SVD>
#include <cmath>
#include <iostream>
#include <limits>
namespace gtsam {
//******************************************************************************
namespace sot {
Matrix99 Dcompose(const SO3& Q) {
Matrix99 H;
auto R = Q.matrix();
H << I_3x3 * R(0, 0), I_3x3 * R(1, 0), I_3x3 * R(2, 0), //
I_3x3 * R(0, 1), I_3x3 * R(1, 1), I_3x3 * R(2, 1), //
I_3x3 * R(0, 2), I_3x3 * R(1, 2), I_3x3 * R(2, 2);
return H;
}
Matrix3 compose(const Matrix3& M, const SO3& R, OptionalJacobian<9, 9> H) {
Matrix3 MR = M * R.matrix();
if (H) *H = Dcompose(R);
return MR;
}
void ExpmapFunctor::init(bool nearZeroApprox) {
nearZero =
nearZeroApprox || (theta2 <= std::numeric_limits<double>::epsilon());
if (!nearZero) {
theta = std::sqrt(theta2); // rotation angle
sin_theta = std::sin(theta);
const double s2 = std::sin(theta / 2.0);
one_minus_cos = 2.0 * s2 * s2; // numerically better than [1 - cos(theta)]
}
}
ExpmapFunctor::ExpmapFunctor(const Vector3& omega, bool nearZeroApprox)
: theta2(omega.dot(omega)) {
const double wx = omega.x(), wy = omega.y(), wz = omega.z();
W << 0.0, -wz, +wy, +wz, 0.0, -wx, -wy, +wx, 0.0;
init(nearZeroApprox);
if (!nearZero) {
K = W / theta;
KK = K * K;
}
}
ExpmapFunctor::ExpmapFunctor(const Vector3& axis, double angle,
bool nearZeroApprox)
: theta2(angle * angle) {
const double ax = axis.x(), ay = axis.y(), az = axis.z();
K << 0.0, -az, +ay, +az, 0.0, -ax, -ay, +ax, 0.0;
W = K * angle;
init(nearZeroApprox);
if (!nearZero) {
KK = K * K;
}
}
SO3 ExpmapFunctor::expmap() const {
if (nearZero)
return SO3(I_3x3 + W);
else
return SO3(I_3x3 + sin_theta * K + one_minus_cos * KK);
}
DexpFunctor::DexpFunctor(const Vector3& omega, bool nearZeroApprox)
: ExpmapFunctor(omega, nearZeroApprox), omega(omega) {
if (nearZero)
dexp_ = I_3x3 - 0.5 * W;
else {
a = one_minus_cos / theta;
b = 1.0 - sin_theta / theta;
dexp_ = I_3x3 - a * K + b * KK;
}
}
Vector3 DexpFunctor::applyDexp(const Vector3& v, OptionalJacobian<3, 3> H1,
OptionalJacobian<3, 3> H2) const {
if (H1) {
if (nearZero) {
*H1 = 0.5 * skewSymmetric(v);
} else {
// TODO(frank): Iserles hints that there should be a form I + c*K + d*KK
const Vector3 Kv = K * v;
const double Da = (sin_theta - 2.0 * a) / theta2;
const double Db = (one_minus_cos - 3.0 * b) / theta2;
*H1 = (Db * K - Da * I_3x3) * Kv * omega.transpose() -
skewSymmetric(Kv * b / theta) +
(a * I_3x3 - b * K) * skewSymmetric(v / theta);
}
}
if (H2) *H2 = dexp_;
return dexp_ * v;
}
Vector3 DexpFunctor::applyInvDexp(const Vector3& v, OptionalJacobian<3, 3> H1,
OptionalJacobian<3, 3> H2) const {
const Matrix3 invDexp = dexp_.inverse();
const Vector3 c = invDexp * v;
if (H1) {
Matrix3 D_dexpv_omega;
applyDexp(c, D_dexpv_omega); // get derivative H of forward mapping
*H1 = -invDexp * D_dexpv_omega;
}
if (H2) *H2 = invDexp;
return c;
}
} // namespace sot
//******************************************************************************
template <>
SO3 SO3::AxisAngle(const Vector3& axis, double theta) {
return sot::ExpmapFunctor(axis, theta).expmap();
}
//******************************************************************************
template <>
SO3 SO3::ClosestTo(const Matrix3& M) {
Eigen::JacobiSVD<Matrix3> svd(M, Eigen::ComputeFullU | Eigen::ComputeFullV);
const auto& U = svd.matrixU();
const auto& V = svd.matrixV();
const double det = (U * V.transpose()).determinant();
return SO3(U * Vector3(1, 1, det).asDiagonal() * V.transpose());
}
//******************************************************************************
template <>
SO3 SO3::ChordalMean(const std::vector<SO3>& rotations) {
// See Hartley13ijcv:
// Cost function C(R) = \sum sqr(|R-R_i|_F)
// Closed form solution = ClosestTo(C_e), where C_e = \sum R_i !!!!
Matrix3 C_e{Z_3x3};
for (const auto& R_i : rotations) {
C_e += R_i.matrix();
}
return ClosestTo(C_e);
}
//******************************************************************************
template <>
Matrix3 SO3::Hat(const Vector3& xi) {
// skew symmetric matrix X = xi^
Matrix3 Y = Z_3x3;
Y(0, 1) = -xi(2);
Y(0, 2) = +xi(1);
Y(1, 2) = -xi(0);
return Y - Y.transpose();
}
//******************************************************************************
template <>
Vector3 SO3::Vee(const Matrix3& X) {
Vector3 xi;
xi(0) = -X(1, 2);
xi(1) = +X(0, 2);
xi(2) = -X(0, 1);
return xi;
}
//******************************************************************************
template <>
SO3 SO3::Expmap(const Vector3& omega, ChartJacobian H) {
if (H) {
sot::DexpFunctor impl(omega);
*H = impl.dexp();
return impl.expmap();
} else {
return sot::ExpmapFunctor(omega).expmap();
}
}
template <>
Matrix3 SO3::ExpmapDerivative(const Vector3& omega) {
return sot::DexpFunctor(omega).dexp();
}
//******************************************************************************
/* Right Jacobian for Log map in SO(3) - equation (10.86) and following
equations in G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie
Groups", Volume 2, 2008.
logmap( Rhat * expmap(omega) ) \approx logmap(Rhat) + Jrinv * omega
where Jrinv = LogmapDerivative(omega). This maps a perturbation on the
manifold (expmap(omega)) to a perturbation in the tangent space (Jrinv *
omega)
*/
template <>
Matrix3 SO3::LogmapDerivative(const Vector3& omega) {
using std::cos;
using std::sin;
double theta2 = omega.dot(omega);
if (theta2 <= std::numeric_limits<double>::epsilon()) return I_3x3;
double theta = std::sqrt(theta2); // rotation angle
// element of Lie algebra so(3): W = omega^
const Matrix3 W = Hat(omega);
return I_3x3 + 0.5 * W +
(1 / (theta * theta) - (1 + cos(theta)) / (2 * theta * sin(theta))) *
W * W;
}
//******************************************************************************
template <>
Vector3 SO3::Logmap(const SO3& Q, ChartJacobian H) {
using std::sin;
using std::sqrt;
// note switch to base 1
const Matrix3& R = Q.matrix();
const double &R11 = R(0, 0), R12 = R(0, 1), R13 = R(0, 2);
const double &R21 = R(1, 0), R22 = R(1, 1), R23 = R(1, 2);
const double &R31 = R(2, 0), R32 = R(2, 1), R33 = R(2, 2);
// Get trace(R)
const double tr = R.trace();
Vector3 omega;
// when trace == -1, i.e., when theta = +-pi, +-3pi, +-5pi, etc.
// we do something special
if (std::abs(tr + 1.0) < 1e-10) {
if (std::abs(R33 + 1.0) > 1e-10)
omega = (M_PI / sqrt(2.0 + 2.0 * R33)) * Vector3(R13, R23, 1.0 + R33);
else if (std::abs(R22 + 1.0) > 1e-10)
omega = (M_PI / sqrt(2.0 + 2.0 * R22)) * Vector3(R12, 1.0 + R22, R32);
else
// if(std::abs(R.r1_.x()+1.0) > 1e-10) This is implicit
omega = (M_PI / sqrt(2.0 + 2.0 * R11)) * Vector3(1.0 + R11, R21, R31);
} else {
double magnitude;
const double tr_3 = tr - 3.0; // always negative
if (tr_3 < -1e-7) {
double theta = acos((tr - 1.0) / 2.0);
magnitude = theta / (2.0 * sin(theta));
} else {
// when theta near 0, +-2pi, +-4pi, etc. (trace near 3.0)
// use Taylor expansion: theta \approx 1/2-(t-3)/12 + O((t-3)^2)
magnitude = 0.5 - tr_3 * tr_3 / 12.0;
}
omega = magnitude * Vector3(R32 - R23, R13 - R31, R21 - R12);
}
if (H) *H = LogmapDerivative(omega);
return omega;
}
//******************************************************************************
// local vectorize
static Vector9 vec3(const Matrix3& R) {
return Eigen::Map<const Vector9>(R.data());
}
// so<3> generators
static const std::vector<const Matrix3> G3({SO3::Hat(Vector3::Unit(0)),
SO3::Hat(Vector3::Unit(1)),
SO3::Hat(Vector3::Unit(2))});
// vectorized generators
static const Matrix93 P3 =
(Matrix93() << vec3(G3[0]), vec3(G3[1]), vec3(G3[2])).finished();
//******************************************************************************
template <>
Vector9 SO3::vec(OptionalJacobian<9, 3> H) const {
const Matrix3& R = matrix_;
if (H) {
// As Luca calculated (for SO4), this is (I3 \oplus R) * P3
*H << R * P3.block<3, 3>(0, 0), R * P3.block<3, 3>(3, 0),
R * P3.block<3, 3>(6, 0);
}
return gtsam::vec3(R);
}
//******************************************************************************
} // end namespace gtsam

187
gtsam/geometry/SOt.h
View File

@ -1,187 +0,0 @@
/* ----------------------------------------------------------------------------
* 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
* -------------------------------------------------------------------------- */
/**
* @file SO3.h
* @brief 3*3 matrix representation of SO(3)
* @author Frank Dellaert
* @author Luca Carlone
* @author Duy Nguyen Ta
* @date December 2014
*/
#pragma once
#include <gtsam/geometry/SOn.h>
#include <gtsam/base/Lie.h>
#include <gtsam/base/Matrix.h>
#include <cmath>
#include <iosfwd>
#include <vector>
namespace gtsam {
using SO3 = SO<3>;
// Below are all declarations of SO<3> specializations.
// They are *defined* in SO3.cpp.
template <>
SO3 SO3::AxisAngle(const Vector3& axis, double theta);
template <>
SO3 SO3::ClosestTo(const Matrix3& M);
template <>
SO3 SO3::ChordalMean(const std::vector<SO3>& rotations);
template <>
Matrix3 SO3::Hat(const Vector3& xi); ///< make skew symmetric matrix
template <>
Vector3 SO3::Vee(const Matrix3& X); ///< inverse of Hat
/// Adjoint map
template <>
Matrix3 SO3::AdjointMap() const {
return matrix_;
}
/**
* Exponential map at identity - create a rotation from canonical coordinates
* \f$ [R_x,R_y,R_z] \f$ using Rodrigues' formula
*/
template <>
SO3 SO3::Expmap(const Vector3& omega, ChartJacobian H);
/// Derivative of Expmap
template <>
Matrix3 SO3::ExpmapDerivative(const Vector3& omega);
/**
* Log map at identity - returns the canonical coordinates
* \f$ [R_x,R_y,R_z] \f$ of this rotation
*/
template <>
Vector3 SO3::Logmap(const SO3& R, ChartJacobian H);
/// Derivative of Logmap
template <>
Matrix3 SO3::LogmapDerivative(const Vector3& omega);
// Chart at origin for SO3 is *not* Cayley but actual Expmap/Logmap
template <>
SO3 SO3::ChartAtOrigin::Retract(const Vector3& omega, ChartJacobian H) {
return Expmap(omega, H);
}
template <>
Vector3 SO3::ChartAtOrigin::Local(const SO3& R, ChartJacobian H) {
return Logmap(R, H);
}
template <>
Vector9 SO3::vec(OptionalJacobian<9, 3> H) const;
/** Serialization function */
template <class Archive>
void serialize(Archive& ar, SO3& R, const unsigned int /*version*/) {
Matrix3& M = R.matrix_;
ar& boost::serialization::make_nvp("R11", M(0, 0));
ar& boost::serialization::make_nvp("R12", M(0, 1));
ar& boost::serialization::make_nvp("R13", M(0, 2));
ar& boost::serialization::make_nvp("R21", M(1, 0));
ar& boost::serialization::make_nvp("R22", M(1, 1));
ar& boost::serialization::make_nvp("R23", M(1, 2));
ar& boost::serialization::make_nvp("R31", M(2, 0));
ar& boost::serialization::make_nvp("R32", M(2, 1));
ar& boost::serialization::make_nvp("R33", M(2, 2));
}
namespace sot {
/**
* Compose general matrix with an SO(3) element.
* We only provide the 9*9 derivative in the first argument M.
*/
Matrix3 compose(const Matrix3& M, const SO3& R,
OptionalJacobian<9, 9> H = boost::none);
/// (constant) Jacobian of compose wrpt M
Matrix99 Dcompose(const SO3& R);
// Below are 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).
/// Functor implementing Exponential map
class GTSAM_EXPORT 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:
/// Constructor with element of Lie algebra so(3)
explicit ExpmapFunctor(const Vector3& omega, bool nearZeroApprox = false);
/// Constructor with axis-angle
ExpmapFunctor(const Vector3& axis, double angle, bool nearZeroApprox = false);
/// Rodrigues formula
SO3 expmap() const;
};
/// Functor that implements Exponential map *and* its derivatives
class GTSAM_EXPORT DexpFunctor : public ExpmapFunctor {
const Vector3 omega;
double a, b;
Matrix3 dexp_;
public:
/// Constructor with element of Lie algebra so(3)
explicit 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_; }
/// Multiplies with dexp(), with optional derivatives
Vector3 applyDexp(const Vector3& v, OptionalJacobian<3, 3> H1 = boost::none,
OptionalJacobian<3, 3> H2 = boost::none) const;
/// Multiplies with dexp().inverse(), with optional derivatives
Vector3 applyInvDexp(const Vector3& v,
OptionalJacobian<3, 3> H1 = boost::none,
OptionalJacobian<3, 3> H2 = boost::none) const;
};
} // namespace sot
/*
* Define the traits. internal::LieGroup provides both Lie group and Testable
*/
template <>
struct traits<SO3> : public internal::LieGroup<SO3> {};
template <>
struct traits<const SO3> : public internal::LieGroup<SO3> {};
} // end namespace gtsam

12
gtsam/geometry/tests/testSO3.cpp
View File

@ -18,7 +18,7 @@
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/testLie.h>
#include <gtsam/geometry/SOt.h>
#include <gtsam/geometry/SO3.h>
using namespace std;
using namespace gtsam;
@ -152,7 +152,7 @@ TEST(SO3, ChartDerivatives) {
}
/* ************************************************************************* */
TEST(SO3, Expmap) {
TEST(SO3, ExpmapFunctor) {
Vector axis = Vector3(0., 1., 0.); // rotation around Y
double angle = 3.14 / 4.0;
Matrix expected(3,3);
@ -306,11 +306,11 @@ TEST(SO3, ApplyDexp) {
for (bool nearZeroApprox : {true, false}) {
boost::function<Vector3(const Vector3&, const Vector3&)> f =
[=](const Vector3& omega, const Vector3& v) {
return sot::DexpFunctor(omega, nearZeroApprox).applyDexp(v);
return so3::DexpFunctor(omega, nearZeroApprox).applyDexp(v);
};
for (Vector3 omega : {Vector3(0, 0, 0), Vector3(1, 0, 0), Vector3(0, 1, 0),
Vector3(0, 0, 1), Vector3(0.1, 0.2, 0.3)}) {
sot::DexpFunctor local(omega, nearZeroApprox);
so3::DexpFunctor local(omega, nearZeroApprox);
for (Vector3 v : {Vector3(1, 0, 0), Vector3(0, 1, 0), Vector3(0, 0, 1),
Vector3(0.4, 0.3, 0.2)}) {
EXPECT(assert_equal(Vector3(local.dexp() * v),
@ -329,11 +329,11 @@ TEST(SO3, ApplyInvDexp) {
for (bool nearZeroApprox : {true, false}) {
boost::function<Vector3(const Vector3&, const Vector3&)> f =
[=](const Vector3& omega, const Vector3& v) {
return sot::DexpFunctor(omega, nearZeroApprox).applyInvDexp(v);
return so3::DexpFunctor(omega, nearZeroApprox).applyInvDexp(v);
};
for (Vector3 omega : {Vector3(0, 0, 0), Vector3(1, 0, 0), Vector3(0, 1, 0),
Vector3(0, 0, 1), Vector3(0.1, 0.2, 0.3)}) {
sot::DexpFunctor local(omega, nearZeroApprox);
so3::DexpFunctor local(omega, nearZeroApprox);
Matrix invDexp = local.dexp().inverse();
for (Vector3 v : {Vector3(1, 0, 0), Vector3(0, 1, 0), Vector3(0, 0, 1),
Vector3(0.4, 0.3, 0.2)}) {

2
gtsam/geometry/tests/testSO4.cpp
View File

@ -20,7 +20,7 @@
#include <gtsam/base/lieProxies.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/geometry/SO4.h>
#include <gtsam/geometry/SOt.h>
#include <gtsam/geometry/SO3.h>
#include <CppUnitLite/TestHarness.h>

2
gtsam/geometry/tests/testSOn.cpp
View File

@ -16,7 +16,7 @@
**/
#include <gtsam/geometry/SOn.h>
#include <gtsam/geometry/SOt.h>
#include <gtsam/geometry/SO3.h>
#include <gtsam/base/Lie.h>
#include <gtsam/base/Manifold.h>

24
gtsam/navigation/TangentPreintegration.cpp
View File

@ -68,30 +68,30 @@ Vector9 TangentPreintegration::UpdatePreintegrated(const Vector3& a_body,
Matrix3 w_tangent_H_theta, invH;
const Vector3 w_tangent = // angular velocity mapped back to tangent space
local.applyInvDexp(w_body, A ? &w_tangent_H_theta : 0, C ? &invH : 0);
const SO3 R = local.expmap();
const Rot3 R(local.expmap());
const Vector3 a_nav = R * a_body;
const double dt22 = 0.5 * dt * dt;
Vector9 preintegratedPlus;
preintegratedPlus << // new preintegrated vector:
theta + w_tangent * dt, // theta
position + velocity * dt + a_nav * dt22, // position
velocity + a_nav * dt; // velocity
preintegratedPlus << // new preintegrated vector:
theta + w_tangent * dt, // theta
position + velocity * dt + a_nav * dt22, // position
velocity + a_nav * dt; // velocity
if (A) {
// Exact derivative of R*a with respect to theta:
const Matrix3 a_nav_H_theta = R * skewSymmetric(-a_body) * local.dexp();
const Matrix3 a_nav_H_theta = R.matrix() * skewSymmetric(-a_body) * local.dexp();
A->setIdentity();
A->block<3, 3>(0, 0).noalias() += w_tangent_H_theta * dt; // theta
A->block<3, 3>(3, 0) = a_nav_H_theta * dt22; // position wrpt theta...
A->block<3, 3>(3, 6) = I_3x3 * dt; // .. and velocity
A->block<3, 3>(6, 0) = a_nav_H_theta * dt; // velocity wrpt theta
A->block<3, 3>(0, 0).noalias() += w_tangent_H_theta * dt; // theta
A->block<3, 3>(3, 0) = a_nav_H_theta * dt22; // position wrpt theta...
A->block<3, 3>(3, 6) = I_3x3 * dt; // .. and velocity
A->block<3, 3>(6, 0) = a_nav_H_theta * dt; // velocity wrpt theta
}
if (B) {
B->block<3, 3>(0, 0) = Z_3x3;
B->block<3, 3>(3, 0) = R * dt22;
B->block<3, 3>(6, 0) = R * dt;
B->block<3, 3>(3, 0) = R.matrix() * dt22;
B->block<3, 3>(6, 0) = R.matrix() * dt;
}
if (C) {
C->block<3, 3>(0, 0) = invH * dt;

2
gtsam/slam/tests/testKarcherMeanFactor.cpp
View File

@ -98,7 +98,7 @@ TEST(KarcherMean, FactorSO4) {
FindKarcherMean<SO4>({result.at<SO4>(1), result.at<SO4>(2)});
EXPECT(assert_equal(expected, actual));
EXPECT(assert_equal((Matrix)(Q * Q * Q).matrix(),
result.at<SO4>(1).between(result.at<SO4>(2))));
result.at<SO4>(1).between(result.at<SO4>(2)).matrix()));
}
/* ************************************************************************* */