From 05c1e572b6b8c2d0de9326626a1debaf7f475cff Mon Sep 17 00:00:00 2001 From: dellaert Date: Mon, 5 May 2014 10:14:36 -0400 Subject: [PATCH 1/8] Moved TSAMFactors.h from tsam to gtsam --- gtsam_unstable/slam/TSAMFactors.h | 167 ++++++++++++++++++ gtsam_unstable/slam/tests/testTSAMFactors.cpp | 149 ++++++++++++++++ 2 files changed, 316 insertions(+) create mode 100644 gtsam_unstable/slam/TSAMFactors.h create mode 100644 gtsam_unstable/slam/tests/testTSAMFactors.cpp diff --git a/gtsam_unstable/slam/TSAMFactors.h b/gtsam_unstable/slam/TSAMFactors.h new file mode 100644 index 000000000..d4c5f8172 --- /dev/null +++ b/gtsam_unstable/slam/TSAMFactors.h @@ -0,0 +1,167 @@ +/* ---------------------------------------------------------------------------- + + * 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 TSAMFactors.h + * @brief TSAM 1 Factors, simpler than the hierarchical TSAM 2 + * @author Frank Dellaert + * @date May 2014 + */ + +#pragma once + +#include +#include + +namespace gtsam { + +/** + * DeltaFactor: relative 2D measurement between Pose2 and Point2 + */ +class DeltaFactor: public NoiseModelFactor2 { + +public: + typedef DeltaFactor This; + typedef NoiseModelFactor2 Base; + typedef boost::shared_ptr shared_ptr; + +private: + Point2 measured_; ///< the measurement + +public: + + /// Constructor + DeltaFactor(Key i, Key j, const Point2& measured, + const SharedNoiseModel& model) : + Base(model, i, j), measured_(measured) { + } + + /// Evaluate measurement error h(x)-z + Vector evaluateError(const Pose2& pose, const Point2& point, + boost::optional H1 = boost::none, boost::optional H2 = + boost::none) const { + Point2 d = pose.transform_to(point, H1, H2); + Point2 e = measured_.between(d); + return e.vector(); + } +}; + +/** + * DeltaFactorBase: relative 2D measurement between Pose2 and Point2, with Basenodes + */ +class DeltaFactorBase: public NoiseModelFactor4 { + +public: + typedef DeltaFactorBase This; + typedef NoiseModelFactor4 Base; + typedef boost::shared_ptr shared_ptr; + +private: + Point2 measured_; ///< the measurement + +public: + + /// Constructor + DeltaFactorBase(Key b1, Key i, Key b2, Key j, const Point2& measured, + const SharedNoiseModel& model) : + Base(model, b1, i, b2, j), measured_(measured) { + } + + /// Evaluate measurement error h(x)-z + Vector evaluateError(const Pose2& base1, const Pose2& pose, + const Pose2& base2, const Point2& point, // + boost::optional H1 = boost::none, // + boost::optional H2 = boost::none, // + boost::optional H3 = boost::none, // + boost::optional H4 = boost::none) const { + if (H1 || H2 || H3 || H4) { + // TODO use fixed-size matrices + Matrix D_pose_g_base1, D_pose_g_pose; + Pose2 pose_g = base1.compose(pose, D_pose_g_base1, D_pose_g_pose); + Matrix D_point_g_base2, D_point_g_point; + Point2 point_g = base2.transform_from(point, D_point_g_base2, + D_point_g_point); + Matrix D_e_pose_g, D_e_point_g; + Point2 d = pose_g.transform_to(point_g, D_e_pose_g, D_e_point_g); + if (H1) + *H1 = D_e_pose_g * D_pose_g_base1; + if (H2) + *H2 = D_e_pose_g * D_pose_g_pose; + if (H3) + *H3 = D_e_point_g * D_point_g_base2; + if (H4) + *H4 = D_e_point_g * D_point_g_point; + return measured_.localCoordinates(d); + } else { + Pose2 pose_g = base1.compose(pose); + Point2 point_g = base2.transform_from(point); + Point2 d = pose_g.transform_to(point_g); + return measured_.localCoordinates(d); + } + } +}; + +/** + * OdometryFactorBase: Pose2 odometry, with Basenodes + */ +class OdometryFactorBase: public NoiseModelFactor4 { + +public: + typedef OdometryFactorBase This; + typedef NoiseModelFactor4 Base; + typedef boost::shared_ptr shared_ptr; + +private: + Pose2 measured_; ///< the measurement + +public: + + /// Constructor + OdometryFactorBase(Key b1, Key i, Key b2, Key j, const Pose2& measured, + const SharedNoiseModel& model) : + Base(model, b1, i, b2, j), measured_(measured) { + } + + /// Evaluate measurement error h(x)-z + Vector evaluateError(const Pose2& base1, const Pose2& pose1, + const Pose2& base2, const Pose2& pose2, // + boost::optional H1 = boost::none, // + boost::optional H2 = boost::none, // + boost::optional H3 = boost::none, // + boost::optional H4 = boost::none) const { + if (H1 || H2 || H3 || H4) { + // TODO use fixed-size matrices + Matrix D_pose1_g_base1, D_pose1_g_pose1; + Pose2 pose1_g = base1.compose(pose1, D_pose1_g_base1, D_pose1_g_pose1); + Matrix D_pose2_g_base2, D_pose2_g_pose2; + Pose2 pose2_g = base2.compose(pose2, D_pose2_g_base2, D_pose2_g_pose2); + Matrix D_e_pose1_g, D_e_pose2_g; + Pose2 d = pose1_g.between(pose2_g, D_e_pose1_g, D_e_pose2_g); + if (H1) + *H1 = D_e_pose1_g * D_pose1_g_base1; + if (H2) + *H2 = D_e_pose1_g * D_pose1_g_pose1; + if (H3) + *H3 = D_e_pose2_g * D_pose2_g_base2; + if (H4) + *H4 = D_e_pose2_g * D_pose2_g_pose2; + return measured_.localCoordinates(d); + } else { + Pose2 pose1_g = base1.compose(pose1); + Pose2 pose2_g = base2.compose(pose2); + Pose2 d = pose1_g.between(pose2_g); + return measured_.localCoordinates(d); + } + } +}; + +} + diff --git a/gtsam_unstable/slam/tests/testTSAMFactors.cpp b/gtsam_unstable/slam/tests/testTSAMFactors.cpp new file mode 100644 index 000000000..44dca9b8e --- /dev/null +++ b/gtsam_unstable/slam/tests/testTSAMFactors.cpp @@ -0,0 +1,149 @@ +/* ---------------------------------------------------------------------------- + + * 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 testTSAMFactors.cpp + * @brief Unit tests for TSAM 1 Factors + * @author Frank Dellaert + * @date May 2014 + */ + +#include +#include +#include + +using namespace std; +using namespace gtsam; + +Key i(1), j(2); // Key for pose and point + +//************************************************************************* +TEST( DeltaFactor, all ) { + // Create a factor + Point2 measurement(1, 1); + static SharedNoiseModel model(noiseModel::Unit::Create(2)); + DeltaFactor factor(i, j, measurement, model); + + // Set the linearization point + Pose2 pose(1, 2, 0); + Point2 point(4, 11); + Vector2 expected(4 - 1 - 1, 11 - 2 - 1); + + // Use the factor to calculate the Jacobians + Matrix H1Actual, H2Actual; + Vector actual = factor.evaluateError(pose, point, H1Actual, H2Actual); + EXPECT(assert_equal(expected, actual, 1e-9)); + + // Use numerical derivatives to calculate the Jacobians + Matrix H1Expected, H2Expected; + H1Expected = numericalDerivative11( + boost::bind(&DeltaFactor::evaluateError, &factor, _1, point, boost::none, + boost::none), pose); + H2Expected = numericalDerivative11( + boost::bind(&DeltaFactor::evaluateError, &factor, pose, _1, boost::none, + boost::none), point); + + // Verify the Jacobians are correct + EXPECT(assert_equal(H1Expected, H1Actual, 1e-9)); + EXPECT(assert_equal(H2Expected, H2Actual, 1e-9)); +} + +//************************************************************************* +TEST( DeltaFactorBase, all ) { + // Create a factor + Key b1(10), b2(20); + Point2 measurement(1, 1); + static SharedNoiseModel model(noiseModel::Unit::Create(2)); + DeltaFactorBase factor(b1, i, b2, j, measurement, model); + + // Set the linearization point + Pose2 base1, base2(1, 0, 0); + Pose2 pose(1, 2, 0); + Point2 point(4, 11); + Vector2 expected(4 + 1 - 1 - 1, 11 - 2 - 1); + + // Use the factor to calculate the Jacobians + Matrix H1Actual, H2Actual, H3Actual, H4Actual; + Vector actual = factor.evaluateError(base1, pose, base2, point, H1Actual, + H2Actual, H3Actual, H4Actual); + EXPECT(assert_equal(expected, actual, 1e-9)); + + // Use numerical derivatives to calculate the Jacobians + Matrix H1Expected, H2Expected, H3Expected, H4Expected; + H1Expected = numericalDerivative11( + boost::bind(&DeltaFactorBase::evaluateError, &factor, _1, pose, base2, + point, boost::none, boost::none, boost::none, boost::none), base1); + H2Expected = numericalDerivative11( + boost::bind(&DeltaFactorBase::evaluateError, &factor, base1, _1, base2, + point, boost::none, boost::none, boost::none, boost::none), pose); + H3Expected = numericalDerivative11( + boost::bind(&DeltaFactorBase::evaluateError, &factor, base1, pose, _1, + point, boost::none, boost::none, boost::none, boost::none), base2); + H4Expected = numericalDerivative11( + boost::bind(&DeltaFactorBase::evaluateError, &factor, base1, pose, base2, + _1, boost::none, boost::none, boost::none, boost::none), point); + + // Verify the Jacobians are correct + EXPECT(assert_equal(H1Expected, H1Actual, 1e-9)); + EXPECT(assert_equal(H2Expected, H2Actual, 1e-9)); + EXPECT(assert_equal(H3Expected, H3Actual, 1e-9)); + EXPECT(assert_equal(H4Expected, H4Actual, 1e-9)); +} + +//************************************************************************* +TEST( OdometryFactorBase, all ) { + // Create a factor + Key b1(10), b2(20); + Pose2 measurement(1, 1, 0); + static SharedNoiseModel model(noiseModel::Unit::Create(2)); + OdometryFactorBase factor(b1, i, b2, j, measurement, model); + + // Set the linearization pose2 + Pose2 base1, base2(1, 0, 0); + Pose2 pose1(1, 2, 0), pose2(4, 11, 0); + Vector3 expected(4 + 1 - 1 - 1, 11 - 2 - 1, 0); + + // Use the factor to calculate the Jacobians + Matrix H1Actual, H2Actual, H3Actual, H4Actual; + Vector actual = factor.evaluateError(base1, pose1, base2, pose2, H1Actual, + H2Actual, H3Actual, H4Actual); + EXPECT(assert_equal(expected, actual, 1e-9)); + + // Use numerical derivatives to calculate the Jacobians + Matrix H1Expected, H2Expected, H3Expected, H4Expected; + H1Expected = numericalDerivative11( + boost::bind(&OdometryFactorBase::evaluateError, &factor, _1, pose1, base2, + pose2, boost::none, boost::none, boost::none, boost::none), base1); + H2Expected = numericalDerivative11( + boost::bind(&OdometryFactorBase::evaluateError, &factor, base1, _1, base2, + pose2, boost::none, boost::none, boost::none, boost::none), pose1); + H3Expected = numericalDerivative11( + boost::bind(&OdometryFactorBase::evaluateError, &factor, base1, pose1, _1, + pose2, boost::none, boost::none, boost::none, boost::none), base2); + H4Expected = numericalDerivative11( + boost::bind(&OdometryFactorBase::evaluateError, &factor, base1, pose1, + base2, _1, boost::none, boost::none, boost::none, boost::none), + pose2); + + // Verify the Jacobians are correct + EXPECT(assert_equal(H1Expected, H1Actual, 1e-9)); + EXPECT(assert_equal(H2Expected, H2Actual, 1e-9)); + EXPECT(assert_equal(H3Expected, H3Actual, 1e-9)); + EXPECT(assert_equal(H4Expected, H4Actual, 1e-9)); +} + +//************************************************************************* +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +//************************************************************************* + From 0a2385711b2a2dc9df85eb29a691ef7ade1d2990 Mon Sep 17 00:00:00 2001 From: dellaert Date: Mon, 5 May 2014 10:14:56 -0400 Subject: [PATCH 2/8] MATLAB wrapping of TSAMFactors --- .cproject | 438 +++++++++--------- gtsam_unstable/gtsam_unstable.h | 33 +- matlab/unstable_examples/TSAMFactorsExample.m | 44 ++ 3 files changed, 305 insertions(+), 210 deletions(-) create mode 100644 matlab/unstable_examples/TSAMFactorsExample.m diff --git a/.cproject b/.cproject index 97e36d81f..31d4d9143 100644 --- a/.cproject +++ b/.cproject @@ -1,17 +1,19 @@ - + + + + + - - @@ -60,13 +62,13 @@ + + - - @@ -116,13 +118,13 @@ + + - - @@ -540,14 +542,6 @@ true true - - make - -j2 - testGaussianFactor.run - true - true - true - make -j2 @@ -574,6 +568,7 @@ make + tests/testBayesTree.run true false @@ -581,6 +576,7 @@ make + testBinaryBayesNet.run true false @@ -628,6 +624,7 @@ make + testSymbolicBayesNet.run true false @@ -635,6 +632,7 @@ make + tests/testSymbolicFactor.run true false @@ -642,6 +640,7 @@ make + testSymbolicFactorGraph.run true false @@ -657,11 +656,20 @@ make + tests/testBayesTree true false true + + make + -j2 + testGaussianFactor.run + true + true + true + make -j5 @@ -758,22 +766,6 @@ false true - - make - -j2 - all - true - true - true - - - make - -j2 - clean - true - true - true - make -j2 @@ -790,6 +782,22 @@ true true + + make + -j2 + all + true + true + true + + + make + -j2 + clean + true + true + true + make -j2 @@ -814,42 +822,26 @@ true true - + make - -j5 - testValues.run + -j2 + all true true true - + make - -j5 - testOrdering.run + -j2 + check true true true - + make - -j5 - testKey.run - true - true - true - - - make - -j5 - testLinearContainerFactor.run - true - true - true - - - make - -j6 -j8 - testWhiteNoiseFactor.run + -j2 + clean true true true @@ -918,26 +910,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 @@ -1086,6 +1094,14 @@ true true + + make + -j5 + testTSAMFactors.run + true + true + true + make -j5 @@ -1328,6 +1344,7 @@ make + testGraph.run true false @@ -1335,6 +1352,7 @@ make + testJunctionTree.run true false @@ -1342,6 +1360,7 @@ make + testSymbolicBayesNetB.run true false @@ -1509,6 +1528,7 @@ make + testErrors.run true false @@ -1554,22 +1574,6 @@ true true - - make - -j5 - testParticleFactor.run - true - true - true - - - make - -j2 - testGaussianFactor.run - true - true - true - make -j2 @@ -1650,6 +1654,22 @@ true true + + make + -j2 + testGaussianFactor.run + true + true + true + + + make + -j5 + testParticleFactor.run + true + true + true + make -j2 @@ -1930,22 +1950,6 @@ true true - - make - -j5 - testImuFactor.run - true - true - true - - - make - -j5 - testCombinedImuFactor.run - true - true - true - make -j2 @@ -2028,7 +2032,6 @@ make - testSimulated2DOriented.run true false @@ -2068,7 +2071,6 @@ make - testSimulated2D.run true false @@ -2076,7 +2078,6 @@ make - testSimulated3D.run true false @@ -2090,6 +2091,22 @@ true true + + make + -j5 + testImuFactor.run + true + true + true + + + make + -j5 + testCombinedImuFactor.run + true + true + true + make -j5 @@ -2364,7 +2381,6 @@ make - tests/testGaussianISAM2 true false @@ -2386,102 +2402,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 -j3 @@ -2683,6 +2603,7 @@ cpack + -G DEB true false @@ -2690,6 +2611,7 @@ cpack + -G RPM true false @@ -2697,6 +2619,7 @@ cpack + -G TGZ true false @@ -2704,6 +2627,7 @@ cpack + --config CPackSourceConfig.cmake true false @@ -2877,34 +2801,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 + -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 @@ -2948,6 +2936,38 @@ 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/gtsam_unstable.h b/gtsam_unstable/gtsam_unstable.h index 80ee41b22..1aa840825 100644 --- a/gtsam_unstable/gtsam_unstable.h +++ b/gtsam_unstable/gtsam_unstable.h @@ -15,8 +15,11 @@ virtual class gtsam::Pose3; virtual class gtsam::noiseModel::Base; virtual class gtsam::noiseModel::Gaussian; virtual class gtsam::imuBias::ConstantBias; -virtual class gtsam::NoiseModelFactor; virtual class gtsam::NonlinearFactor; +virtual class gtsam::NoiseModelFactor; +virtual class gtsam::NoiseModelFactor2; +virtual class gtsam::NoiseModelFactor3; +virtual class gtsam::NoiseModelFactor4; virtual class gtsam::GaussianFactor; virtual class gtsam::HessianFactor; virtual class gtsam::JacobianFactor; @@ -709,4 +712,32 @@ class AHRS { void print(string s) const; }; +// Tectonic SAM Factors + +#include +#include + +//typedef gtsam::NoiseModelFactor2 NLPosePose; +virtual class DeltaFactor : gtsam::NoiseModelFactor { + DeltaFactor(size_t i, size_t j, const gtsam::Point2& measured, + const gtsam::noiseModel::Base* noiseModel); + void print(string s) const; +}; + +//typedef gtsam::NoiseModelFactor4 NLPosePosePosePoint; +virtual class DeltaFactorBase : gtsam::NoiseModelFactor { + DeltaFactorBase(size_t b1, size_t i, size_t b2, size_t j, + const gtsam::Point2& measured, const gtsam::noiseModel::Base* noiseModel); + void print(string s) const; +}; + +//typedef gtsam::NoiseModelFactor4 NLPosePosePosePose; +virtual class OdometryFactorBase : gtsam::NoiseModelFactor { + OdometryFactorBase(size_t b1, size_t i, size_t b2, size_t j, + const gtsam::Pose2& measured, const gtsam::noiseModel::Base* noiseModel); + void print(string s) const; +}; + } //\namespace gtsam diff --git a/matlab/unstable_examples/TSAMFactorsExample.m b/matlab/unstable_examples/TSAMFactorsExample.m new file mode 100644 index 000000000..5ee9f3909 --- /dev/null +++ b/matlab/unstable_examples/TSAMFactorsExample.m @@ -0,0 +1,44 @@ +% TSAMFactorsExample +% Frank Dellaert, May 2014 + +import gtsam.*; + +% noise models +noisePrior = noiseModel.Diagonal.Sigmas([0; 0; 0]); +noiseDelta = noiseModel.Isotropic.Sigma(2, 0.1); +noiseOdom = noiseModel.Diagonal.Sigmas([0.1; 0.1; 0.05]); + +% Example is 1, landmark, 2 poses 10 and 20, 2 base nodes 100 and 200 +% + + +% - b - p - l - p - b +% +---+-------+---+ +% Create a graph +graph = NonlinearFactorGraph +origin = Pose2; +graph.add(gtsam.PriorFactorPose2(100, origin, noisePrior)) +graph.add(gtsam.PriorFactorPose2(10, origin, noisePrior)) +graph.add(gtsam.PriorFactorPose2(20, origin, noisePrior)) +graph.add(DeltaFactor(10, 1, Point2(1,0), noiseDelta)) +graph.add(DeltaFactor(20, 1, Point2(-1,0), noiseDelta)) +graph.add(OdometryFactorBase(100,10,200,20, Pose2(2,0,0), noiseOdom)) +graph + +% Initial values +initial = Values; +initial.insert(100,origin); +initial.insert(10,origin); +initial.insert(1,Point2(2,0)); +initial.insert(20,origin); +initial.insert(200,origin); + +graph.error(initial) +graph.at(0).error(initial) +graph.at(1).error(initial) +graph.at(2).error(initial) +graph.at(3).error(initial) + +% optimize +params = LevenbergMarquardtParams; +params.setVerbosity('ERROR'); +optimizer = LevenbergMarquardtOptimizer(graph, initial, params); +result = optimizer.optimize() \ No newline at end of file From f5e0a7f2b19f03cbabf97f7dcbcc78089434df85 Mon Sep 17 00:00:00 2001 From: dellaert Date: Mon, 5 May 2014 10:15:23 -0400 Subject: [PATCH 3/8] ignore --- matlab/unstable_examples/.gitignore | 1 + 1 file changed, 1 insertion(+) create mode 100644 matlab/unstable_examples/.gitignore diff --git a/matlab/unstable_examples/.gitignore b/matlab/unstable_examples/.gitignore new file mode 100644 index 000000000..6d725d9bc --- /dev/null +++ b/matlab/unstable_examples/.gitignore @@ -0,0 +1 @@ +*.m~ From 9409357fe76b1ef4b6483fda76c91041eec4aede Mon Sep 17 00:00:00 2001 From: dellaert Date: Mon, 5 May 2014 11:26:16 -0400 Subject: [PATCH 4/8] Works, but is really a test, not an example --- matlab/unstable_examples/TSAMFactorsExample.m | 54 ++++++++++++------- 1 file changed, 36 insertions(+), 18 deletions(-) diff --git a/matlab/unstable_examples/TSAMFactorsExample.m b/matlab/unstable_examples/TSAMFactorsExample.m index 5ee9f3909..abdfc5922 100644 --- a/matlab/unstable_examples/TSAMFactorsExample.m +++ b/matlab/unstable_examples/TSAMFactorsExample.m @@ -4,41 +4,59 @@ import gtsam.*; % noise models -noisePrior = noiseModel.Diagonal.Sigmas([0; 0; 0]); +noisePrior = noiseModel.Diagonal.Sigmas([100; 100; 100]); noiseDelta = noiseModel.Isotropic.Sigma(2, 0.1); noiseOdom = noiseModel.Diagonal.Sigmas([0.1; 0.1; 0.05]); -% Example is 1, landmark, 2 poses 10 and 20, 2 base nodes 100 and 200 +% Example is 2 landmarks 1 and 2, 2 poses 10 and 20, 2 base nodes 100 and 200 +% l1 l2 % + + -% - b - p - l - p - b -% +---+-------+---+ +% - b - p - p - b +% +---+---+---+ + +% True values +b1 = Pose2(0,0,0); +b2 = Pose2(2,0,0); +l1 = Point2(0,1); +l2 = Point2(2,1); + % Create a graph -graph = NonlinearFactorGraph +graph = NonlinearFactorGraph; origin = Pose2; -graph.add(gtsam.PriorFactorPose2(100, origin, noisePrior)) graph.add(gtsam.PriorFactorPose2(10, origin, noisePrior)) graph.add(gtsam.PriorFactorPose2(20, origin, noisePrior)) -graph.add(DeltaFactor(10, 1, Point2(1,0), noiseDelta)) -graph.add(DeltaFactor(20, 1, Point2(-1,0), noiseDelta)) +graph.add(gtsam.PriorFactorPose2(100, origin, noisePrior)) +graph.add(DeltaFactor(10, 1, b1.transform_to(l1), noiseDelta)) +graph.add(DeltaFactor(20, 1, b2.transform_to(l2), noiseDelta)) +graph.add(DeltaFactorBase(100,10, 200,2, b1.transform_to(l2), noiseDelta)) +graph.add(DeltaFactorBase(200,20, 100,1, b2.transform_to(l1), noiseDelta)) graph.add(OdometryFactorBase(100,10,200,20, Pose2(2,0,0), noiseOdom)) -graph % Initial values initial = Values; initial.insert(100,origin); initial.insert(10,origin); -initial.insert(1,Point2(2,0)); +initial.insert(1,l1); +initial.insert(2,l2); initial.insert(20,origin); initial.insert(200,origin); -graph.error(initial) -graph.at(0).error(initial) -graph.at(1).error(initial) -graph.at(2).error(initial) -graph.at(3).error(initial) - % optimize params = LevenbergMarquardtParams; -params.setVerbosity('ERROR'); +% params.setVerbosity('ERROR'); optimizer = LevenbergMarquardtOptimizer(graph, initial, params); -result = optimizer.optimize() \ No newline at end of file +result = optimizer.optimize(); + +% Check result +CHECK('error',result.at(100).equals(b1,1e-5)) +CHECK('error',result.at(10).equals(origin,1e-5)) +CHECK('error',result.at(1).equals(Point2(0,1),1e-5)) +CHECK('error',result.at(2).equals(Point2(0,1),1e-5)) +CHECK('error',result.at(20).equals(origin,1e-5)) +CHECK('error',result.at(200).equals(b2,1e-5)) + +% Check error +CHECK('error',abs(graph.error(result))<1e-9) +for i=0:7 + CHECK('error',abs(graph.at(i).error(result))<1e-9) +end From 3b1f947909c4fc141f72606b540a7f105f1f949b Mon Sep 17 00:00:00 2001 From: dellaert Date: Mon, 5 May 2014 11:29:02 -0400 Subject: [PATCH 5/8] Renamed to test --- .../unstable_examples/{TSAMFactorsExample.m => testTSAMFactors.m} | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename matlab/unstable_examples/{TSAMFactorsExample.m => testTSAMFactors.m} (100%) diff --git a/matlab/unstable_examples/TSAMFactorsExample.m b/matlab/unstable_examples/testTSAMFactors.m similarity index 100% rename from matlab/unstable_examples/TSAMFactorsExample.m rename to matlab/unstable_examples/testTSAMFactors.m From cea4aef9f207475a929b0083cb645d6cef533fda Mon Sep 17 00:00:00 2001 From: dellaert Date: Tue, 6 May 2014 00:23:20 -0400 Subject: [PATCH 6/8] New perturbPose2 utility --- gtsam.h | 1 + matlab.h | 20 +++++++++++++++----- 2 files changed, 16 insertions(+), 5 deletions(-) diff --git a/gtsam.h b/gtsam.h index 56a1a601d..05cf7ffb5 100644 --- a/gtsam.h +++ b/gtsam.h @@ -2363,6 +2363,7 @@ namespace utilities { gtsam::Values allPose3s(gtsam::Values& values); Matrix extractPose3(const gtsam::Values& values); void perturbPoint2(gtsam::Values& values, double sigma, int seed); + void perturbPose2 (gtsam::Values& values, double sigmaT, double sigmaR, int seed); void perturbPoint3(gtsam::Values& values, double sigma, int seed); void insertBackprojections(gtsam::Values& values, const gtsam::SimpleCamera& c, Vector J, Matrix Z, double depth); void insertProjectionFactors(gtsam::NonlinearFactorGraph& graph, size_t i, Vector J, Matrix Z, const gtsam::noiseModel::Base* model, const gtsam::Cal3_S2* K); diff --git a/matlab.h b/matlab.h index 6c2b137d7..100f5a242 100644 --- a/matlab.h +++ b/matlab.h @@ -84,7 +84,7 @@ namespace gtsam { return result; } - /// perturb all Point2 using normally distributed noise + /// Perturb all Point2 values using normally distributed noise void perturbPoint2(Values& values, double sigma, int32_t seed = 42u) { noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(2,sigma); Sampler sampler(model, seed); @@ -93,7 +93,17 @@ namespace gtsam { } } - /// perturb all Point3 using normally distributed noise + /// Perturb all Pose2 values using normally distributed noise + void perturbPose2(Values& values, double sigmaT, double sigmaR, int32_t seed = 42u) { + noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas( + Vector3(sigmaT, sigmaT, sigmaR)); + Sampler sampler(model, seed); + BOOST_FOREACH(const Values::ConstFiltered::KeyValuePair& key_value, values.filter()) { + values.update(key_value.key, key_value.value.retract(sampler.sample())); + } + } + + /// Perturb all Point3 values using normally distributed noise void perturbPoint3(Values& values, double sigma, int32_t seed = 42u) { noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(3,sigma); Sampler sampler(model, seed); @@ -102,7 +112,7 @@ namespace gtsam { } } - /// insert a number of initial point values by backprojecting + /// Insert a number of initial point values by backprojecting void insertBackprojections(Values& values, const SimpleCamera& camera, const Vector& J, const Matrix& Z, double depth) { if (Z.rows() != 2) throw std::invalid_argument("insertBackProjections: Z must be 2*K"); if (Z.cols() != J.size()) throw std::invalid_argument("insertBackProjections: J and Z must have same number of entries"); @@ -113,7 +123,7 @@ namespace gtsam { } } - /// insert multiple projection factors for a single pose key + /// Insert multiple projection factors for a single pose key void insertProjectionFactors(NonlinearFactorGraph& graph, Key i, const Vector& J, const Matrix& Z, const SharedNoiseModel& model, const Cal3_S2::shared_ptr K, const Pose3& body_P_sensor = Pose3()) { if (Z.rows() != 2) throw std::invalid_argument("addMeasurements: Z must be 2*K"); @@ -126,7 +136,7 @@ namespace gtsam { } } - /// calculate the errors of all projection factors in a graph + /// Calculate the errors of all projection factors in a graph Matrix reprojectionErrors(const NonlinearFactorGraph& graph, const Values& values) { // first count size_t K = 0, k=0; From 779d6ad2afcdf41a76a116dbda27d47e7737b340 Mon Sep 17 00:00:00 2001 From: dellaert Date: Tue, 6 May 2014 00:23:34 -0400 Subject: [PATCH 7/8] Added utilities to Contents.m --- matlab/+gtsam/Contents.m | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/matlab/+gtsam/Contents.m b/matlab/+gtsam/Contents.m index 8d15bc913..5fec7721c 100644 --- a/matlab/+gtsam/Contents.m +++ b/matlab/+gtsam/Contents.m @@ -173,3 +173,16 @@ % symbol - create a Symbol key % symbolChr - get character from a symbol key % symbolIndex - get index from a symbol key +% +%% Wrapped C++ Convenience Functions for use within MATLAB +% utilities.extractPoint2 - Extract all Point2 values into a single matrix [x y] +% utilities.extractPoint3 - Extract all Point3 values into a single matrix [x y z] +% utilities.extractPose2 - Extract all Pose2 values into a single matrix [x y theta] +% utilities.allPose3s - Extract all Pose3 values +% utilities.extractPose3 - Extract all Pose3 values into a single matrix [r11 r12 r13 r21 r22 r23 r31 r32 r33 x y z] +% utilities.perturbPoint2 - Perturb all Point2 values using normally distributed noise +% utilities.perturbPose2 - Perturb all Pose2 values using normally distributed noise +% utilities.perturbPoint3 - Perturb all Point3 values using normally distributed noise +% utilities.insertBackprojections - Insert a number of initial point values by backprojecting +% utilities.insertProjectionFactors - Insert multiple projection factors for a single pose key +% utilities.reprojectionErrors - Calculate the errors of all projection factors in a graph From 6c6c545d99f2542864e7fe3ea6bce3319f44905c Mon Sep 17 00:00:00 2001 From: dellaert Date: Tue, 6 May 2014 01:33:09 -0400 Subject: [PATCH 8/8] Comments --- gtsam/linear/GaussianConditional.cpp | 13 ++++++++++++- 1 file changed, 12 insertions(+), 1 deletion(-) diff --git a/gtsam/linear/GaussianConditional.cpp b/gtsam/linear/GaussianConditional.cpp index c6bd3967b..a5c651a44 100644 --- a/gtsam/linear/GaussianConditional.cpp +++ b/gtsam/linear/GaussianConditional.cpp @@ -119,14 +119,20 @@ namespace gtsam { /* ************************************************************************* */ VectorValues GaussianConditional::solve(const VectorValues& x) const { - // Solve matrix + // Concatenate all vector values that correspond to parent variables Vector xS = x.vector(FastVector(beginParents(), endParents())); + + // Update right-hand-side xS = getb() - get_S() * xS; + + // Solve matrix Vector soln = get_R().triangularView().solve(xS); // Check for indeterminant solution if(soln.hasNaN()) throw IndeterminantLinearSystemException(keys().front()); + // TODO, do we not have to scale by sigmas here? Copy/paste bug + // Insert solution into a VectorValues VectorValues result; DenseIndex vectorPosition = 0; @@ -142,9 +148,14 @@ namespace gtsam { VectorValues GaussianConditional::solveOtherRHS( const VectorValues& parents, const VectorValues& rhs) const { + // Concatenate all vector values that correspond to parent variables Vector xS = parents.vector(FastVector(beginParents(), endParents())); + + // Instead of updating getb(), update the right-hand-side from the given rhs const Vector rhsR = rhs.vector(FastVector(beginFrontals(), endFrontals())); xS = rhsR - get_S() * xS; + + // Solve Matrix Vector soln = get_R().triangularView().solve(xS); // Scale by sigmas