PoseRTV as ProductManifold works
dellaert
parent
b8a8a16348
commit
b23a51db6d
|
|
@ -14,7 +14,7 @@ namespace gtsam {
|
|||
|
||||
using namespace std;
|
||||
|
||||
static const Vector g = delta(3, 2, 9.81);
|
||||
static const Vector kGravity = delta(3, 2, 9.81);
|
||||
|
||||
/* ************************************************************************* */
|
||||
double bound(double a, double min, double max) {
|
||||
|
|
@ -24,28 +24,30 @@ double bound(double a, double min, double max) {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
PoseRTV::PoseRTV(double roll, double pitch, double yaw, double x, double y, double z,
|
||||
double vx, double vy, double vz)
|
||||
: Rt_(Rot3::RzRyRx(roll, pitch, yaw), Point3(x, y, z)), v_(vx, vy, vz) {}
|
||||
PoseRTV::PoseRTV(double roll, double pitch, double yaw, double x, double y,
|
||||
double z, double vx, double vy, double vz) :
|
||||
Base(Pose3(Rot3::RzRyRx(roll, pitch, yaw), Point3(x, y, z)),
|
||||
Velocity3(vx, vy, vz)) {
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
PoseRTV::PoseRTV(const Vector& rtv)
|
||||
: Rt_(Rot3::RzRyRx(rtv.head(3)), Point3(rtv.segment(3, 3))), v_(rtv.tail(3))
|
||||
{
|
||||
PoseRTV::PoseRTV(const Vector& rtv) :
|
||||
Base(Pose3(Rot3::RzRyRx(rtv.head(3)), Point3(rtv.segment(3, 3))),
|
||||
Velocity3(rtv.tail(3))) {
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Vector PoseRTV::vector() const {
|
||||
Vector rtv(9);
|
||||
rtv.head(3) = Rt_.rotation().xyz();
|
||||
rtv.segment(3,3) = Rt_.translation().vector();
|
||||
rtv.tail(3) = v_.vector();
|
||||
rtv.head(3) = rotation().xyz();
|
||||
rtv.segment(3,3) = translation().vector();
|
||||
rtv.tail(3) = velocity().vector();
|
||||
return rtv;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
bool PoseRTV::equals(const PoseRTV& other, double tol) const {
|
||||
return Rt_.equals(other.Rt_, tol) && v_.equals(other.v_, tol);
|
||||
return pose().equals(other.pose(), tol) && velocity().equals(other.velocity(), tol);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -53,7 +55,7 @@ void PoseRTV::print(const string& s) const {
|
|||
cout << s << ":" << endl;
|
||||
gtsam::print((Vector)R().xyz(), " R:rpy");
|
||||
t().print(" T");
|
||||
v_.print(" V");
|
||||
velocity().print(" V");
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -67,8 +69,8 @@ PoseRTV PoseRTV::Expmap(const Vector9& v, ChartJacobian H) {
|
|||
/* ************************************************************************* */
|
||||
Vector9 PoseRTV::Logmap(const PoseRTV& p, ChartJacobian H) {
|
||||
if (H) CONCEPT_NOT_IMPLEMENTED;
|
||||
Vector6 Lx = Pose3::Logmap(p.Rt_);
|
||||
Vector3 Lv = p.v_.vector();
|
||||
Vector6 Lx = Pose3::Logmap(p.pose());
|
||||
Vector3 Lv = p.velocity().vector();
|
||||
return (Vector9() << Lx, Lv).finished();
|
||||
}
|
||||
|
||||
|
|
@ -79,8 +81,8 @@ PoseRTV PoseRTV::retract(const Vector& v,
|
|||
if (Horigin || Hv) CONCEPT_NOT_IMPLEMENTED;
|
||||
assert(v.size() == 9);
|
||||
// First order approximation
|
||||
Pose3 newPose = Rt_.retract(sub(v, 0, 6));
|
||||
Velocity3 newVel = v_ + Rt_.rotation() * Point3(sub(v, 6, 9));
|
||||
Pose3 newPose = pose().retract(sub(v, 0, 6));
|
||||
Velocity3 newVel = velocity() + rotation() * Point3(sub(v, 6, 9));
|
||||
return PoseRTV(newPose, newVel);
|
||||
}
|
||||
|
||||
|
|
@ -92,7 +94,7 @@ Vector PoseRTV::localCoordinates(const PoseRTV& p1,
|
|||
const Pose3& x0 = pose(), &x1 = p1.pose();
|
||||
// First order approximation
|
||||
Vector6 poseLogmap = x0.localCoordinates(x1);
|
||||
Vector3 lv = rotation().unrotate(p1.velocity() - v_).vector();
|
||||
Vector3 lv = rotation().unrotate(p1.velocity() - velocity()).vector();
|
||||
return (Vector(9) << poseLogmap, lv).finished();
|
||||
}
|
||||
|
||||
|
|
@ -100,7 +102,7 @@ Vector PoseRTV::localCoordinates(const PoseRTV& p1,
|
|||
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(Rt_.inverse(), - v_);
|
||||
return PoseRTV(pose().inverse(), - velocity());
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -109,7 +111,7 @@ 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_);
|
||||
return PoseRTV(pose().compose(p.pose()), velocity()+p.velocity());
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -187,7 +189,7 @@ PoseRTV PoseRTV::generalDynamics(
|
|||
Rot3 r2 = rotation().retract(gyro * dt);
|
||||
|
||||
// Integrate Velocity Equations
|
||||
Velocity3 v2 = v_ + (Velocity3(dt * (r2.matrix() * accel + g)));
|
||||
Velocity3 v2 = velocity() + Velocity3(dt * (r2.matrix() * accel + kGravity));
|
||||
|
||||
// Integrate Position Equations
|
||||
Point3 t2 = translationIntegration(r2, v2, dt);
|
||||
|
|
@ -205,7 +207,7 @@ Vector6 PoseRTV::imuPrediction(const PoseRTV& x2, double dt) const {
|
|||
|
||||
// acceleration
|
||||
Vector3 accel = (v2-v1).vector() / dt;
|
||||
imu.head<3>() = r2.transpose() * (accel - g);
|
||||
imu.head<3>() = r2.transpose() * (accel - kGravity);
|
||||
|
||||
// rotation rates
|
||||
// just using euler angles based on matlab code
|
||||
|
|
@ -249,7 +251,7 @@ 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);
|
||||
Velocity3 newvel = trans.rotation().rotate(velocity(), DVr, DTt);
|
||||
if (!Dglobal && !Dtrans)
|
||||
return PoseRTV(trans.compose(pose()), newvel);
|
||||
|
||||
|
|
@ -273,7 +275,7 @@ PoseRTV PoseRTV::transformed_from(const Pose3& trans, ChartJacobian Dglobal,
|
|||
// insertSub(*Dtrans, DTc, 0, 0); // correct in tests
|
||||
//
|
||||
// // rotating the velocity
|
||||
// Matrix vRhat = skewSymmetric(-v_.x(), -v_.y(), -v_.z());
|
||||
// Matrix vRhat = skewSymmetric(-velocity().x(), -velocity().y(), -velocity().z());
|
||||
// trans.rotation().print("Transform rotation");
|
||||
// gtsam::print(vRhat, "vRhat");
|
||||
// gtsam::print(DVr, "DVr");
|
||||
|
|
|
|||
|
|
@ -7,11 +7,46 @@
|
|||
#pragma once
|
||||
|
||||
#include <gtsam_unstable/base/dllexport.h>
|
||||
#include <gtsam/base/DerivedValue.h>
|
||||
#include <gtsam/geometry/Pose3.h>
|
||||
#include <gtsam/base/Lie.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;
|
||||
|
||||
|
|
@ -19,12 +54,10 @@ typedef Point3 Velocity3;
|
|||
* Robot state for use with IMU measurements
|
||||
* - contains translation, translational velocity and rotation
|
||||
*/
|
||||
class GTSAM_UNSTABLE_EXPORT PoseRTV {
|
||||
class GTSAM_UNSTABLE_EXPORT PoseRTV : private ProductLieGroup<PoseRTV,Pose3,Velocity3> {
|
||||
protected:
|
||||
|
||||
Pose3 Rt_;
|
||||
Velocity3 v_;
|
||||
|
||||
typedef ProductLieGroup<PoseRTV,Pose3,Velocity3> Base;
|
||||
typedef OptionalJacobian<9, 9> ChartJacobian;
|
||||
|
||||
public:
|
||||
|
|
@ -32,16 +65,16 @@ public:
|
|||
|
||||
// constructors - with partial versions
|
||||
PoseRTV() {}
|
||||
PoseRTV(const Point3& pt, const Rot3& rot, const Velocity3& vel)
|
||||
: Rt_(rot, pt), v_(vel) {}
|
||||
PoseRTV(const Rot3& rot, const Point3& pt, const Velocity3& vel)
|
||||
: Rt_(rot, pt), v_(vel) {}
|
||||
explicit PoseRTV(const Point3& pt)
|
||||
: Rt_(Rot3::identity(), pt) {}
|
||||
PoseRTV(const Point3& t, const Rot3& rot, const Velocity3& vel)
|
||||
: Base(Pose3(rot, t), vel) {}
|
||||
PoseRTV(const Rot3& rot, const Point3& t, const Velocity3& vel)
|
||||
: Base(Pose3(rot, t), vel) {}
|
||||
explicit PoseRTV(const Point3& t)
|
||||
: Base(Pose3(Rot3(), t),Velocity3()) {}
|
||||
PoseRTV(const Pose3& pose, const Velocity3& vel)
|
||||
: Rt_(pose), v_(vel) {}
|
||||
: Base(pose, vel) {}
|
||||
explicit PoseRTV(const Pose3& pose)
|
||||
: Rt_(pose) {}
|
||||
: Base(pose,Velocity3()) {}
|
||||
|
||||
/** build from components - useful for data files */
|
||||
PoseRTV(double roll, double pitch, double yaw, double x, double y, double z,
|
||||
|
|
@ -51,21 +84,21 @@ public:
|
|||
explicit PoseRTV(const Vector& v);
|
||||
|
||||
// access
|
||||
const Point3& t() const { return Rt_.translation(); }
|
||||
const Rot3& R() const { return Rt_.rotation(); }
|
||||
const Velocity3& v() const { return v_; }
|
||||
const Pose3& pose() const { return Rt_; }
|
||||
const Pose3& pose() const { return first; }
|
||||
const Velocity3& v() const { return second; }
|
||||
const Point3& t() const { return pose().translation(); }
|
||||
const Rot3& R() const { return pose().rotation(); }
|
||||
|
||||
// longer function names
|
||||
const Point3& translation() const { return Rt_.translation(); }
|
||||
const Rot3& rotation() const { return Rt_.rotation(); }
|
||||
const Velocity3& velocity() const { return v_; }
|
||||
const Point3& translation() const { return pose().translation(); }
|
||||
const Rot3& rotation() const { return pose().rotation(); }
|
||||
const Velocity3& velocity() const { return second; }
|
||||
|
||||
// Access to vector for ease of use with Matlab
|
||||
// and avoidance of Point3
|
||||
Vector vector() const;
|
||||
Vector translationVec() const { return Rt_.translation().vector(); }
|
||||
Vector velocityVec() const { return v_.vector(); }
|
||||
Vector translationVec() const { return pose().translation().vector(); }
|
||||
Vector velocityVec() const { return velocity().vector(); }
|
||||
|
||||
// testable
|
||||
bool equals(const PoseRTV& other, double tol=1e-6) const;
|
||||
|
|
@ -183,8 +216,8 @@ private:
|
|||
friend class boost::serialization::access;
|
||||
template<class Archive>
|
||||
void serialize(Archive & ar, const unsigned int /*version*/) {
|
||||
ar & BOOST_SERIALIZATION_NVP(Rt_);
|
||||
ar & BOOST_SERIALIZATION_NVP(v_);
|
||||
ar & BOOST_SERIALIZATION_NVP(first);
|
||||
ar & BOOST_SERIALIZATION_NVP(second);
|
||||
}
|
||||
};
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue