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

Fixed dimensions of Vectors

release/4.3a0
dellaert 2014-11-04 15:41:14 +01:00
parent 8b86626113
commit 4afe132b1a
10 changed files with 101 additions and 109 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
gtsam_unstable/dynamics/FullIMUFactor.h
View File

@ -28,20 +28,20 @@ public:
protected: protected:
/** measurements from the IMU */ /** measurements from the IMU */
Vector accel_, gyro_; Vector3 accel_, gyro_;
double dt_; /// time between measurements double dt_; /// time between measurements
public: public:
/** Standard constructor */ /** Standard constructor */
FullIMUFactor(const Vector& accel, const Vector& gyro, FullIMUFactor(const Vector3& accel, const Vector3& gyro,
double dt, const Key& key1, const Key& key2, const SharedNoiseModel& model) double dt, const Key& key1, const Key& key2, const SharedNoiseModel& model)
: Base(model, key1, key2), accel_(accel), gyro_(gyro), dt_(dt) { : Base(model, key1, key2), accel_(accel), gyro_(gyro), dt_(dt) {
assert(model->dim() == 9); assert(model->dim() == 9);
} }
/** Single IMU vector - imu = [accel, gyro] */ /** Single IMU vector - imu = [accel, gyro] */
FullIMUFactor(const Vector& imu, FullIMUFactor(const Vector6& imu,
double dt, const Key& key1, const Key& key2, const SharedNoiseModel& model) double dt, const Key& key1, const Key& key2, const SharedNoiseModel& model)
: Base(model, key1, key2), accel_(imu.head(3)), gyro_(imu.tail(3)), dt_(dt) { : Base(model, key1, key2), accel_(imu.head(3)), gyro_(imu.tail(3)), dt_(dt) {
assert(imu.size() == 6); assert(imu.size() == 6);
@ -67,15 +67,15 @@ public:
void print(const std::string& s="", const gtsam::KeyFormatter& formatter = gtsam::DefaultKeyFormatter) const { void print(const std::string& s="", const gtsam::KeyFormatter& formatter = gtsam::DefaultKeyFormatter) const {
std::string a = "FullIMUFactor: " + s; std::string a = "FullIMUFactor: " + s;
Base::print(a, formatter); Base::print(a, formatter);
gtsam::print(accel_, "accel"); gtsam::print((Vector)accel_, "accel");
gtsam::print(gyro_, "gyro"); gtsam::print((Vector)gyro_, "gyro");
std::cout << "dt: " << dt_ << std::endl; std::cout << "dt: " << dt_ << std::endl;
} }
// access // access
const Vector& gyro() const { return gyro_; } const Vector3& gyro() const { return gyro_; }
const Vector& accel() const { return accel_; } const Vector3& accel() const { return accel_; }
Vector z() const { return concatVectors(2, &accel_, &gyro_); } Vector6 z() const { return (Vector6() << accel_, gyro_); }
/** /**
* Error evaluation with optional derivatives - calculates * Error evaluation with optional derivatives - calculates
@ -84,13 +84,13 @@ public:
virtual Vector evaluateError(const PoseRTV& x1, const PoseRTV& x2, virtual Vector evaluateError(const PoseRTV& x1, const PoseRTV& x2,
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const { boost::optional<Matrix&> H2 = boost::none) const {
Vector z(9); Vector9 z;
z.head(3).operator=(accel_); // Strange syntax to work around ambiguous operator error with clang z.head(3).operator=(accel_); // Strange syntax to work around ambiguous operator error with clang
z.segment(3, 3).operator=(gyro_); // Strange syntax to work around ambiguous operator error with clang z.segment(3, 3).operator=(gyro_); // Strange syntax to work around ambiguous operator error with clang
z.tail(3).operator=(x2.t().vector()); // Strange syntax to work around ambiguous operator error with clang z.tail(3).operator=(x2.t().vector()); // Strange syntax to work around ambiguous operator error with clang
if (H1) *H1 = numericalDerivative21<Vector3, PoseRTV, PoseRTV>( if (H1) *H1 = numericalDerivative21<Vector9, PoseRTV, PoseRTV>(
boost::bind(This::predict_proxy, _1, _2, dt_), x1, x2, 1e-5); boost::bind(This::predict_proxy, _1, _2, dt_), x1, x2, 1e-5);
if (H2) *H2 = numericalDerivative22<Vector3, PoseRTV, PoseRTV>( if (H2) *H2 = numericalDerivative22<Vector9, PoseRTV, PoseRTV>(
boost::bind(This::predict_proxy, _1, _2, dt_), x1, x2, 1e-5); boost::bind(This::predict_proxy, _1, _2, dt_), x1, x2, 1e-5);
return z - predict_proxy(x1, x2, dt_); return z - predict_proxy(x1, x2, dt_);
} }
@ -106,11 +106,11 @@ public:
private: private:
/** copy of the measurement function formulated for numerical derivatives */ /** copy of the measurement function formulated for numerical derivatives */
static Vector3 predict_proxy(const PoseRTV& x1, const PoseRTV& x2, double dt) { static Vector9 predict_proxy(const PoseRTV& x1, const PoseRTV& x2, double dt) {
Vector hx(9); Vector9 hx;
hx.head(6).operator=(x1.imuPrediction(x2, dt)); // Strange syntax to work around ambiguous operator error with clang hx.head(6).operator=(x1.imuPrediction(x2, dt)); // Strange syntax to work around ambiguous operator error with clang
hx.tail(3).operator=(x1.translationIntegration(x2, dt).vector()); // Strange syntax to work around ambiguous operator error with clang hx.tail(3).operator=(x1.translationIntegration(x2, dt).vector()); // Strange syntax to work around ambiguous operator error with clang
return Vector3(hx); return hx;
} }
}; };

