diff --git a/gtsam_unstable/gtsam_unstable.h b/gtsam_unstable/gtsam_unstable.h index a459925fd..077d6c4e7 100644 --- a/gtsam_unstable/gtsam_unstable.h +++ b/gtsam_unstable/gtsam_unstable.h @@ -199,4 +199,9 @@ virtual class RelativeElevationFactor: gtsam::NonlinearFactor { void print(string s) const; }; +#include +virtual class DummyFactor : gtsam::NonlinearFactor { + DummyFactor(size_t key1, size_t dim1, size_t key2, size_t dim2); +}; + } //\namespace gtsam diff --git a/gtsam_unstable/slam/DummyFactor.cpp b/gtsam_unstable/slam/DummyFactor.cpp new file mode 100644 index 000000000..c4b973ea5 --- /dev/null +++ b/gtsam_unstable/slam/DummyFactor.cpp @@ -0,0 +1,64 @@ +/** + * @file DummyFactor.cpp + * + * @date Sep 10, 2012 + * @author Alex Cunningham + */ + +#include + +#include + +namespace gtsam { + +/* ************************************************************************* */ +DummyFactor::DummyFactor(const Key& key1, size_t dim1, const Key& key2, size_t dim2) +: NonlinearFactor(key1, key2) +{ + dims_.push_back(dim1); + dims_.push_back(dim2); + if (dim1 > dim2) + rowDim_ = dim1; + else + rowDim_ = dim2; +} + +/* ************************************************************************* */ +void DummyFactor::print(const std::string& s, const KeyFormatter& keyFormatter) const { + std::cout << s << " DummyFactor dim = " << rowDim_ << ", keys = { "; + BOOST_FOREACH(Key key, this->keys()) { std::cout << keyFormatter(key) << " "; } + std::cout << "}" << std::endl; +} + +/* ************************************************************************* */ +bool DummyFactor::equals(const NonlinearFactor& f, double tol) const { + const DummyFactor* e = dynamic_cast(&f); + return e && Base::equals(f, tol) && dims_ == e->dims_ && rowDim_ == e->rowDim_; +} + +/* ************************************************************************* */ +boost::shared_ptr +DummyFactor::linearize(const Values& c, const Ordering& ordering) const { + // Only linearize if the factor is active + if (!this->active(c)) + return boost::shared_ptr(); + + // Fill in terms with zero matrices + std::vector > terms(this->size()); + for(size_t j=0; jsize(); ++j) { + terms[j].first = ordering[this->keys()[j]]; + terms[j].second = zeros(rowDim_, dims_[j]); + } + + noiseModel::Diagonal::shared_ptr model = noiseModel::Unit::Create(rowDim_); + return GaussianFactor::shared_ptr( + new JacobianFactor(terms, zero(rowDim_), model)); +} + +/* ************************************************************************* */ + +} // \namespace gtsam + + + + diff --git a/gtsam_unstable/slam/DummyFactor.h b/gtsam_unstable/slam/DummyFactor.h new file mode 100644 index 000000000..7d3b1a8da --- /dev/null +++ b/gtsam_unstable/slam/DummyFactor.h @@ -0,0 +1,76 @@ +/** + * @file DummyFactor.h + * + * @brief A simple factor that can be used to trick gtsam solvers into believing a graph is connected. + * + * @date Sep 10, 2012 + * @author Alex Cunningham + */ + +#pragma once + +#include + +namespace gtsam { + +class DummyFactor : public NonlinearFactor { +protected: + + // Store the dimensions of the variables and the dimension of the full system + std::vector dims_; + size_t rowDim_; ///< choose dimension for the rows + +public: + + /** Default constructor: don't use directly */ + DummyFactor() : rowDim_(1) { } + + /** standard binary constructor */ + DummyFactor(const Key& key1, size_t dim1, const Key& key2, size_t dim2); + + virtual ~DummyFactor() {} + + // testable + + /** print */ + virtual void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; + + /** Check if two factors are equal */ + virtual bool equals(const NonlinearFactor& f, double tol = 1e-9) const; + + // access + + const std::vector& dims() const { return dims_; } + + // factor interface + + /** + * Calculate the error of the factor - zero for dummy factors + */ + virtual double error(const Values& c) const { return 0.0; } + + /** get the dimension of the factor (number of rows on linearization) */ + virtual size_t dim() const { return rowDim_; } + + /** linearize to a GaussianFactor */ + virtual boost::shared_ptr + linearize(const Values& c, const Ordering& ordering) const; + + /** + * Creates a shared_ptr clone of the factor - needs to be specialized to allow + * for subclasses + * + * By default, throws exception if subclass does not implement the function. + */ + virtual NonlinearFactor::shared_ptr clone() const { + return boost::static_pointer_cast( + NonlinearFactor::shared_ptr(new DummyFactor(*this))); + } + +}; + +} // \namespace gtsam + + + + diff --git a/gtsam_unstable/slam/tests/testDummyFactor.cpp b/gtsam_unstable/slam/tests/testDummyFactor.cpp new file mode 100644 index 000000000..e4b43c496 --- /dev/null +++ b/gtsam_unstable/slam/tests/testDummyFactor.cpp @@ -0,0 +1,58 @@ +/** + * @file testDummyFactor.cpp + * + * @brief A simple dummy nonlinear factor that can be used to enforce connectivity + * without adding any real information + * + * @date Sep 10, 2012 + * @author Alex Cunningham + */ + +#include + +#include + +#include + +using namespace gtsam; + +const double tol = 1e-9; + +/* ************************************************************************* */ +TEST( testDummyFactor, basics ) { + + gtsam::Key key1 = 7, key2 = 9; + size_t dim1 = 3, dim2 = 3; + DummyFactor dummyfactor(key1, dim1, key2, dim2); + + // verify contents + LONGS_EQUAL(2, dummyfactor.size()); + EXPECT_LONGS_EQUAL(key1, dummyfactor.keys()[0]); + EXPECT_LONGS_EQUAL(key2, dummyfactor.keys()[1]); + + LONGS_EQUAL(2, dummyfactor.dims().size()); + EXPECT_LONGS_EQUAL(dim1, dummyfactor.dims()[0]); + EXPECT_LONGS_EQUAL(dim2, dummyfactor.dims()[1]); + + Values c; + c.insert(key1, Point3(1.0, 2.0, 3.0)); + c.insert(key2, Point3(4.0, 5.0, 6.0)); + + // errors - all zeros + DOUBLES_EQUAL(0.0, dummyfactor.error(c), tol); + + Ordering ordering; + ordering += key1, key2; + + // linearization: all zeros + GaussianFactor::shared_ptr actLinearization = dummyfactor.linearize(c, ordering); + CHECK(actLinearization); + noiseModel::Diagonal::shared_ptr model3 = noiseModel::Unit::Create(3); + GaussianFactor::shared_ptr expLinearization(new JacobianFactor( + ordering[key1], zeros(3,3), ordering[key2], zeros(3,3), zero(3), model3)); + EXPECT(assert_equal(*expLinearization, *actLinearization, tol)); +} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +/* ************************************************************************* */