Rot3 modernization: now derives from LieGroup, SLOW_CAYLEY is gone, retract and localCoordinates auto-generated so no more flag. Might re-add instance-based expmap and logmap in LieGroup for convenienece.
dellaert
parent
72c539fa9c
commit
8191ad5078
|
|
@ -660,19 +660,6 @@ Matrix expm(const Matrix& A, size_t K) {
|
|||
return E;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Matrix Cayley(const Matrix& A) {
|
||||
Matrix::Index n = A.cols();
|
||||
assert(A.rows() == n);
|
||||
|
||||
// original
|
||||
// const Matrix I = eye(n);
|
||||
// return (I-A)*inverse(I+A);
|
||||
|
||||
// inlined to let Eigen do more optimization
|
||||
return (Matrix::Identity(n, n) - A)*(Matrix::Identity(n, n) + A).inverse();
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
std::string formatMatrixIndented(const std::string& label, const Matrix& matrix, bool makeVectorHorizontal)
|
||||
{
|
||||
|
|
|
|||
|
|
@ -528,17 +528,6 @@ DLT(const Matrix& A, double rank_tol = 1e-9);
|
|||
*/
|
||||
GTSAM_EXPORT Matrix expm(const Matrix& A, size_t K=7);
|
||||
|
||||
/// Cayley transform
|
||||
GTSAM_EXPORT Matrix Cayley(const Matrix& A);
|
||||
|
||||
/// Implementation of Cayley transform using fixed size matrices to let
|
||||
/// Eigen do more optimization
|
||||
template<int N>
|
||||
Eigen::Matrix<double, N, N> CayleyFixed(const Eigen::Matrix<double, N, N>& A) {
|
||||
typedef Eigen::Matrix<double, N, N> FMat;
|
||||
return (FMat::Identity() - A)*(FMat::Identity() + A).inverse();
|
||||
}
|
||||
|
||||
std::string formatMatrixIndented(const std::string& label, const Matrix& matrix, bool makeVectorHorizontal = false);
|
||||
|
||||
} // namespace gtsam
|
||||
|
|
|
|||
|
|
@ -16,7 +16,7 @@
|
|||
**/
|
||||
|
||||
#include <gtsam/base/Lie.h>
|
||||
#include <gtsam/base/Matrix.h>
|
||||
#include <gtsam/base/concepts.h>
|
||||
|
||||
#define QUATERNION_TYPE Eigen::Quaternion<_Scalar,_Options>
|
||||
|
||||
|
|
@ -96,8 +96,7 @@ struct traits_x<QUATERNION_TYPE> {
|
|||
_Scalar angle = omega.norm();
|
||||
return Q(Eigen::AngleAxis<_Scalar>(angle, omega / angle));
|
||||
}
|
||||
if (H)
|
||||
throw std::runtime_error("TODO: implement Jacobian");
|
||||
if (H) CONCEPT_NOT_IMPLEMENTED;
|
||||
}
|
||||
|
||||
/// We use our own Logmap, as there is a slight bug in Eigen
|
||||
|
|
@ -129,8 +128,7 @@ struct traits_x<QUATERNION_TYPE> {
|
|||
return (angle / s) * q.vec();
|
||||
}
|
||||
|
||||
if (H)
|
||||
throw std::runtime_error("TODO: implement Jacobian");
|
||||
if (H) CONCEPT_NOT_IMPLEMENTED;
|
||||
}
|
||||
|
||||
/// @}
|
||||
|
|
|
|||
|
|
@ -58,20 +58,6 @@ Rot3 Rot3::rodriguez(const Vector3& w) {
|
|||
return rodriguez(w/t, t);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Rot3 Rot3::retract(const Vector& omega, OptionalJacobian<3, 3> Hthis,
|
||||
OptionalJacobian<3, 3> Hv, Rot3::CoordinatesMode mode) const {
|
||||
if (Hthis || Hv) CONCEPT_NOT_IMPLEMENTED;
|
||||
return retract(omega, mode);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Vector3 Rot3::localCoordinates(const Rot3& R2, OptionalJacobian<3, 3> Horigin,
|
||||
OptionalJacobian<3, 3> H2, Rot3::CoordinatesMode mode) const {
|
||||
if (Horigin || H2) CONCEPT_NOT_IMPLEMENTED;
|
||||
return localCoordinates(R2, mode);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
bool Rot3::equals(const Rot3 & R, double tol) const {
|
||||
return equal_with_abs_tol(matrix(), R.matrix(), tol);
|
||||
|
|
@ -248,8 +234,8 @@ ostream &operator<<(ostream &os, const Rot3& R) {
|
|||
Rot3 Rot3::slerp(double t, const Rot3& other) const {
|
||||
// amazingly simple in GTSAM :-)
|
||||
assert(t>=0 && t<=1);
|
||||
Vector3 omega = localCoordinates(other, Rot3::EXPMAP);
|
||||
return retract(t * omega, Rot3::EXPMAP);
|
||||
Vector3 omega = Logmap(between(other));
|
||||
return compose(Expmap(t * omega));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -52,7 +52,7 @@ namespace gtsam {
|
|||
* @addtogroup geometry
|
||||
* \nosubgrouping
|
||||
*/
|
||||
class GTSAM_EXPORT Rot3 {
|
||||
class GTSAM_EXPORT Rot3 : public LieGroup<Rot3,3> {
|
||||
|
||||
private:
|
||||
|
||||
|
|
@ -64,8 +64,6 @@ namespace gtsam {
|
|||
#endif
|
||||
|
||||
public:
|
||||
/// The intrinsic dimension of this manifold.
|
||||
enum { dimension = 3 };
|
||||
|
||||
/// @name Constructors and named constructors
|
||||
/// @{
|
||||
|
|
@ -210,16 +208,13 @@ namespace gtsam {
|
|||
return Rot3();
|
||||
}
|
||||
|
||||
/// derivative of inverse rotation R^T s.t. inverse(R)*R = identity
|
||||
Rot3 inverse(OptionalJacobian<3,3> H1=boost::none) const;
|
||||
|
||||
/// Compose two rotations i.e., R= (*this) * R2
|
||||
Rot3 compose(const Rot3& R2, OptionalJacobian<3, 3> H1 = boost::none,
|
||||
OptionalJacobian<3, 3> H2 = boost::none) const;
|
||||
|
||||
/** compose two rotations */
|
||||
Rot3 operator*(const Rot3& R2) const;
|
||||
|
||||
Rot3 inverse() const {
|
||||
return Rot3(Matrix3(transpose()));
|
||||
}
|
||||
|
||||
/**
|
||||
* Conjugation: given a rotation acting in frame B, compute rotation c1Rc2 acting in a frame C
|
||||
* @param cRb rotation from B frame to C frame
|
||||
|
|
@ -230,23 +225,10 @@ namespace gtsam {
|
|||
return cRb * (*this) * cRb.inverse();
|
||||
}
|
||||
|
||||
/**
|
||||
* Return relative rotation D s.t. R2=D*R1, i.e. D=R2*R1'
|
||||
*/
|
||||
Rot3 between(const Rot3& R2,
|
||||
OptionalJacobian<3,3> H1=boost::none,
|
||||
OptionalJacobian<3,3> H2=boost::none) const;
|
||||
|
||||
/// @}
|
||||
/// @name Manifold
|
||||
/// @{
|
||||
|
||||
/// dimension of the variable - used to autodetect sizes
|
||||
static size_t Dim() { return 3; }
|
||||
|
||||
/// return dimensionality of tangent space, DOF = 3
|
||||
size_t dim() const { return 3; }
|
||||
|
||||
/**
|
||||
* The method retract() is used to map from the tangent space back to the manifold.
|
||||
* Its inverse, is localCoordinates(). For Lie groups, an obvious retraction is the
|
||||
|
|
@ -260,21 +242,29 @@ namespace gtsam {
|
|||
EXPMAP, ///< Use the Lie group exponential map to retract
|
||||
#ifndef GTSAM_USE_QUATERNIONS
|
||||
CAYLEY, ///< Retract and localCoordinates using the Cayley transform.
|
||||
SLOW_CAYLEY ///< Slow matrix implementation of Cayley transform (for tests only).
|
||||
#endif
|
||||
};
|
||||
|
||||
#ifndef GTSAM_USE_QUATERNIONS
|
||||
|
||||
// Cayley chart around origin, no derivatives
|
||||
struct CayleyChart {
|
||||
static Rot3 Retract(const Vector3& v);
|
||||
static Vector3 Local(const Rot3& r);
|
||||
};
|
||||
|
||||
/// Retraction from R^3 to Rot3 manifold using the Cayley transform
|
||||
Rot3 retractCayley(const Vector& omega) const;
|
||||
Rot3 retractCayley(const Vector& omega) const {
|
||||
return compose(CayleyChart::Retract(omega));
|
||||
}
|
||||
|
||||
/// Inverse of retractCayley
|
||||
Vector3 localCayley(const Rot3& other) const {
|
||||
return CayleyChart::Local(between(other));
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
/// Retraction from R^3 \f$ [R_x,R_y,R_z] \f$ to Rot3 manifold neighborhood around current rotation
|
||||
Rot3 retract(const Vector& omega, Rot3::CoordinatesMode mode = ROT3_DEFAULT_COORDINATES_MODE) const;
|
||||
|
||||
/// Returns local retract coordinates \f$ [R_x,R_y,R_z] \f$ in neighborhood around current rotation
|
||||
Vector3 localCoordinates(const Rot3& t2, Rot3::CoordinatesMode mode = ROT3_DEFAULT_COORDINATES_MODE) const;
|
||||
|
||||
/// @}
|
||||
/// @name Lie Group
|
||||
/// @{
|
||||
|
|
@ -300,27 +290,16 @@ namespace gtsam {
|
|||
/// Derivative of Logmap
|
||||
static Matrix3 LogmapDerivative(const Vector3& x);
|
||||
|
||||
Rot3 retract(const Vector& omega, OptionalJacobian<3, 3> Hthis,
|
||||
OptionalJacobian<3, 3> Hv = boost::none, Rot3::CoordinatesMode mode =
|
||||
ROT3_DEFAULT_COORDINATES_MODE) const;
|
||||
|
||||
Vector3 localCoordinates(const Rot3& R2, OptionalJacobian<3, 3> Horigin,
|
||||
OptionalJacobian<3, 3> H2 = boost::none, Rot3::CoordinatesMode mode =
|
||||
ROT3_DEFAULT_COORDINATES_MODE) const;
|
||||
|
||||
/** Calculate Adjoint map */
|
||||
Matrix3 AdjointMap() const { return matrix(); }
|
||||
|
||||
/**
|
||||
* 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.
|
||||
*/
|
||||
static Matrix3 rightJacobianExpMapSO3(const Vector3& x);
|
||||
// Chart at origin, depends on compile-time flag ROT3_DEFAULT_COORDINATES_MODE
|
||||
struct ChartAtOrigin {
|
||||
static Rot3 Retract(const Vector3& v, ChartJacobian H = boost::none);
|
||||
static Vector3 Local(const Rot3& r, ChartJacobian H = boost::none);
|
||||
};
|
||||
|
||||
/** 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.
|
||||
*/
|
||||
static Matrix3 rightJacobianExpMapSO3inverse(const Vector3& x);
|
||||
using LieGroup<Rot3, 3>::inverse; // version with derivative
|
||||
|
||||
/// @}
|
||||
/// @name Group Action on Point3
|
||||
|
|
|
|||
|
|
@ -139,13 +139,6 @@ Rot3 Rot3::rodriguez(const Vector3& w, double theta) {
|
|||
-swy + C02, swx + C12, c + C22);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Rot3 Rot3::compose(const Rot3& R2, OptionalJacobian<3, 3> H1, OptionalJacobian<3, 3> H2) const {
|
||||
if (H1) *H1 = R2.transpose();
|
||||
if (H2) *H2 = I_3x3;
|
||||
return *this * R2;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Rot3 Rot3::operator*(const Rot3& R2) const {
|
||||
return Rot3(Matrix3(rot_*R2.rot_));
|
||||
|
|
@ -157,21 +150,6 @@ Matrix3 Rot3::transpose() const {
|
|||
return rot_.transpose();
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Rot3 Rot3::inverse(OptionalJacobian<3,3> H1) const {
|
||||
if (H1) *H1 = -rot_;
|
||||
return Rot3(Matrix3(transpose()));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Rot3 Rot3::between (const Rot3& R2,
|
||||
OptionalJacobian<3,3> H1, OptionalJacobian<3,3> H2) const {
|
||||
if (H1) *H1 = -(R2.transpose()*rot_);
|
||||
if (H2) *H2 = I_3x3;
|
||||
Matrix3 R12 = transpose()*R2.rot_;
|
||||
return Rot3(R12);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Point3 Rot3::rotate(const Point3& p,
|
||||
OptionalJacobian<3,3> H1, OptionalJacobian<3,3> H2) const {
|
||||
|
|
@ -228,26 +206,41 @@ Vector3 Rot3::Logmap(const Rot3& R, OptionalJacobian<3,3> H) {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Rot3 Rot3::retractCayley(const Vector& omega) const {
|
||||
Rot3 Rot3::CayleyChart::Retract(const Vector3& omega) {
|
||||
const double x = omega(0), y = omega(1), z = omega(2);
|
||||
const double x2 = x * x, y2 = y * y, z2 = z * z;
|
||||
const double xy = x * y, xz = x * z, yz = y * z;
|
||||
const double f = 1.0 / (4.0 + x2 + y2 + z2), _2f = 2.0 * f;
|
||||
return (*this)
|
||||
* Rot3((4 + x2 - y2 - z2) * f, (xy - 2 * z) * _2f, (xz + 2 * y) * _2f,
|
||||
return Rot3((4 + x2 - y2 - z2) * f, (xy - 2 * z) * _2f, (xz + 2 * y) * _2f,
|
||||
(xy + 2 * z) * _2f, (4 - x2 + y2 - z2) * f, (yz - 2 * x) * _2f,
|
||||
(xz - 2 * y) * _2f, (yz + 2 * x) * _2f, (4 - x2 - y2 + z2) * f);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Rot3 Rot3::retract(const Vector& omega, Rot3::CoordinatesMode mode) const {
|
||||
if(mode == Rot3::EXPMAP) {
|
||||
return (*this)*Expmap(omega);
|
||||
} else if(mode == Rot3::CAYLEY) {
|
||||
return retractCayley(omega);
|
||||
} else if(mode == Rot3::SLOW_CAYLEY) {
|
||||
Matrix3 Omega = skewSymmetric(omega);
|
||||
return (*this)*CayleyFixed<3>(-Omega/2);
|
||||
Vector3 Rot3::CayleyChart::Local(const Rot3& R) {
|
||||
// Create a fixed-size matrix
|
||||
Matrix3 A = R.matrix();
|
||||
// Mathematica closed form optimization (procrastination?) gone wild:
|
||||
const double a = A(0, 0), b = A(0, 1), c = A(0, 2);
|
||||
const double d = A(1, 0), e = A(1, 1), f = A(1, 2);
|
||||
const double g = A(2, 0), h = A(2, 1), i = A(2, 2);
|
||||
const double di = d * i, ce = c * e, cd = c * d, fg = f * g;
|
||||
const double M = 1 + e - f * h + i + e * i;
|
||||
const double K = -4.0 / (cd * h + M + a * M - g * (c + ce) - b * (d + di - fg));
|
||||
const double x = a * f - cd + f;
|
||||
const double y = b * f - ce - c;
|
||||
const double z = fg - di - d;
|
||||
return K * Vector3(x, y, z);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Rot3 Rot3::ChartAtOrigin::Retract(const Vector3& omega, ChartJacobian H) {
|
||||
static const CoordinatesMode mode = ROT3_DEFAULT_COORDINATES_MODE;
|
||||
if (mode == Rot3::EXPMAP) return Expmap(omega, H);
|
||||
|
||||
if (H) CONCEPT_NOT_IMPLEMENTED;
|
||||
if(mode == Rot3::CAYLEY) {
|
||||
return CayleyChart::Retract(omega);
|
||||
} else {
|
||||
assert(false);
|
||||
exit(1);
|
||||
|
|
@ -255,29 +248,13 @@ Rot3 Rot3::retract(const Vector& omega, Rot3::CoordinatesMode mode) const {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Vector3 Rot3::localCoordinates(const Rot3& T, Rot3::CoordinatesMode mode) const {
|
||||
if(mode == Rot3::EXPMAP) {
|
||||
return Logmap(between(T));
|
||||
} else if(mode == Rot3::CAYLEY) {
|
||||
// Create a fixed-size matrix
|
||||
Matrix3 A = rot_.transpose() * T.matrix();
|
||||
// Mathematica closed form optimization (procrastination?) gone wild:
|
||||
const double a=A(0,0),b=A(0,1),c=A(0,2);
|
||||
const double d=A(1,0),e=A(1,1),f=A(1,2);
|
||||
const double g=A(2,0),h=A(2,1),i=A(2,2);
|
||||
const double di = d*i, ce = c*e, cd = c*d, fg=f*g;
|
||||
const double M = 1 + e - f*h + i + e*i;
|
||||
const double K = - 4.0 / (cd*h + M + a*M -g*(c + ce) - b*(d + di - fg));
|
||||
const double x = a * f - cd + f;
|
||||
const double y = b * f - ce - c;
|
||||
const double z = fg - di - d;
|
||||
return K * Vector3(x, y, z);
|
||||
} else if(mode == Rot3::SLOW_CAYLEY) {
|
||||
// Create a fixed-size matrix
|
||||
Matrix3 A(between(T).matrix());
|
||||
// using templated version of Cayley
|
||||
Matrix3 Omega = CayleyFixed<3>(A);
|
||||
return -2*Vector3(Omega(2,1),Omega(0,2),Omega(1,0));
|
||||
Vector3 Rot3::ChartAtOrigin::Local(const Rot3& R, ChartJacobian H) {
|
||||
static const CoordinatesMode mode = ROT3_DEFAULT_COORDINATES_MODE;
|
||||
if (mode == Rot3::EXPMAP) return Logmap(R,H);
|
||||
|
||||
if (H) CONCEPT_NOT_IMPLEMENTED;
|
||||
if(mode == Rot3::CAYLEY) {
|
||||
return CayleyChart::Local(R);
|
||||
} else {
|
||||
assert(false);
|
||||
exit(1);
|
||||
|
|
|
|||
|
|
@ -294,12 +294,10 @@ TEST(Rot3, manifold_expmap)
|
|||
Rot3 origin;
|
||||
|
||||
// log behaves correctly
|
||||
Vector d12 = gR1.localCoordinates(gR2, Rot3::EXPMAP);
|
||||
CHECK(assert_equal(gR2, gR1.retract(d12, Rot3::EXPMAP)));
|
||||
Vector d21 = gR2.localCoordinates(gR1, Rot3::EXPMAP);
|
||||
CHECK(assert_equal(gR1, gR2.retract(d21, Rot3::EXPMAP)));
|
||||
Vector d12 = Rot3::Logmap(gR1.between(gR2));
|
||||
Vector d21 = Rot3::Logmap(gR2.between(gR1));
|
||||
|
||||
// Check that it is expmap
|
||||
// Check expmap
|
||||
CHECK(assert_equal(gR2, gR1*Rot3::Expmap(d12)));
|
||||
CHECK(assert_equal(gR1, gR2*Rot3::Expmap(d21)));
|
||||
|
||||
|
|
@ -596,6 +594,12 @@ TEST(Rot3, quaternion) {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Matrix Cayley(const Matrix& A) {
|
||||
Matrix::Index n = A.cols();
|
||||
const Matrix I = eye(n);
|
||||
return (I-A)*inverse(I+A);
|
||||
}
|
||||
|
||||
TEST( Rot3, Cayley ) {
|
||||
Matrix A = skewSymmetric(1,2,-3);
|
||||
Matrix Q = Cayley(A);
|
||||
|
|
|
|||
|
|
@ -10,21 +10,14 @@
|
|||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file testRot3.cpp
|
||||
* @brief Unit tests for Rot3 class
|
||||
* @file testRot3M.cpp
|
||||
* @brief Unit tests for Rot3 class, matrix version
|
||||
* @author Alireza Fathi
|
||||
* @author Frank Dellaert
|
||||
*/
|
||||
|
||||
#include <gtsam/geometry/Point3.h>
|
||||
#include <gtsam/geometry/Rot3.h>
|
||||
|
||||
#include <gtsam/base/Testable.h>
|
||||
#include <gtsam/base/numericalDerivative.h>
|
||||
#include <gtsam/base/lieProxies.h>
|
||||
|
||||
#include <boost/math/constants/constants.hpp>
|
||||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
|
||||
#ifndef GTSAM_USE_QUATERNIONS
|
||||
|
|
@ -46,38 +39,10 @@ TEST(Rot3, manifold_cayley)
|
|||
Rot3 origin;
|
||||
|
||||
// log behaves correctly
|
||||
Vector d12 = gR1.localCoordinates(gR2, Rot3::CAYLEY);
|
||||
CHECK(assert_equal(gR2, gR1.retract(d12, Rot3::CAYLEY)));
|
||||
Vector d21 = gR2.localCoordinates(gR1, Rot3::CAYLEY);
|
||||
CHECK(assert_equal(gR1, gR2.retract(d21, Rot3::CAYLEY)));
|
||||
|
||||
// Check that log(t1,t2)=-log(t2,t1)
|
||||
CHECK(assert_equal(d12,-d21));
|
||||
|
||||
// lines in canonical coordinates correspond to Abelian subgroups in SO(3)
|
||||
Vector d = Vector3(0.1, 0.2, 0.3);
|
||||
// exp(-d)=inverse(exp(d))
|
||||
CHECK(assert_equal(Rot3::Expmap(-d),Rot3::Expmap(d).inverse()));
|
||||
// exp(5d)=exp(2*d+3*d)=exp(2*d)exp(3*d)=exp(3*d)exp(2*d)
|
||||
Rot3 R2 = Rot3::Expmap (2 * d);
|
||||
Rot3 R3 = Rot3::Expmap (3 * d);
|
||||
Rot3 R5 = Rot3::Expmap (5 * d);
|
||||
CHECK(assert_equal(R5,R2*R3));
|
||||
CHECK(assert_equal(R5,R3*R2));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(Rot3, manifold_slow_cayley)
|
||||
{
|
||||
Rot3 gR1 = Rot3::rodriguez(0.1, 0.4, 0.2);
|
||||
Rot3 gR2 = Rot3::rodriguez(0.3, 0.1, 0.7);
|
||||
Rot3 origin;
|
||||
|
||||
// log behaves correctly
|
||||
Vector d12 = gR1.localCoordinates(gR2, Rot3::SLOW_CAYLEY);
|
||||
CHECK(assert_equal(gR2, gR1.retract(d12, Rot3::SLOW_CAYLEY)));
|
||||
Vector d21 = gR2.localCoordinates(gR1, Rot3::SLOW_CAYLEY);
|
||||
CHECK(assert_equal(gR1, gR2.retract(d21, Rot3::SLOW_CAYLEY)));
|
||||
Vector d12 = gR1.localCayley(gR2);
|
||||
CHECK(assert_equal(gR2, gR1.retractCayley(d12)));
|
||||
Vector d21 = gR2.localCayley(gR1);
|
||||
CHECK(assert_equal(gR1, gR2.retractCayley(d21)));
|
||||
|
||||
// Check that log(t1,t2)=-log(t2,t1)
|
||||
CHECK(assert_equal(d12,-d21));
|
||||
|
|
|
|||
|
|
@ -140,8 +140,7 @@ Vector3 AHRSFactor::PreintegratedMeasurements::predict(const Vector3& bias,
|
|||
boost::optional<Matrix&> H) const {
|
||||
const Vector3 biasOmegaIncr = bias - biasHat_;
|
||||
Vector3 delRdelBiasOmega_biasOmegaIncr = delRdelBiasOmega_ * biasOmegaIncr;
|
||||
const Rot3 deltaRij_biascorrected = deltaRij_.retract(
|
||||
delRdelBiasOmega_biasOmegaIncr, Rot3::EXPMAP);
|
||||
const Rot3 deltaRij_biascorrected = deltaRij_ * Rot3::Expmap(delRdelBiasOmega_biasOmegaIncr);
|
||||
const Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected);
|
||||
if (H) {
|
||||
const Matrix3 Jrinv_theta_bc = //
|
||||
|
|
|
|||
|
|
@ -280,7 +280,7 @@ Vector CombinedImuFactor::evaluateError(const Pose3& pose_i, const Vector3& vel_
|
|||
|
||||
// We compute factor's Jacobians, according to [3]
|
||||
/* ---------------------------------------------------------------------------------------------------- */
|
||||
const Rot3 deltaRij_biascorrected = preintegratedMeasurements_.deltaRij_.retract(preintegratedMeasurements_.delRdelBiasOmega_ * biasOmegaIncr, Rot3::EXPMAP);
|
||||
const Rot3 deltaRij_biascorrected = preintegratedMeasurements_.deltaRij_ * Rot3::Expmap(preintegratedMeasurements_.delRdelBiasOmega_ * biasOmegaIncr);
|
||||
// deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr)
|
||||
|
||||
Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected);
|
||||
|
|
@ -480,7 +480,7 @@ PoseVelocityBias CombinedImuFactor::Predict(const Pose3& pose_i, const Vector3&
|
|||
vel_j += - skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij; // 2nd order term for velocity
|
||||
}
|
||||
|
||||
const Rot3 deltaRij_biascorrected = preintegratedMeasurements.deltaRij_.retract(preintegratedMeasurements.delRdelBiasOmega_ * biasOmegaIncr, Rot3::EXPMAP);
|
||||
const Rot3 deltaRij_biascorrected = preintegratedMeasurements.deltaRij_ * Rot3::Expmap(preintegratedMeasurements.delRdelBiasOmega_ * biasOmegaIncr);
|
||||
// deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr)
|
||||
Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected);
|
||||
Vector3 theta_biascorrected_corioliscorrected = theta_biascorrected -
|
||||
|
|
|
|||
|
|
@ -262,7 +262,7 @@ Vector ImuFactor::evaluateError(const Pose3& pose_i, const Vector3& vel_i, const
|
|||
|
||||
// We compute factor's Jacobians
|
||||
/* ---------------------------------------------------------------------------------------------------- */
|
||||
const Rot3 deltaRij_biascorrected = preintegratedMeasurements_.deltaRij_.retract(preintegratedMeasurements_.delRdelBiasOmega_ * biasOmegaIncr, Rot3::EXPMAP);
|
||||
const Rot3 deltaRij_biascorrected = preintegratedMeasurements_.deltaRij_ * Rot3::Expmap(preintegratedMeasurements_.delRdelBiasOmega_ * biasOmegaIncr);
|
||||
// deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr)
|
||||
|
||||
Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected);
|
||||
|
|
@ -422,7 +422,7 @@ PoseVelocity ImuFactor::Predict(const Pose3& pose_i, const Vector3& vel_i,
|
|||
vel_j += - skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij; // 2nd order term for velocity
|
||||
}
|
||||
|
||||
const Rot3 deltaRij_biascorrected = preintegratedMeasurements.deltaRij_.retract(preintegratedMeasurements.delRdelBiasOmega_ * biasOmegaIncr, Rot3::EXPMAP);
|
||||
const Rot3 deltaRij_biascorrected = preintegratedMeasurements.deltaRij_ * Rot3::Expmap(preintegratedMeasurements.delRdelBiasOmega_ * biasOmegaIncr);
|
||||
// deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr)
|
||||
Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected);
|
||||
Vector3 theta_biascorrected_corioliscorrected = theta_biascorrected -
|
||||
|
|
|
|||
Loading…
Reference in New Issue