PoseRTV is now implemented using ProductLieGroup
dellaert
parent
8939adc799
commit
d060d4621e
|
|
@ -3,12 +3,9 @@
|
|||
* @author Alex Cunningham
|
||||
*/
|
||||
|
||||
#include <gtsam/base/numericalDerivative.h>
|
||||
#include <gtsam/base/Vector.h>
|
||||
|
||||
#include <gtsam/geometry/Pose2.h>
|
||||
|
||||
#include <gtsam_unstable/dynamics/PoseRTV.h>
|
||||
#include <gtsam/geometry/Pose2.h>
|
||||
#include <gtsam/base/Vector.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
|
|
@ -58,48 +55,6 @@ void PoseRTV::print(const string& s) const {
|
|||
velocity().print(" 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, ChartJacobian H) {
|
||||
if (H) CONCEPT_NOT_IMPLEMENTED;
|
||||
Vector6 Lx = Pose3::Logmap(p.pose());
|
||||
Vector3 Lv = p.velocity().vector();
|
||||
return (Vector9() << Lx, Lv).finished();
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
PoseRTV inverse_(const PoseRTV& p) { return p.inverse(); }
|
||||
PoseRTV PoseRTV::inverse(ChartJacobian H1) const {
|
||||
if (H1) *H1 = numericalDerivative11<PoseRTV,PoseRTV>(inverse_, *this, 1e-5);
|
||||
return PoseRTV(pose().inverse(), - velocity());
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
PoseRTV compose_(const PoseRTV& p1, const PoseRTV& p2) { return p1.compose(p2); }
|
||||
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(pose().compose(p.pose()), velocity()+p.velocity());
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
PoseRTV between_(const PoseRTV& p1, const PoseRTV& p2) { return p1.between(p2); }
|
||||
PoseRTV PoseRTV::between(const PoseRTV& p,
|
||||
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);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
PoseRTV PoseRTV::planarDynamics(double vel_rate, double heading_rate,
|
||||
double max_accel, double dt) const {
|
||||
|
|
@ -210,54 +165,40 @@ 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,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());
|
||||
Matrix36 D_t1_pose, D_t2_other;
|
||||
const Point3 t1 = pose().translation(H1 ? &D_t1_pose : 0);
|
||||
const Point3 t2 = other.pose().translation(H2 ? &D_t2_other : 0);
|
||||
Matrix13 D_d_t1, D_d_t2;
|
||||
double d = t1.distance(t2, H1 ? &D_d_t1 : 0, H2 ? &D_d_t2 : 0);
|
||||
if (H1) *H1 << D_d_t1 * D_t1_pose, 0,0,0;
|
||||
if (H2) *H2 << D_d_t2 * D_t2_other, 0,0,0;
|
||||
return d;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
PoseRTV transformed_from_(const PoseRTV& global, const Pose3& transform) {
|
||||
return global.transformed_from(transform);
|
||||
}
|
||||
|
||||
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(velocity(), DVr, DTt);
|
||||
if (!Dglobal && !Dtrans)
|
||||
return PoseRTV(trans.compose(pose()), newvel);
|
||||
|
||||
// Pose3 transform is just compose
|
||||
Matrix DTc, DGc;
|
||||
Pose3 newpose = trans.compose(pose(), DTc, DGc);
|
||||
Matrix6 D_newpose_trans, D_newpose_pose;
|
||||
Pose3 newpose = trans.compose(pose(), D_newpose_trans, D_newpose_pose);
|
||||
|
||||
// Note that we rotate the velocity
|
||||
Matrix3 D_newvel_R, D_newvel_v;
|
||||
Velocity3 newvel = trans.rotation().rotate(velocity(), D_newvel_R, D_newvel_v);
|
||||
|
||||
if (Dglobal) {
|
||||
*Dglobal = zeros(9,9);
|
||||
insertSub(*Dglobal, DGc, 0, 0);
|
||||
|
||||
// Rotate velocity
|
||||
insertSub(*Dglobal, eye(3,3), 6, 6); // FIXME: should this actually be an identity matrix?
|
||||
Dglobal->setZero();
|
||||
Dglobal->topLeftCorner<6,6>() = D_newpose_pose;
|
||||
Dglobal->bottomRightCorner<3,3>() = D_newvel_v;
|
||||
}
|
||||
|
||||
if (Dtrans) {
|
||||
*Dtrans = numericalDerivative22(transformed_from_, *this, trans, 1e-8);
|
||||
//
|
||||
// *Dtrans = zeros(9,6);
|
||||
// // directly affecting the pose
|
||||
// insertSub(*Dtrans, DTc, 0, 0); // correct in tests
|
||||
//
|
||||
// // rotating the velocity
|
||||
// Matrix vRhat = skewSymmetric(-velocity().x(), -velocity().y(), -velocity().z());
|
||||
// trans.rotation().print("Transform rotation");
|
||||
// gtsam::print(vRhat, "vRhat");
|
||||
// gtsam::print(DVr, "DVr");
|
||||
// // FIXME: find analytic derivative
|
||||
//// insertSub(*Dtrans, vRhat, 6, 0); // works if PoseRTV.rotation() = I
|
||||
//// insertSub(*Dtrans, trans.rotation().matrix() * vRhat, 6, 0); // FAIL: both tests fail
|
||||
Dtrans->setZero();
|
||||
Dtrans->topLeftCorner<6,6>() = D_newpose_trans;
|
||||
Dtrans->bottomLeftCorner<3,3>() = D_newvel_R;
|
||||
}
|
||||
return PoseRTV(newpose, newvel);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -8,45 +8,10 @@
|
|||
|
||||
#include <gtsam_unstable/base/dllexport.h>
|
||||
#include <gtsam/geometry/Pose3.h>
|
||||
#include <gtsam/base/Lie.h>
|
||||
#include <gtsam/base/ProductLieGroup.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/// CRTP to construct the product Lie group of two other Lie groups, G and H
|
||||
/// Assumes manifold structure from G and H, and binary constructor
|
||||
template<class Derived, typename G, typename H>
|
||||
class ProductLieGroup: public ProductManifold<Derived, G, H> {
|
||||
BOOST_CONCEPT_ASSERT((IsLieGroup<G>));
|
||||
BOOST_CONCEPT_ASSERT((IsLieGroup<H>));
|
||||
typedef ProductManifold<Derived, G, H> Base;
|
||||
|
||||
public:
|
||||
enum {dimension = G::dimension + H::dimension};
|
||||
inline static size_t Dim() {return dimension;}
|
||||
inline size_t dim() const {return dimension;}
|
||||
|
||||
typedef Eigen::Matrix<double, dimension, 1> TangentVector;
|
||||
|
||||
/// Default constructor yields identity
|
||||
ProductLieGroup():Base(traits<G>::Identity(),traits<H>::Identity()) {}
|
||||
|
||||
// Construct from two subgroup elements
|
||||
ProductLieGroup(const G& g, const H& h):Base(g,h) {}
|
||||
|
||||
ProductLieGroup operator*(const Derived& other) const {
|
||||
return Derived(traits<G>::Compose(this->g(),other.g()), traits<H>::Compose(this->h(),other.h()));
|
||||
}
|
||||
ProductLieGroup inverse() const {
|
||||
return Derived(this->g().inverse(), this->h().inverse());
|
||||
}
|
||||
};
|
||||
|
||||
// Define any direct product group to be a model of the multiplicative Group concept
|
||||
template<class Derived, typename G, typename H>
|
||||
struct traits<ProductLieGroup<Derived, G, H> > : internal::LieGroupTraits<
|
||||
ProductLieGroup<Derived, G, H> > {
|
||||
};
|
||||
|
||||
/// Syntactic sugar to clarify components
|
||||
typedef Point3 Velocity3;
|
||||
|
||||
|
|
@ -54,14 +19,13 @@ typedef Point3 Velocity3;
|
|||
* Robot state for use with IMU measurements
|
||||
* - contains translation, translational velocity and rotation
|
||||
*/
|
||||
class GTSAM_UNSTABLE_EXPORT PoseRTV : private ProductLieGroup<PoseRTV,Pose3,Velocity3> {
|
||||
class GTSAM_UNSTABLE_EXPORT PoseRTV : public ProductLieGroup<Pose3,Velocity3> {
|
||||
protected:
|
||||
|
||||
typedef ProductLieGroup<PoseRTV,Pose3,Velocity3> Base;
|
||||
typedef ProductLieGroup<Pose3,Velocity3> Base;
|
||||
typedef OptionalJacobian<9, 9> ChartJacobian;
|
||||
|
||||
public:
|
||||
enum { dimension = 9 };
|
||||
|
||||
// constructors - with partial versions
|
||||
PoseRTV() {}
|
||||
|
|
@ -76,6 +40,10 @@ public:
|
|||
explicit PoseRTV(const Pose3& pose)
|
||||
: Base(pose,Velocity3()) {}
|
||||
|
||||
// Construct from Base
|
||||
PoseRTV(const Base& base)
|
||||
: Base(base) {}
|
||||
|
||||
/** build from components - useful for data files */
|
||||
PoseRTV(double roll, double pitch, double yaw, double x, double y, double z,
|
||||
double vx, double vy, double vz);
|
||||
|
|
@ -104,52 +72,26 @@ public:
|
|||
bool equals(const PoseRTV& other, double tol=1e-6) const;
|
||||
void print(const std::string& s="") const;
|
||||
|
||||
// Manifold
|
||||
static size_t Dim() { return 9; }
|
||||
size_t dim() const { return Dim(); }
|
||||
/// @name Manifold
|
||||
/// @{
|
||||
using Base::dimension;
|
||||
using Base::dim;
|
||||
using Base::Dim;
|
||||
using Base::retract;
|
||||
using Base::localCoordinates;
|
||||
/// @}
|
||||
|
||||
/**
|
||||
* retract/unretract assume independence of components
|
||||
* Tangent space parameterization:
|
||||
* - v(0-2): Rot3 (roll, pitch, yaw)
|
||||
* - v(3-5): Point3
|
||||
* - v(6-8): Translational velocity
|
||||
*/
|
||||
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;
|
||||
/// @name measurement functions
|
||||
/// @{
|
||||
|
||||
// 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, ChartJacobian H = boost::none);
|
||||
static Vector9 Logmap(const PoseRTV& p, ChartJacobian H = boost::none);
|
||||
|
||||
static PoseRTV identity() { return PoseRTV(); }
|
||||
|
||||
/** Derivatives calculated numerically */
|
||||
PoseRTV inverse(ChartJacobian H1=boost::none) const;
|
||||
|
||||
/** Derivatives calculated numerically */
|
||||
PoseRTV compose(const PoseRTV& p,
|
||||
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,
|
||||
ChartJacobian H1=boost::none,
|
||||
ChartJacobian H2=boost::none) const;
|
||||
|
||||
// measurement functions
|
||||
/** Derivatives calculated numerically */
|
||||
/** range between translations */
|
||||
double range(const PoseRTV& other,
|
||||
OptionalJacobian<1,9> H1=boost::none,
|
||||
OptionalJacobian<1,9> H2=boost::none) const;
|
||||
/// @}
|
||||
|
||||
// IMU-specific
|
||||
/// @name IMU-specific
|
||||
/// @{
|
||||
|
||||
/// Dynamics integrator for ground robots
|
||||
/// Always move from time 1 to time 2
|
||||
|
|
@ -197,7 +139,9 @@ public:
|
|||
ChartJacobian Dglobal = boost::none,
|
||||
OptionalJacobian<9, 6> Dtrans = boost::none) const;
|
||||
|
||||
// Utility functions
|
||||
/// @}
|
||||
/// @name Utility functions
|
||||
/// @{
|
||||
|
||||
/// RRTMbn - Function computes the rotation rate transformation matrix from
|
||||
/// body axis rates to euler angle (global) rates
|
||||
|
|
@ -210,6 +154,7 @@ public:
|
|||
static Matrix RRTMnb(const Vector& euler);
|
||||
|
||||
static Matrix RRTMnb(const Rot3& att);
|
||||
/// @}
|
||||
|
||||
private:
|
||||
/** Serialization function */
|
||||
|
|
|
|||
|
|
@ -76,11 +76,12 @@ TEST( testPoseRTV, equals ) {
|
|||
/* ************************************************************************* */
|
||||
TEST( testPoseRTV, Lie ) {
|
||||
// origin and zero deltas
|
||||
EXPECT(assert_equal(PoseRTV(), PoseRTV().retract(zero(9)), tol));
|
||||
EXPECT(assert_equal(zero(9), PoseRTV().localCoordinates(PoseRTV()), tol));
|
||||
PoseRTV identity;
|
||||
EXPECT(assert_equal(identity, (PoseRTV)identity.retract(zero(9)), tol));
|
||||
EXPECT(assert_equal(zero(9), identity.localCoordinates(identity), tol));
|
||||
|
||||
PoseRTV state1(pt, rot, vel);
|
||||
EXPECT(assert_equal(state1, state1.retract(zero(9)), tol));
|
||||
EXPECT(assert_equal(state1, (PoseRTV)state1.retract(zero(9)), tol));
|
||||
EXPECT(assert_equal(zero(9), state1.localCoordinates(state1), tol));
|
||||
|
||||
Vector delta = (Vector(9) << 0.1, 0.1, 0.1, 0.2, 0.3, 0.4,-0.1,-0.2,-0.3).finished();
|
||||
|
|
@ -88,9 +89,9 @@ TEST( testPoseRTV, Lie ) {
|
|||
Point3 pt2 = pt + rot * Point3(0.2, 0.3, 0.4);
|
||||
Velocity3 vel2 = vel + rot * Velocity3(-0.1,-0.2,-0.3);
|
||||
PoseRTV state2(pt2, rot2, vel2);
|
||||
EXPECT(assert_equal(state2, state1.retract(delta), 1e-1));
|
||||
EXPECT(assert_equal(state2, (PoseRTV)state1.retract(delta), 1e-1));
|
||||
EXPECT(assert_equal(delta, state1.localCoordinates(state2), 1e-1));
|
||||
EXPECT(assert_equal(-delta, state2.localCoordinates(state1), 1e-1)); // loose tolerance due to retract approximation
|
||||
EXPECT(assert_equal(delta, -state2.localCoordinates(state1), 1e-1));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
Loading…
Reference in New Issue