30
gtsam_unstable/dynamics/IMUFactor.h
View File

@ -26,18 +26,18 @@ public:
protected: protected:
/** measurements from the IMU */ /** measurements from the IMU */
Vector accel_, gyro_; Vector3 accel_, gyro_;
double dt_; /// time between measurements double dt_; /// time between measurements
public: public:
/** Standard constructor */ /** Standard constructor */
IMUFactor(const Vector& accel, const Vector& gyro, IMUFactor(const Vector3& accel, const Vector3& gyro,
double dt, const Key& key1, const Key& key2, const SharedNoiseModel& model) double dt, const Key& key1, const Key& key2, const SharedNoiseModel& model)
: Base(model, key1, key2), accel_(accel), gyro_(gyro), dt_(dt) {} : Base(model, key1, key2), accel_(accel), gyro_(gyro), dt_(dt) {}
/** Full IMU vector specification */ /** Full IMU vector specification */
IMUFactor(const Vector& imu_vector, IMUFactor(const Vector6& imu_vector,
double dt, const Key& key1, const Key& key2, const SharedNoiseModel& model) double dt, const Key& key1, const Key& key2, const SharedNoiseModel& model)
: Base(model, key1, key2), accel_(imu_vector.head(3)), gyro_(imu_vector.tail(3)), dt_(dt) {} : Base(model, key1, key2), accel_(imu_vector.head(3)), gyro_(imu_vector.tail(3)), dt_(dt) {}
@ -60,15 +60,15 @@ public:
void print(const std::string& s="", const gtsam::KeyFormatter& formatter = gtsam::DefaultKeyFormatter) const { void print(const std::string& s="", const gtsam::KeyFormatter& formatter = gtsam::DefaultKeyFormatter) const {
std::string a = "IMUFactor: " + s; std::string a = "IMUFactor: " + s;
Base::print(a, formatter); Base::print(a, formatter);
gtsam::print(accel_, "accel"); gtsam::print((Vector)accel_, "accel");
gtsam::print(gyro_, "gyro"); gtsam::print((Vector)gyro_, "gyro");
std::cout << "dt: " << dt_ << std::endl; std::cout << "dt: " << dt_ << std::endl;
} }
// access // access
const Vector& gyro() const { return gyro_; } const Vector3& gyro() const { return gyro_; }
const Vector& accel() const { return accel_; } const Vector3& accel() const { return accel_; }
Vector z() const { return concatVectors(2, &accel_, &gyro_); } Vector6 z() const { return (Vector6() << accel_, gyro_);}
/** /**
* Error evaluation with optional derivatives - calculates * Error evaluation with optional derivatives - calculates
@ -77,10 +77,10 @@ public:
virtual Vector evaluateError(const PoseRTV& x1, const PoseRTV& x2, virtual Vector evaluateError(const PoseRTV& x1, const PoseRTV& x2,
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const { boost::optional<Matrix&> H2 = boost::none) const {
const Vector meas = z(); const Vector6 meas = z();
if (H1) *H1 = numericalDerivative21<Vector, PoseRTV, PoseRTV>( if (H1) *H1 = numericalDerivative21<Vector6, PoseRTV, PoseRTV>(
boost::bind(This::predict_proxy, _1, _2, dt_, meas), x1, x2, 1e-5); boost::bind(This::predict_proxy, _1, _2, dt_, meas), x1, x2, 1e-5);
if (H2) *H2 = numericalDerivative22<Vector, PoseRTV, PoseRTV>( if (H2) *H2 = numericalDerivative22<Vector6, PoseRTV, PoseRTV>(
boost::bind(This::predict_proxy, _1, _2, dt_, meas), x1, x2, 1e-5); boost::bind(This::predict_proxy, _1, _2, dt_, meas), x1, x2, 1e-5);
return predict_proxy(x1, x2, dt_, meas); return predict_proxy(x1, x2, dt_, meas);
} }
@ -95,10 +95,10 @@ public:
private: private:
/** copy of the measurement function formulated for numerical derivatives */ /** copy of the measurement function formulated for numerical derivatives */
static Vector predict_proxy(const PoseRTV& x1, const PoseRTV& x2, static Vector6 predict_proxy(const PoseRTV& x1, const PoseRTV& x2,
double dt, const Vector& meas) { double dt, const Vector6& meas) {
Vector hx = x1.imuPrediction(x2, dt); Vector6 hx = x1.imuPrediction(x2, dt);
return Vector(meas - hx); return meas - hx;
} }
}; };

34
gtsam_unstable/dynamics/Pendulum.h
View File

