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

No more LieVector/LieScalar

release/4.3a0
dellaert 2014-11-03 13:52:08 +01:00
parent f1dd554a9d
commit da3677e704
7 changed files with 44 additions and 47 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
gtsam/navigation/ImuBias.h
View File

@ -144,7 +144,7 @@ namespace imuBias {
/// return dimensionality of tangent space
inline size_t dim() const { return dimension; }
/** Update the LieVector with a tangent space update */
/** Update the bias with a tangent space update */
inline ConstantBias retract(const Vector& v) const { return ConstantBias(biasAcc_ + v.head(3), biasGyro_ + v.tail(3)); }
/** @return the local coordinates of another object */

7
gtsam/navigation/MagFactor.h
View File

@ -19,7 +19,6 @@
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/geometry/Rot2.h>
#include <gtsam/geometry/Rot3.h>
#include <gtsam/base/LieScalar.h>
namespace gtsam {
@ -165,7 +164,7 @@ public:
* This version uses model measured bM = scale * bRn * direction + bias
* and optimizes for both scale, direction, and the bias.
*/
class MagFactor3: public NoiseModelFactor3<LieScalar, Unit3, Point3> {
class MagFactor3: public NoiseModelFactor3<double, Unit3, Point3> {
const Point3 measured_; ///< The measured magnetometer values
const Rot3 bRn_; ///< The assumed known rotation from nav to body
@ -175,7 +174,7 @@ public:
/** Constructor */
MagFactor3(Key key1, Key key2, Key key3, const Point3& measured,
const Rot3& nRb, const SharedNoiseModel& model) :
NoiseModelFactor3<LieScalar, Unit3, Point3>(model, key1, key2, key3), //
NoiseModelFactor3<double, Unit3, Point3>(model, key1, key2, key3), //
measured_(measured), bRn_(nRb.inverse()) {
}
@ -190,7 +189,7 @@ public:
* @param nM (unknown) local earth magnetic field vector, in nav frame
* @param bias (unknown) 3D bias
*/
Vector evaluateError(const LieScalar& scale, const Unit3& direction,
Vector evaluateError(double scale, const Unit3& direction,
const Point3& bias, boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none, boost::optional<Matrix&> H3 =
boost::none) const {

4
gtsam/navigation/tests/testCombinedImuFactor.cpp
View File

@ -142,9 +142,9 @@ TEST( CombinedImuFactor, ErrorWithBiases )
imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot)
imuBias::ConstantBias bias2(Vector3(0.2, 0.2, 0), Vector3(1, 0, 0.3)); // Biases (acc, rot)
Pose3 x1(Rot3::Expmap(Vector3(0, 0, M_PI/4.0)), Point3(5.0, 1.0, -50.0));
LieVector v1((Vector(3) << 0.5, 0.0, 0.0));
Vector3 v1(0.5, 0.0, 0.0);
Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI/4.0 + M_PI/10.0)), Point3(5.5, 1.0, -50.0));
LieVector v2((Vector(3) << 0.5, 0.0, 0.0));
Vector3 v2(0.5, 0.0, 0.0);
// Measurements
Vector3 gravity; gravity << 0, 0, 9.81;

6
gtsam/slam/tests/testEssentialMatrixFactor.cpp
View File

@ -211,7 +211,7 @@ TEST (EssentialMatrixFactor2, minimization) {
EssentialMatrix actual = result.at<EssentialMatrix>(100);
EXPECT(assert_equal(trueE, actual, 1e-1));
for (size_t i = 0; i < 5; i++)
EXPECT(assert_equal(truth.at<double>(i), result.at<double>(i), 1e-1));
EXPECT_DOUBLES_EQUAL(truth.at<double>(i), result.at<double>(i), 1e-1);
// Check error at result
EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-4);
@ -284,7 +284,7 @@ TEST (EssentialMatrixFactor3, minimization) {
EssentialMatrix actual = result.at<EssentialMatrix>(100);
EXPECT(assert_equal(bodyE, actual, 1e-1));
for (size_t i = 0; i < 5; i++)
EXPECT(assert_equal(truth.at<double>(i), result.at<double>(i), 1e-1));
EXPECT_DOUBLES_EQUAL(truth.at<double>(i), result.at<double>(i), 1e-1);
// Check error at result
EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-4);
@ -427,7 +427,7 @@ TEST (EssentialMatrixFactor2, extraMinimization) {
EssentialMatrix actual = result.at<EssentialMatrix>(100);
EXPECT(assert_equal(trueE, actual, 1e-1));
for (size_t i = 0; i < data.number_tracks(); i++)
EXPECT(assert_equal(truth.at<double>(i), result.at<double>(i), 1e-1));
EXPECT_DOUBLES_EQUAL(truth.at<double>(i), result.at<double>(i), 1e-1);
// Check error at result
EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-4);

8
gtsam_unstable/dynamics/IMUFactor.h
View File

@ -78,9 +78,9 @@ public:
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const {
const Vector meas = z();
if (H1) *H1 = numericalDerivative21<LieVector, PoseRTV, PoseRTV>(
if (H1) *H1 = numericalDerivative21<Vector, PoseRTV, PoseRTV>(
boost::bind(This::predict_proxy, _1, _2, dt_, meas), x1, x2, 1e-5);
if (H2) *H2 = numericalDerivative22<LieVector, PoseRTV, PoseRTV>(
if (H2) *H2 = numericalDerivative22<Vector, PoseRTV, PoseRTV>(
boost::bind(This::predict_proxy, _1, _2, dt_, meas), x1, x2, 1e-5);
return predict_proxy(x1, x2, dt_, meas);
}
@ -95,10 +95,10 @@ public:
private:
/** copy of the measurement function formulated for numerical derivatives */
static LieVector predict_proxy(const PoseRTV& x1, const PoseRTV& x2,
static Vector predict_proxy(const PoseRTV& x1, const PoseRTV& x2,
double dt, const Vector& meas) {
Vector hx = x1.imuPrediction(x2, dt);
return LieVector(meas - hx);
return Vector(meas - hx);
}
};

49
gtsam_unstable/dynamics/Pendulum.h
View File

@ -9,7 +9,6 @@
#pragma once
#include <gtsam/base/LieScalar.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
namespace gtsam {
@ -21,11 +20,11 @@ namespace gtsam {
* - For implicit Euler method: q_{k+1} = q_k + h*v_{k+1}
* - For sympletic Euler method: q_{k+1} = q_k + h*v_{k+1}
*/
class PendulumFactor1: public NoiseModelFactor3<LieScalar, LieScalar, LieScalar> {
class PendulumFactor1: public NoiseModelFactor3<double, double, double> {
public:
protected:
typedef NoiseModelFactor3<LieScalar, LieScalar, LieScalar> Base;
typedef NoiseModelFactor3<double, double, double> Base;
/** default constructor to allow for serialization */
PendulumFactor1() {}
@ -38,7 +37,7 @@ public:
///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)
: Base(noiseModel::Constrained::All(LieScalar::Dim(), fabs(mu)), k1, k, velKey), h_(h) {}
: Base(noiseModel::Constrained::All(double::Dim(), fabs(mu)), k1, k, velKey), h_(h) {}
/// @return a deep copy of this factor
virtual gtsam::NonlinearFactor::shared_ptr clone() const {
@ -46,15 +45,15 @@ public:
gtsam::NonlinearFactor::shared_ptr(new PendulumFactor1(*this))); }
/** q_k + h*v - q_k1 = 0, with optional derivatives */
Vector evaluateError(const LieScalar& qk1, const LieScalar& qk, const LieScalar& v,
Vector evaluateError(double qk1, double qk, double v,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none,
boost::optional<Matrix&> H3 = boost::none) const {
const size_t p = LieScalar::Dim();
const size_t p = double::Dim();
if (H1) *H1 = -eye(p);
if (H2) *H2 = eye(p);
if (H3) *H3 = eye(p)*h_;
return qk1.localCoordinates(qk.compose(LieScalar(v*h_)));
return qk1.localCoordinates(qk.compose(double(v*h_)));
}
}; // \PendulumFactor1
@ -67,11 +66,11 @@ public:
* - For implicit Euler method: v_{k+1} = v_k - h*g/L*sin(q_{k+1})
* - For sympletic Euler method: v_{k+1} = v_k - h*g/L*sin(q_k)
*/
class PendulumFactor2: public NoiseModelFactor3<LieScalar, LieScalar, LieScalar> {
class PendulumFactor2: public NoiseModelFactor3<double, double, double> {
public:
protected:
typedef NoiseModelFactor3<LieScalar, LieScalar, LieScalar> Base;
typedef NoiseModelFactor3<double, double, double> Base;
/** default constructor to allow for serialization */
PendulumFactor2() {}
@ -86,7 +85,7 @@ public:
///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)
: Base(noiseModel::Constrained::All(LieScalar::Dim(), fabs(mu)), vk1, vk, qkey), h_(h), g_(g), r_(r) {}
: Base(noiseModel::Constrained::All(double::Dim(), fabs(mu)), vk1, vk, qkey), h_(h), g_(g), r_(r) {}
/// @return a deep copy of this factor
virtual gtsam::NonlinearFactor::shared_ptr clone() const {
@ -94,15 +93,15 @@ public:
gtsam::NonlinearFactor::shared_ptr(new PendulumFactor2(*this))); }
/** v_k - h*g/L*sin(q) - v_k1 = 0, with optional derivatives */
Vector evaluateError(const LieScalar& vk1, const LieScalar& vk, const LieScalar& q,
Vector evaluateError(double vk1, double vk, double q,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none,
boost::optional<Matrix&> H3 = boost::none) const {
const size_t p = LieScalar::Dim();
const size_t p = double::Dim();
if (H1) *H1 = -eye(p);
if (H2) *H2 = eye(p);
if (H3) *H3 = -eye(p)*h_*g_/r_*cos(q.value());
return vk1.localCoordinates(LieScalar(vk - h_*g_/r_*sin(q)));
return vk1.localCoordinates(double(vk - h_*g_/r_*sin(q)));
}
}; // \PendulumFactor2
@ -114,11 +113,11 @@ public:
* p_k = -D_1 L_d(q_k,q_{k+1},h) = \frac{1}{h}mr^{2}\left(q_{k+1}-q_{k}\right)+mgrh(1-\alpha)\,\sin\left((1-\alpha)q_{k}+\alpha q_{k+1}\right)
* = (1/h)mr^2 (q_{k+1}-q_k) + mgrh(1-alpha) sin ((1-alpha)q_k+\alpha q_{k+1})
*/
class PendulumFactorPk: public NoiseModelFactor3<LieScalar, LieScalar, LieScalar> {
class PendulumFactorPk: public NoiseModelFactor3<double, double, double> {
public:
protected:
typedef NoiseModelFactor3<LieScalar, LieScalar, LieScalar> Base;
typedef NoiseModelFactor3<double, double, double> Base;
/** default constructor to allow for serialization */
PendulumFactorPk() {}
@ -136,7 +135,7 @@ public:
///Constructor
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)
: Base(noiseModel::Constrained::All(LieScalar::Dim(), fabs(mu)), pKey, qKey, qKey1),
: Base(noiseModel::Constrained::All(double::Dim(), fabs(mu)), pKey, qKey, qKey1),
h_(h), m_(m), r_(r), g_(g), alpha_(alpha) {}
/// @return a deep copy of this factor
@ -145,11 +144,11 @@ public:
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 */
Vector evaluateError(const LieScalar& pk, const LieScalar& qk, const LieScalar& qk1,
Vector evaluateError(double pk, double qk, double qk1,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none,
boost::optional<Matrix&> H3 = boost::none) const {
const size_t p = LieScalar::Dim();
const size_t p = double::Dim();
double qmid = (1-alpha_)*qk + alpha_*qk1;
double mr2_h = 1/h_*m_*r_*r_;
@ -159,7 +158,7 @@ public:
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));
return pk.localCoordinates(LieScalar(mr2_h*(qk1-qk) + mgrh*(1-alpha_)*sin(qmid)));
return pk.localCoordinates(double(mr2_h*(qk1-qk) + mgrh*(1-alpha_)*sin(qmid)));
}
}; // \PendulumFactorPk
@ -170,11 +169,11 @@ public:
* p_k1 = D_2 L_d(q_k,q_{k+1},h) = \frac{1}{h}mr^{2}\left(q_{k+1}-q_{k}\right)-mgrh\alpha\sin\left((1-\alpha)q_{k}+\alpha q_{k+1}\right)
* = (1/h)mr^2 (q_{k+1}-q_k) - mgrh alpha sin ((1-alpha)q_k+\alpha q_{k+1})
*/
class PendulumFactorPk1: public NoiseModelFactor3<LieScalar, LieScalar, LieScalar> {
class PendulumFactorPk1: public NoiseModelFactor3<double, double, double> {
public:
protected:
typedef NoiseModelFactor3<LieScalar, LieScalar, LieScalar> Base;
typedef NoiseModelFactor3<double, double, double> Base;
/** default constructor to allow for serialization */
PendulumFactorPk1() {}
@ -192,7 +191,7 @@ public:
///Constructor
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)
: Base(noiseModel::Constrained::All(LieScalar::Dim(), fabs(mu)), pKey1, qKey, qKey1),
: Base(noiseModel::Constrained::All(double::Dim(), fabs(mu)), pKey1, qKey, qKey1),
h_(h), m_(m), r_(r), g_(g), alpha_(alpha) {}
/// @return a deep copy of this factor
@ -201,11 +200,11 @@ public:
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 */
Vector evaluateError(const LieScalar& pk1, const LieScalar& qk, const LieScalar& qk1,
Vector evaluateError(double pk1, double qk, double qk1,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none,
boost::optional<Matrix&> H3 = boost::none) const {
const size_t p = LieScalar::Dim();
const size_t p = double::Dim();
double qmid = (1-alpha_)*qk + alpha_*qk1;
double mr2_h = 1/h_*m_*r_*r_;
@ -215,7 +214,7 @@ public:
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));
return pk1.localCoordinates(LieScalar(mr2_h*(qk1-qk) - mgrh*alpha_*sin(qmid)));
return pk1.localCoordinates(double(mr2_h*(qk1-qk) - mgrh*alpha_*sin(qmid)));
}
}; // \PendulumFactorPk1

15
gtsam_unstable/dynamics/VelocityConstraint3.h
View File

@ -1,21 +1,20 @@
/**
* @file VelocityConstraint3.h
* @brief A simple 3-way factor constraining LieScalar poses and velocity
* @brief A simple 3-way factor constraining double poses and velocity
* @author Duy-Nguyen Ta
*/
#pragma once
#include <gtsam/base/LieScalar.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
namespace gtsam {
class VelocityConstraint3 : public NoiseModelFactor3<LieScalar, LieScalar, LieScalar> {
class VelocityConstraint3 : public NoiseModelFactor3<double, double, double> {
public:
protected:
typedef NoiseModelFactor3<LieScalar, LieScalar, LieScalar> Base;
typedef NoiseModelFactor3<double, double, double> Base;
/** default constructor to allow for serialization */
VelocityConstraint3() {}
@ -28,7 +27,7 @@ public:
///TODO: comment
VelocityConstraint3(Key key1, Key key2, Key velKey, double dt, double mu = 1000.0)
: Base(noiseModel::Constrained::All(LieScalar::Dim(), fabs(mu)), key1, key2, velKey), dt_(dt) {}
: Base(noiseModel::Constrained::All(double::Dim(), fabs(mu)), key1, key2, velKey), dt_(dt) {}
virtual ~VelocityConstraint3() {}
/// @return a deep copy of this factor
@ -37,15 +36,15 @@ public:
gtsam::NonlinearFactor::shared_ptr(new VelocityConstraint3(*this))); }
/** x1 + v*dt - x2 = 0, with optional derivatives */
Vector evaluateError(const LieScalar& x1, const LieScalar& x2, const LieScalar& v,
Vector evaluateError(double x1, double x2, double v,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none,
boost::optional<Matrix&> H3 = boost::none) const {
const size_t p = LieScalar::Dim();
const size_t p = double::Dim();
if (H1) *H1 = eye(p);
if (H2) *H2 = -eye(p);
if (H3) *H3 = eye(p)*dt_;
return x2.localCoordinates(x1.compose(LieScalar(v*dt_)));
return x2.localCoordinates(x1.compose(double(v*dt_)));
}
private: