diff --git a/.cproject b/.cproject index b5d5685ab..19005c63f 100644 --- a/.cproject +++ b/.cproject @@ -542,6 +542,14 @@ true true + + make + -j2 + testGaussianFactor.run + true + true + true + make -j2 @@ -568,7 +576,6 @@ make - tests/testBayesTree.run true false @@ -576,7 +583,6 @@ make - testBinaryBayesNet.run true false @@ -624,7 +630,6 @@ make - testSymbolicBayesNet.run true false @@ -632,7 +637,6 @@ make - tests/testSymbolicFactor.run true false @@ -640,7 +644,6 @@ make - testSymbolicFactorGraph.run true false @@ -656,20 +659,11 @@ make - tests/testBayesTree true false true - - make - -j2 - testGaussianFactor.run - true - true - true - make -j5 @@ -766,22 +760,6 @@ false true - - make - -j2 - tests/testPose2.run - true - true - true - - - make - -j2 - tests/testPose3.run - true - true - true - make -j2 @@ -798,6 +776,22 @@ true true + + make + -j2 + tests/testPose2.run + true + true + true + + + make + -j2 + tests/testPose3.run + true + true + true + make -j2 @@ -822,26 +816,42 @@ true true - + make - -j2 - all + -j5 + testValues.run true true true - + make - -j2 - check + -j5 + testOrdering.run true true true - + make - -j2 - clean + -j5 + testKey.run + true + true + true + + + make + -j5 + testLinearContainerFactor.run + true + true + true + + + make + -j6 -j8 + testWhiteNoiseFactor.run true true true @@ -902,42 +912,34 @@ true true - + make -j5 - testValues.run + testRotateFactor.run true true true - + make - -j5 - testOrdering.run + -j2 + all true true true - + make - -j5 - testKey.run + -j2 + check true true true - + make - -j5 - testLinearContainerFactor.run - true - true - true - - - make - -j6 -j8 - testWhiteNoiseFactor.run + -j2 + clean true true true @@ -1328,7 +1330,6 @@ make - testGraph.run true false @@ -1336,7 +1337,6 @@ make - testJunctionTree.run true false @@ -1344,7 +1344,6 @@ make - testSymbolicBayesNetB.run true false @@ -1512,7 +1511,6 @@ make - testErrors.run true false @@ -1558,6 +1556,22 @@ true true + + make + -j5 + testParticleFactor.run + true + true + true + + + make + -j2 + testGaussianFactor.run + true + true + true + make -j2 @@ -1638,14 +1652,6 @@ true true - - make - -j2 - testGaussianFactor.run - true - true - true - make -j2 @@ -1910,6 +1916,22 @@ true true + + make + -j5 + testSphere2.run + true + true + true + + + make + -j5 + testEssentialMatrix.run + true + true + true + make -j2 @@ -1992,6 +2014,7 @@ make + testSimulated2DOriented.run true false @@ -2031,6 +2054,7 @@ make + testSimulated2D.run true false @@ -2038,6 +2062,7 @@ make + testSimulated3D.run true false @@ -2147,6 +2172,14 @@ true true + + make + -j5 + testSampler.run + true + true + true + make -j2 @@ -2299,14 +2332,6 @@ true true - - make - -j5 - CreateSFMExampleData.run - true - true - true - make -j4 @@ -2325,6 +2350,7 @@ make + tests/testGaussianISAM2 true false @@ -2346,6 +2372,102 @@ 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 -j3 @@ -2547,7 +2669,6 @@ cpack - -G DEB true false @@ -2555,7 +2676,6 @@ cpack - -G RPM true false @@ -2563,7 +2683,6 @@ cpack - -G TGZ true false @@ -2571,7 +2690,6 @@ cpack - --config CPackSourceConfig.cmake true false @@ -2737,98 +2855,34 @@ true true - + make - -j2 - testRot3.run + -j5 + testSpirit.run true true true - + make - -j2 - testRot2.run + -j5 + testWrap.run true true true - + make - -j2 - testPose3.run + -j5 + check.wrap 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 + -j5 + wrap true true true @@ -2872,38 +2926,6 @@ 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 - true - true - true - diff --git a/gtsam_unstable/nonlinear/tests/testParticleFactor.cpp b/gtsam_unstable/nonlinear/tests/testParticleFactor.cpp new file mode 100644 index 000000000..cea3c0184 --- /dev/null +++ b/gtsam_unstable/nonlinear/tests/testParticleFactor.cpp @@ -0,0 +1,201 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file testParticleFactor.cpp + * @brief Unit tests for the Particle factor + * @author Frank Dellaert + * @date Dec 9, 2013 + */ + +#include +#include + +namespace gtsam { + +/** + * A factor approximating a density on a variable as a set of weighted particles + */ +template +class ParticleFactor { + +public: + typedef ParticleFactor This; ///< Typedef to this class + typedef boost::shared_ptr shared_ptr; ///< shared_ptr to this class + +}; + +/** + * A particle filter class, styled on functional KalmanFilter + * A state is created, which is functionally updates through [predict] and [update] + */ +template +class ParticleFilter { + +public: + + /** + * The Particle filter state is simply a ParticleFactor + */ + typedef typename ParticleFactor::shared_ptr State; + + /** + * Create initial state, i.e., prior density at time k=0 + * In Bayes Filter notation, these are x_{0|0} and P_{0|0} + * @param x0 estimate at time 0 + * @param P0 covariance at time 0, given as a diagonal Gaussian 'model' + */ + State init(const Vector& x0, const SharedDiagonal& P0) { + return boost::make_shared >(); + } + +}; +// ParticleFilter + +}// namespace gtsam + +#include +#include +#include +#include + +using namespace std; +using namespace gtsam; + +//****************************************************************************** + +TEST( particleFactor, constructor ) { +// ParticleFactor pf; + //CHECK(assert_equal(expected, actual)); +} + +//****************************************************************************** +// Tests to do: +// Take two variables pf-x-*-y, eliminate x, multiply and sample then marginalize +TEST( particleFactor, eliminate) { +// ParticleFactor fx; + BetweenFactor fxy; + +} + +//****************************************************************************** + +/** Small 2D point class implemented as a Vector */ +struct State: Vector { + State(double x, double y) : + Vector((Vector(2) << x, y)) { + } +}; + +//****************************************************************************** +TEST( ParticleFilter, constructor) { + +// Create a Kalman filter of dimension 2 + ParticleFilter pf1; + +// Create inital mean/covariance + State x_initial(0.0, 0.0); + SharedDiagonal P1 = noiseModel::Isotropic::Sigma(2, 0.1); + +// Get initial state by passing initial mean/covariance to the filter + ParticleFilter::State p1 = pf1.init(x_initial, P1); + +// // Assert it has the correct mean, covariance and information +// EXPECT(assert_equal(x_initial, p1->mean())); +// Matrix Sigma = (Mat(2, 2) << 0.01, 0.0, 0.0, 0.01); +// EXPECT(assert_equal(Sigma, p1->covariance())); +// EXPECT(assert_equal(inverse(Sigma), p1->information())); +// +// // Create one with a sharedGaussian +// KalmanFilter::State p2 = pf1.init(x_initial, Sigma); +// EXPECT(assert_equal(Sigma, p2->covariance())); +// +// // Now make sure both agree +// EXPECT(assert_equal(p1->covariance(), p2->covariance())); +} + +//****************************************************************************** +TEST( ParticleFilter, linear1 ) { + + // Create the controls and measurement properties for our example + Matrix F = eye(2, 2); + Matrix B = eye(2, 2); + Vector u = (Vector(2) << 1.0, 0.0); + SharedDiagonal modelQ = noiseModel::Isotropic::Sigma(2, 0.1); + Matrix Q = 0.01 * eye(2, 2); + Matrix H = eye(2, 2); + State z1(1.0, 0.0); + State z2(2.0, 0.0); + State z3(3.0, 0.0); + SharedDiagonal modelR = noiseModel::Isotropic::Sigma(2, 0.1); + Matrix R = 0.01 * eye(2, 2); + +// Create the set of expected output TestValues + State expected0(0.0, 0.0); + Matrix P00 = 0.01 * eye(2, 2); + + State expected1(1.0, 0.0); + Matrix P01 = P00 + Q; + Matrix I11 = inverse(P01) + inverse(R); + + State expected2(2.0, 0.0); + Matrix P12 = inverse(I11) + Q; + Matrix I22 = inverse(P12) + inverse(R); + + State expected3(3.0, 0.0); + Matrix P23 = inverse(I22) + Q; + Matrix I33 = inverse(P23) + inverse(R); + +// Create a Kalman filter of dimension 2 + KalmanFilter kf(2); + +// Create the Kalman Filter initialization point + State x_initial(0.0, 0.0); + SharedDiagonal P_initial = noiseModel::Isotropic::Sigma(2, 0.1); + +// Create initial KalmanFilter object + KalmanFilter::State p0 = kf.init(x_initial, P_initial); + EXPECT(assert_equal(expected0, p0->mean())); + EXPECT(assert_equal(P00, p0->covariance())); + +// Run iteration 1 + KalmanFilter::State p1p = kf.predict(p0, F, B, u, modelQ); + EXPECT(assert_equal(expected1, p1p->mean())); + EXPECT(assert_equal(P01, p1p->covariance())); + + KalmanFilter::State p1 = kf.update(p1p, H, z1, modelR); + EXPECT(assert_equal(expected1, p1->mean())); + EXPECT(assert_equal(I11, p1->information())); + +// Run iteration 2 (with full covariance) + KalmanFilter::State p2p = kf.predictQ(p1, F, B, u, Q); + EXPECT(assert_equal(expected2, p2p->mean())); + + KalmanFilter::State p2 = kf.update(p2p, H, z2, modelR); + EXPECT(assert_equal(expected2, p2->mean())); + +// Run iteration 3 + KalmanFilter::State p3p = kf.predict(p2, F, B, u, modelQ); + EXPECT(assert_equal(expected3, p3p->mean())); + LONGS_EQUAL(3, (long)KalmanFilter::step(p3p)); + + KalmanFilter::State p3 = kf.update(p3p, H, z3, modelR); + EXPECT(assert_equal(expected3, p3->mean())); + LONGS_EQUAL(3, (long)KalmanFilter::step(p3)); +} + +//****************************************************************************** +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +//****************************************************************************** +