diff --git a/nonlinear/Makefile.am b/nonlinear/Makefile.am index c36a97a4e..5abd05a6f 100644 --- a/nonlinear/Makefile.am +++ b/nonlinear/Makefile.am @@ -17,7 +17,7 @@ check_PROGRAMS = # Lie Groups headers += Key.h LieValues.h LieValues-inl.h TupleValues.h TupleValues-inl.h -check_PROGRAMS += tests/testLieValues +check_PROGRAMS += tests/testLieValues tests/testKey # Nonlinear nonlinear headers += NonlinearFactorGraph.h NonlinearFactorGraph-inl.h diff --git a/inference/tests/testKey.cpp b/nonlinear/tests/testKey.cpp similarity index 98% rename from inference/tests/testKey.cpp rename to nonlinear/tests/testKey.cpp index 13e3d138c..589599a73 100644 --- a/inference/tests/testKey.cpp +++ b/nonlinear/tests/testKey.cpp @@ -7,7 +7,7 @@ using namespace boost::assign; #include -#include +#include using namespace std; using namespace gtsam; diff --git a/slam/LinearApproxFactor-inl.h b/slam/LinearApproxFactor-inl.h index c678db376..c58ba7950 100644 --- a/slam/LinearApproxFactor-inl.h +++ b/slam/LinearApproxFactor-inl.h @@ -1,4 +1,4 @@ -/* +/** * @file LinearApproxFactor.h * @brief A dummy factor that allows a linear factor to act as a nonlinear factor * @author Alex Cunningham @@ -15,51 +15,83 @@ namespace gtsam { /* ************************************************************************* */ template -LinearApproxFactor::LinearApproxFactor(GaussianFactor::shared_ptr lin_factor, const Values& lin_points) +LinearApproxFactor::LinearApproxFactor( + GaussianFactor::shared_ptr lin_factor, const Ordering& ordering, const Values& lin_points) : Base(noiseModel::Unit::Create(lin_factor->get_model()->dim())), - lin_factor_(lin_factor), lin_points_(lin_points) - { - // create the keys and store them - BOOST_FOREACH(Symbol key, lin_factor->keys()) { - nonlinearKeys_.push_back(Key(key.index())); - this->keys_.push_back(key); + b_(lin_factor->getb()), model_(lin_factor->get_model()), lin_points_(lin_points) +{ + BOOST_FOREACH(const Ordering::Map::value_type& p, ordering) { + Symbol key = p.first; + varid_t var = p.second; + + // check if actually in factor + Factor::const_iterator it = lin_factor->find(var); + if (it != lin_factor->end()) { + // store matrix + Matrix A = lin_factor->getA(it); + matrices_.insert(make_pair(key, A)); + + // store keys + nonlinearKeys_.push_back(Key(key.index())); + this->keys_.push_back(key); + } } - } +} /* ************************************************************************* */ template Vector LinearApproxFactor::unwhitenedError(const Values& c) const { // extract the points in the new config VectorValues delta; - BOOST_FOREACH(const Key& key, nonlinearKeys_) { - X newPt = c[key], linPt = lin_points_[key]; - Vector d = linPt.logmap(newPt); - delta.insert(key, d); - } + // BOOST_FOREACH(const Key& key, nonlinearKeys_) { + // X newPt = c[key], linPt = lin_points_[key]; + // Vector d = linPt.logmap(newPt); + // delta.insert(key, d); + // } - return lin_factor_->unweighted_error(delta); + return zero(b_.size()); //FIXME: PLACEHOLDER! } /* ************************************************************************* */ template boost::shared_ptr -LinearApproxFactor::linearize(const Values& c) const { - Vector b = lin_factor_->getb(); - SharedDiagonal model = lin_factor_->get_model(); - std::vector > terms; - BOOST_FOREACH(Symbol key, lin_factor_->keys()) { - terms.push_back(std::make_pair(key, lin_factor_->get_A(key))); - } +LinearApproxFactor::linearize(const Values& c, const Ordering& ordering) const { - return boost::shared_ptr( - new GaussianFactor(terms, b, model)); + // sort by varid - only known at linearization time + typedef std::map VarMatrixMap; + VarMatrixMap sorting_terms; + BOOST_FOREACH(const SymbolMatrixMap::value_type& p, matrices_) + sorting_terms.insert(std::make_pair(ordering[p.first], p.second)); + + // move into terms + std::vector > terms; + BOOST_FOREACH(const VarMatrixMap::value_type& p, sorting_terms) + terms.push_back(p); + + return boost::shared_ptr(new GaussianFactor(terms, b_, model_)); +} + +/* ************************************************************************* */ +template +Factor::shared_ptr +LinearApproxFactor::symbolic(const Ordering& ordering) const { + std::vector key_ids(this->keys_.size()); + size_t i=0; + BOOST_FOREACH(const Symbol& key, this->keys_) + key_ids[i++] = ordering[key]; + std::sort(key_ids.begin(), key_ids.end()); + return boost::shared_ptr(new Factor(key_ids.begin(), key_ids.end())); } /* ************************************************************************* */ template void LinearApproxFactor::print(const std::string& s) const { LinearApproxFactor::Base::print(s); - lin_factor_->print(); + BOOST_FOREACH(const SymbolMatrixMap::value_type& p, matrices_) { + gtsam::print(p.second, (std::string) p.first); + } + gtsam::print(b_, std::string("b")); + model_->print("model"); } } // \namespace gtsam diff --git a/slam/LinearApproxFactor.h b/slam/LinearApproxFactor.h index 7b958c60e..80ce02a2c 100644 --- a/slam/LinearApproxFactor.h +++ b/slam/LinearApproxFactor.h @@ -1,4 +1,4 @@ -/* +/** * @file LinearApproxFactor.h * @brief A dummy factor that allows a linear factor to act as a nonlinear factor * @author Alex Cunningham @@ -8,8 +8,6 @@ #include #include -#include -#include namespace gtsam { @@ -35,7 +33,12 @@ public: protected: /** hold onto the factor itself */ - GaussianFactor::shared_ptr lin_factor_; +// GaussianFactor::shared_ptr lin_factor_; + // store components of a linear factor that can be reordered + typedef std::map SymbolMatrixMap; + SymbolMatrixMap matrices_; + Vector b_; + SharedDiagonal model_; /** linearization points for error calculation */ Values lin_points_; @@ -51,8 +54,14 @@ protected: public: - /** use this constructor when starting with nonlinear keys */ - LinearApproxFactor(GaussianFactor::shared_ptr lin_factor, const Values& lin_points); + /** + * use this constructor when starting with nonlinear keys + * + * Note that you need to have the ordering used to construct the factor + * initially in order to find the actual keys + */ + LinearApproxFactor(GaussianFactor::shared_ptr lin_factor, + const Ordering& ordering, const Values& lin_points); virtual ~LinearApproxFactor() {} @@ -61,11 +70,17 @@ public: /** * linearize to a GaussianFactor - * Just returns a copy of the existing factor - * NOTE: copies to avoid actual factor getting destroyed - * during elimination + * Reconstructs the linear factor from components to ensure that + * the ordering is correct */ - virtual boost::shared_ptr linearize(const Values& c) const; + virtual boost::shared_ptr linearize( + const Values& c, const Ordering& ordering) const; + + /** + * Create a symbolic factor using the given ordering to determine the + * variable indices. + */ + Factor::shared_ptr symbolic(const Ordering& ordering) const; /** get access to nonlinear keys */ KeyVector nonlinearKeys() const { return nonlinearKeys_; } @@ -74,7 +89,7 @@ public: virtual void print(const std::string& s="") const; /** access to b vector of gaussian */ - Vector get_b() const { return lin_factor_->getb(); } + Vector get_b() const { return b_; } }; } // \namespace gtsam diff --git a/tests/Makefile.am b/tests/Makefile.am index 0a8f43949..c6c26c463 100644 --- a/tests/Makefile.am +++ b/tests/Makefile.am @@ -22,7 +22,7 @@ check_PROGRAMS += testTupleValues #check_PROGRAMS += testNonlinearEqualityConstraint #check_PROGRAMS += testBoundingConstraint check_PROGRAMS += testTransformConstraint -#check_PROGRAMS += testLinearApproxFactor +check_PROGRAMS += testLinearApproxFactor # only if serialization is available if ENABLE_SERIALIZATION diff --git a/tests/testLinearApproxFactor.cpp b/tests/testLinearApproxFactor.cpp index ed3f75324..f04d6f67d 100644 --- a/tests/testLinearApproxFactor.cpp +++ b/tests/testLinearApproxFactor.cpp @@ -18,22 +18,21 @@ typedef LinearApproxFactor ApproxFactor /* ************************************************************************* */ TEST ( LinearApproxFactor, basic ) { - Symbol key1('x', 1); + Symbol key1('l', 1); Matrix A1 = eye(2); Vector b = repeat(2, 1.2); SharedDiagonal model = noiseModel::Unit::Create(2); - GaussianFactor::shared_ptr lin_factor(new GaussianFactor(1, A1, b, model)); + GaussianFactor::shared_ptr lin_factor(new GaussianFactor(0, A1, b, model)); + Ordering ordering; + ordering.push_back(key1); planarSLAM::Values lin_points; - ApproxFactor f1(lin_factor, lin_points); + ApproxFactor f1(lin_factor, ordering, lin_points); EXPECT(f1.size() == 1); ApproxFactor::KeyVector expKeyVec; expKeyVec.push_back(planarSLAM::PointKey(key1.index())); - EXPECT(assert_equal(expKeyVec, f1.nonlinearKeys())); planarSLAM::Values config; // doesn't really need to have any data - Ordering ordering; - ordering.push_back(key1); GaussianFactor::shared_ptr actual = f1.linearize(config, ordering); // Check the linearization