diff --git a/.cproject b/.cproject index 677af8430..dd5f6dc09 100644 --- a/.cproject +++ b/.cproject @@ -597,6 +597,14 @@ true true + + make + -j5 + testLinearContainerFactor.run + true + true + true + make -j5 @@ -1345,14 +1353,6 @@ true true - - make - -j5 - testLinearContainerFactor.run - true - true - true - make -j2 @@ -2356,14 +2356,6 @@ true true - - make - -j5 - check.nonlinear_unstable - true - true - true - make -j5 diff --git a/gtsam.h b/gtsam.h index 065435bd5..c4950e4e4 100644 --- a/gtsam.h +++ b/gtsam.h @@ -1590,6 +1590,42 @@ class JointMarginal { void print() const; }; +#include +virtual class LinearContainerFactor : gtsam::NonlinearFactor { + + LinearContainerFactor(gtsam::GaussianFactor* factor, const gtsam::Ordering& ordering, + const gtsam::Values& linearizationPoint); + LinearContainerFactor(gtsam::GaussianFactor* factor, const gtsam::Values& linearizationPoint); + LinearContainerFactor(gtsam::GaussianFactor* factor, const gtsam::InvertedOrdering& ordering, + const gtsam::Values& linearizationPoint); + + LinearContainerFactor(gtsam::GaussianFactor* factor, const gtsam::Ordering& ordering); + LinearContainerFactor(gtsam::GaussianFactor* factor); + LinearContainerFactor(gtsam::GaussianFactor* factor, const gtsam::InvertedOrdering& ordering); + + gtsam::GaussianFactor* factor() const; +// const boost::optional& linearizationPoint() const; + + gtsam::GaussianFactor* order(const gtsam::Ordering& ordering) const; + gtsam::GaussianFactor* negate(const gtsam::Ordering& ordering) const; + gtsam::NonlinearFactor* negate() const; + + bool isJacobian() const; + gtsam::JacobianFactor* toJacobian() const; + gtsam::HessianFactor* toHessian() const; + + static gtsam::NonlinearFactorGraph convertLinearGraph(const gtsam::GaussianFactorGraph& linear_graph, + const gtsam::Ordering& ordering, const gtsam::Values& linearizationPoint); + static gtsam::NonlinearFactorGraph convertLinearGraph(const gtsam::GaussianFactorGraph& linear_graph, + const gtsam::InvertedOrdering& invOrdering, const gtsam::Values& linearizationPoint); + + static gtsam::NonlinearFactorGraph convertLinearGraph(const gtsam::GaussianFactorGraph& linear_graph, + const gtsam::Ordering& ordering); + static gtsam::NonlinearFactorGraph convertLinearGraph(const gtsam::GaussianFactorGraph& linear_graph, + const gtsam::InvertedOrdering& invOrdering); + +}; // \class LinearContainerFactor + //************************************************************************* // Nonlinear optimizers //************************************************************************* diff --git a/gtsam_unstable/nonlinear/LinearContainerFactor.cpp b/gtsam/nonlinear/LinearContainerFactor.cpp similarity index 99% rename from gtsam_unstable/nonlinear/LinearContainerFactor.cpp rename to gtsam/nonlinear/LinearContainerFactor.cpp index fa7813bb9..732c807b3 100644 --- a/gtsam_unstable/nonlinear/LinearContainerFactor.cpp +++ b/gtsam/nonlinear/LinearContainerFactor.cpp @@ -5,7 +5,7 @@ * @author Alex Cunningham */ -#include +#include #include diff --git a/gtsam_unstable/nonlinear/LinearContainerFactor.h b/gtsam/nonlinear/LinearContainerFactor.h similarity index 100% rename from gtsam_unstable/nonlinear/LinearContainerFactor.h rename to gtsam/nonlinear/LinearContainerFactor.h diff --git a/gtsam_unstable/nonlinear/tests/testLinearContainerFactor.cpp b/gtsam/nonlinear/tests/testLinearContainerFactor.cpp similarity index 99% rename from gtsam_unstable/nonlinear/tests/testLinearContainerFactor.cpp rename to gtsam/nonlinear/tests/testLinearContainerFactor.cpp index 9ff2cec12..f25e86f23 100644 --- a/gtsam_unstable/nonlinear/tests/testLinearContainerFactor.cpp +++ b/gtsam/nonlinear/tests/testLinearContainerFactor.cpp @@ -9,7 +9,7 @@ #include -#include +#include #include #include diff --git a/gtsam_unstable/CMakeLists.txt b/gtsam_unstable/CMakeLists.txt index 9d3d0e643..4016caa39 100644 --- a/gtsam_unstable/CMakeLists.txt +++ b/gtsam_unstable/CMakeLists.txt @@ -6,7 +6,7 @@ set (gtsam_unstable_subdirs discrete # linear dynamics - nonlinear +# nonlinear slam ) @@ -36,7 +36,7 @@ set(gtsam_unstable_srcs ${discrete_srcs} ${dynamics_srcs} #${linear_srcs} - ${nonlinear_srcs} + #${nonlinear_srcs} ${slam_srcs} ) diff --git a/gtsam_unstable/gtsam_unstable.h b/gtsam_unstable/gtsam_unstable.h index 3639283f5..9100413fc 100644 --- a/gtsam_unstable/gtsam_unstable.h +++ b/gtsam_unstable/gtsam_unstable.h @@ -177,41 +177,7 @@ virtual class DGroundConstraint : gtsam::NonlinearFactor { //************************************************************************* // nonlinear //************************************************************************* -#include -virtual class LinearContainerFactor : gtsam::NonlinearFactor { - LinearContainerFactor(gtsam::GaussianFactor* factor, const gtsam::Ordering& ordering, - const gtsam::Values& linearizationPoint); - LinearContainerFactor(gtsam::GaussianFactor* factor, const gtsam::Values& linearizationPoint); - LinearContainerFactor(gtsam::GaussianFactor* factor, const gtsam::InvertedOrdering& ordering, - const gtsam::Values& linearizationPoint); - - LinearContainerFactor(gtsam::GaussianFactor* factor, const gtsam::Ordering& ordering); - LinearContainerFactor(gtsam::GaussianFactor* factor); - LinearContainerFactor(gtsam::GaussianFactor* factor, const gtsam::InvertedOrdering& ordering); - - gtsam::GaussianFactor* factor() const; -// const boost::optional& linearizationPoint() const; - - gtsam::GaussianFactor* order(const gtsam::Ordering& ordering) const; - gtsam::GaussianFactor* negate(const gtsam::Ordering& ordering) const; - gtsam::NonlinearFactor* negate() const; - - bool isJacobian() const; - gtsam::JacobianFactor* toJacobian() const; - gtsam::HessianFactor* toHessian() const; - - static gtsam::NonlinearFactorGraph convertLinearGraph(const gtsam::GaussianFactorGraph& linear_graph, - const gtsam::Ordering& ordering, const gtsam::Values& linearizationPoint); - static gtsam::NonlinearFactorGraph convertLinearGraph(const gtsam::GaussianFactorGraph& linear_graph, - const gtsam::InvertedOrdering& invOrdering, const gtsam::Values& linearizationPoint); - - static gtsam::NonlinearFactorGraph convertLinearGraph(const gtsam::GaussianFactorGraph& linear_graph, - const gtsam::Ordering& ordering); - static gtsam::NonlinearFactorGraph convertLinearGraph(const gtsam::GaussianFactorGraph& linear_graph, - const gtsam::InvertedOrdering& invOrdering); - -}; // \class LinearContainerFactor //************************************************************************* // slam diff --git a/gtsam_unstable/nonlinear/LinearizedFactor.cpp b/gtsam_unstable/nonlinear/LinearizedFactor.cpp deleted file mode 100644 index 942cb1f1e..000000000 --- a/gtsam_unstable/nonlinear/LinearizedFactor.cpp +++ /dev/null @@ -1,148 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 NonlinearFactor& other, double tol) const { - const LinearizedFactor* e = dynamic_cast(&other); - if (!e || !Base::equals(other, tol) - || !lin_points_.equals(e->lin_points_, tol) - || !equal_with_abs_tol(b_, e->b_, tol) - || !model_->equals(*e->model_, tol) - || matrices_.size() != e->matrices_.size()) - return false; - - KeyMatrixMap::const_iterator map1 = matrices_.begin(), map2 = e->matrices_.begin(); - for (; map1 != matrices_.end() && map2 != e->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 deleted file mode 100644 index 802765d51..000000000 --- a/gtsam_unstable/nonlinear/LinearizedFactor.h +++ /dev/null @@ -1,125 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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() {} - - /// @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))); } - - /** - * 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 NonlinearFactor& 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/testLinearizedFactor.cpp b/gtsam_unstable/nonlinear/tests/testLinearizedFactor.cpp deleted file mode 100644 index 3eebe2fe7..000000000 --- a/gtsam_unstable/nonlinear/tests/testLinearizedFactor.cpp +++ /dev/null @@ -1,69 +0,0 @@ -/** - * @file testLinearizedFactor - * @author Alex Cunningham - */ - -#include -#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); } -/* ************************************************************************* */