diff --git a/.cproject b/.cproject index 5689b0b48..af740032b 100644 --- a/.cproject +++ b/.cproject @@ -311,14 +311,6 @@ true true - - make - -j2 - testGaussianFactor.run - true - true - true - make -j2 @@ -345,6 +337,7 @@ make + tests/testBayesTree.run true false @@ -352,6 +345,7 @@ make + testBinaryBayesNet.run true false @@ -399,6 +393,7 @@ make + testSymbolicBayesNet.run true false @@ -406,6 +401,7 @@ make + tests/testSymbolicFactor.run true false @@ -413,6 +409,7 @@ make + testSymbolicFactorGraph.run true false @@ -428,11 +425,20 @@ make + tests/testBayesTree true false true + + make + -j2 + testGaussianFactor.run + true + true + true + make -j5 @@ -441,6 +447,14 @@ true true + + make + -j5 + testIMUSystem.run + true + true + true + make -j5 @@ -513,22 +527,6 @@ false true - - make - -j2 - all - true - true - true - - - make - -j2 - clean - true - true - true - make -j2 @@ -545,6 +543,22 @@ true true + + make + -j2 + all + true + true + true + + + make + -j2 + clean + true + true + true + make -j2 @@ -569,18 +583,26 @@ true true - + make - -j5 - testValues.run + -j2 + all true true true - + make - -j5 - testOrdering.run + -j2 + check + true + true + true + + + make + -j2 + clean true true true @@ -665,26 +687,18 @@ true true - + make - -j2 - all + -j5 + testValues.run true true true - + make - -j2 - check - true - true - true - - - make - -j2 - clean + -j5 + testOrdering.run true true true @@ -931,6 +945,7 @@ make + testGraph.run true false @@ -938,6 +953,7 @@ make + testJunctionTree.run true false @@ -945,6 +961,7 @@ make + testSymbolicBayesNetB.run true false @@ -1064,6 +1081,7 @@ make + testErrors.run true false @@ -1519,7 +1537,6 @@ make - testSimulated2DOriented.run true false @@ -1559,7 +1576,6 @@ make - testSimulated2D.run true false @@ -1567,7 +1583,6 @@ make - testSimulated3D.run true false @@ -1783,7 +1798,6 @@ make - tests/testGaussianISAM2 true false @@ -1805,102 +1819,6 @@ true true - - make - -j2 - testRot3.run - true - true - true - - - make - -j2 - testRot2.run - true - true - true - - - make - -j2 - testPose3.run - true - true - true - - - make - -j2 - timeRot3.run - true - true - true - - - make - -j2 - testPose2.run - true - true - true - - - make - -j2 - testCal3_S2.run - true - true - true - - - make - -j2 - testSimpleCamera.run - true - true - true - - - make - -j2 - testHomography2.run - true - true - true - - - make - -j2 - testCalibratedCamera.run - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - testPoint2.run - true - true - true - make -j2 @@ -2102,6 +2020,7 @@ cpack + -G DEB true false @@ -2109,6 +2028,7 @@ cpack + -G RPM true false @@ -2116,6 +2036,7 @@ cpack + -G TGZ true false @@ -2123,6 +2044,7 @@ cpack + --config CPackSourceConfig.cmake true false @@ -2216,42 +2138,98 @@ true true - + make - -j5 - testSpirit.run + -j2 + testRot3.run true true true - + make - -j5 - testWrap.run + -j2 + testRot2.run true true true - + make - -j5 - check.wrap + -j2 + testPose3.run true true true - + make - -j5 - wrap_gtsam + -j2 + timeRot3.run true true true - + make - -j5 - wrap + -j2 + testPose2.run + true + true + true + + + make + -j2 + testCal3_S2.run + true + true + true + + + make + -j2 + testSimpleCamera.run + true + true + true + + + make + -j2 + testHomography2.run + true + true + true + + + make + -j2 + testCalibratedCamera.run + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + testPoint2.run true true true @@ -2295,6 +2273,46 @@ false true + + make + -j5 + testSpirit.run + true + true + true + + + make + -j5 + testWrap.run + true + true + true + + + make + -j5 + check.wrap + true + true + true + + + make + -j5 + wrap_gtsam + true + true + true + + + make + -j5 + wrap + true + true + true + diff --git a/gtsam/nonlinear/tests/testKey.cpp b/gtsam/nonlinear/tests/testKey.cpp index dedb45332..601176935 100644 --- a/gtsam/nonlinear/tests/testKey.cpp +++ b/gtsam/nonlinear/tests/testKey.cpp @@ -25,6 +25,8 @@ using namespace boost::assign; using namespace std; using namespace gtsam; +Key aKey = gtsam::symbol_shorthand::X(4); // FIXME: throws index too large exception in Symbol.key() + /* ************************************************************************* */ TEST(Key, KeySymbolConversion) { Symbol original('j', 4); diff --git a/gtsam_unstable/dynamics/imuSystem.cpp b/gtsam_unstable/dynamics/imuSystem.cpp index 209615bcc..376868583 100644 --- a/gtsam_unstable/dynamics/imuSystem.cpp +++ b/gtsam_unstable/dynamics/imuSystem.cpp @@ -13,44 +13,44 @@ using namespace gtsam; /* ************************************************************************* */ void Graph::addPrior(Key key, const PoseRTV& pose, const SharedNoiseModel& noiseModel) { - add(Prior(PoseKey(key), pose, noiseModel)); + add(Prior(key, pose, noiseModel)); } /* ************************************************************************* */ void Graph::addConstraint(Key key, const PoseRTV& pose) { - add(Constraint(PoseKey(key), pose)); + add(Constraint(key, pose)); } /* ************************************************************************* */ void Graph::addHeightPrior(Key key, double z, const SharedNoiseModel& noiseModel) { - add(DHeightPrior(PoseKey(key), z, noiseModel)); + add(DHeightPrior(key, z, noiseModel)); } /* ************************************************************************* */ void Graph::addFullIMUMeasurement(Key key1, Key key2, const Vector& accel, const Vector& gyro, double dt, const SharedNoiseModel& noiseModel) { - add(FullIMUMeasurement(accel, gyro, dt, PoseKey(key1), PoseKey(key2), noiseModel)); + add(FullIMUMeasurement(accel, gyro, dt, key1, key2, noiseModel)); } /* ************************************************************************* */ void Graph::addIMUMeasurement(Key key1, Key key2, const Vector& accel, const Vector& gyro, double dt, const SharedNoiseModel& noiseModel) { - add(IMUMeasurement(accel, gyro, dt, PoseKey(key1), PoseKey(key2), noiseModel)); + add(IMUMeasurement(accel, gyro, dt, key1, key2, noiseModel)); } /* ************************************************************************* */ void Graph::addVelocityConstraint(Key key1, Key key2, double dt, const SharedNoiseModel& noiseModel) { - add(VelocityConstraint(PoseKey(key1), PoseKey(key2), dt, noiseModel)); + add(VelocityConstraint(key1, key2, dt, noiseModel)); } /* ************************************************************************* */ void Graph::addBetween(Key key1, Key key2, const PoseRTV& z, const SharedNoiseModel& noiseModel) { - add(Between(PoseKey(key1), PoseKey(key2), z, noiseModel)); + add(Between(key1, key2, z, noiseModel)); } /* ************************************************************************* */ void Graph::addRange(Key key1, Key key2, double z, const SharedNoiseModel& noiseModel) { - add(Range(PoseKey(key1), PoseKey(key2), z, noiseModel)); + add(Range(key1, key2, z, noiseModel)); } /* ************************************************************************* */ diff --git a/gtsam_unstable/dynamics/imuSystem.h b/gtsam_unstable/dynamics/imuSystem.h index b0bee8873..ef5437fdf 100644 --- a/gtsam_unstable/dynamics/imuSystem.h +++ b/gtsam_unstable/dynamics/imuSystem.h @@ -6,11 +6,6 @@ #pragma once -#include - -#include -#include -#include #include #include #include @@ -34,11 +29,6 @@ */ namespace imu { -using namespace gtsam; - -// Values: just poses -inline Symbol PoseKey(Index j) { return Symbol('x', j); } - struct Values : public gtsam::Values { typedef gtsam::Values Base; @@ -46,39 +36,39 @@ struct Values : public gtsam::Values { Values(const Values& values) : Base(values) {} Values(const Base& values) : Base(values) {} - void insertPose(Index key, const PoseRTV& pose) { insert(PoseKey(key), pose); } - PoseRTV pose(Index key) const { return at(PoseKey(key)); } + void insertPose(gtsam::Key key, const gtsam::PoseRTV& pose) { insert(key, pose); } + gtsam::PoseRTV pose(gtsam::Key key) const { return at(key); } }; // factors -typedef IMUFactor IMUMeasurement; // IMU between measurements -typedef FullIMUFactor FullIMUMeasurement; // Full-state IMU between measurements -typedef BetweenFactor Between; // full odometry (including velocity) -typedef NonlinearEquality Constraint; -typedef PriorFactor Prior; -typedef RangeFactor Range; +typedef gtsam::IMUFactor IMUMeasurement; // IMU between measurements +typedef gtsam::FullIMUFactor FullIMUMeasurement; // Full-state IMU between measurements +typedef gtsam::BetweenFactor Between; // full odometry (including velocity) +typedef gtsam::NonlinearEquality Constraint; +typedef gtsam::PriorFactor Prior; +typedef gtsam::RangeFactor Range; // graph components -struct Graph : public NonlinearFactorGraph { - typedef NonlinearFactorGraph Base; +struct Graph : public gtsam::NonlinearFactorGraph { + typedef gtsam::NonlinearFactorGraph Base; Graph() {} Graph(const Base& graph) : Base(graph) {} Graph(const Graph& graph) : Base(graph) {} // prior factors - void addPrior(size_t key, const PoseRTV& pose, const SharedNoiseModel& noiseModel); - void addConstraint(size_t key, const PoseRTV& pose); - void addHeightPrior(size_t key, double z, const SharedNoiseModel& noiseModel); + void addPrior(size_t key, const gtsam::PoseRTV& pose, const gtsam::SharedNoiseModel& noiseModel); + void addConstraint(size_t key, const gtsam::PoseRTV& pose); + void addHeightPrior(size_t key, double z, const gtsam::SharedNoiseModel& noiseModel); // inertial factors - void addFullIMUMeasurement(size_t key1, size_t key2, const Vector& accel, const Vector& gyro, double dt, const SharedNoiseModel& noiseModel); - void addIMUMeasurement(size_t key1, size_t key2, const Vector& accel, const Vector& gyro, double dt, const SharedNoiseModel& noiseModel); - void addVelocityConstraint(size_t key1, size_t key2, double dt, const SharedNoiseModel& noiseModel); + void addFullIMUMeasurement(size_t key1, size_t key2, const gtsam::Vector& accel, const gtsam::Vector& gyro, double dt, const gtsam::SharedNoiseModel& noiseModel); + void addIMUMeasurement(size_t key1, size_t key2, const gtsam::Vector& accel, const gtsam::Vector& gyro, double dt, const gtsam::SharedNoiseModel& noiseModel); + void addVelocityConstraint(size_t key1, size_t key2, double dt, const gtsam::SharedNoiseModel& noiseModel); // other measurements - void addBetween(size_t key1, size_t key2, const PoseRTV& z, const SharedNoiseModel& noiseModel); - void addRange(size_t key1, size_t key2, double z, const SharedNoiseModel& noiseModel); + void addBetween(size_t key1, size_t key2, const gtsam::PoseRTV& z, const gtsam::SharedNoiseModel& noiseModel); + void addRange(size_t key1, size_t key2, double z, const gtsam::SharedNoiseModel& noiseModel); // optimization Values optimize(const Values& init) const; diff --git a/gtsam_unstable/dynamics/tests/testIMUSystem.cpp b/gtsam_unstable/dynamics/tests/testIMUSystem.cpp index 916d07559..783c420d4 100644 --- a/gtsam_unstable/dynamics/tests/testIMUSystem.cpp +++ b/gtsam_unstable/dynamics/tests/testIMUSystem.cpp @@ -3,6 +3,8 @@ * @author Alex Cunningham */ +#include + #include #include @@ -13,10 +15,9 @@ using namespace imu; const double tol=1e-5; +static const Key x0 = 0, x1 = 1, x2 = 2, x3 = 3, x4 = 4; static const Vector g = delta(3, 2, -9.81); -Key x1 = PoseKey(1), x2 = PoseKey(2), x3 = PoseKey(3), x4 = PoseKey(4); - /* ************************************************************************* */ TEST(testIMUSystem, instantiations) { // just checking for compilation @@ -149,14 +150,14 @@ TEST( testIMUSystem, linear_trajectory) { imu::Values true_traj, init_traj; Graph graph; - graph.add(Constraint(PoseKey(0), start)); - true_traj.insert(PoseKey(0), start); - init_traj.insert(PoseKey(0), start); + graph.add(Constraint(x0, start)); + true_traj.insert(x0, start); + init_traj.insert(x0, start); size_t nrPoses = 10; PoseRTV cur_pose = start; for (size_t i=1; i fg = example::createGaussianFactorGraph(ordering); @@ -275,6 +279,7 @@ TEST( GaussianFactor, matrix ) /* ************************************************************************* */ TEST( GaussianFactor, matrix_aug ) { + const Key kx1 = X(1), kx2 = X(2), kl1 = L(1); // create a small linear factor graph Ordering ordering; ordering += kx1,kx2,kl1; FactorGraph fg = example::createGaussianFactorGraph(ordering); @@ -387,6 +392,7 @@ void print(const list& i) { TEST( GaussianFactor, size ) { // create a linear factor graph + const Key kx1 = X(1), kx2 = X(2), kl1 = L(1); Ordering ordering; ordering += kx1,kx2,kl1; GaussianFactorGraph fg = example::createGaussianFactorGraph(ordering);