From aea4f31096fe14b71dfe542150d5c14e13dbc920 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 24 Jun 2013 15:31:13 +0000 Subject: [PATCH 1/8] Fixed Jacobians, optimization works --- gtsam_unstable/slam/SmartRangeFactor.h | 49 ++++++++++++------- .../slam/tests/testSmartRangeFactor.cpp | 48 +++++++++++++----- 2 files changed, 68 insertions(+), 29 deletions(-) diff --git a/gtsam_unstable/slam/SmartRangeFactor.h b/gtsam_unstable/slam/SmartRangeFactor.h index dd1de5ef5..0bec8ab4d 100644 --- a/gtsam_unstable/slam/SmartRangeFactor.h +++ b/gtsam_unstable/slam/SmartRangeFactor.h @@ -33,8 +33,10 @@ protected: double radius; }; - /// Range measurements - std::vector measurements_; + typedef SmartRangeFactor This; + + std::vector measurements_; ///< Range measurements + double sigma_; ///< standard deviation on noise public: @@ -43,7 +45,7 @@ public: } /** standard binary constructor */ - SmartRangeFactor(const SharedNoiseModel& model) { + SmartRangeFactor(double sigma) : NoiseModelFactor(noiseModel::Isotropic::Sigma(1,sigma_)), sigma_(sigma) { } virtual ~SmartRangeFactor() { @@ -53,6 +55,7 @@ public: void addRange(Key key, double measuredRange) { keys_.push_back(key); measurements_.push_back(measuredRange); + noiseModel_ = noiseModel::Isotropic::Sigma(keys_.size(),sigma_); } // Testable @@ -100,30 +103,40 @@ public: */ virtual Vector unwhitenedError(const Values& x, boost::optional&> H = boost::none) const { - size_t K = size(); - Vector errors = zero(K); - if (K >= 3) { + size_t n = size(); + if (H) assert(H->size()==n); + Vector errors = zero(n); + if (n >= 3) { + // create n circles corresponding to measured range around each pose std::list circles; - for (size_t i = 0; i < K; i++) { - const Pose2& pose = x.at(keys_[i]); - circles.push_back(Circle2(pose.translation(), measurements_[i])); + for (size_t j = 0; j < n; j++) { + const Pose2& pose = x.at(keys_[j]); + circles.push_back(Circle2(pose.translation(), measurements_[j])); } + // triangulate to get the optimized point Point2 optimizedPoint = triangulate(circles); - if (H) - *H = std::vector(); - for (size_t i = 0; i < K; i++) { - const Pose2& pose = x.at(keys_[i]); + // now evaluate the errors between predicted and measured range + for (size_t j = 0; j < n; j++) { + const Pose2& pose = x.at(keys_[j]); if (H) { - Matrix Hi; - errors[i] = pose.range(optimizedPoint, Hi) - measurements_[i]; - H->push_back(Hi); - } else - errors[i] = pose.range(optimizedPoint) - measurements_[i]; + // calculate n*3 derivative for each of the n poses + (*H)[j] = zeros(n,3); + Matrix Hj; + errors[j] = pose.range(optimizedPoint, Hj) - measurements_[j]; + (*H)[j].row(j) = Hj; + } + else + errors[j] = pose.range(optimizedPoint) - measurements_[j]; } } return errors; } + /// @return a deep copy of this factor + virtual gtsam::NonlinearFactor::shared_ptr clone() const { + return boost::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new This(*this))); } + }; } // \namespace gtsam diff --git a/gtsam_unstable/slam/tests/testSmartRangeFactor.cpp b/gtsam_unstable/slam/tests/testSmartRangeFactor.cpp index 7eb01eedc..933148cf2 100644 --- a/gtsam_unstable/slam/tests/testSmartRangeFactor.cpp +++ b/gtsam_unstable/slam/tests/testSmartRangeFactor.cpp @@ -17,27 +17,28 @@ */ #include +#include +#include #include using namespace std; using namespace gtsam; -const noiseModel::Base::shared_ptr gaussian = noiseModel::Isotropic::Sigma(1, - 2.0); +static const double sigma = 2.0; /* ************************************************************************* */ TEST( SmartRangeFactor, constructor ) { SmartRangeFactor f1; LONGS_EQUAL(0, f1.size()) - SmartRangeFactor f2(gaussian); + SmartRangeFactor f2(sigma); LONGS_EQUAL(0, f2.size()) } /* ************************************************************************* */ TEST( SmartRangeFactor, addRange ) { - SmartRangeFactor f(gaussian); + SmartRangeFactor f(sigma); f.addRange(1, 10); f.addRange(1, 12); LONGS_EQUAL(2, f.size()) @@ -45,7 +46,7 @@ TEST( SmartRangeFactor, addRange ) { /* ************************************************************************* */ -TEST( SmartRangeFactor, unwhitenedError ) { +TEST( SmartRangeFactor, allAtOnce ) { // Test situation: Point2 p(0, 10); Pose2 pose1(0, 0, 0), pose2(5, 0, 0), pose3(5, 5, 0); @@ -59,7 +60,7 @@ TEST( SmartRangeFactor, unwhitenedError ) { values.insert(2, pose2); values.insert(3, pose3); - SmartRangeFactor f(gaussian); + SmartRangeFactor f(sigma); f.addRange(1, r1); // Whenever there are two ranges or less, error should be zero @@ -70,14 +71,39 @@ TEST( SmartRangeFactor, unwhitenedError ) { EXPECT(assert_equal(Vector2(0,0), actual2)); f.addRange(3, r3); - vector H; - Vector actual3 = f.unwhitenedError(values,H); + vector H(3); + Vector actual3 = f.unwhitenedError(values); + EXPECT_LONGS_EQUAL(3,f.keys().size()); EXPECT(assert_equal(Vector3(0,0,0), actual3)); // Check keys and Jacobian - EXPECT_LONGS_EQUAL(3,f.keys().size()); - EXPECT(assert_equal(Matrix_(1,3,0.0,-1.0,0.0), H.front())); - EXPECT(assert_equal(Matrix_(1,3,sqrt(2)/2,-sqrt(2)/2,0.0), H.back())); + Vector actual4 = f.unwhitenedError(values,H); // with H now ! + EXPECT(assert_equal(Vector3(0,0,0), actual4)); + CHECK(assert_equal(Matrix_(3,3, 0.0,-1.0,0.0, 0.0,0.0,0.0, 0.0,0.0,0.0), H.front())); + CHECK(assert_equal(Matrix_(3,3, 0.0,0.0,0.0, 0.0,0.0,0.0, sqrt(2)/2,-sqrt(2)/2,0.0), H.back())); + + // Test clone + NonlinearFactor::shared_ptr clone = f.clone(); + EXPECT_LONGS_EQUAL(3,clone->keys().size()); + + // Create initial value for optimization + Values initial; + initial.insert(1, Pose2(0, 0, 0)); + initial.insert(2, Pose2(5, 0, 0)); + initial.insert(3, Pose2(5, 6, 0)); + Vector actual5 = f.unwhitenedError(initial); + EXPECT(assert_equal(Vector3(0,0,sqrt(25+16)-sqrt(50)), actual5)); + + // Try optimizing + NonlinearFactorGraph graph; + graph.add(f); + LevenbergMarquardtParams params; + //params.setVerbosity("ERROR"); + Values result = LevenbergMarquardtOptimizer(graph, initial, params).optimize(); + EXPECT(assert_equal(values.at(1), result.at(1))); + EXPECT(assert_equal(values.at(2), result.at(2))); + // only the third pose will be changed, converges on following: + EXPECT(assert_equal(Pose2(5.52157630366, 5.58273895707, 0), result.at(3))); } /* ************************************************************************* */ From 75751cc5fac6c86a4c844a2b0cc4ef494c4dde0a Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 24 Jun 2013 15:33:39 +0000 Subject: [PATCH 2/8] Comments --- gtsam/linear/JacobianFactor.cpp | 19 ++++++++++++++----- 1 file changed, 14 insertions(+), 5 deletions(-) diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index 25d0be736..5de446d25 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -140,17 +140,26 @@ namespace gtsam { GaussianFactor(GetKeys(terms.size(), terms.begin(), terms.end())), model_(model), Ab_(matrix_) { + // get number of measurements and variables involved in this factor + size_t m = b.size(), n = terms.size(); - if(model->dim() != (size_t) b.size()) + if(model->dim() != (size_t) m) throw InvalidNoiseModel(b.size(), model->dim()); - size_t* dims = (size_t*)alloca(sizeof(size_t)*(terms.size()+1)); // FIXME: alloca is bad, just ask Google. - for(size_t j=0; j Date: Mon, 24 Jun 2013 15:57:03 +0000 Subject: [PATCH 3/8] Made more efficient by adding errors -> Jacobians back to 1*3, always. This is big savings if a landmark is seen from many poses. --- gtsam_unstable/slam/SmartRangeFactor.h | 35 +++++++++++-------- .../slam/tests/testSmartRangeFactor.cpp | 30 +++++++++------- 2 files changed, 38 insertions(+), 27 deletions(-) diff --git a/gtsam_unstable/slam/SmartRangeFactor.h b/gtsam_unstable/slam/SmartRangeFactor.h index 0bec8ab4d..ff5b57a40 100644 --- a/gtsam_unstable/slam/SmartRangeFactor.h +++ b/gtsam_unstable/slam/SmartRangeFactor.h @@ -36,7 +36,7 @@ protected: typedef SmartRangeFactor This; std::vector measurements_; ///< Range measurements - double sigma_; ///< standard deviation on noise + double variance_; ///< variance on noise public: @@ -44,8 +44,12 @@ public: SmartRangeFactor() { } - /** standard binary constructor */ - SmartRangeFactor(double sigma) : NoiseModelFactor(noiseModel::Isotropic::Sigma(1,sigma_)), sigma_(sigma) { + /** + * Constructor + * @param s standard deviation of range measurement noise + */ + SmartRangeFactor(double s) : + NoiseModelFactor(noiseModel::Isotropic::Sigma(1, s)), variance_(s * s) { } virtual ~SmartRangeFactor() { @@ -55,7 +59,9 @@ public: void addRange(Key key, double measuredRange) { keys_.push_back(key); measurements_.push_back(measuredRange); - noiseModel_ = noiseModel::Isotropic::Sigma(keys_.size(),sigma_); + size_t n = keys_.size(); + // Since we add the errors, the noise variance adds + noiseModel_ = noiseModel::Isotropic::Variance(1, n * variance_); } // Testable @@ -104,8 +110,9 @@ public: virtual Vector unwhitenedError(const Values& x, boost::optional&> H = boost::none) const { size_t n = size(); - if (H) assert(H->size()==n); - Vector errors = zero(n); + if (H) + assert(H->size()==n); + Vector errors = zero(1); if (n >= 3) { // create n circles corresponding to measured range around each pose std::list circles; @@ -114,19 +121,16 @@ public: circles.push_back(Circle2(pose.translation(), measurements_[j])); } // triangulate to get the optimized point + // TODO: Should we have a (better?) variant that does this in relative coordinates ? Point2 optimizedPoint = triangulate(circles); // now evaluate the errors between predicted and measured range for (size_t j = 0; j < n; j++) { const Pose2& pose = x.at(keys_[j]); - if (H) { - // calculate n*3 derivative for each of the n poses - (*H)[j] = zeros(n,3); - Matrix Hj; - errors[j] = pose.range(optimizedPoint, Hj) - measurements_[j]; - (*H)[j].row(j) = Hj; - } + if (H) + // also calculate 1*3 derivative for each of the n poses + errors[0] += pose.range(optimizedPoint, (*H)[j]) - measurements_[j]; else - errors[j] = pose.range(optimizedPoint) - measurements_[j]; + errors[0] += pose.range(optimizedPoint) - measurements_[j]; } } return errors; @@ -135,7 +139,8 @@ public: /// @return a deep copy of this factor virtual gtsam::NonlinearFactor::shared_ptr clone() const { return boost::static_pointer_cast( - gtsam::NonlinearFactor::shared_ptr(new This(*this))); } + gtsam::NonlinearFactor::shared_ptr(new This(*this))); + } }; diff --git a/gtsam_unstable/slam/tests/testSmartRangeFactor.cpp b/gtsam_unstable/slam/tests/testSmartRangeFactor.cpp index 933148cf2..354eef59c 100644 --- a/gtsam_unstable/slam/tests/testSmartRangeFactor.cpp +++ b/gtsam_unstable/slam/tests/testSmartRangeFactor.cpp @@ -19,6 +19,7 @@ #include #include #include +#include #include using namespace std; @@ -68,23 +69,23 @@ TEST( SmartRangeFactor, allAtOnce ) { EXPECT(assert_equal(Vector_(1,0.0), actual1)); f.addRange(2, r2); Vector actual2 = f.unwhitenedError(values); - EXPECT(assert_equal(Vector2(0,0), actual2)); + EXPECT(assert_equal(Vector_(1,0.0), actual2)); f.addRange(3, r3); vector H(3); Vector actual3 = f.unwhitenedError(values); - EXPECT_LONGS_EQUAL(3,f.keys().size()); - EXPECT(assert_equal(Vector3(0,0,0), actual3)); + EXPECT_LONGS_EQUAL(3, f.keys().size()); + EXPECT(assert_equal(Vector_(1,0.0), actual3)); // Check keys and Jacobian - Vector actual4 = f.unwhitenedError(values,H); // with H now ! - EXPECT(assert_equal(Vector3(0,0,0), actual4)); - CHECK(assert_equal(Matrix_(3,3, 0.0,-1.0,0.0, 0.0,0.0,0.0, 0.0,0.0,0.0), H.front())); - CHECK(assert_equal(Matrix_(3,3, 0.0,0.0,0.0, 0.0,0.0,0.0, sqrt(2)/2,-sqrt(2)/2,0.0), H.back())); + Vector actual4 = f.unwhitenedError(values, H); // with H now ! + EXPECT(assert_equal(Vector_(1,0.0), actual4)); + CHECK(assert_equal(Matrix_(1,3, 0.0,-1.0,0.0), H.front())); + CHECK(assert_equal(Matrix_(1,3, sqrt(2)/2,-sqrt(2)/2,0.0), H.back())); // Test clone NonlinearFactor::shared_ptr clone = f.clone(); - EXPECT_LONGS_EQUAL(3,clone->keys().size()); + EXPECT_LONGS_EQUAL(3, clone->keys().size()); // Create initial value for optimization Values initial; @@ -92,18 +93,23 @@ TEST( SmartRangeFactor, allAtOnce ) { initial.insert(2, Pose2(5, 0, 0)); initial.insert(3, Pose2(5, 6, 0)); Vector actual5 = f.unwhitenedError(initial); - EXPECT(assert_equal(Vector3(0,0,sqrt(25+16)-sqrt(50)), actual5)); + EXPECT(assert_equal(Vector_(1,sqrt(25+16)-sqrt(50)), actual5)); // Try optimizing NonlinearFactorGraph graph; graph.add(f); + const noiseModel::Base::shared_ptr // + priorNoise = noiseModel::Diagonal::Sigmas(Vector3(1, 1, M_PI)); + graph.add(PriorFactor(1, pose1, priorNoise)); + graph.add(PriorFactor(2, pose2, priorNoise)); LevenbergMarquardtParams params; - //params.setVerbosity("ERROR"); - Values result = LevenbergMarquardtOptimizer(graph, initial, params).optimize(); + // params.setVerbosity("ERROR"); + Values result = + LevenbergMarquardtOptimizer(graph, initial, params).optimize(); EXPECT(assert_equal(values.at(1), result.at(1))); EXPECT(assert_equal(values.at(2), result.at(2))); // only the third pose will be changed, converges on following: - EXPECT(assert_equal(Pose2(5.52157630366, 5.58273895707, 0), result.at(3))); + EXPECT(assert_equal(Pose2(5.52159, 5.582727, 0), result.at(3),1e-5)); } /* ************************************************************************* */ From 3383e52c5fdb5db00f7e3b0d7659c0a3f2c7baac Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 24 Jun 2013 16:02:17 +0000 Subject: [PATCH 4/8] Split unit tests.... --- .../slam/tests/testSmartRangeFactor.cpp | 43 ++++++++++++------- 1 file changed, 28 insertions(+), 15 deletions(-) diff --git a/gtsam_unstable/slam/tests/testSmartRangeFactor.cpp b/gtsam_unstable/slam/tests/testSmartRangeFactor.cpp index 354eef59c..646354452 100644 --- a/gtsam_unstable/slam/tests/testSmartRangeFactor.cpp +++ b/gtsam_unstable/slam/tests/testSmartRangeFactor.cpp @@ -27,6 +27,12 @@ using namespace gtsam; static const double sigma = 2.0; +// Test situation: +static const Point2 p(0, 10); +static const Pose2 pose1(0, 0, 0), pose2(5, 0, 0), pose3(5, 5, 0); +static const double r1 = pose1.range(p), r2 = pose2.range(p), r3 = pose3.range( + p); + /* ************************************************************************* */ TEST( SmartRangeFactor, constructor ) { @@ -35,7 +41,6 @@ TEST( SmartRangeFactor, constructor ) { SmartRangeFactor f2(sigma); LONGS_EQUAL(0, f2.size()) } - /* ************************************************************************* */ TEST( SmartRangeFactor, addRange ) { @@ -44,18 +49,16 @@ TEST( SmartRangeFactor, addRange ) { f.addRange(1, 12); LONGS_EQUAL(2, f.size()) } - /* ************************************************************************* */ -TEST( SmartRangeFactor, allAtOnce ) { - // Test situation: - Point2 p(0, 10); - Pose2 pose1(0, 0, 0), pose2(5, 0, 0), pose3(5, 5, 0); - double r1 = pose1.range(p), r2 = pose2.range(p), r3 = pose3.range(p); +TEST( SmartRangeFactor, scenario ) { DOUBLES_EQUAL(10, r1, 1e-9); DOUBLES_EQUAL(sqrt(100+25), r2, 1e-9); DOUBLES_EQUAL(sqrt(50), r3, 1e-9); +} +/* ************************************************************************* */ +TEST( SmartRangeFactor, unwhitenedError ) { Values values; // all correct values.insert(1, pose1); values.insert(2, pose2); @@ -86,28 +89,38 @@ TEST( SmartRangeFactor, allAtOnce ) { // Test clone NonlinearFactor::shared_ptr clone = f.clone(); EXPECT_LONGS_EQUAL(3, clone->keys().size()); +} +/* ************************************************************************* */ + +TEST( SmartRangeFactor, optimization ) { + SmartRangeFactor f(sigma); + f.addRange(1, r1); + f.addRange(2, r2); + f.addRange(3, r3); // Create initial value for optimization Values initial; - initial.insert(1, Pose2(0, 0, 0)); - initial.insert(2, Pose2(5, 0, 0)); - initial.insert(3, Pose2(5, 6, 0)); + initial.insert(1, pose1); + initial.insert(2, pose2); + initial.insert(3, Pose2(5, 6, 0)); // does not satisfy range measurement Vector actual5 = f.unwhitenedError(initial); EXPECT(assert_equal(Vector_(1,sqrt(25+16)-sqrt(50)), actual5)); - // Try optimizing + // Create Factor graph NonlinearFactorGraph graph; graph.add(f); const noiseModel::Base::shared_ptr // priorNoise = noiseModel::Diagonal::Sigmas(Vector3(1, 1, M_PI)); graph.add(PriorFactor(1, pose1, priorNoise)); graph.add(PriorFactor(2, pose2, priorNoise)); + + // Try optimizing LevenbergMarquardtParams params; // params.setVerbosity("ERROR"); - Values result = - LevenbergMarquardtOptimizer(graph, initial, params).optimize(); - EXPECT(assert_equal(values.at(1), result.at(1))); - EXPECT(assert_equal(values.at(2), result.at(2))); + LevenbergMarquardtOptimizer optimizer(graph, initial, params); + Values result = optimizer.optimize(); + EXPECT(assert_equal(initial.at(1), result.at(1))); + EXPECT(assert_equal(initial.at(2), result.at(2))); // only the third pose will be changed, converges on following: EXPECT(assert_equal(Pose2(5.52159, 5.582727, 0), result.at(3),1e-5)); } From 34db3008024647893743106a11ced2eb4afae037 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 24 Jun 2013 16:18:48 +0000 Subject: [PATCH 5/8] Fixed n<3 Jacobians --- gtsam_unstable/slam/SmartRangeFactor.h | 34 ++++++++++++++----- .../slam/tests/testSmartRangeFactor.cpp | 5 +++ 2 files changed, 30 insertions(+), 9 deletions(-) diff --git a/gtsam_unstable/slam/SmartRangeFactor.h b/gtsam_unstable/slam/SmartRangeFactor.h index ff5b57a40..37c29ac83 100644 --- a/gtsam_unstable/slam/SmartRangeFactor.h +++ b/gtsam_unstable/slam/SmartRangeFactor.h @@ -86,21 +86,29 @@ public: Circle2 circle1 = circles.front(); boost::optional best_fh; boost::optional best_circle; + + // loop over all circles BOOST_FOREACH(const Circle2& it, circles) { // distance between circle centers. double d = circle1.center.dist(it.center); if (d < 1e-9) - continue; + continue; // skip circles that are in the same location + // Find f and h, the intersection points in normalized circles boost::optional fh = Point2::CircleCircleIntersection( circle1.radius / d, it.radius / d); + // Check if this pair is better by checking h = fh->y() + // if h is large, the intersections are well defined. if (fh && (!best_fh || fh->y() > best_fh->y())) { best_fh = fh; best_circle = it; } } + + // use best fh to find actual intersection points std::list solutions = Point2::CircleCircleIntersection( circle1.center, best_circle->center, best_fh); - // TODO, pick winner based on other measurement + + // pick winner based on other measurement return solutions.front(); } @@ -110,30 +118,38 @@ public: virtual Vector unwhitenedError(const Values& x, boost::optional&> H = boost::none) const { size_t n = size(); - if (H) - assert(H->size()==n); - Vector errors = zero(1); - if (n >= 3) { + if (n < 3) { + if (H) + // set Jacobians to zero for n<3 + for (size_t j = 0; j < n; j++) + (*H)[j] = zeros(3, 1); + return zero(1); + } else { + Vector error = zero(1); + // create n circles corresponding to measured range around each pose std::list circles; for (size_t j = 0; j < n; j++) { const Pose2& pose = x.at(keys_[j]); circles.push_back(Circle2(pose.translation(), measurements_[j])); } + // triangulate to get the optimized point // TODO: Should we have a (better?) variant that does this in relative coordinates ? Point2 optimizedPoint = triangulate(circles); + + // TODO: triangulation should be followed by an optimization given poses // now evaluate the errors between predicted and measured range for (size_t j = 0; j < n; j++) { const Pose2& pose = x.at(keys_[j]); if (H) // also calculate 1*3 derivative for each of the n poses - errors[0] += pose.range(optimizedPoint, (*H)[j]) - measurements_[j]; + error[0] += pose.range(optimizedPoint, (*H)[j]) - measurements_[j]; else - errors[0] += pose.range(optimizedPoint) - measurements_[j]; + error[0] += pose.range(optimizedPoint) - measurements_[j]; } + return error; } - return errors; } /// @return a deep copy of this factor diff --git a/gtsam_unstable/slam/tests/testSmartRangeFactor.cpp b/gtsam_unstable/slam/tests/testSmartRangeFactor.cpp index 646354452..8f485738b 100644 --- a/gtsam_unstable/slam/tests/testSmartRangeFactor.cpp +++ b/gtsam_unstable/slam/tests/testSmartRangeFactor.cpp @@ -67,6 +67,11 @@ TEST( SmartRangeFactor, unwhitenedError ) { SmartRangeFactor f(sigma); f.addRange(1, r1); + // Check Jacobian for n==1 + vector H1(1); + f.unwhitenedError(values, H1); // with H now ! + CHECK(assert_equal(zeros(3,1), H1.front())); + // Whenever there are two ranges or less, error should be zero Vector actual1 = f.unwhitenedError(values); EXPECT(assert_equal(Vector_(1,0.0), actual1)); From 39910717d6557cf78642314c65ce499db9d7aed9 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 24 Jun 2013 16:25:02 +0000 Subject: [PATCH 6/8] Picked the winner --- gtsam_unstable/slam/SmartRangeFactor.h | 12 +++++++++--- 1 file changed, 9 insertions(+), 3 deletions(-) diff --git a/gtsam_unstable/slam/SmartRangeFactor.h b/gtsam_unstable/slam/SmartRangeFactor.h index 37c29ac83..be3735961 100644 --- a/gtsam_unstable/slam/SmartRangeFactor.h +++ b/gtsam_unstable/slam/SmartRangeFactor.h @@ -105,11 +105,17 @@ public: } // use best fh to find actual intersection points - std::list solutions = Point2::CircleCircleIntersection( + std::list intersections = Point2::CircleCircleIntersection( circle1.center, best_circle->center, best_fh); - // pick winner based on other measurement - return solutions.front(); + // pick winner based on other measurements + double error1 = 0, error2 = 0; + Point2 p1 = intersections.front(), p2 = intersections.back(); + BOOST_FOREACH(const Circle2& it, circles) { + error1 += it.center.dist(p1); + error2 += it.center.dist(p2); + } + return (error1 < error2) ? p1 : p2; } /** From d8527f7332f4f2bfb3b74d4d613772b2f5580a43 Mon Sep 17 00:00:00 2001 From: Chris Beall Date: Mon, 24 Jun 2013 18:15:29 +0000 Subject: [PATCH 7/8] documentation error --- gtsam/geometry/Cal3_S2.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/geometry/Cal3_S2.h b/gtsam/geometry/Cal3_S2.h index 660774180..b8cfda9ba 100644 --- a/gtsam/geometry/Cal3_S2.h +++ b/gtsam/geometry/Cal3_S2.h @@ -87,7 +87,7 @@ namespace gtsam { /// focal length x inline double fx() const { return fx_;} - /// focal length x + /// focal length y inline double fy() const { return fy_;} /// skew From f6036242488d058b9535e29b8a135e0c366afdce Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Mon, 24 Jun 2013 19:29:54 +0000 Subject: [PATCH 8/8] Fixed bug in assert_equal for vector - was always returning true without checking vector contents! --- gtsam/base/TestableAssertions.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/base/TestableAssertions.h b/gtsam/base/TestableAssertions.h index b62372957..42001b6f4 100644 --- a/gtsam/base/TestableAssertions.h +++ b/gtsam/base/TestableAssertions.h @@ -91,7 +91,7 @@ bool assert_equal(const std::vector& expected, const std::vector& actual, if(match) { size_t i = 0; BOOST_FOREACH(const V& a, expected) { - if (!assert_equal(a, expected[i++], tol)) { + if (!assert_equal(a, actual[i++], tol)) { match = false; break; }