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

Tightened what is needed for Lie, i.e., fewer versions of retract/localCoordinates

release/4.3a0
dellaert 2014-12-22 23:42:52 +01:00
parent f5c9c24330
commit c6ae119414
14 changed files with 191 additions and 139 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
gtsam/base/Lie.h
View File

@ -53,26 +53,16 @@ struct LieGroup : Testable<T> {
typedef OptionalJacobian<dimension, dimension> ChartJacobian;
static int GetDimension(const ManifoldType& m){ return m.dim(); }
static TangentVector Local(const ManifoldType& origin,
const ManifoldType& other) {
return origin.localCoordinates(other);
}
static ManifoldType Retract(const ManifoldType& origin,
const TangentVector& v) {
return origin.retract(v);
}
static TangentVector Local(const ManifoldType& origin,
const ManifoldType& other,
ChartJacobian Horigin,
ChartJacobian Horigin = boost::none,
ChartJacobian Hother = boost::none) {
return origin.localCoordinates(other, Horigin, Hother);
}
static ManifoldType Retract(const ManifoldType& origin,
const TangentVector& v,
ChartJacobian Horigin,
ChartJacobian Horigin = boost::none,
ChartJacobian Hv = boost::none) {
return origin.retract(v, Horigin, Hv);
}
@ -82,19 +72,11 @@ struct LieGroup : Testable<T> {
/// @name Lie Group
/// @{
static TangentVector Logmap(const ManifoldType& m) {
return ManifoldType::Logmap(m);
}
static ManifoldType Expmap(const TangentVector& v) {
return ManifoldType::Expmap(v);
}
static TangentVector Logmap(const ManifoldType& m, ChartJacobian Hm) {
static TangentVector Logmap(const ManifoldType& m, ChartJacobian Hm = boost::none) {
return ManifoldType::Logmap(m, Hm);
}
static ManifoldType Expmap(const TangentVector& v, ChartJacobian Hv) {
static ManifoldType Expmap(const TangentVector& v, ChartJacobian Hv = boost::none) {
return ManifoldType::Expmap(v, Hv);
}

26
gtsam/geometry/Pose2.cpp
View File

@ -17,7 +17,10 @@
#include <gtsam/geometry/concepts.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/concepts.h>
#include <boost/foreach.hpp>
#include <cmath>
#include <iostream>
#include <iomanip>
@ -56,7 +59,8 @@ bool Pose2::equals(const Pose2& q, double tol) const {
}
/* ************************************************************************* */
Pose2 Pose2::Expmap(const Vector& xi) {
Pose2 Pose2::Expmap(const Vector& xi, OptionalJacobian<3, 3> H) {
if (H) CONCEPT_NOT_IMPLEMENTED;
assert(xi.size() == 3);
Point2 v(xi(0),xi(1));
double w = xi(2);
@ -71,7 +75,8 @@ Pose2 Pose2::Expmap(const Vector& xi) {
}
/* ************************************************************************* */
Vector3 Pose2::Logmap(const Pose2& p) {
Vector3 Pose2::Logmap(const Pose2& p, OptionalJacobian<3, 3> H) {
if (H) CONCEPT_NOT_IMPLEMENTED;
const Rot2& R = p.r();
const Point2& t = p.t();
double w = R.theta();
@ -87,7 +92,9 @@ Vector3 Pose2::Logmap(const Pose2& p) {
}
/* ************************************************************************* */
Pose2 Pose2::retract(const Vector& v) const {
Pose2 Pose2::retract(const Vector& v, OptionalJacobian<3, 3> Hthis,
OptionalJacobian<3, 3> Hv) const {
if (Hthis || Hv) CONCEPT_NOT_IMPLEMENTED;
#ifdef SLOW_BUT_CORRECT_EXPMAP
return compose(Expmap(v));
#else
@ -97,7 +104,9 @@ Pose2 Pose2::retract(const Vector& v) const {
}
/* ************************************************************************* */
Vector3 Pose2::localCoordinates(const Pose2& p2) const {
Vector Pose2::localCoordinates(const Pose2& p2, OptionalJacobian<3, 3> Hthis,
OptionalJacobian<3, 3> Hother) const {
if (Hthis || Hother) CONCEPT_NOT_IMPLEMENTED;
#ifdef SLOW_BUT_CORRECT_EXPMAP
return Logmap(between(p2));
#else
@ -106,15 +115,6 @@ Vector3 Pose2::localCoordinates(const Pose2& p2) const {
#endif
}
/// Local 3D coordinates \f$ [T_x,T_y,\theta] \f$ of Pose2 manifold neighborhood around current pose
Vector Pose2::localCoordinates(const Pose2& p2, OptionalJacobian<3, 3> Hthis,
OptionalJacobian<3, 3> Hother) const {
if (Hthis || Hother)
throw std::runtime_error(
"Pose2::localCoordinates derivatives not implemented");
return localCoordinates(p2);
}
/* ************************************************************************* */
// Calculate Adjoint map
// Ad_pose is 3*3 matrix that when applied to twist xi, returns Ad_pose(xi)

13
gtsam/geometry/Pose2.h
View File

@ -136,23 +136,22 @@ public:
inline size_t dim() const { return 3; }
/// Retraction from R^3 \f$ [T_x,T_y,\theta] \f$ to Pose2 manifold neighborhood around current pose
Pose2 retract(const Vector& v) const;
Pose2 retract(const Vector& v, OptionalJacobian<3, 3> Hthis =
boost::none, OptionalJacobian<3, 3> Hv = boost::none) const;
/// Local 3D coordinates \f$ [T_x,T_y,\theta] \f$ of Pose2 manifold neighborhood around current pose
Vector3 localCoordinates(const Pose2& p2) const;
/// Local 3D coordinates \f$ [T_x,T_y,\theta] \f$ of Pose2 manifold neighborhood around current pose
Vector localCoordinates(const Pose2& p2, OptionalJacobian<3,3> Hthis, OptionalJacobian<3,3> Hother) const;
Vector localCoordinates(const Pose2& p2, OptionalJacobian<3, 3> Hthis =
boost::none, OptionalJacobian<3, 3> Hother = boost::none) const;
/// @}
/// @name Lie Group
/// @{
///Exponential map at identity - create a rotation from canonical coordinates \f$ [T_x,T_y,\theta] \f$
static Pose2 Expmap(const Vector& xi);
static Pose2 Expmap(const Vector& xi, OptionalJacobian<3, 3> H = boost::none);
///Log map at identity - return the canonical coordinates \f$ [T_x,T_y,\theta] \f$ of this rotation
static Vector3 Logmap(const Pose2& p);
static Vector3 Logmap(const Pose2& p, OptionalJacobian<3, 3> H = boost::none);
/**
* Calculate Adjoint map

22
gtsam/geometry/Pose3.cpp
View File

@ -17,6 +17,7 @@
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/concepts.h>
#include <gtsam/base/concepts.h>
#include <boost/foreach.hpp>
#include <iostream>
@ -124,7 +125,8 @@ bool Pose3::equals(const Pose3& pose, double tol) const {
/* ************************************************************************* */
/** Modified from Murray94book version (which assumes w and v normalized?) */
Pose3 Pose3::Expmap(const Vector& xi) {
Pose3 Pose3::Expmap(const Vector& xi, OptionalJacobian<6, 6> H) {
if (H) CONCEPT_NOT_IMPLEMENTED;
// get angular velocity omega and translational velocity v from twist xi
Point3 w(xi(0), xi(1), xi(2)), v(xi(3), xi(4), xi(5));
@ -144,7 +146,9 @@ Pose3 Pose3::Expmap(const Vector& xi) {
}
/* ************************************************************************* */
Vector6 Pose3::Logmap(const Pose3& p) {
Vector6 Pose3::Logmap(const Pose3& p, OptionalJacobian<6, 6> H) {
if (H) CONCEPT_NOT_IMPLEMENTED;
Vector3 w = Rot3::Logmap(p.rotation()), T = p.translation().vector();
double t = w.norm();
if (t < 1e-10) {
@ -218,6 +222,20 @@ Vector6 Pose3::localCoordinates(const Pose3& T,
}
}
/* ************************************************************************* */
Pose3 Pose3::retract(const Vector& d, OptionalJacobian<6, 6> Hthis,
OptionalJacobian<6, 6> Hd, Pose3::CoordinatesMode mode) const {
if (Hthis || Hd) CONCEPT_NOT_IMPLEMENTED;
return retract(d, mode);
}
/* ************************************************************************* */
Vector6 Pose3::localCoordinates(const Pose3& T2, OptionalJacobian<6, 6> Horigin,
OptionalJacobian<6, 6> Hother, Pose3::CoordinatesMode mode) const {
if (Horigin || Hother) CONCEPT_NOT_IMPLEMENTED;
return localCoordinates(T2, mode);
}
/* ************************************************************************* */
Matrix4 Pose3::matrix() const {
const Matrix3 R = R_.matrix();

19
gtsam/geometry/Pose3.h
View File

@ -155,7 +155,8 @@ public:
POSE3_DEFAULT_COORDINATES_MODE) const;
/// Local 6D coordinates \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$ of Pose3 manifold neighborhood around current pose
Vector6 localCoordinates(const Pose3& T2, Pose3::CoordinatesMode mode =POSE3_DEFAULT_COORDINATES_MODE) const;
Vector6 localCoordinates(const Pose3& T2, Pose3::CoordinatesMode mode =
POSE3_DEFAULT_COORDINATES_MODE) const;
/// @}
@ -163,18 +164,20 @@ public:
/// @{
/// Exponential map at identity - create a rotation from canonical coordinates \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$
static Pose3 Expmap(const Vector& xi);
static Pose3 Expmap(const Vector& xi, OptionalJacobian<6, 6> H = boost::none);
/// Log map at identity - return the canonical coordinates \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$ of this rotation
static Vector6 Logmap(const Pose3& p);
static Vector6 Logmap(const Pose3& p, OptionalJacobian<6, 6> H = boost::none);
/// Local 6D coordinates \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$ of Pose3 manifold neighborhood around current pose
Pose3 retract(const Vector& d, OptionalJacobian<6, 6> Hthis,
OptionalJacobian<6, 6> Hd = boost::none, Pose3::CoordinatesMode mode =
POSE3_DEFAULT_COORDINATES_MODE) const;
/// Local 6D coordinates \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$ of Pose3 manifold neighborhood around current pose
Vector6 localCoordinates(const Pose3& T2, OptionalJacobian<6, 6> Horigin,
OptionalJacobian<6,6> Hother = boost::none) const {
if (Horigin || Hother)
throw std::runtime_error("Pose3::localCoordinates derivatives not implemented");
return localCoordinates(T2);
}
OptionalJacobian<6, 6> Hother = boost::none, Pose3::CoordinatesMode mode =
POSE3_DEFAULT_COORDINATES_MODE) const;
/**
* Calculate Adjoint map, transforming a twist in the this pose's (i.e, body) frame to the world spatial frame

58
gtsam/geometry/Rot2.cpp
View File

@ -59,6 +59,64 @@ Rot2& Rot2::normalize() {
return *this;
}
/* ************************************************************************* */
Rot2 Rot2::compose(const Rot2& R, OptionalJacobian<1, 1> H1,
OptionalJacobian<1, 1> H2) const {
if (H1)
*H1 = I_1x1;
if (H2)
*H2 = I_1x1;
return fromCosSin(c_ * R.c_ - s_ * R.s_, s_ * R.c_ + c_ * R.s_);
}
/* ************************************************************************* */
Rot2 Rot2::between(const Rot2& R, OptionalJacobian<1, 1> H1,
OptionalJacobian<1, 1> H2) const {
if (H1)
*H1 = -I_1x1;
if (H2)
*H2 = I_1x1;
return fromCosSin(c_ * R.c_ + s_ * R.s_, -s_ * R.c_ + c_ * R.s_);
}
/* ************************************************************************* */
Rot2 Rot2::retract(const Vector& v, OptionalJacobian<1, 1> H1,
OptionalJacobian<1, 1> H2) const {
if (H1)
*H1 = I_1x1;
if (H2)
*H2 = I_1x1;
return *this * Expmap(v);
}
/* ************************************************************************* */
Vector1 Rot2::localCoordinates(const Rot2& t2, OptionalJacobian<1, 1> H1,
OptionalJacobian<1, 1> H2) const {
if (H1)
*H1 = -I_1x1;
if (H2)
*H2 = I_1x1;
return Logmap(between(t2));
}
/* ************************************************************************* */
Rot2 Rot2::Expmap(const Vector& v, OptionalJacobian<1, 1> H) {
if (H)
*H = I_1x1;
if (zero(v))
return (Rot2());
else
return Rot2::fromAngle(v(0));
}
/* ************************************************************************* */
Vector1 Rot2::Logmap(const Rot2& r, OptionalJacobian<1, 1> H) {
if (H)
*H = I_1x1;
Vector1 v;
v << r.theta();
return v;
}
/* ************************************************************************* */
Matrix2 Rot2::matrix() const {
Matrix2 rvalue_;

39
gtsam/geometry/Rot2.h
View File

@ -115,26 +115,18 @@ namespace gtsam {
return Rot2(c_, -s_);
}
/** Compose - make a new rotation by adding angles */
inline Rot2 compose(const Rot2& R, OptionalJacobian<1,1> H1 =
boost::none, OptionalJacobian<1,1> H2 = boost::none) const {
if (H1) *H1 = I_1x1; // set to 1x1 identity matrix
if (H2) *H2 = I_1x1; // set to 1x1 identity matrix
return fromCosSin(c_ * R.c_ - s_ * R.s_, s_ * R.c_ + c_ * R.s_);
}
/** Compose - make a new rotation by adding angles */
Rot2 operator*(const Rot2& R) const {
return fromCosSin(c_ * R.c_ - s_ * R.s_, s_ * R.c_ + c_ * R.s_);
}
/** Compose - make a new rotation by adding angles */
Rot2 compose(const Rot2& R, OptionalJacobian<1,1> H1 =
boost::none, OptionalJacobian<1,1> H2 = boost::none) const;
/** Between using the default implementation */
inline Rot2 between(const Rot2& R, OptionalJacobian<1,1> H1 =
boost::none, OptionalJacobian<1,1> H2 = boost::none) const {
if (H1) *H1 = -I_1x1; // set to 1x1 identity matrix
if (H2) *H2 = I_1x1; // set to 1x1 identity matrix
return fromCosSin(c_ * R.c_ + s_ * R.s_, -s_ * R.c_ + c_ * R.s_);
}
Rot2 between(const Rot2& R, OptionalJacobian<1,1> H1 =
boost::none, OptionalJacobian<1,1> H2 = boost::none) const;
/// @}
/// @name Manifold
@ -151,29 +143,22 @@ namespace gtsam {
}
/// Updates a with tangent space delta
inline Rot2 retract(const Vector& v) const { return *this * Expmap(v); }
Rot2 retract(const Vector& v, OptionalJacobian<1, 1> H1 = boost::none,
OptionalJacobian<1, 1> H2 = boost::none) const;
/// Returns inverse retraction
inline Vector1 localCoordinates(const Rot2& t2) const { return Logmap(between(t2)); }
Vector1 localCoordinates(const Rot2& t2, OptionalJacobian<1, 1> H1 =
boost::none, OptionalJacobian<1, 1> H2 = boost::none) const;
/// @}
/// @name Lie Group
/// @{
///Exponential map at identity - create a rotation from canonical coordinates
static Rot2 Expmap(const Vector& v) {
if (zero(v))
return (Rot2());
else
return Rot2::fromAngle(v(0));
}
static Rot2 Expmap(const Vector& v, OptionalJacobian<1, 1> H = boost::none);
///Log map at identity - return the canonical coordinates of this rotation
static inline Vector1 Logmap(const Rot2& r) {
Vector1 v;
v << r.theta();
return v;
}
static Vector1 Logmap(const Rot2& r, OptionalJacobian<1, 1> H = boost::none);
/// Left-trivialized derivative of the exponential map
static Matrix dexpL(const Vector& v) {

14
gtsam/geometry/Rot3.cpp
View File

@ -58,6 +58,20 @@ 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);

26
gtsam/geometry/Rot3.h
View File

@ -21,8 +21,10 @@
#pragma once
#include <gtsam/config.h> // Get GTSAM_USE_QUATERNIONS macro
#include <gtsam/geometry/Quaternion.h>
#include <gtsam/geometry/Unit3.h>
#include <gtsam/base/concepts.h>
#include <gtsam/config.h> // Get GTSAM_USE_QUATERNIONS macro
// You can override the default coordinate mode using this flag
#ifndef ROT3_DEFAULT_COORDINATES_MODE
@ -40,11 +42,6 @@
#endif
#endif
#include <gtsam/base/DerivedValue.h>
#include <gtsam/base/Matrix.h>
#include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/Unit3.h>
namespace gtsam {
/**
@ -285,7 +282,8 @@ namespace gtsam {
* Exponential map at identity - create a rotation from canonical coordinates
* \f$ [R_x,R_y,R_z] \f$ using Rodriguez' formula
*/
static Rot3 Expmap(const Vector& v) {
static Rot3 Expmap(const Vector& v, OptionalJacobian<3, 3> H = boost::none) {
if (H) CONCEPT_NOT_IMPLEMENTED;
if(zero(v)) return Rot3();
else return rodriguez(v);
}
@ -293,7 +291,7 @@ namespace gtsam {
/**
* Log map at identity - return the canonical coordinates \f$ [R_x,R_y,R_z] \f$ of this rotation
*/
static Vector3 Logmap(const Rot3& R);
static Vector3 Logmap(const Rot3& R, OptionalJacobian<3, 3> H = boost::none);
/// Left-trivialized derivative of the exponential map
static Matrix3 dexpL(const Vector3& v);
@ -301,13 +299,13 @@ namespace gtsam {
/// Left-trivialized derivative inverse of the exponential map
static Matrix3 dexpInvL(const Vector3& v);
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, Rot3::CoordinatesMode mode =
ROT3_DEFAULT_COORDINATES_MODE) const {
if (Horigin || H2)
throw std::runtime_error("Rot3::localCoordinates derivatives not implemented");
return localCoordinates(R2, mode);
}
OptionalJacobian<3, 3> H2 = boost::none, Rot3::CoordinatesMode mode =
ROT3_DEFAULT_COORDINATES_MODE) const;
/**
* Right Jacobian for Exponential map in SO(3) - equation (10.86) and following equations in

4
gtsam/geometry/Rot3M.cpp
View File

@ -184,7 +184,9 @@ Point3 Rot3::rotate(const Point3& p,
/* ************************************************************************* */
// Log map at identity - return the canonical coordinates of this rotation
Vector3 Rot3::Logmap(const Rot3& R) {
Vector3 Rot3::Logmap(const Rot3& R, OptionalJacobian<3, 3> H) {
if (H) CONCEPT_NOT_IMPLEMENTED;
static const double PI = boost::math::constants::pi<double>();

3
gtsam/geometry/Rot3Q.cpp
View File

@ -134,7 +134,8 @@ namespace gtsam {
}
/* ************************************************************************* */
Vector3 Rot3::Logmap(const Rot3& R) {
Vector3 Rot3::Logmap(const Rot3& R, OptionalJacobian<3, 3> H) {
if (H) CONCEPT_NOT_IMPLEMENTED;
return QuaternionChart::Logmap(R.quaternion_);
}

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

@ -231,7 +231,7 @@ TEST( Rot3, rightJacobianExpMapSO3 )
Vector3 thetahat; thetahat << 0.1, 0, 0;
Matrix expectedJacobian = numericalDerivative11<Rot3, Vector3>(
boost::bind(&Rot3::Expmap, _1), thetahat);
boost::bind(&Rot3::Expmap, _1, boost::none), thetahat);
Matrix actualJacobian = Rot3::rightJacobianExpMapSO3(thetahat);
CHECK(assert_equal(expectedJacobian, actualJacobian));
}

46
gtsam_unstable/dynamics/PoseRTV.cpp
View File

@ -57,14 +57,16 @@ void PoseRTV::print(const string& s) const {
}
/* ************************************************************************* */
PoseRTV PoseRTV::Expmap(const Vector9& v) {
PoseRTV PoseRTV::Expmap(const Vector9& v, ChartJacobian H) {
if (H) CONCEPT_NOT_IMPLEMENTED;
Pose3 newPose = Pose3::Expmap(v.head<6>());
Velocity3 newVel = Velocity3(v.tail<3>());
return PoseRTV(newPose, newVel);
}
/* ************************************************************************* */
Vector9 PoseRTV::Logmap(const PoseRTV& p) {
Vector9 PoseRTV::Logmap(const PoseRTV& p, ChartJacobian H) {
if (H) CONCEPT_NOT_IMPLEMENTED;
Vector6 Lx = Pose3::Logmap(p.Rt_);
Vector3 Lv = p.v_.vector();
return (Vector9() << Lx, Lv).finished();
@ -72,14 +74,9 @@ Vector9 PoseRTV::Logmap(const PoseRTV& p) {
/* ************************************************************************* */
PoseRTV PoseRTV::retract(const Vector& v,
OptionalJacobian<dimension, dimension> Horigin,
OptionalJacobian<dimension, dimension> Hv) const {
if (Horigin) {
CONCEPT_NOT_IMPLEMENTED;
}
if (Hv) {
CONCEPT_NOT_IMPLEMENTED;
}
ChartJacobian Horigin,
ChartJacobian Hv) const {
if (Horigin || Hv) CONCEPT_NOT_IMPLEMENTED;
assert(v.size() == 9);
// First order approximation
Pose3 newPose = Rt_.retract(sub(v, 0, 6));
@ -89,14 +86,9 @@ PoseRTV PoseRTV::retract(const Vector& v,
/* ************************************************************************* */
Vector PoseRTV::localCoordinates(const PoseRTV& p1,
OptionalJacobian<dimension, dimension> Horigin,
OptionalJacobian<dimension, dimension> Hp) const {
if (Horigin) {
CONCEPT_NOT_IMPLEMENTED;
}
if (Hp) {
CONCEPT_NOT_IMPLEMENTED;
}
ChartJacobian Horigin,
ChartJacobian Hp) const {
if (Horigin || Hp) CONCEPT_NOT_IMPLEMENTED;
const Pose3& x0 = pose(), &x1 = p1.pose();
// First order approximation
Vector6 poseLogmap = x0.localCoordinates(x1);
@ -106,16 +98,15 @@ Vector PoseRTV::localCoordinates(const PoseRTV& p1,
/* ************************************************************************* */
PoseRTV inverse_(const PoseRTV& p) { return p.inverse(); }
PoseRTV PoseRTV::inverse(OptionalJacobian<dimension,dimension> H1) const {
PoseRTV PoseRTV::inverse(ChartJacobian H1) const {
if (H1) *H1 = numericalDerivative11<PoseRTV,PoseRTV>(inverse_, *this, 1e-5);
return PoseRTV(Rt_.inverse(), - v_);
}
/* ************************************************************************* */
PoseRTV compose_(const PoseRTV& p1, const PoseRTV& p2) { return p1.compose(p2); }
PoseRTV PoseRTV::compose(const PoseRTV& p,
OptionalJacobian<dimension,dimension> H1,
OptionalJacobian<dimension,dimension> H2) const {
PoseRTV PoseRTV::compose(const PoseRTV& p, ChartJacobian H1,
ChartJacobian H2) const {
if (H1) *H1 = numericalDerivative21(compose_, *this, p, 1e-5);
if (H2) *H2 = numericalDerivative22(compose_, *this, p, 1e-5);
return PoseRTV(Rt_.compose(p.Rt_), v_+p.v_);
@ -124,8 +115,8 @@ PoseRTV PoseRTV::compose(const PoseRTV& p,
/* ************************************************************************* */
PoseRTV between_(const PoseRTV& p1, const PoseRTV& p2) { return p1.between(p2); }
PoseRTV PoseRTV::between(const PoseRTV& p,
OptionalJacobian<dimension,dimension> H1,
OptionalJacobian<dimension,dimension> H2) const {
ChartJacobian H1,
ChartJacobian H2) const {
if (H1) *H1 = numericalDerivative21(between_, *this, p, 1e-5);
if (H2) *H2 = numericalDerivative22(between_, *this, p, 1e-5);
return inverse().compose(p);
@ -243,7 +234,7 @@ Point3 PoseRTV::translationIntegration(const Rot3& r2, const Velocity3& v2, doub
/* ************************************************************************* */
double range_(const PoseRTV& A, const PoseRTV& B) { return A.range(B); }
double PoseRTV::range(const PoseRTV& other,
OptionalJacobian<1,dimension> H1, OptionalJacobian<1,dimension> H2) const {
OptionalJacobian<1,9> H1, OptionalJacobian<1,9> H2) const {
if (H1) *H1 = numericalDerivative21(range_, *this, other, 1e-5);
if (H2) *H2 = numericalDerivative22(range_, *this, other, 1e-5);
return t().distance(other.t());
@ -254,9 +245,8 @@ PoseRTV transformed_from_(const PoseRTV& global, const Pose3& transform) {
return global.transformed_from(transform);
}
PoseRTV PoseRTV::transformed_from(const Pose3& trans,
OptionalJacobian<dimension,dimension> Dglobal,
OptionalJacobian<dimension,traits_x<Pose3>::dimension> Dtrans) const {
PoseRTV PoseRTV::transformed_from(const Pose3& trans, ChartJacobian Dglobal,
OptionalJacobian<9, 6> Dtrans) const {
// Note that we rotate the velocity
Matrix DVr, DTt;
Velocity3 newvel = trans.rotation().rotate(v_, DVr, DTt);

28
gtsam_unstable/dynamics/PoseRTV.h
View File

@ -25,6 +25,8 @@ protected:
Pose3 Rt_;
Velocity3 v_;
typedef OptionalJacobian<9, 9> ChartJacobian;
public:
enum { dimension = 9 };
@ -80,39 +82,39 @@ public:
* - v(3-5): Point3
* - v(6-8): Translational velocity
*/
PoseRTV retract(const Vector& v, OptionalJacobian<dimension, dimension> Horigin=boost::none, OptionalJacobian<dimension, dimension> Hv=boost::none) const;
Vector localCoordinates(const PoseRTV& p, OptionalJacobian<dimension, dimension> Horigin=boost::none,OptionalJacobian<dimension, dimension> Hp=boost::none) const;
PoseRTV retract(const Vector& v, ChartJacobian Horigin=boost::none, ChartJacobian Hv=boost::none) const;
Vector localCoordinates(const PoseRTV& p, ChartJacobian Horigin=boost::none,ChartJacobian Hp=boost::none) const;
// Lie TODO IS this a Lie group or just a Manifold????
/**
* expmap/logmap are poor approximations that assume independence of components
* Currently implemented using the poor retract/unretract approximations
*/
static PoseRTV Expmap(const Vector9& v);
static Vector9 Logmap(const PoseRTV& p);
static PoseRTV Expmap(const Vector9& v, ChartJacobian H = boost::none);
static Vector9 Logmap(const PoseRTV& p, ChartJacobian H = boost::none);
static PoseRTV identity() { return PoseRTV(); }
/** Derivatives calculated numerically */
PoseRTV inverse(OptionalJacobian<dimension,dimension> H1=boost::none) const;
PoseRTV inverse(ChartJacobian H1=boost::none) const;
/** Derivatives calculated numerically */
PoseRTV compose(const PoseRTV& p,
OptionalJacobian<dimension,dimension> H1=boost::none,
OptionalJacobian<dimension,dimension> H2=boost::none) const;
ChartJacobian H1=boost::none,
ChartJacobian H2=boost::none) const;
PoseRTV operator*(const PoseRTV& p) const { return compose(p); }
/** Derivatives calculated numerically */
PoseRTV between(const PoseRTV& p,
OptionalJacobian<dimension,dimension> H1=boost::none,
OptionalJacobian<dimension,dimension> H2=boost::none) const;
ChartJacobian H1=boost::none,
ChartJacobian H2=boost::none) const;
// measurement functions
/** Derivatives calculated numerically */
double range(const PoseRTV& other,
OptionalJacobian<1,dimension> H1=boost::none,
OptionalJacobian<1,dimension> H2=boost::none) const;
OptionalJacobian<1,9> H1=boost::none,
OptionalJacobian<1,9> H2=boost::none) const;
// IMU-specific
@ -159,8 +161,8 @@ public:
* Note: the transform jacobian convention is flipped
*/
PoseRTV transformed_from(const Pose3& trans,
OptionalJacobian<dimension,dimension> Dglobal=boost::none,
OptionalJacobian<dimension,traits_x<Pose3>::dimension> Dtrans=boost::none) const;
ChartJacobian Dglobal = boost::none,
OptionalJacobian<9, 6> Dtrans = boost::none) const;
// Utility functions