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

No more Lie types

release/4.3a0
dellaert 2014-11-03 09:27:40 +01:00
parent 64d0a3b4dc
commit 492c607f9e
7 changed files with 124 additions and 131 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
gtsam/geometry/tests/testPoint3.cpp
View File

@ -16,7 +16,6 @@
#include <gtsam/geometry/Point3.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/LieScalar.h>
#include <gtsam/base/numericalDerivative.h>
#include <CppUnitLite/TestHarness.h>
@ -90,8 +89,8 @@ TEST (Point3, normalize) {
}
//*************************************************************************
LieScalar norm_proxy(const Point3& point) {
return LieScalar(point.norm());
double norm_proxy(const Point3& point) {
return double(point.norm());
}
TEST (Point3, norm) {
@ -99,7 +98,7 @@ TEST (Point3, norm) {
Point3 point(3,4,5); // arbitrary point
double expected = sqrt(50);
EXPECT_DOUBLES_EQUAL(expected, point.norm(actualH), 1e-8);
Matrix expectedH = numericalDerivative11<LieScalar, Point3>(norm_proxy, point);
Matrix expectedH = numericalDerivative11<double, Point3>(norm_proxy, point);
EXPECT(assert_equal(expectedH, actualH, 1e-8));
}

15
gtsam/navigation/ImuFactor.h
View File

@ -21,7 +21,6 @@
#include <gtsam/linear/GaussianFactor.h>
#include <gtsam/navigation/ImuBias.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/base/LieVector.h>
#include <gtsam/base/debug.h>
/* External or standard includes */
@ -46,7 +45,7 @@ namespace gtsam {
* [3] L. Carlone, S. Williams, R. Roberts, "Preintegrated IMU factor: Computation of the Jacobian Matrices", Tech. Report, 2013.
*/
class ImuFactor: public NoiseModelFactor5<Pose3,LieVector,Pose3,LieVector,imuBias::ConstantBias> {
class ImuFactor: public NoiseModelFactor5<Pose3,Vector3,Pose3,Vector3,imuBias::ConstantBias> {
public:
@ -203,13 +202,13 @@ namespace gtsam {
Matrix H_vel_vel = I_3x3;
Matrix H_vel_angles = - deltaRij.matrix() * skewSymmetric(correctedAcc) * Jr_theta_i * deltaT;
// analytic expression corresponding to the following numerical derivative
// Matrix H_vel_angles = numericalDerivative11<LieVector, LieVector>(boost::bind(&PreIntegrateIMUObservations_delta_vel, correctedOmega, correctedAcc, deltaT, _1, deltaVij), theta_i);
// Matrix H_vel_angles = numericalDerivative11<Vector3, Vector3>(boost::bind(&PreIntegrateIMUObservations_delta_vel, correctedOmega, correctedAcc, deltaT, _1, deltaVij), theta_i);
Matrix H_angles_pos = Z_3x3;
Matrix H_angles_vel = Z_3x3;
Matrix H_angles_angles = Jrinv_theta_j * Rincr.inverse().matrix() * Jr_theta_i;
// analytic expression corresponding to the following numerical derivative
// Matrix H_angles_angles = numericalDerivative11<LieVector, LieVector>(boost::bind(&PreIntegrateIMUObservations_delta_angles, correctedOmega, deltaT, _1), thetaij);
// Matrix H_angles_angles = numericalDerivative11<Vector3, Vector3>(boost::bind(&PreIntegrateIMUObservations_delta_angles, correctedOmega, deltaT, _1), thetaij);
// overall Jacobian wrt preintegrated measurements (df/dx)
Matrix F(9,9);
@ -285,7 +284,7 @@ namespace gtsam {
private:
typedef ImuFactor This;
typedef NoiseModelFactor5<Pose3,LieVector,Pose3,LieVector,imuBias::ConstantBias> Base;
typedef NoiseModelFactor5<Pose3,Vector3,Pose3,Vector3,imuBias::ConstantBias> Base;
PreintegratedMeasurements preintegratedMeasurements_;
Vector3 gravity_;
@ -373,7 +372,7 @@ namespace gtsam {
/** implement functions needed to derive from Factor */
/** vector of errors */
Vector evaluateError(const Pose3& pose_i, const LieVector& vel_i, const Pose3& pose_j, const LieVector& vel_j,
Vector evaluateError(const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j,
const imuBias::ConstantBias& bias,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none,
@ -525,7 +524,7 @@ namespace gtsam {
/** predicted states from IMU */
static void Predict(const Pose3& pose_i, const LieVector& vel_i, Pose3& pose_j, LieVector& vel_j,
static void Predict(const Pose3& pose_i, const Vector3& vel_i, Pose3& pose_j, Vector3& vel_j,
const imuBias::ConstantBias& bias, const PreintegratedMeasurements preintegratedMeasurements,
const Vector3& gravity, const Vector3& omegaCoriolis, boost::optional<const Pose3&> body_P_sensor = boost::none,
const bool use2ndOrderCoriolis = false)
@ -547,7 +546,7 @@ namespace gtsam {
- skewSymmetric(omegaCoriolis) * vel_i * deltaTij*deltaTij // Coriolis term - we got rid of the 2 wrt ins paper
+ 0.5 * gravity * deltaTij*deltaTij;
vel_j = LieVector(vel_i + Rot_i.matrix() * (preintegratedMeasurements.deltaVij
vel_j = Vector3(vel_i + Rot_i.matrix() * (preintegratedMeasurements.deltaVij
+ preintegratedMeasurements.delVdelBiasAcc * biasAccIncr
+ preintegratedMeasurements.delVdelBiasOmega * biasOmegaIncr)
- 2 * skewSymmetric(omegaCoriolis) * vel_i * deltaTij // Coriolis term

7
gtsam/nonlinear/WhiteNoiseFactor.h
View File

@ -19,7 +19,6 @@
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/linear/HessianFactor.h>
#include <gtsam/base/LieScalar.h>
#include <cmath>
namespace gtsam {
@ -126,7 +125,7 @@ namespace gtsam {
/// Calculate the error of the factor, typically equal to log-likelihood
inline double error(const Values& x) const {
return f(z_, x.at<LieScalar>(meanKey_), x.at<LieScalar>(precisionKey_));
return f(z_, x.at<double>(meanKey_), x.at<double>(precisionKey_));
}
/**
@ -155,8 +154,8 @@ namespace gtsam {
/// linearize returns a Hessianfactor that is an approximation of error(p)
virtual boost::shared_ptr<GaussianFactor> linearize(const Values& x) const {
double u = x.at<LieScalar>(meanKey_);
double p = x.at<LieScalar>(precisionKey_);
double u = x.at<double>(meanKey_);
double p = x.at<double>(precisionKey_);
Key j1 = meanKey_;
Key j2 = precisionKey_;
return linearize(z_, u, p, j1, j2);

101
gtsam/nonlinear/tests/testValues.cpp
View File

@ -21,7 +21,6 @@
#include <gtsam/geometry/Pose3.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/TestableAssertions.h>
#include <gtsam/base/LieVector.h>
#include <CppUnitLite/TestHarness.h>
#include <boost/assign/std/list.hpp> // for operator +=
@ -74,7 +73,7 @@ struct dimension<TestValue> : public boost::integral_constant<int, 0> {};
TEST( Values, equals1 )
{
Values expected;
LieVector v((Vector(3) << 5.0, 6.0, 7.0));
Vector3 v(5.0, 6.0, 7.0);
expected.insert(key1, v);
Values actual;
@ -86,8 +85,8 @@ TEST( Values, equals1 )
TEST( Values, equals2 )
{
Values cfg1, cfg2;
LieVector v1((Vector(3) << 5.0, 6.0, 7.0));
LieVector v2((Vector(3) << 5.0, 6.0, 8.0));
Vector3 v1(5.0, 6.0, 7.0);
Vector3 v2(5.0, 6.0, 8.0);
cfg1.insert(key1, v1);
cfg2.insert(key1, v2);
@ -99,8 +98,8 @@ TEST( Values, equals2 )
TEST( Values, equals_nan )
{
Values cfg1, cfg2;
LieVector v1((Vector(3) << 5.0, 6.0, 7.0));
LieVector v2((Vector(3) << inf, inf, inf));
Vector3 v1(5.0, 6.0, 7.0);
Vector3 v2(inf, inf, inf);
cfg1.insert(key1, v1);
cfg2.insert(key1, v2);
@ -112,10 +111,10 @@ TEST( Values, equals_nan )
TEST( Values, insert_good )
{
Values cfg1, cfg2, expected;
LieVector v1((Vector(3) << 5.0, 6.0, 7.0));
LieVector v2((Vector(3) << 8.0, 9.0, 1.0));
LieVector v3((Vector(3) << 2.0, 4.0, 3.0));
LieVector v4((Vector(3) << 8.0, 3.0, 7.0));
Vector3 v1(5.0, 6.0, 7.0);
Vector3 v2(8.0, 9.0, 1.0);
Vector3 v3(2.0, 4.0, 3.0);
Vector3 v4(8.0, 3.0, 7.0);
cfg1.insert(key1, v1);
cfg1.insert(key2, v2);
@ -134,10 +133,10 @@ TEST( Values, insert_good )
TEST( Values, insert_bad )
{
Values cfg1, cfg2;
LieVector v1((Vector(3) << 5.0, 6.0, 7.0));
LieVector v2((Vector(3) << 8.0, 9.0, 1.0));
LieVector v3((Vector(3) << 2.0, 4.0, 3.0));
LieVector v4((Vector(3) << 8.0, 3.0, 7.0));
Vector3 v1(5.0, 6.0, 7.0);
Vector3 v2(8.0, 9.0, 1.0);
Vector3 v3(2.0, 4.0, 3.0);
Vector3 v4(8.0, 3.0, 7.0);
cfg1.insert(key1, v1);
cfg1.insert(key2, v2);
@ -151,16 +150,16 @@ TEST( Values, insert_bad )
TEST( Values, update_element )
{
Values cfg;
LieVector v1((Vector(3) << 5.0, 6.0, 7.0));
LieVector v2((Vector(3) << 8.0, 9.0, 1.0));
Vector3 v1(5.0, 6.0, 7.0);
Vector3 v2(8.0, 9.0, 1.0);
cfg.insert(key1, v1);
CHECK(cfg.size() == 1);
CHECK(assert_equal(v1, cfg.at<LieVector>(key1)));
CHECK(assert_equal(v1, cfg.at<Vector3>(key1)));
cfg.update(key1, v2);
CHECK(cfg.size() == 1);
CHECK(assert_equal(v2, cfg.at<LieVector>(key1)));
CHECK(assert_equal(v2, cfg.at<Vector3>(key1)));
}
/* ************************************************************************* */
@ -168,10 +167,10 @@ TEST(Values, basic_functions)
{
Values values;
const Values& values_c = values;
values.insert(2, LieVector());
values.insert(4, LieVector());
values.insert(6, LieVector());
values.insert(8, LieVector());
values.insert(2, Vector3());
values.insert(4, Vector3());
values.insert(6, Vector3());
values.insert(8, Vector3());
// find
EXPECT_LONGS_EQUAL(4, values.find(4)->key);
@ -195,8 +194,8 @@ TEST(Values, basic_functions)
//TEST(Values, dim_zero)
//{
// Values config0;
// config0.insert(key1, LieVector((Vector(2) << 2.0, 3.0));
// config0.insert(key2, LieVector((Vector(3) << 5.0, 6.0, 7.0));
// config0.insert(key1, Vector2((Vector(2) << 2.0, 3.0));
// config0.insert(key2, Vector3(5.0, 6.0, 7.0));
// LONGS_EQUAL(5, config0.dim());
//
// VectorValues expected;
@ -209,16 +208,16 @@ TEST(Values, basic_functions)
TEST(Values, expmap_a)
{
Values config0;
config0.insert(key1, LieVector((Vector(3) << 1.0, 2.0, 3.0)));
config0.insert(key2, LieVector((Vector(3) << 5.0, 6.0, 7.0)));
config0.insert(key1, Vector3(1.0, 2.0, 3.0));
config0.insert(key2, Vector3(5.0, 6.0, 7.0));
VectorValues increment = pair_list_of<Key, Vector>
(key1, (Vector(3) << 1.0, 1.1, 1.2))
(key2, (Vector(3) << 1.3, 1.4, 1.5));
Values expected;
expected.insert(key1, LieVector((Vector(3) << 2.0, 3.1, 4.2)));
expected.insert(key2, LieVector((Vector(3) << 6.3, 7.4, 8.5)));
expected.insert(key1, Vector3(2.0, 3.1, 4.2));
expected.insert(key2, Vector3(6.3, 7.4, 8.5));
CHECK(assert_equal(expected, config0.retract(increment)));
}
@ -227,15 +226,15 @@ TEST(Values, expmap_a)
TEST(Values, expmap_b)
{
Values config0;
config0.insert(key1, LieVector((Vector(3) << 1.0, 2.0, 3.0)));
config0.insert(key2, LieVector((Vector(3) << 5.0, 6.0, 7.0)));
config0.insert(key1, Vector3(1.0, 2.0, 3.0));
config0.insert(key2, Vector3(5.0, 6.0, 7.0));
VectorValues increment = pair_list_of<Key, Vector>
(key2, (Vector(3) << 1.3, 1.4, 1.5));
Values expected;
expected.insert(key1, LieVector((Vector(3) << 1.0, 2.0, 3.0)));
expected.insert(key2, LieVector((Vector(3) << 6.3, 7.4, 8.5)));
expected.insert(key1, Vector3(1.0, 2.0, 3.0));
expected.insert(key2, Vector3(6.3, 7.4, 8.5));
CHECK(assert_equal(expected, config0.retract(increment)));
}
@ -244,16 +243,16 @@ TEST(Values, expmap_b)
//TEST(Values, expmap_c)
//{
// Values config0;
// config0.insert(key1, LieVector((Vector(3) << 1.0, 2.0, 3.0)));
// config0.insert(key2, LieVector((Vector(3) << 5.0, 6.0, 7.0)));
// config0.insert(key1, Vector3(1.0, 2.0, 3.0));
// config0.insert(key2, Vector3(5.0, 6.0, 7.0));
//
// Vector increment = LieVector(6,
// Vector increment = Vector6(
// 1.0, 1.1, 1.2,
// 1.3, 1.4, 1.5);
//
// Values expected;
// expected.insert(key1, LieVector((Vector(3) << 2.0, 3.1, 4.2)));
// expected.insert(key2, LieVector((Vector(3) << 6.3, 7.4, 8.5)));
// expected.insert(key1, Vector3(2.0, 3.1, 4.2));
// expected.insert(key2, Vector3(6.3, 7.4, 8.5));
//
// CHECK(assert_equal(expected, config0.retract(increment)));
//}
@ -262,8 +261,8 @@ TEST(Values, expmap_b)
TEST(Values, expmap_d)
{
Values config0;
config0.insert(key1, LieVector((Vector(3) << 1.0, 2.0, 3.0)));
config0.insert(key2, LieVector((Vector(3) << 5.0, 6.0, 7.0)));
config0.insert(key1, Vector3(1.0, 2.0, 3.0));
config0.insert(key2, Vector3(5.0, 6.0, 7.0));
//config0.print("config0");
CHECK(equal(config0, config0));
CHECK(config0.equals(config0));
@ -280,8 +279,8 @@ TEST(Values, expmap_d)
TEST(Values, localCoordinates)
{
Values valuesA;
valuesA.insert(key1, LieVector((Vector(3) << 1.0, 2.0, 3.0)));
valuesA.insert(key2, LieVector((Vector(3) << 5.0, 6.0, 7.0)));
valuesA.insert(key1, Vector3(1.0, 2.0, 3.0));
valuesA.insert(key2, Vector3(5.0, 6.0, 7.0));
VectorValues expDelta = pair_list_of<Key, Vector>
(key1, (Vector(3) << 0.1, 0.2, 0.3))
@ -317,28 +316,28 @@ TEST(Values, extract_keys)
TEST(Values, exists_)
{
Values config0;
config0.insert(key1, LieVector((Vector(1) << 1.)));
config0.insert(key2, LieVector((Vector(1) << 2.)));
config0.insert(key1, 1.0);
config0.insert(key2, 2.0);
boost::optional<const LieVector&> v = config0.exists<LieVector>(key1);
CHECK(assert_equal((Vector(1) << 1.),*v));
boost::optional<const double&> v = config0.exists<double>(key1);
DOUBLES_EQUAL(1.0,*v,1e-9);
}
/* ************************************************************************* */
TEST(Values, update)
{
Values config0;
config0.insert(key1, LieVector((Vector(1) << 1.)));
config0.insert(key2, LieVector((Vector(1) << 2.)));
config0.insert(key1, 1.0);
config0.insert(key2, 2.0);
Values superset;
superset.insert(key1, LieVector((Vector(1) << -1.)));
superset.insert(key2, LieVector((Vector(1) << -2.)));
superset.insert(key1, -1.0);
superset.insert(key2, -2.0);
config0.update(superset);
Values expected;
expected.insert(key1, LieVector((Vector(1) << -1.)));
expected.insert(key2, LieVector((Vector(1) << -2.)));
expected.insert(key1, -1.0);
expected.insert(key2, -2.0);
CHECK(assert_equal(expected, config0));
}

13
gtsam/slam/EssentialMatrixFactor.h
View File

@ -10,7 +10,6 @@
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/geometry/EssentialMatrix.h>
#include <gtsam/geometry/SimpleCamera.h>
#include <gtsam/base/LieScalar.h>
#include <iostream>
namespace gtsam {
@ -85,14 +84,13 @@ public:
* Binary factor that optimizes for E and inverse depth d: assumes measurement
* in image 2 is perfect, and returns re-projection error in image 1
*/
class EssentialMatrixFactor2: public NoiseModelFactor2<EssentialMatrix,
LieScalar> {
class EssentialMatrixFactor2: public NoiseModelFactor2<EssentialMatrix, double> {
Point3 dP1_; ///< 3D point corresponding to measurement in image 1
Point2 pn_; ///< Measurement in image 2, in ideal coordinates
double f_; ///< approximate conversion factor for error scaling
typedef NoiseModelFactor2<EssentialMatrix, LieScalar> Base;
typedef NoiseModelFactor2<EssentialMatrix, double> Base;
typedef EssentialMatrixFactor2 This;
public:
@ -149,7 +147,7 @@ public:
* @param E essential matrix
* @param d inverse depth d
*/
Vector evaluateError(const EssentialMatrix& E, const LieScalar& d,
Vector evaluateError(const EssentialMatrix& E, const double& d,
boost::optional<Matrix&> DE = boost::none, boost::optional<Matrix&> Dd =
boost::none) const {
@ -237,7 +235,8 @@ public:
*/
template<class CALIBRATION>
EssentialMatrixFactor3(Key key1, Key key2, const Point2& pA, const Point2& pB,
const Rot3& cRb, const SharedNoiseModel& model, boost::shared_ptr<CALIBRATION> K) :
const Rot3& cRb, const SharedNoiseModel& model,
boost::shared_ptr<CALIBRATION> K) :
EssentialMatrixFactor2(key1, key2, pA, pB, model, K), cRb_(cRb) {
}
@ -259,7 +258,7 @@ public:
* @param E essential matrix
* @param d inverse depth d
*/
Vector evaluateError(const EssentialMatrix& E, const LieScalar& d,
Vector evaluateError(const EssentialMatrix& E, const double& d,
boost::optional<Matrix&> DE = boost::none, boost::optional<Matrix&> Dd =
boost::none) const {
if (!DE) {

23
gtsam_unstable/dynamics/SimpleHelicopter.h
View File

@ -9,7 +9,6 @@
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/base/LieVector.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
namespace gtsam {
@ -24,10 +23,10 @@ namespace gtsam {
* Note: this factor is necessary if one needs to smooth the entire graph. It's not needed
* in sequential update method.
*/
class Reconstruction : public NoiseModelFactor3<Pose3, Pose3, LieVector> {
class Reconstruction : public NoiseModelFactor3<Pose3, Pose3, Vector6> {
double h_; // time step
typedef NoiseModelFactor3<Pose3, Pose3, LieVector> Base;
typedef NoiseModelFactor3<Pose3, Pose3, Vector6> Base;
public:
Reconstruction(Key gKey1, Key gKey, Key xiKey, double h, double mu = 1000.0) :
Base(noiseModel::Constrained::All(Pose3::Dim(), fabs(mu)), gKey1, gKey,
@ -41,7 +40,7 @@ public:
gtsam::NonlinearFactor::shared_ptr(new Reconstruction(*this))); }
/** \f$ log((g_k\exp(h\xi_k))^{-1}g_{k+1}) = 0, with optional derivatives */
Vector evaluateError(const Pose3& gk1, const Pose3& gk, const LieVector& xik,
Vector evaluateError(const Pose3& gk1, const Pose3& gk, const Vector6& xik,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none,
boost::optional<Matrix&> H3 = boost::none) const {
@ -74,7 +73,7 @@ public:
/**
* Implement the Discrete Euler-Poincare' equation:
*/
class DiscreteEulerPoincareHelicopter : public NoiseModelFactor3<LieVector, LieVector, Pose3> {
class DiscreteEulerPoincareHelicopter : public NoiseModelFactor3<Vector6, Vector6, Pose3> {
double h_; /// time step
Matrix Inertia_; /// Inertia tensors Inertia = [ J 0; 0 M ]
@ -87,7 +86,7 @@ class DiscreteEulerPoincareHelicopter : public NoiseModelFactor3<LieVector, LieV
// This might be needed in control or system identification problems.
// We treat them as constant here, since the control inputs are to specify.
typedef NoiseModelFactor3<LieVector, LieVector, Pose3> Base;
typedef NoiseModelFactor3<Vector6, Vector6, Pose3> Base;
public:
DiscreteEulerPoincareHelicopter(Key xiKey1, Key xiKey_1, Key gKey,
@ -108,7 +107,7 @@ public:
* where pk = CT_TLN(h*xi_k)*Inertia*xi_k
* pk_1 = CT_TLN(-h*xi_k_1)*Inertia*xi_k_1
* */
Vector evaluateError(const LieVector& xik, const LieVector& xik_1, const Pose3& gk,
Vector evaluateError(const Vector6& xik, const Vector6& xik_1, const Pose3& gk,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none,
boost::optional<Matrix&> H3 = boost::none) const {
@ -149,7 +148,7 @@ public:
}
#if 0
Vector computeError(const LieVector& xik, const LieVector& xik_1, const Pose3& gk) const {
Vector computeError(const Vector6& xik, const Vector6& xik_1, const Pose3& gk) const {
Vector pk = Pose3::dExpInv_exp(h_*xik).transpose()*Inertia_*xik;
Vector pk_1 = Pose3::dExpInv_exp(-h_*xik_1).transpose()*Inertia_*xik_1;
@ -161,13 +160,13 @@ public:
return hx;
}
Vector evaluateError(const LieVector& xik, const LieVector& xik_1, const Pose3& gk,
Vector evaluateError(const Vector6& xik, const Vector6& xik_1, const Pose3& gk,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none,
boost::optional<Matrix&> H3 = boost::none) const {
if (H1) {
(*H1) = numericalDerivative31(
boost::function<Vector(const LieVector&, const LieVector&, const Pose3&)>(
boost::function<Vector(const Vector6&, const Vector6&, const Pose3&)>(
boost::bind(&DiscreteEulerPoincareHelicopter::computeError, *this, _1, _2, _3)
),
xik, xik_1, gk, 1e-5
@ -175,7 +174,7 @@ public:
}
if (H2) {
(*H2) = numericalDerivative32(
boost::function<Vector(const LieVector&, const LieVector&, const Pose3&)>(
boost::function<Vector(const Vector6&, const Vector6&, const Pose3&)>(
boost::bind(&DiscreteEulerPoincareHelicopter::computeError, *this, _1, _2, _3)
),
xik, xik_1, gk, 1e-5
@ -183,7 +182,7 @@ public:
}
if (H3) {
(*H3) = numericalDerivative33(
boost::function<Vector(const LieVector&, const LieVector&, const Pose3&)>(
boost::function<Vector(const Vector6&, const Vector6&, const Pose3&)>(
boost::bind(&DiscreteEulerPoincareHelicopter::computeError, *this, _1, _2, _3)
),
xik, xik_1, gk, 1e-5

89
tests/testGaussianISAM2.cpp
View File

@ -8,7 +8,6 @@
#include <gtsam/base/debug.h>
#include <gtsam/base/TestableAssertions.h>
#include <gtsam/base/LieVector.h>
#include <gtsam/base/treeTraversal-inst.h>
#include <gtsam/geometry/Point2.h>
#include <gtsam/geometry/Pose2.h>
@ -285,19 +284,19 @@ bool isam_check(const NonlinearFactorGraph& fullgraph, const Values& fullinit, c
TEST(ISAM2, AddFactorsStep1)
{
NonlinearFactorGraph nonlinearFactors;
nonlinearFactors += PriorFactor<LieVector>(10, LieVector(), gtsam::SharedNoiseModel());
nonlinearFactors += PriorFactor<double>(10, 0.0, gtsam::SharedNoiseModel());
nonlinearFactors += NonlinearFactor::shared_ptr();
nonlinearFactors += PriorFactor<LieVector>(11, LieVector(), gtsam::SharedNoiseModel());
nonlinearFactors += PriorFactor<double>(11, 0.0, gtsam::SharedNoiseModel());
NonlinearFactorGraph newFactors;
newFactors += PriorFactor<LieVector>(1, LieVector(), gtsam::SharedNoiseModel());
newFactors += PriorFactor<LieVector>(2, LieVector(), gtsam::SharedNoiseModel());
newFactors += PriorFactor<double>(1, 0.0, gtsam::SharedNoiseModel());
newFactors += PriorFactor<double>(2, 0.0, gtsam::SharedNoiseModel());
NonlinearFactorGraph expectedNonlinearFactors;
expectedNonlinearFactors += PriorFactor<LieVector>(10, LieVector(), gtsam::SharedNoiseModel());
expectedNonlinearFactors += PriorFactor<LieVector>(1, LieVector(), gtsam::SharedNoiseModel());
expectedNonlinearFactors += PriorFactor<LieVector>(11, LieVector(), gtsam::SharedNoiseModel());
expectedNonlinearFactors += PriorFactor<LieVector>(2, LieVector(), gtsam::SharedNoiseModel());
expectedNonlinearFactors += PriorFactor<double>(10, 0.0, gtsam::SharedNoiseModel());
expectedNonlinearFactors += PriorFactor<double>(1, 0.0, gtsam::SharedNoiseModel());
expectedNonlinearFactors += PriorFactor<double>(11, 0.0, gtsam::SharedNoiseModel());
expectedNonlinearFactors += PriorFactor<double>(2, 0.0, gtsam::SharedNoiseModel());
const FastVector<size_t> expectedNewFactorIndices = list_of(1)(3);
@ -696,16 +695,16 @@ TEST(ISAM2, marginalizeLeaves1)
ISAM2 isam;
NonlinearFactorGraph factors;
factors += PriorFactor<LieVector>(0, LieVector(0.0), noiseModel::Unit::Create(1));
factors += PriorFactor<double>(0, 0.0, noiseModel::Unit::Create(1));
factors += BetweenFactor<LieVector>(0, 1, LieVector(0.0), noiseModel::Unit::Create(1));
factors += BetweenFactor<LieVector>(1, 2, LieVector(0.0), noiseModel::Unit::Create(1));
factors += BetweenFactor<LieVector>(0, 2, LieVector(0.0), noiseModel::Unit::Create(1));
factors += BetweenFactor<double>(0, 1, 0.0, noiseModel::Unit::Create(1));
factors += BetweenFactor<double>(1, 2, 0.0, noiseModel::Unit::Create(1));
factors += BetweenFactor<double>(0, 2, 0.0, noiseModel::Unit::Create(1));
Values values;
values.insert(0, LieVector(0.0));
values.insert(1, LieVector(0.0));
values.insert(2, LieVector(0.0));
values.insert(0, 0.0);
values.insert(1, 0.0);
values.insert(2, 0.0);
FastMap<Key,int> constrainedKeys;
constrainedKeys.insert(make_pair(0,0));
@ -724,18 +723,18 @@ TEST(ISAM2, marginalizeLeaves2)
ISAM2 isam;
NonlinearFactorGraph factors;
factors += PriorFactor<LieVector>(0, LieVector(0.0), noiseModel::Unit::Create(1));
factors += PriorFactor<double>(0, 0.0, noiseModel::Unit::Create(1));
factors += BetweenFactor<LieVector>(0, 1, LieVector(0.0), noiseModel::Unit::Create(1));
factors += BetweenFactor<LieVector>(1, 2, LieVector(0.0), noiseModel::Unit::Create(1));
factors += BetweenFactor<LieVector>(0, 2, LieVector(0.0), noiseModel::Unit::Create(1));
factors += BetweenFactor<LieVector>(2, 3, LieVector(0.0), noiseModel::Unit::Create(1));
factors += BetweenFactor<double>(0, 1, 0.0, noiseModel::Unit::Create(1));
factors += BetweenFactor<double>(1, 2, 0.0, noiseModel::Unit::Create(1));
factors += BetweenFactor<double>(0, 2, 0.0, noiseModel::Unit::Create(1));
factors += BetweenFactor<double>(2, 3, 0.0, noiseModel::Unit::Create(1));
Values values;
values.insert(0, LieVector(0.0));
values.insert(1, LieVector(0.0));
values.insert(2, LieVector(0.0));
values.insert(3, LieVector(0.0));
values.insert(0, 0.0);
values.insert(1, 0.0);
values.insert(2, 0.0);
values.insert(3, 0.0);
FastMap<Key,int> constrainedKeys;
constrainedKeys.insert(make_pair(0,0));
@ -755,25 +754,25 @@ TEST(ISAM2, marginalizeLeaves3)
ISAM2 isam;
NonlinearFactorGraph factors;
factors += PriorFactor<LieVector>(0, LieVector(0.0), noiseModel::Unit::Create(1));
factors += PriorFactor<double>(0, 0.0, noiseModel::Unit::Create(1));
factors += BetweenFactor<LieVector>(0, 1, LieVector(0.0), noiseModel::Unit::Create(1));
factors += BetweenFactor<LieVector>(1, 2, LieVector(0.0), noiseModel::Unit::Create(1));
factors += BetweenFactor<LieVector>(0, 2, LieVector(0.0), noiseModel::Unit::Create(1));
factors += BetweenFactor<double>(0, 1, 0.0, noiseModel::Unit::Create(1));
factors += BetweenFactor<double>(1, 2, 0.0, noiseModel::Unit::Create(1));
factors += BetweenFactor<double>(0, 2, 0.0, noiseModel::Unit::Create(1));
factors += BetweenFactor<LieVector>(2, 3, LieVector(0.0), noiseModel::Unit::Create(1));
factors += BetweenFactor<double>(2, 3, 0.0, noiseModel::Unit::Create(1));
factors += BetweenFactor<LieVector>(3, 4, LieVector(0.0), noiseModel::Unit::Create(1));
factors += BetweenFactor<LieVector>(4, 5, LieVector(0.0), noiseModel::Unit::Create(1));
factors += BetweenFactor<LieVector>(3, 5, LieVector(0.0), noiseModel::Unit::Create(1));
factors += BetweenFactor<double>(3, 4, 0.0, noiseModel::Unit::Create(1));
factors += BetweenFactor<double>(4, 5, 0.0, noiseModel::Unit::Create(1));
factors += BetweenFactor<double>(3, 5, 0.0, noiseModel::Unit::Create(1));
Values values;
values.insert(0, LieVector(0.0));
values.insert(1, LieVector(0.0));
values.insert(2, LieVector(0.0));
values.insert(3, LieVector(0.0));
values.insert(4, LieVector(0.0));
values.insert(5, LieVector(0.0));
values.insert(0, 0.0);
values.insert(1, 0.0);
values.insert(2, 0.0);
values.insert(3, 0.0);
values.insert(4, 0.0);
values.insert(5, 0.0);
FastMap<Key,int> constrainedKeys;
constrainedKeys.insert(make_pair(0,0));
@ -795,14 +794,14 @@ TEST(ISAM2, marginalizeLeaves4)
ISAM2 isam;
NonlinearFactorGraph factors;
factors += PriorFactor<LieVector>(0, LieVector(0.0), noiseModel::Unit::Create(1));
factors += BetweenFactor<LieVector>(0, 2, LieVector(0.0), noiseModel::Unit::Create(1));
factors += BetweenFactor<LieVector>(1, 2, LieVector(0.0), noiseModel::Unit::Create(1));
factors += PriorFactor<double>(0, 0.0, noiseModel::Unit::Create(1));
factors += BetweenFactor<double>(0, 2, 0.0, noiseModel::Unit::Create(1));
factors += BetweenFactor<double>(1, 2, 0.0, noiseModel::Unit::Create(1));
Values values;
values.insert(0, LieVector(0.0));
values.insert(1, LieVector(0.0));
values.insert(2, LieVector(0.0));
values.insert(0, 0.0);
values.insert(1, 0.0);
values.insert(2, 0.0);
FastMap<Key,int> constrainedKeys;
constrainedKeys.insert(make_pair(0,0));