@ -37,7 +37,7 @@ public:
///Constructor. k1: q_{k+1}, k: q_k, velKey: velocity variable depending on the chosen method, h: time step ///Constructor. k1: q_{k+1}, k: q_k, velKey: velocity variable depending on the chosen method, h: time step
PendulumFactor1(Key k1, Key k, Key velKey, double h, double mu = 1000.0) PendulumFactor1(Key k1, Key k, Key velKey, double h, double mu = 1000.0)
: Base(noiseModel::Constrained::All(double::Dim(), fabs(mu)), k1, k, velKey), h_(h) {} : Base(noiseModel::Constrained::All(1, fabs(mu)), k1, k, velKey), h_(h) {}
/// @return a deep copy of this factor /// @return a deep copy of this factor
virtual gtsam::NonlinearFactor::shared_ptr clone() const { virtual gtsam::NonlinearFactor::shared_ptr clone() const {
@ -45,15 +45,15 @@ public:
gtsam::NonlinearFactor::shared_ptr(new PendulumFactor1(*this))); } gtsam::NonlinearFactor::shared_ptr(new PendulumFactor1(*this))); }
/** q_k + h*v - q_k1 = 0, with optional derivatives */ /** q_k + h*v - q_k1 = 0, with optional derivatives */
Vector evaluateError(double qk1, double qk, double v, Vector evaluateError(const double& qk1, const double& qk, const double& v,
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none, boost::optional<Matrix&> H2 = boost::none,
boost::optional<Matrix&> H3 = boost::none) const { boost::optional<Matrix&> H3 = boost::none) const {
const size_t p = double::Dim(); const size_t p = 1;
if (H1) *H1 = -eye(p); if (H1) *H1 = -eye(p);
if (H2) *H2 = eye(p); if (H2) *H2 = eye(p);
if (H3) *H3 = eye(p)*h_; if (H3) *H3 = eye(p)*h_;
return qk1.localCoordinates(qk.compose(double(v*h_))); return (Vector(1) << qk+v*h_-qk1);
} }
}; // \PendulumFactor1 }; // \PendulumFactor1
@ -85,7 +85,7 @@ public:
///Constructor. vk1: v_{k+1}, vk: v_k, qkey: q's key depending on the chosen method, h: time step ///Constructor. vk1: v_{k+1}, vk: v_k, qkey: q's key depending on the chosen method, h: time step
PendulumFactor2(Key vk1, Key vk, Key qkey, double h, double r = 1.0, double g = 9.81, double mu = 1000.0) PendulumFactor2(Key vk1, Key vk, Key qkey, double h, double r = 1.0, double g = 9.81, double mu = 1000.0)
: Base(noiseModel::Constrained::All(double::Dim(), fabs(mu)), vk1, vk, qkey), h_(h), g_(g), r_(r) {} : Base(noiseModel::Constrained::All(1, fabs(mu)), vk1, vk, qkey), h_(h), g_(g), r_(r) {}
/// @return a deep copy of this factor /// @return a deep copy of this factor
virtual gtsam::NonlinearFactor::shared_ptr clone() const { virtual gtsam::NonlinearFactor::shared_ptr clone() const {
@ -93,15 +93,15 @@ public:
gtsam::NonlinearFactor::shared_ptr(new PendulumFactor2(*this))); } gtsam::NonlinearFactor::shared_ptr(new PendulumFactor2(*this))); }
/** v_k - h*g/L*sin(q) - v_k1 = 0, with optional derivatives */ /** v_k - h*g/L*sin(q) - v_k1 = 0, with optional derivatives */
Vector evaluateError(double vk1, double vk, double q, Vector evaluateError(const double & vk1, const double & vk, const double & q,
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none, boost::optional<Matrix&> H2 = boost::none,
boost::optional<Matrix&> H3 = boost::none) const { boost::optional<Matrix&> H3 = boost::none) const {
const size_t p = double::Dim(); const size_t p = 1;
if (H1) *H1 = -eye(p); if (H1) *H1 = -eye(p);
if (H2) *H2 = eye(p); if (H2) *H2 = eye(p);
if (H3) *H3 = -eye(p)*h_*g_/r_*cos(q.value()); if (H3) *H3 = -eye(p)*h_*g_/r_*cos(q);
return vk1.localCoordinates(double(vk - h_*g_/r_*sin(q))); return (Vector(1) << vk - h_ * g_ / r_ * sin(q) - vk1);
} }
}; // \PendulumFactor2 }; // \PendulumFactor2
@ -135,7 +135,7 @@ public:
///Constructor ///Constructor
PendulumFactorPk(Key pKey, Key qKey, Key qKey1, PendulumFactorPk(Key pKey, Key qKey, Key qKey1,
double h, double m = 1.0, double r = 1.0, double g = 9.81, double alpha = 0.0, double mu = 1000.0) double h, double m = 1.0, double r = 1.0, double g = 9.81, double alpha = 0.0, double mu = 1000.0)
: Base(noiseModel::Constrained::All(double::Dim(), fabs(mu)), pKey, qKey, qKey1), : Base(noiseModel::Constrained::All(1, fabs(mu)), pKey, qKey, qKey1),
h_(h), m_(m), r_(r), g_(g), alpha_(alpha) {} h_(h), m_(m), r_(r), g_(g), alpha_(alpha) {}
/// @return a deep copy of this factor /// @return a deep copy of this factor
@ -144,11 +144,11 @@ public:
gtsam::NonlinearFactor::shared_ptr(new PendulumFactorPk(*this))); } gtsam::NonlinearFactor::shared_ptr(new PendulumFactorPk(*this))); }
/** 1/h mr^2 (qk1-qk)+mgrh (1-a) sin((1-a)pk + a*pk1) - pk = 0, with optional derivatives */ /** 1/h mr^2 (qk1-qk)+mgrh (1-a) sin((1-a)pk + a*pk1) - pk = 0, with optional derivatives */
Vector evaluateError(double pk, double qk, double qk1, Vector evaluateError(const double & pk, const double & qk, const double & qk1,
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none, boost::optional<Matrix&> H2 = boost::none,
boost::optional<Matrix&> H3 = boost::none) const { boost::optional<Matrix&> H3 = boost::none) const {
const size_t p = double::Dim(); const size_t p = 1;
double qmid = (1-alpha_)*qk + alpha_*qk1; double qmid = (1-alpha_)*qk + alpha_*qk1;
double mr2_h = 1/h_*m_*r_*r_; double mr2_h = 1/h_*m_*r_*r_;
@ -158,7 +158,7 @@ public:
if (H2) *H2 = eye(p)*(-mr2_h + mgrh*(1-alpha_)*(1-alpha_)*cos(qmid)); if (H2) *H2 = eye(p)*(-mr2_h + mgrh*(1-alpha_)*(1-alpha_)*cos(qmid));
if (H3) *H3 = eye(p)*( mr2_h + mgrh*(1-alpha_)*(alpha_)*cos(qmid)); if (H3) *H3 = eye(p)*( mr2_h + mgrh*(1-alpha_)*(alpha_)*cos(qmid));
return pk.localCoordinates(double(mr2_h*(qk1-qk) + mgrh*(1-alpha_)*sin(qmid))); return (Vector(1) << mr2_h * (qk1 - qk) + mgrh * (1 - alpha_) * sin(qmid) - pk);
} }
}; // \PendulumFactorPk }; // \PendulumFactorPk
@ -191,7 +191,7 @@ public:
///Constructor ///Constructor
PendulumFactorPk1(Key pKey1, Key qKey, Key qKey1, PendulumFactorPk1(Key pKey1, Key qKey, Key qKey1,
double h, double m = 1.0, double r = 1.0, double g = 9.81, double alpha = 0.0, double mu = 1000.0) double h, double m = 1.0, double r = 1.0, double g = 9.81, double alpha = 0.0, double mu = 1000.0)
: Base(noiseModel::Constrained::All(double::Dim(), fabs(mu)), pKey1, qKey, qKey1), : Base(noiseModel::Constrained::All(1, fabs(mu)), pKey1, qKey, qKey1),
h_(h), m_(m), r_(r), g_(g), alpha_(alpha) {} h_(h), m_(m), r_(r), g_(g), alpha_(alpha) {}
/// @return a deep copy of this factor /// @return a deep copy of this factor
@ -200,11 +200,11 @@ public:
gtsam::NonlinearFactor::shared_ptr(new PendulumFactorPk1(*this))); } gtsam::NonlinearFactor::shared_ptr(new PendulumFactorPk1(*this))); }
/** 1/h mr^2 (qk1-qk) - mgrh a sin((1-a)pk + a*pk1) - pk1 = 0, with optional derivatives */ /** 1/h mr^2 (qk1-qk) - mgrh a sin((1-a)pk + a*pk1) - pk1 = 0, with optional derivatives */
Vector evaluateError(double pk1, double qk, double qk1, Vector evaluateError(const double & pk1, const double & qk, const double & qk1,
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none, boost::optional<Matrix&> H2 = boost::none,
boost::optional<Matrix&> H3 = boost::none) const { boost::optional<Matrix&> H3 = boost::none) const {
const size_t p = double::Dim(); const size_t p = 1;
double qmid = (1-alpha_)*qk + alpha_*qk1; double qmid = (1-alpha_)*qk + alpha_*qk1;
double mr2_h = 1/h_*m_*r_*r_; double mr2_h = 1/h_*m_*r_*r_;
@ -214,7 +214,7 @@ public:
if (H2) *H2 = eye(p)*(-mr2_h - mgrh*(1-alpha_)*alpha_*cos(qmid)); if (H2) *H2 = eye(p)*(-mr2_h - mgrh*(1-alpha_)*alpha_*cos(qmid));
if (H3) *H3 = eye(p)*( mr2_h - mgrh*alpha_*alpha_*cos(qmid)); if (H3) *H3 = eye(p)*( mr2_h - mgrh*alpha_*alpha_*cos(qmid));
return pk1.localCoordinates(double(mr2_h*(qk1-qk) - mgrh*alpha_*sin(qmid))); return (Vector(1) << mr2_h * (qk1 - qk) - mgrh * alpha_ * sin(qmid) - pk1);
} }
}; // \PendulumFactorPk1 }; // \PendulumFactorPk1

