diff --git a/.cproject b/.cproject index abe692394..36e736354 100644 --- a/.cproject +++ b/.cproject @@ -2114,6 +2114,14 @@ true true + + make + -j5 + check.nonlinear_unstable + true + true + true + make -j2 diff --git a/gtsam_unstable/CMakeLists.txt b/gtsam_unstable/CMakeLists.txt index f5cbf9ac6..679551dbe 100644 --- a/gtsam_unstable/CMakeLists.txt +++ b/gtsam_unstable/CMakeLists.txt @@ -5,6 +5,7 @@ set (gtsam_unstable_subdirs discrete dynamics linear + nonlinear slam ) diff --git a/gtsam_unstable/nonlinear/CMakeLists.txt b/gtsam_unstable/nonlinear/CMakeLists.txt new file mode 100644 index 000000000..a815e956d --- /dev/null +++ b/gtsam_unstable/nonlinear/CMakeLists.txt @@ -0,0 +1,25 @@ +# Install headers +file(GLOB nonlinear_headers "*.h") +install(FILES ${nonlinear_headers} DESTINATION include/gtsam_unstable/nonlinear) + +# Components to link tests in this subfolder against +set(nonlinear_local_libs + nonlinear_unstable + nonlinear + linear + inference + geometry + base + ccolamd +) + +set (nonlinear_full_libs + gtsam-static + gtsam_unstable-static) + +# Exclude tests that don't work +set (nonlinear_excluded_tests "") + +# Add all tests +gtsam_add_subdir_tests(nonlinear_unstable "${nonlinear_local_libs}" "${nonlinear_full_libs}" "${nonlinear_excluded_tests}") +add_dependencies(check.unstable check.nonlinear_unstable) diff --git a/gtsam_unstable/nonlinear/LinearizedFactor.cpp b/gtsam_unstable/nonlinear/LinearizedFactor.cpp new file mode 100644 index 000000000..82e5c9eef --- /dev/null +++ b/gtsam_unstable/nonlinear/LinearizedFactor.cpp @@ -0,0 +1,147 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file LinearizedFactor.cpp + * @brief A dummy factor that allows a linear factor to act as a nonlinear factor + * @author Alex Cunningham + */ + +#include +#include + +#include + +namespace gtsam { + +using namespace std; + +/* ************************************************************************* */ +LinearizedFactor::LinearizedFactor(JacobianFactor::shared_ptr lin_factor, + const KeyLookup& decoder, const Values& lin_points) +: Base(lin_factor->get_model()), b_(lin_factor->getb()), model_(lin_factor->get_model()) { + BOOST_FOREACH(const Index& idx, *lin_factor) { + // find full symbol + KeyLookup::const_iterator decode_it = decoder.find(idx); + if (decode_it == decoder.end()) + throw runtime_error("LinearizedFactor: could not find index in decoder!"); + Key key(decode_it->second); + + // extract linearization point + assert(lin_points.exists(key)); + this->lin_points_.insert(key, lin_points.at(key)); // NOTE: will not overwrite + + // extract Jacobian + Matrix A = lin_factor->getA(lin_factor->find(idx)); + this->matrices_.insert(std::make_pair(key, A)); + + // store keys + this->keys_.push_back(key); + } +} + +/* ************************************************************************* */ +LinearizedFactor::LinearizedFactor(JacobianFactor::shared_ptr lin_factor, + const Ordering& ordering, const Values& lin_points) +: Base(lin_factor->get_model()), b_(lin_factor->getb()), model_(lin_factor->get_model()) { + const KeyLookup decoder = ordering.invert(); + BOOST_FOREACH(const Index& idx, *lin_factor) { + // find full symbol + KeyLookup::const_iterator decode_it = decoder.find(idx); + if (decode_it == decoder.end()) + throw runtime_error("LinearizedFactor: could not find index in decoder!"); + Key key(decode_it->second); + + // extract linearization point + assert(lin_points.exists(key)); + this->lin_points_.insert(key, lin_points.at(key)); // NOTE: will not overwrite + + // extract Jacobian + Matrix A = lin_factor->getA(lin_factor->find(idx)); + this->matrices_.insert(std::make_pair(key, A)); + + // store keys + this->keys_.push_back(key); + } +} + +/* ************************************************************************* */ +Vector LinearizedFactor::unwhitenedError(const Values& c, + boost::optional&> H) const { + // extract the points in the new values + Vector ret = - b_; + + if (H) H->resize(this->size()); + size_t i=0; + BOOST_FOREACH(Key key, this->keys()) { + const Value& newPt = c.at(key); + const Value& linPt = lin_points_.at(key); + Vector d = linPt.localCoordinates_(newPt); + const Matrix& A = matrices_.at(key); + ret += A * d; + if (H) (*H)[i++] = A; + } + + return ret; +} + +/* ************************************************************************* */ +boost::shared_ptr +LinearizedFactor::linearize(const Values& c, const Ordering& ordering) const { + + // sort by varid - only known at linearization time + typedef std::map VarMatrixMap; + VarMatrixMap sorting_terms; + BOOST_FOREACH(const KeyMatrixMap::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); + + // compute rhs: adjust current by whitened update + Vector b = unwhitenedError(c) + b_; // remove original b + this->noiseModel_->whitenInPlace(b); + + return boost::shared_ptr(new JacobianFactor(terms, b - b_, model_)); +} + +/* ************************************************************************* */ +void LinearizedFactor::print(const std::string& s, const KeyFormatter& keyFormatter) const { + this->noiseModel_->print(s + std::string(" model")); + BOOST_FOREACH(const KeyMatrixMap::value_type& p, matrices_) { + gtsam::print(p.second, keyFormatter(p.first)); + } + gtsam::print(b_, std::string("b")); + std::cout << " nonlinear keys: "; + BOOST_FOREACH(const Key& key, this->keys()) + cout << keyFormatter(key) << " "; + lin_points_.print("Linearization Point"); +} + +/* ************************************************************************* */ +bool LinearizedFactor::equals(const LinearizedFactor& other, double tol) const { + if (!Base::equals(other, tol) + || !lin_points_.equals(other.lin_points_, tol) + || !equal_with_abs_tol(b_, other.b_, tol) + || !model_->equals(*other.model_, tol) + || matrices_.size() != other.matrices_.size()) + return false; + + KeyMatrixMap::const_iterator map1 = matrices_.begin(), map2 = other.matrices_.begin(); + for (; map1 != matrices_.end(), map2 != other.matrices_.end(); ++map1, ++map2) + if ((map1->first != map2->first) || !equal_with_abs_tol(map1->second, map2->second, tol)) + return false; + return true; +} + +} // \namespace gtsam diff --git a/gtsam_unstable/nonlinear/LinearizedFactor.h b/gtsam_unstable/nonlinear/LinearizedFactor.h new file mode 100644 index 000000000..3fa46701d --- /dev/null +++ b/gtsam_unstable/nonlinear/LinearizedFactor.h @@ -0,0 +1,122 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file LinearizedFactor.h + * @brief A dummy factor that allows a linear factor to act as a nonlinear factor + * @author Alex Cunningham + */ + +#pragma once + +#include +#include +#include + +namespace gtsam { + +/** + * A dummy factor that takes a linearized factor and inserts it into + * a nonlinear graph. + */ +class LinearizedFactor : public gtsam::NoiseModelFactor { + +public: + /** base type */ + typedef gtsam::NoiseModelFactor Base; + typedef LinearizedFactor This; + + /** shared pointer for convenience */ + typedef boost::shared_ptr shared_ptr; + + + /** decoder for keys - avoids the use of a full ordering */ + typedef gtsam::Ordering::InvertedMap KeyLookup; + +protected: + /** hold onto the factor itself */ + // store components of a jacobian factor + typedef std::map KeyMatrixMap; + KeyMatrixMap matrices_; + gtsam::Vector b_; + gtsam::SharedDiagonal model_; /// separate from the noisemodel in NonlinearFactor due to Diagonal/Gaussian + + /** linearization points for error calculation */ + gtsam::Values lin_points_; + + /** default constructor for serialization */ + LinearizedFactor() {} + +public: + + virtual ~LinearizedFactor() {} + + ADD_CLONE_NONLINEAR_FACTOR(This); + + /** + * Use this constructor with pre-constructed decoders + * @param lin_factor is a gaussian factor with linear keys (no labels baked in) + * @param decoder is a structure to look up the correct keys + * @param values is assumed to have the correct key structure with labels + */ + LinearizedFactor(gtsam::JacobianFactor::shared_ptr lin_factor, + const KeyLookup& decoder, const gtsam::Values& lin_points); + + /** + * Use this constructor with the ordering used to linearize the graph + * @param lin_factor is a gaussian factor with linear keys (no labels baked in) + * @param ordering is the full ordering used to linearize the graph + * @param values is assumed to have the correct key structure with labels + */ + LinearizedFactor(gtsam::JacobianFactor::shared_ptr lin_factor, + const gtsam::Ordering& ordering, const gtsam::Values& lin_points); + + /** Vector of errors, unwhitened, with optional derivatives (ordered by key) */ + virtual gtsam::Vector unwhitenedError(const gtsam::Values& c, + boost::optional&> H = boost::none) const; + + /** + * linearize to a GaussianFactor + * Reimplemented from NoiseModelFactor to directly copy out the + * matrices and only update the RHS b with an updated residual + */ + virtual boost::shared_ptr linearize( + const gtsam::Values& c, const gtsam::Ordering& ordering) const; + + // Testable + + /** print function */ + virtual void print(const std::string& s="", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; + + /** equals function with optional tolerance */ + virtual bool equals(const LinearizedFactor& other, double tol = 1e-9) const; + + // access functions + + const gtsam::Vector& b() const { return b_; } + inline const gtsam::Values& linearizationPoint() const { return lin_points_; } + +private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & boost::serialization::make_nvp("NonlinearFactor", + boost::serialization::base_object(*this)); + ar & BOOST_SERIALIZATION_NVP(matrices_); + ar & BOOST_SERIALIZATION_NVP(b_); + ar & BOOST_SERIALIZATION_NVP(model_); + ar & BOOST_SERIALIZATION_NVP(lin_points_); + } +}; + +} // \namespace gtsam + diff --git a/gtsam_unstable/nonlinear/tests/testLinearContainerFactors.cpp b/gtsam_unstable/nonlinear/tests/testLinearContainerFactors.cpp new file mode 100644 index 000000000..05fabc66a --- /dev/null +++ b/gtsam_unstable/nonlinear/tests/testLinearContainerFactors.cpp @@ -0,0 +1,22 @@ +/** + * @file testLinearContainerFactors.cpp + * + * @brief Tests the use of nonlinear containers for simple integration of linear factors into nonlinear graphs + * + * @date Jun 7, 2012 + * @author Alex Cunningham + */ + +#include + + +/* ************************************************************************* */ +TEST( testLinearContainerFactors, jacobian_factor ) { + +// LinearContainerFactor actualLinFactor(); + +} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +/* ************************************************************************* */ diff --git a/gtsam_unstable/nonlinear/tests/testLinearizedFactor.cpp b/gtsam_unstable/nonlinear/tests/testLinearizedFactor.cpp new file mode 100644 index 000000000..d15bcd150 --- /dev/null +++ b/gtsam_unstable/nonlinear/tests/testLinearizedFactor.cpp @@ -0,0 +1,68 @@ +/** + * @file testLinearizedFactor + * @author Alex Cunningham + */ + +#include +#include +#include +#include + +#include + +using namespace std; +using namespace boost::assign; +using namespace gtsam; + +static const double tol = 1e-5; + +/* ************************************************************************* */ +TEST( testLinearizedFactor, creation ) { + // Create a set of local keys (No robot label) + Key l1 = 11, l2 = 12, + l3 = 13, l4 = 14, + l5 = 15, l6 = 16, + l7 = 17, l8 = 18; + + // creating an ordering to decode the linearized factor + Ordering ordering; + ordering += l1,l2,l3,l4,l5,l6,l7,l8; + + // create a decoder for only the relevant variables + LinearizedFactor::KeyLookup decoder; + decoder[2] = l3; + decoder[4] = l5; + + // create a linear factor + SharedDiagonal model = noiseModel::Unit::Create(2); + JacobianFactor::shared_ptr linear_factor(new JacobianFactor( + ordering[l3], eye(2,2), ordering[l5], 2.0 * eye(2,2), zero(2), model)); + + // create a set of values - build with full set of values + gtsam::Values full_values, exp_values; + full_values.insert(l3, Point2(1.0, 2.0)); + full_values.insert(l5, Point2(4.0, 3.0)); + exp_values = full_values; + full_values.insert(l1, Point2(3.0, 7.0)); + + // create the test LinearizedFactor + // This is called in the constructor of DDFSlot for each linear factor + LinearizedFactor actual1(linear_factor, decoder, full_values); + LinearizedFactor actual2(linear_factor, ordering, full_values); + + EXPECT(assert_equal(actual1, actual2, tol)); + + // Verify the keys + vector expKeys; + expKeys += l3, l5; + EXPECT(assert_container_equality(expKeys, actual1.keys())); + EXPECT(assert_container_equality(expKeys, actual2.keys())); + + // Verify subset of linearization points + EXPECT(assert_equal(exp_values, actual1.linearizationPoint(), tol)); + EXPECT(assert_equal(exp_values, actual2.linearizationPoint(), tol)); +} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +/* ************************************************************************* */