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

PoseRTV is now implemented using ProductLieGroup

release/4.3a0
dellaert 2015-05-25 23:50:00 -07:00
parent 8939adc799
commit d060d4621e
3 changed files with 53 additions and 166 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

103
gtsam_unstable/dynamics/PoseRTV.cpp
View File

@ -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);
}

105
gtsam_unstable/dynamics/PoseRTV.h
View File

@ -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 */

11
gtsam_unstable/dynamics/tests/testPoseRTV.cpp
View File

@ -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));
}
/* ************************************************************************* */