29
gtsam_unstable/dynamics/PoseRTV.cpp
View File

@ -57,18 +57,17 @@ void PoseRTV::print(const string& s) const {
} }
/* ************************************************************************* */ /* ************************************************************************* */
PoseRTV PoseRTV::Expmap(const Vector& v) { PoseRTV PoseRTV::Expmap(const Vector9& v) {
assert(v.size() == 9); Pose3 newPose = Pose3::Expmap(v.head<6>());
Pose3 newPose = Pose3::Expmap(sub(v, 0, 6)); Velocity3 newVel = Velocity3::Expmap(v.tail<3>());
Velocity3 newVel = Velocity3::Expmap(sub(v, 6, 9));
return PoseRTV(newPose, newVel); return PoseRTV(newPose, newVel);
} }
/* ************************************************************************* */ /* ************************************************************************* */
Vector PoseRTV::Logmap(const PoseRTV& p) { Vector9 PoseRTV::Logmap(const PoseRTV& p) {
Vector Lx = Pose3::Logmap(p.Rt_); Vector6 Lx = Pose3::Logmap(p.Rt_);
Vector Lv = Velocity3::Logmap(p.v_); Vector3 Lv = Velocity3::Logmap(p.v_);
return concatVectors(2, &Lx, &Lv); return (Vector9() << Lx, Lv);
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -84,9 +83,9 @@ PoseRTV PoseRTV::retract(const Vector& v) const {
Vector PoseRTV::localCoordinates(const PoseRTV& p1) const { Vector PoseRTV::localCoordinates(const PoseRTV& p1) const {
const Pose3& x0 = pose(), &x1 = p1.pose(); const Pose3& x0 = pose(), &x1 = p1.pose();
// First order approximation // First order approximation
Vector poseLogmap = x0.localCoordinates(x1); Vector6 poseLogmap = x0.localCoordinates(x1);
Vector lv = rotation().unrotate(p1.velocity() - v_).vector(); Vector3 lv = rotation().unrotate(p1.velocity() - v_).vector();
return concatVectors(2, &poseLogmap, &lv); return (Vector(9) << poseLogmap, lv);
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -190,16 +189,16 @@ PoseRTV PoseRTV::generalDynamics(
} }
/* ************************************************************************* */ /* ************************************************************************* */
Vector PoseRTV::imuPrediction(const PoseRTV& x2, double dt) const { Vector6 PoseRTV::imuPrediction(const PoseRTV& x2, double dt) const {
// split out states // split out states
const Rot3 &r1 = R(), &r2 = x2.R(); const Rot3 &r1 = R(), &r2 = x2.R();
const Velocity3 &v1 = v(), &v2 = x2.v(); const Velocity3 &v1 = v(), &v2 = x2.v();
Vector imu(6); Vector6 imu;
// acceleration // acceleration
Vector accel = v1.localCoordinates(v2) / dt; Vector accel = v1.localCoordinates(v2) / dt;
imu.head(3) = r2.transpose() * (accel - g); imu.head<3>() = r2.transpose() * (accel - g);
// rotation rates // rotation rates
// just using euler angles based on matlab code // just using euler angles based on matlab code
@ -211,7 +210,7 @@ Vector PoseRTV::imuPrediction(const PoseRTV& x2, double dt) const {
// normalize yaw in difference (as per Mitch's code) // normalize yaw in difference (as per Mitch's code)
dR(2) = Rot2::fromAngle(dR(2)).theta(); dR(2) = Rot2::fromAngle(dR(2)).theta();
dR /= dt; dR /= dt;
imu.tail(3) = Enb * dR; imu.tail<3>() = Enb * dR;
// imu.tail(3) = r1.transpose() * dR; // imu.tail(3) = r1.transpose() * dR;
return imu; return imu;

6
gtsam_unstable/dynamics/PoseRTV.h
View File

@ -86,8 +86,8 @@ public:
* expmap/logmap are poor approximations that assume independence of components * expmap/logmap are poor approximations that assume independence of components
* Currently implemented using the poor retract/unretract approximations * Currently implemented using the poor retract/unretract approximations
*/ */
static PoseRTV Expmap(const Vector& v); static PoseRTV Expmap(const Vector9& v);
static Vector Logmap(const PoseRTV& p); static Vector9 Logmap(const PoseRTV& p);
static PoseRTV identity() { return PoseRTV(); } static PoseRTV identity() { return PoseRTV(); }
@ -129,7 +129,7 @@ public:
/// Dynamics predictor for both ground and flying robots, given states at 1 and 2 /// Dynamics predictor for both ground and flying robots, given states at 1 and 2
/// Always move from time 1 to time 2 /// Always move from time 1 to time 2
/// @return imu measurement, as [accel, gyro] /// @return imu measurement, as [accel, gyro]
Vector imuPrediction(const PoseRTV& x2, double dt) const; Vector6 imuPrediction(const PoseRTV& x2, double dt) const;
/// predict measurement and where Point3 for x2 should be, as a way /// predict measurement and where Point3 for x2 should be, as a way
/// of enforcing a velocity constraint /// of enforcing a velocity constraint

8
gtsam_unstable/dynamics/VelocityConstraint3.h
View File

@ -27,7 +27,7 @@ public:
///TODO: comment ///TODO: comment
VelocityConstraint3(Key key1, Key key2, Key velKey, double dt, double mu = 1000.0) VelocityConstraint3(Key key1, Key key2, Key velKey, double dt, double mu = 1000.0)
: Base(noiseModel::Constrained::All(double::Dim(), fabs(mu)), key1, key2, velKey), dt_(dt) {} : Base(noiseModel::Constrained::All(1, fabs(mu)), key1, key2, velKey), dt_(dt) {}
virtual ~VelocityConstraint3() {} virtual ~VelocityConstraint3() {}
/// @return a deep copy of this factor /// @return a deep copy of this factor
@ -36,15 +36,15 @@ public:
gtsam::NonlinearFactor::shared_ptr(new VelocityConstraint3(*this))); } gtsam::NonlinearFactor::shared_ptr(new VelocityConstraint3(*this))); }
/** x1 + v*dt - x2 = 0, with optional derivatives */ /** x1 + v*dt - x2 = 0, with optional derivatives */
Vector evaluateError(double x1, double x2, double v, Vector evaluateError(const double& x1, const double& x2, const double& v,
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none, boost::optional<Matrix&> H2 = boost::none,
boost::optional<Matrix&> H3 = boost::none) const { boost::optional<Matrix&> H3 = boost::none) const {
const size_t p = double::Dim(); const size_t p = 1;
if (H1) *H1 = eye(p); if (H1) *H1 = eye(p);
if (H2) *H2 = -eye(p); if (H2) *H2 = -eye(p);
if (H3) *H3 = eye(p)*dt_; if (H3) *H3 = eye(p)*dt_;
return x2.localCoordinates(x1.compose(double(v*dt_))); return (Vector(1) << x1+v*dt_-x2);
} }
private: private:

14
gtsam_unstable/dynamics/tests/testIMUSystem.cpp
View File

@ -62,10 +62,9 @@ TEST( testIMUSystem, optimize_chain ) {
// create measurements // create measurements
SharedDiagonal model = noiseModel::Unit::Create(6); SharedDiagonal model = noiseModel::Unit::Create(6);
Vector imu12(6), imu23(6), imu34(6); Vector6 imu12 = pose1.imuPrediction(pose2, dt);
imu12 = pose1.imuPrediction(pose2, dt); Vector6 imu23 = pose2.imuPrediction(pose3, dt);
imu23 = pose2.imuPrediction(pose3, dt); Vector6 imu34 = pose3.imuPrediction(pose4, dt);
imu34 = pose3.imuPrediction(pose4, dt);
// assemble simple graph with IMU measurements and velocity constraints // assemble simple graph with IMU measurements and velocity constraints
NonlinearFactorGraph graph; NonlinearFactorGraph graph;
@ -109,10 +108,9 @@ TEST( testIMUSystem, optimize_chain_fullfactor ) {
// create measurements // create measurements
SharedDiagonal model = noiseModel::Isotropic::Sigma(9, 1.0); SharedDiagonal model = noiseModel::Isotropic::Sigma(9, 1.0);
Vector imu12(6), imu23(6), imu34(6); Vector6 imu12 = pose1.imuPrediction(pose2, dt);
imu12 = pose1.imuPrediction(pose2, dt); Vector6 imu23 = pose2.imuPrediction(pose3, dt);
imu23 = pose2.imuPrediction(pose3, dt); Vector6 imu34 = pose3.imuPrediction(pose4, dt);
imu34 = pose3.imuPrediction(pose4, dt);
// assemble simple graph with IMU measurements and velocity constraints // assemble simple graph with IMU measurements and velocity constraints
NonlinearFactorGraph graph; NonlinearFactorGraph graph;

8
gtsam_unstable/dynamics/tests/testPendulumFactors.cpp
View File

@ -18,8 +18,8 @@ namespace {
const double g = 9.81, l = 1.0; const double g = 9.81, l = 1.0;
const double deg2rad = M_PI/180.0; const double deg2rad = M_PI/180.0;
LieScalar origin, q1(deg2rad*30.0), q2(deg2rad*31.0); double q1(deg2rad*30.0), q2(deg2rad*31.0);
LieScalar v1(deg2rad*1.0/h), v2((v1-h*g/l*sin(q1))); double v1(deg2rad*1.0/h), v2((v1-h*g/l*sin(q1)));
} }
@ -46,7 +46,7 @@ TEST( testPendulumFactorPk, evaluateError) {
// hard constraints don't need a noise model // hard constraints don't need a noise model
PendulumFactorPk constraint(P(1), Q(1), Q(2), h); PendulumFactorPk constraint(P(1), Q(1), Q(2), h);
LieScalar pk( 1/h * (q2-q1) + h*g*sin(q1) ); double pk( 1/h * (q2-q1) + h*g*sin(q1) );
// verify error function // verify error function
EXPECT(assert_equal(zero(1), constraint.evaluateError(pk, q1, q2), tol)); EXPECT(assert_equal(zero(1), constraint.evaluateError(pk, q1, q2), tol));
@ -57,7 +57,7 @@ TEST( testPendulumFactorPk1, evaluateError) {
// hard constraints don't need a noise model // hard constraints don't need a noise model
PendulumFactorPk1 constraint(P(2), Q(1), Q(2), h); PendulumFactorPk1 constraint(P(2), Q(1), Q(2), h);
LieScalar pk1( 1/h * (q2-q1) ); double pk1( 1/h * (q2-q1) );
// verify error function // verify error function
EXPECT(assert_equal(zero(1), constraint.evaluateError(pk1, q1, q2), tol)); EXPECT(assert_equal(zero(1), constraint.evaluateError(pk1, q1, q2), tol));

23
gtsam_unstable/dynamics/tests/testVelocityConstraint3.cpp
View File

@ -8,23 +8,16 @@
#include <gtsam/inference/Symbol.h> #include <gtsam/inference/Symbol.h>
#include <gtsam_unstable/dynamics/VelocityConstraint3.h> #include <gtsam_unstable/dynamics/VelocityConstraint3.h>
/* ************************************************************************* */
using namespace gtsam; using namespace gtsam;
using namespace gtsam::symbol_shorthand;
namespace {
const double tol=1e-5;
const double dt = 1.0;
LieScalar origin,
x1(1.0), x2(2.0), v(1.0);
}
/* ************************************************************************* */ /* ************************************************************************* */
// evaluateError
TEST( testVelocityConstraint3, evaluateError) { TEST( testVelocityConstraint3, evaluateError) {
using namespace gtsam::symbol_shorthand; const double tol = 1e-5;
const double dt = 1.0;
double x1(1.0), x2(2.0), v(1.0);
// hard constraints don't need a noise model // hard constraints don't need a noise model
VelocityConstraint3 constraint(X(1), X(2), V(1), dt); VelocityConstraint3 constraint(X(1), X(2), V(1), dt);
@ -33,7 +26,9 @@ TEST( testVelocityConstraint3, evaluateError) {
EXPECT(assert_equal(zero(1), constraint.evaluateError(x1, x2, v), tol)); EXPECT(assert_equal(zero(1), constraint.evaluateError(x1, x2, v), tol));
} }
/* ************************************************************************* */ /* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); } int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */ /* ************************************************************************* */

30
gtsam_unstable/geometry/tests/testInvDepthCamera3.cpp
View File

@ -30,9 +30,9 @@ TEST( InvDepthFactor, Project1) {
InvDepthCamera3<Cal3_S2> inv_camera(level_pose, K); InvDepthCamera3<Cal3_S2> inv_camera(level_pose, K);
Vector5 inv_landmark((Vector(5) << 1., 0., 1., 0., 0.)); Vector5 inv_landmark((Vector(5) << 1., 0., 1., 0., 0.));
LieScalar inv_depth(1./4); double inv_depth(1./4);
Point2 actual_uv = inv_camera.project(inv_landmark, inv_depth); Point2 actual_uv = inv_camera.project(inv_landmark, inv_depth);
EXPECT(assert_equal(expected_uv, actual_uv)); EXPECT_DOUBLES_EQUAL(expected_uv, actual_uv,1e-8);
EXPECT(assert_equal(Point2(640,480), actual_uv)); EXPECT(assert_equal(Point2(640,480), actual_uv));
} }
@ -46,7 +46,7 @@ TEST( InvDepthFactor, Project2) {
InvDepthCamera3<Cal3_S2> inv_camera(level_pose, K); InvDepthCamera3<Cal3_S2> inv_camera(level_pose, K);
Vector5 diag_landmark((Vector(5) << 0., 0., 1., M_PI/4., atan(1.0/sqrt(2.0)))); Vector5 diag_landmark((Vector(5) << 0., 0., 1., M_PI/4., atan(1.0/sqrt(2.0))));
LieScalar inv_depth(1/sqrt(3.0)); double inv_depth(1/sqrt(3.0));
Point2 actual = inv_camera.project(diag_landmark, inv_depth); Point2 actual = inv_camera.project(diag_landmark, inv_depth);
EXPECT(assert_equal(expected, actual)); EXPECT(assert_equal(expected, actual));
} }
@ -61,7 +61,7 @@ TEST( InvDepthFactor, Project3) {
InvDepthCamera3<Cal3_S2> inv_camera(level_pose, K); InvDepthCamera3<Cal3_S2> inv_camera(level_pose, K);
Vector5 diag_landmark((Vector(5) << 0., 0., 0., M_PI/4., atan(2./sqrt(2.0)))); Vector5 diag_landmark((Vector(5) << 0., 0., 0., M_PI/4., atan(2./sqrt(2.0))));
LieScalar inv_depth( 1./sqrt(1.0+1+4)); double inv_depth( 1./sqrt(1.0+1+4));
Point2 actual = inv_camera.project(diag_landmark, inv_depth); Point2 actual = inv_camera.project(diag_landmark, inv_depth);
EXPECT(assert_equal(expected, actual)); EXPECT(assert_equal(expected, actual));
} }
@ -76,20 +76,20 @@ TEST( InvDepthFactor, Project4) {
InvDepthCamera3<Cal3_S2> inv_camera(level_pose, K); InvDepthCamera3<Cal3_S2> inv_camera(level_pose, K);
Vector5 diag_landmark((Vector(5) << 0., 0., 0., atan(4.0/1), atan(2./sqrt(1.+16.)))); Vector5 diag_landmark((Vector(5) << 0., 0., 0., atan(4.0/1), atan(2./sqrt(1.+16.))));
LieScalar inv_depth(1./sqrt(1.+16.+4.)); double inv_depth(1./sqrt(1.+16.+4.));
Point2 actual = inv_camera.project(diag_landmark, inv_depth); Point2 actual = inv_camera.project(diag_landmark, inv_depth);
EXPECT(assert_equal(expected, actual)); EXPECT(assert_equal(expected, actual));
} }
/* ************************************************************************* */ /* ************************************************************************* */
Point2 project_(const Pose3& pose, const Vector5& landmark, const LieScalar& inv_depth) { Point2 project_(const Pose3& pose, const Vector5& landmark, const double& inv_depth) {
return InvDepthCamera3<Cal3_S2>(pose,K).project(landmark, inv_depth); } return InvDepthCamera3<Cal3_S2>(pose,K).project(landmark, inv_depth); }
TEST( InvDepthFactor, Dproject_pose) TEST( InvDepthFactor, Dproject_pose)
{ {
Vector5 landmark((Vector(5) << 0.1,0.2,0.3, 0.1,0.2)); Vector5 landmark((Vector(5) << 0.1,0.2,0.3, 0.1,0.2));
LieScalar inv_depth(1./4); double inv_depth(1./4);
Matrix expected = numericalDerivative31(project_,level_pose, landmark, inv_depth); Matrix expected = numericalDerivative31(project_,level_pose, landmark, inv_depth);
InvDepthCamera3<Cal3_S2> inv_camera(level_pose,K); InvDepthCamera3<Cal3_S2> inv_camera(level_pose,K);
Matrix actual; Matrix actual;
@ -101,7 +101,7 @@ TEST( InvDepthFactor, Dproject_pose)
TEST( InvDepthFactor, Dproject_landmark) TEST( InvDepthFactor, Dproject_landmark)
{ {
Vector5 landmark((Vector(5) << 0.1,0.2,0.3, 0.1,0.2)); Vector5 landmark((Vector(5) << 0.1,0.2,0.3, 0.1,0.2));
LieScalar inv_depth(1./4); double inv_depth(1./4);
Matrix expected = numericalDerivative32(project_,level_pose, landmark, inv_depth); Matrix expected = numericalDerivative32(project_,level_pose, landmark, inv_depth);
InvDepthCamera3<Cal3_S2> inv_camera(level_pose,K); InvDepthCamera3<Cal3_S2> inv_camera(level_pose,K);
Matrix actual; Matrix actual;
@ -113,7 +113,7 @@ TEST( InvDepthFactor, Dproject_landmark)
TEST( InvDepthFactor, Dproject_inv_depth) TEST( InvDepthFactor, Dproject_inv_depth)
{ {
Vector5 landmark((Vector(5) << 0.1,0.2,0.3, 0.1,0.2)); Vector5 landmark((Vector(5) << 0.1,0.2,0.3, 0.1,0.2));
LieScalar inv_depth(1./4); double inv_depth(1./4);
Matrix expected = numericalDerivative33(project_,level_pose, landmark, inv_depth); Matrix expected = numericalDerivative33(project_,level_pose, landmark, inv_depth);
InvDepthCamera3<Cal3_S2> inv_camera(level_pose,K); InvDepthCamera3<Cal3_S2> inv_camera(level_pose,K);
Matrix actual; Matrix actual;
@ -125,15 +125,15 @@ TEST( InvDepthFactor, Dproject_inv_depth)
TEST(InvDepthFactor, backproject) TEST(InvDepthFactor, backproject)
{ {
Vector expected((Vector(5) << 0.,0.,1., 0.1,0.2)); Vector expected((Vector(5) << 0.,0.,1., 0.1,0.2));
LieScalar inv_depth(1./4); double inv_depth(1./4);
InvDepthCamera3<Cal3_S2> inv_camera(level_pose,K); InvDepthCamera3<Cal3_S2> inv_camera(level_pose,K);
Point2 z = inv_camera.project(expected, inv_depth); Point2 z = inv_camera.project(expected, inv_depth);
Vector5 actual_vec; Vector5 actual_vec;
LieScalar actual_inv; double actual_inv;
boost::tie(actual_vec, actual_inv) = inv_camera.backproject(z, 4); boost::tie(actual_vec, actual_inv) = inv_camera.backproject(z, 4);
EXPECT(assert_equal(expected,actual_vec,1e-7)); EXPECT(assert_equal(expected,actual_vec,1e-7));
EXPECT(assert_equal(inv_depth,actual_inv,1e-7)); EXPECT_DOUBLES_EQUAL(inv_depth,actual_inv,1e-7);
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -141,15 +141,15 @@ TEST(InvDepthFactor, backproject2)
{ {
// backwards facing camera // backwards facing camera
Vector expected((Vector(5) << -5.,-5.,2., 3., -0.1)); Vector expected((Vector(5) << -5.,-5.,2., 3., -0.1));
LieScalar inv_depth(1./10); double inv_depth(1./10);
InvDepthCamera3<Cal3_S2> inv_camera(Pose3(Rot3::ypr(1.5,0.1, -1.5), Point3(-5, -5, 2)),K); InvDepthCamera3<Cal3_S2> inv_camera(Pose3(Rot3::ypr(1.5,0.1, -1.5), Point3(-5, -5, 2)),K);
Point2 z = inv_camera.project(expected, inv_depth); Point2 z = inv_camera.project(expected, inv_depth);
Vector5 actual_vec; Vector5 actual_vec;
LieScalar actual_inv; double actual_inv;
boost::tie(actual_vec, actual_inv) = inv_camera.backproject(z, 10); boost::tie(actual_vec, actual_inv) = inv_camera.backproject(z, 10);
EXPECT(assert_equal(expected,actual_vec,1e-7)); EXPECT(assert_equal(expected,actual_vec,1e-7));
EXPECT(assert_equal(inv_depth,actual_inv,1e-7)); EXPECT_DOUBLES_EQUAL(inv_depth,actual_inv,1e-7);
} }
/* ************************************************************************* */ /* ************************************************************************* */