From 6b4d2321b48a566093427a5086d135ad6ccb5176 Mon Sep 17 00:00:00 2001 From: Paul Furgale Date: Mon, 1 Dec 2014 15:29:40 +0100 Subject: [PATCH 01/11] Fixed the prior factor to use charts and traits --- gtsam/slam/PriorFactor.h | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/gtsam/slam/PriorFactor.h b/gtsam/slam/PriorFactor.h index 888dcb76b..bc5e9c87f 100644 --- a/gtsam/slam/PriorFactor.h +++ b/gtsam/slam/PriorFactor.h @@ -67,14 +67,14 @@ namespace gtsam { /** print */ virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { std::cout << s << "PriorFactor on " << keyFormatter(this->key()) << "\n"; - prior_.print(" prior mean: "); + traits::print()(prior_, " prior mean: "); this->noiseModel_->print(" noise model: "); } /** equals */ virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { const This* e = dynamic_cast (&expected); - return e != NULL && Base::equals(*e, tol) && this->prior_.equals(e->prior_, tol); + return e != NULL && Base::equals(*e, tol) && traits::equals()(prior_, e->prior_, tol); } /** implement functions needed to derive from Factor */ @@ -83,7 +83,8 @@ namespace gtsam { Vector evaluateError(const T& p, boost::optional H = boost::none) const { if (H) (*H) = eye(p.dim()); // manifold equivalent of h(x)-z -> log(z,h(x)) - return prior_.localCoordinates(p); + DefaultChart chart; + return chart.local(prior_,p); } const VALUE & prior() const { return prior_; } From 1f171cf1a1d81c6d969305e4664e41226be92004 Mon Sep 17 00:00:00 2001 From: Christian Forster Date: Mon, 1 Dec 2014 10:56:55 -0500 Subject: [PATCH 02/11] Removed LieScalar from prior factor test and added new test for constructor with Vector3 --- gtsam/slam/tests/testPriorFactor.cpp | 14 ++++++++++---- 1 file changed, 10 insertions(+), 4 deletions(-) diff --git a/gtsam/slam/tests/testPriorFactor.cpp b/gtsam/slam/tests/testPriorFactor.cpp index b3e54bedb..d40ec3025 100644 --- a/gtsam/slam/tests/testPriorFactor.cpp +++ b/gtsam/slam/tests/testPriorFactor.cpp @@ -5,8 +5,8 @@ * @date Nov 4, 2014 */ +#include #include -#include #include using namespace std; @@ -14,10 +14,16 @@ using namespace gtsam; /* ************************************************************************* */ -// Constructor -TEST(PriorFactor, Constructor) { +// Constructor scalar +TEST(PriorFactor, ConstructorScalar) { SharedNoiseModel model; - PriorFactor factor(1, LieScalar(1.0), model); + PriorFactor factor(1, 1.0, model); +} + +// Constructor vector3 +TEST(PriorFactor, ConstructorVector3) { + SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 1.0); + PriorFactor factor(1, Vector3(1,2,3), model); } /* ************************************************************************* */ From c472c8673038d295e5a78ee7a23d296fedffff62 Mon Sep 17 00:00:00 2001 From: Christian Forster Date: Mon, 1 Dec 2014 12:10:45 -0500 Subject: [PATCH 03/11] get dimension of prior factor with traits --- gtsam/slam/PriorFactor.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/slam/PriorFactor.h b/gtsam/slam/PriorFactor.h index bc5e9c87f..7f7aef8cb 100644 --- a/gtsam/slam/PriorFactor.h +++ b/gtsam/slam/PriorFactor.h @@ -81,7 +81,7 @@ namespace gtsam { /** vector of errors */ Vector evaluateError(const T& p, boost::optional H = boost::none) const { - if (H) (*H) = eye(p.dim()); + if (H) (*H) = eye(traits::dimension()); // manifold equivalent of h(x)-z -> log(z,h(x)) DefaultChart chart; return chart.local(prior_,p); From 85122bed95db5d34a34a9d0a8e3673a25cdd2168 Mon Sep 17 00:00:00 2001 From: Christian Forster Date: Mon, 1 Dec 2014 14:44:22 -0500 Subject: [PATCH 04/11] using traits and Chart in BetweenFactor --- gtsam/slam/BetweenFactor.h | 11 +++++++---- 1 file changed, 7 insertions(+), 4 deletions(-) diff --git a/gtsam/slam/BetweenFactor.h b/gtsam/slam/BetweenFactor.h index 1b630374c..2aa0b50d2 100644 --- a/gtsam/slam/BetweenFactor.h +++ b/gtsam/slam/BetweenFactor.h @@ -74,14 +74,14 @@ namespace gtsam { std::cout << s << "BetweenFactor(" << keyFormatter(this->key1()) << "," << keyFormatter(this->key2()) << ")\n"; - measured_.print(" measured: "); + traits::print()(measured_, " measured: "); this->noiseModel_->print(" noise model: "); } /** equals */ virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { const This *e = dynamic_cast (&expected); - return e != NULL && Base::equals(*e, tol) && this->measured_.equals(e->measured_, tol); + return e != NULL && Base::equals(*e, tol) && traits::equals()(this->measured_, e->measured_, tol); } /** implement functions needed to derive from Factor */ @@ -92,7 +92,8 @@ namespace gtsam { boost::none) const { T hx = p1.between(p2, H1, H2); // h(x) // manifold equivalent of h(x)-z -> log(z,h(x)) - return measured_.localCoordinates(hx); + DefaultChart chart; + return chart.local(measured_, hx); } /** return the measured */ @@ -129,7 +130,9 @@ namespace gtsam { /** Syntactic sugar for constrained version */ BetweenConstraint(const VALUE& measured, Key key1, Key key2, double mu = 1000.0) : - BetweenFactor(key1, key2, measured, noiseModel::Constrained::All(VALUE::Dim(), fabs(mu))) {} + BetweenFactor(key1, key2, measured, + noiseModel::Constrained::All(traits::dimension(), fabs(mu))) + {} private: From 3d32b9c26041d47a2184939675987295339c4eca Mon Sep 17 00:00:00 2001 From: Christian Forster Date: Mon, 1 Dec 2014 21:17:11 -0500 Subject: [PATCH 05/11] added simple constructor tests for BetweenFactor --- gtsam/slam/tests/testBetweenFactor.cpp | 23 +++++++++++++++++++++++ 1 file changed, 23 insertions(+) diff --git a/gtsam/slam/tests/testBetweenFactor.cpp b/gtsam/slam/tests/testBetweenFactor.cpp index d0dbe7908..5169d90bc 100644 --- a/gtsam/slam/tests/testBetweenFactor.cpp +++ b/gtsam/slam/tests/testBetweenFactor.cpp @@ -44,6 +44,29 @@ TEST(BetweenFactor, Rot3) { EXPECT(assert_equal(numericalH2,actualH2, 1E-5)); } +/* ************************************************************************* */ + +// Constructor scalar +TEST(BetweenFactor, ConstructorScalar) { + SharedNoiseModel model; + double measured_value = 0.0; + BetweenFactor factor(1, 2, measured_value, model); +} + +// Constructor vector3 +TEST(BetweenFactor, ConstructorVector3) { + SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 1.0); + Vector3 measured_value(1, 2, 3); + BetweenFactor factor(1, 2, measured_value, model); +} + +// Constructor dynamic sized vector +TEST(BetweenFactor, ConstructorDynamicSizeVector) { + SharedNoiseModel model = noiseModel::Isotropic::Sigma(5, 1.0); + Vector measured_value(5); measured_value << 1, 2, 3, 4, 5; + BetweenFactor factor(1, 2, measured_value, model); +} + /* ************************************************************************* */ int main() { TestResult tr; From 0fcd76c9fdb85bf649c239221a3e8847fd7596d5 Mon Sep 17 00:00:00 2001 From: Christian Forster Date: Mon, 1 Dec 2014 21:17:40 -0500 Subject: [PATCH 06/11] added test for dynamic size constructor --- gtsam/slam/tests/testPriorFactor.cpp | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/gtsam/slam/tests/testPriorFactor.cpp b/gtsam/slam/tests/testPriorFactor.cpp index d40ec3025..b26d633f5 100644 --- a/gtsam/slam/tests/testPriorFactor.cpp +++ b/gtsam/slam/tests/testPriorFactor.cpp @@ -26,6 +26,13 @@ TEST(PriorFactor, ConstructorVector3) { PriorFactor factor(1, Vector3(1,2,3), model); } +// Constructor dynamic sized vector +TEST(PriorFactor, ConstructorDynamicSizeVector) { + Vector v(5); v << 1, 2, 3, 4, 5; + SharedNoiseModel model = noiseModel::Isotropic::Sigma(5, 1.0); + PriorFactor factor(1, v, model); +} + /* ************************************************************************* */ int main() { TestResult tr; From 1d7c89503129d95f90841933831ce1816e07c28a Mon Sep 17 00:00:00 2001 From: Christian Forster Date: Mon, 1 Dec 2014 21:18:09 -0500 Subject: [PATCH 07/11] using chart.getDimsion() instead of traits::dim --- gtsam/slam/PriorFactor.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/slam/PriorFactor.h b/gtsam/slam/PriorFactor.h index 7f7aef8cb..8fbbd4d88 100644 --- a/gtsam/slam/PriorFactor.h +++ b/gtsam/slam/PriorFactor.h @@ -81,9 +81,9 @@ namespace gtsam { /** vector of errors */ Vector evaluateError(const T& p, boost::optional H = boost::none) const { - if (H) (*H) = eye(traits::dimension()); - // manifold equivalent of h(x)-z -> log(z,h(x)) DefaultChart chart; + if (H) (*H) = eye(chart.getDimension(p)); + // manifold equivalent of h(x)-z -> log(z,h(x)) return chart.local(prior_,p); } From c06334af5fa8fc8aa59417dee6f30db5b1bb1cf1 Mon Sep 17 00:00:00 2001 From: Christian Forster Date: Mon, 1 Dec 2014 21:18:46 -0500 Subject: [PATCH 08/11] attempt to fix evaluateError in BetweenFactor. Doesn't compile --- gtsam/slam/BetweenFactor.h | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) diff --git a/gtsam/slam/BetweenFactor.h b/gtsam/slam/BetweenFactor.h index 2aa0b50d2..651b00404 100644 --- a/gtsam/slam/BetweenFactor.h +++ b/gtsam/slam/BetweenFactor.h @@ -88,11 +88,16 @@ namespace gtsam { /** vector of errors */ Vector evaluateError(const T& p1, const T& p2, - boost::optional H1 = boost::none, boost::optional H2 = + boost::optional H1 = boost::none,boost::optional H2 = boost::none) const { - T hx = p1.between(p2, H1, H2); // h(x) - // manifold equivalent of h(x)-z -> log(z,h(x)) DefaultChart chart; + // TODO check: + //T hx = p1.between(p2, H1, H2); // h(x) + T hx = chart.local(p2, p1); + if(H1) (*H1) = -eye(chart.getDimension(p1)); + if(H2) (*H2) = eye(chart.getDimension(p2)); + + // manifold equivalent of h(x)-z -> log(z,h(x)) return chart.local(measured_, hx); } From c43e77cdfa2222f466ef738f43b030eb7e46b373 Mon Sep 17 00:00:00 2001 From: Christian Forster Date: Tue, 2 Dec 2014 18:30:12 -0500 Subject: [PATCH 09/11] Restored old version of between factor. Uncommented tests --- gtsam/slam/BetweenFactor.h | 10 +++++----- gtsam/slam/tests/testBetweenFactor.cpp | 3 ++- 2 files changed, 7 insertions(+), 6 deletions(-) diff --git a/gtsam/slam/BetweenFactor.h b/gtsam/slam/BetweenFactor.h index 651b00404..bc9cd9d6c 100644 --- a/gtsam/slam/BetweenFactor.h +++ b/gtsam/slam/BetweenFactor.h @@ -90,12 +90,12 @@ namespace gtsam { Vector evaluateError(const T& p1, const T& p2, boost::optional H1 = boost::none,boost::optional H2 = boost::none) const { + + T hx = p1.between(p2, H1, H2); // h(x) DefaultChart chart; - // TODO check: - //T hx = p1.between(p2, H1, H2); // h(x) - T hx = chart.local(p2, p1); - if(H1) (*H1) = -eye(chart.getDimension(p1)); - if(H2) (*H2) = eye(chart.getDimension(p2)); + //T hx = chart.local(p2, p1); + //if(H1) (*H1) = -eye(chart.getDimension(p1)); + //if(H2) (*H2) = eye(chart.getDimension(p2)); // manifold equivalent of h(x)-z -> log(z,h(x)) return chart.local(measured_, hx); diff --git a/gtsam/slam/tests/testBetweenFactor.cpp b/gtsam/slam/tests/testBetweenFactor.cpp index 5169d90bc..b22763099 100644 --- a/gtsam/slam/tests/testBetweenFactor.cpp +++ b/gtsam/slam/tests/testBetweenFactor.cpp @@ -45,7 +45,7 @@ TEST(BetweenFactor, Rot3) { } /* ************************************************************************* */ - +/* // Constructor scalar TEST(BetweenFactor, ConstructorScalar) { SharedNoiseModel model; @@ -66,6 +66,7 @@ TEST(BetweenFactor, ConstructorDynamicSizeVector) { Vector measured_value(5); measured_value << 1, 2, 3, 4, 5; BetweenFactor factor(1, 2, measured_value, model); } +*/ /* ************************************************************************* */ int main() { From 6c92914db1dd016bb37680dcf52f7e25fdc3d078 Mon Sep 17 00:00:00 2001 From: Christian Forster Date: Tue, 2 Dec 2014 18:41:50 -0500 Subject: [PATCH 10/11] remove commented code --- gtsam/slam/BetweenFactor.h | 5 ----- 1 file changed, 5 deletions(-) diff --git a/gtsam/slam/BetweenFactor.h b/gtsam/slam/BetweenFactor.h index bc9cd9d6c..3587f7b71 100644 --- a/gtsam/slam/BetweenFactor.h +++ b/gtsam/slam/BetweenFactor.h @@ -90,13 +90,8 @@ namespace gtsam { Vector evaluateError(const T& p1, const T& p2, boost::optional H1 = boost::none,boost::optional H2 = boost::none) const { - T hx = p1.between(p2, H1, H2); // h(x) DefaultChart chart; - //T hx = chart.local(p2, p1); - //if(H1) (*H1) = -eye(chart.getDimension(p1)); - //if(H2) (*H2) = eye(chart.getDimension(p2)); - // manifold equivalent of h(x)-z -> log(z,h(x)) return chart.local(measured_, hx); } From f7cc4f2337452ca22c815e954c7968d3efe876e7 Mon Sep 17 00:00:00 2001 From: Christian Forster Date: Tue, 2 Dec 2014 18:59:30 -0500 Subject: [PATCH 11/11] use DefaultChart to get dimension in BetweenConstraint constructor --- gtsam/slam/BetweenFactor.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/slam/BetweenFactor.h b/gtsam/slam/BetweenFactor.h index 3587f7b71..9c5df7ea0 100644 --- a/gtsam/slam/BetweenFactor.h +++ b/gtsam/slam/BetweenFactor.h @@ -131,7 +131,7 @@ namespace gtsam { /** Syntactic sugar for constrained version */ BetweenConstraint(const VALUE& measured, Key key1, Key key2, double mu = 1000.0) : BetweenFactor(key1, key2, measured, - noiseModel::Constrained::All(traits::dimension(), fabs(mu))) + noiseModel::Constrained::All(DefaultChart::getDimension(measured), fabs(mu))) {} private: