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; } 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 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 measurements_; + typedef SmartRangeFactor This; + + std::vector measurements_; ///< Range measurements + double variance_; ///< variance on noise public: @@ -42,8 +44,12 @@ public: SmartRangeFactor() { } - /** standard binary constructor */ - SmartRangeFactor(const SharedNoiseModel& model) { + /** + * Constructor + * @param s standard deviation of range measurement noise + */ + SmartRangeFactor(double s) : + NoiseModelFactor(noiseModel::Isotropic::Sigma(1, s)), variance_(s * s) { } virtual ~SmartRangeFactor() { @@ -53,6 +59,9 @@ public: void addRange(Key key, double measuredRange) { keys_.push_back(key); measurements_.push_back(measuredRange); + size_t n = keys_.size(); + // Since we add the errors, the noise variance adds + noiseModel_ = noiseModel::Isotropic::Variance(1, n * variance_); } // Testable @@ -77,22 +86,36 @@ 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; } } - std::list solutions = Point2::CircleCircleIntersection( + + // use best fh to find actual intersection points + std::list intersections = Point2::CircleCircleIntersection( circle1.center, best_circle->center, best_fh); - // TODO, 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; } /** @@ -100,28 +123,45 @@ public: */ virtual Vector unwhitenedError(const Values& x, boost::optional&> H = boost::none) const { - size_t K = size(); - Vector errors = zero(K); - if (K >= 3) { - 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])); - } - Point2 optimizedPoint = triangulate(circles); + size_t n = size(); + if (n < 3) { if (H) - *H = std::vector(); - for (size_t i = 0; i < K; i++) { - const Pose2& pose = x.at(keys_[i]); - if (H) { - Matrix Hi; - errors[i] = pose.range(optimizedPoint, Hi) - measurements_[i]; - H->push_back(Hi); - } else - errors[i] = pose.range(optimizedPoint) - measurements_[i]; + // 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 + error[0] += pose.range(optimizedPoint, (*H)[j]) - measurements_[j]; + else + error[0] += pose.range(optimizedPoint) - measurements_[j]; + } + return error; } - 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))); } }; diff --git a/gtsam_unstable/slam/tests/testSmartRangeFactor.cpp b/gtsam_unstable/slam/tests/testSmartRangeFactor.cpp index 7eb01eedc..8f485738b 100644 --- a/gtsam_unstable/slam/tests/testSmartRangeFactor.cpp +++ b/gtsam_unstable/slam/tests/testSmartRangeFactor.cpp @@ -17,67 +17,117 @@ */ #include +#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 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 ) { 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()) } - /* ************************************************************************* */ -TEST( SmartRangeFactor, unwhitenedError ) { - // 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); values.insert(3, pose3); - SmartRangeFactor f(gaussian); + 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)); 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; - Vector actual3 = f.unwhitenedError(values,H); - EXPECT(assert_equal(Vector3(0,0,0), actual3)); + vector H(3); + Vector actual3 = f.unwhitenedError(values); + EXPECT_LONGS_EQUAL(3, f.keys().size()); + EXPECT(assert_equal(Vector_(1,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(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()); +} +/* ************************************************************************* */ + +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, 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)); + + // 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"); + 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)); } /* ************************************************************************* */