diff --git a/cpp/BearingRangeFactor.h b/cpp/BearingRangeFactor.h index da268caa7..d4d0767df 100644 --- a/cpp/BearingRangeFactor.h +++ b/cpp/BearingRangeFactor.h @@ -21,20 +21,18 @@ namespace gtsam { PointKey, Point2> { private: - // the bearing factor - BearingFactor bearing_; - - // the range factor - RangeFactor range_; + // the measurement + Rot2 bearing_; + double range_; typedef NonlinearFactor2 Base; public: BearingRangeFactor(); /* Default constructor */ - BearingRangeFactor(const PoseKey& i, const PointKey& j, const std::pair& z, + BearingRangeFactor(const PoseKey& i, const PointKey& j, const Rot2& bearing, const double range, const SharedGaussian& model) : - Base(model, i, j), bearing_(i, j, z.first, model), range_(i, j, z.second, model) { + Base(model, i, j), bearing_(bearing), range_(range) { } /** h(x)-z -> between(z,h(x)) for Rot2 manifold */ @@ -45,24 +43,22 @@ namespace gtsam { boost::optional H21_ = H1 ? boost::optional(H21) : boost::optional(); boost::optional H12_ = H2 ? boost::optional(H12) : boost::optional(); boost::optional H22_ = H2 ? boost::optional(H22) : boost::optional(); - Vector e1 = bearing_.evaluateError(pose, point, H11_, H12_); - Vector e2 = range_.evaluateError(pose, point, H21_, H22_); - if (H1) *H1 = stack_matrices(H11, H21); - if (H2) *H2 = stack_matrices(H12, H22); + + Rot2 y1 = gtsam::bearing(pose, point, H11_, H12_); + Vector e1 = logmap(between(bearing_, y1)); + + double y2 = gtsam::range(pose, point, H21_, H22_); + Vector e2 = Vector_(1, y2 - range_); + + if (H1) *H1 = gtsam::stack(2, &H11, &H21); + if (H2) *H2 = gtsam::stack(2, &H12, &H22); return concatVectors(2, &e1, &e2); } /** return the measured */ inline const std::pair measured() const { - return concatVectors(2, bearing_.measured(), range_.measured()); + return std::make_pair(bearing_, range_); } - - /** return the bearing factor */ - const BearingFactor& bearing() const { return bearing_; } - - /** return the range factor */ - const RangeFactor& range() const { return range_; } - }; // BearingRangeFactor } // namespace gtsam diff --git a/cpp/FactorGraph-inl.h b/cpp/FactorGraph-inl.h index 93069b319..3633f5862 100644 --- a/cpp/FactorGraph-inl.h +++ b/cpp/FactorGraph-inl.h @@ -134,6 +134,41 @@ Ordering FactorGraph::keys() const { return keys; } +/* ************************************************************************* */ +template +std::pair, set > FactorGraph::removeSingletons() { + FactorGraph singletonGraph; + set singletons; + + while(true) { + // find all the singleton variables + Ordering new_singletons; + Symbol key; + list indices; + BOOST_FOREACH(boost::tie(key, indices), indices_) { + // find out the number of factors associated with the current key + int numValidFactors = 0; + BOOST_FOREACH(const int& i, indices) + if (factors_[i]!=NULL) numValidFactors++; + + if (numValidFactors == 1) { + new_singletons.push_back(key); + BOOST_FOREACH(const int& i, indices) + if (factors_[i]!=NULL) singletonGraph.push_back(factors_[i]); + } + } + singletons.insert(new_singletons.begin(), new_singletons.end()); + + BOOST_FOREACH(const Symbol& singleton, new_singletons) + findAndRemoveFactors(singleton); + + // exit when there are no more singletons + if (new_singletons.empty()) break; + } + + return make_pair(singletonGraph, singletons); +} + /* ************************************************************************* */ /** * Call colamd given a column-major symbolic matrix A @@ -248,6 +283,9 @@ FactorGraph::findAndRemoveFactors(const Symbol& key) { found.push_back(fi); // add to found fi.reset(); // set factor to NULL == remove(i) } + + indices_.erase(key); + return found; } diff --git a/cpp/FactorGraph.h b/cpp/FactorGraph.h index 170e97c27..00d9f9c0e 100644 --- a/cpp/FactorGraph.h +++ b/cpp/FactorGraph.h @@ -97,6 +97,11 @@ namespace gtsam { return !(indices_.find(key)==indices_.end()); } + /** check whether a variable is a singleton, i.e. it only involve*/ + + /** remove singleton variables and the related factors */ + std::pair, std::set > removeSingletons(); + /** * Compute colamd ordering, including I/O and shared pointer version */ diff --git a/cpp/Matrix.cpp b/cpp/Matrix.cpp index 765ef4d62..6261952c4 100644 --- a/cpp/Matrix.cpp +++ b/cpp/Matrix.cpp @@ -742,11 +742,6 @@ Matrix stack(size_t nrMatrices, ...) return A; } -/* ************************************************************************* */ -Matrix stack_matrices(const Matrix& A, const Matrix& B) { - return stack(2, &A, &B); -} - /* ************************************************************************* */ Matrix collect(const std::vector& matrices, size_t m, size_t n) { diff --git a/cpp/Matrix.h b/cpp/Matrix.h index 571c161aa..3bba01839 100644 --- a/cpp/Matrix.h +++ b/cpp/Matrix.h @@ -287,9 +287,6 @@ Vector backSubstituteLower(const Matrix& L, const Vector& d, bool unit=false); */ Matrix stack(size_t nrMatrices, ...); -/** a shortcut to prevent the name confliction with STL stack */ -Matrix stack_matrices(const Matrix& A, const Matrix& B); - /** * create a matrix by concatenating * Given a set of matrices: A1, A2, A3... diff --git a/cpp/testFactorgraph.cpp b/cpp/testFactorgraph.cpp index f386e84d5..05b76d219 100644 --- a/cpp/testFactorgraph.cpp +++ b/cpp/testFactorgraph.cpp @@ -9,6 +9,9 @@ #include #include #include +#include // for operator += +using namespace boost::assign; + #include #define GTSAM_MAGIC_KEY @@ -44,6 +47,41 @@ TEST( FactorGraph, splitMinimumSpanningTree ) CHECK(assert_equal(expectedC,C)); } +/* ************************************************************************* */ +/** + * x1 - x2 - x3 - x4 - x5 + * | | / | + * l1 l2 l3 + */ +TEST( FactorGraph, removeSingletons ) +{ + SymbolicFactorGraph G; + G.push_factor("x1", "x2"); + G.push_factor("x2", "x3"); + G.push_factor("x3", "x4"); + G.push_factor("x4", "x5"); + G.push_factor("x2", "l1"); + G.push_factor("x3", "l2"); + G.push_factor("x4", "l2"); + G.push_factor("x4", "l3"); + + SymbolicFactorGraph singletonGraph; + set singletons; + boost::tie(singletonGraph, singletons) = G.removeSingletons(); + + set singletons_excepted; singletons_excepted += "x1", "x2", "x5", "l1", "l3"; + CHECK(singletons_excepted == singletons); + + SymbolicFactorGraph singletonGraph_excepted; + singletonGraph_excepted.push_factor("x2", "l1"); + singletonGraph_excepted.push_factor("x4", "l3"); + singletonGraph_excepted.push_factor("x1", "x2"); + singletonGraph_excepted.push_factor("x4", "x5"); + singletonGraph_excepted.push_factor("x2", "x3"); + CHECK(singletonGraph_excepted.equals(singletonGraph)); +} + + /* ************************************************************************* */ int main() { TestResult tr; diff --git a/cpp/testPlanarSLAM.cpp b/cpp/testPlanarSLAM.cpp index efb8918c5..cd325c955 100644 --- a/cpp/testPlanarSLAM.cpp +++ b/cpp/testPlanarSLAM.cpp @@ -61,7 +61,7 @@ TEST( planarSLAM, BearingRangeFactor ) // Create factor Rot2 r = Rot2::fromAngle(M_PI_4 + 0.1); // h(x) - z = -0.1 double b(sqrt(2) - 0.22); // h(x) - z = 0.22 - planarSLAM::BearingRange factor(2, 3, make_pair(r,b), sigma2); + planarSLAM::BearingRange factor(2, 3, r, b, sigma2); // create config planarSLAM::Config c;