diff --git a/slam/LinearApproxFactor.h b/slam/LinearApproxFactor.h new file mode 100644 index 000000000..2d2416824 --- /dev/null +++ b/slam/LinearApproxFactor.h @@ -0,0 +1,112 @@ +/* + * @file LinearApproxFactor.h + * @brief A dummy factor that allows a linear factor to act as a nonlinear factor + * @author Alex Cunningham + */ + +#pragma once + +#include +#include +#include +#include "NonlinearFactor.h" +#include "VectorConfig.h" +#include "Matrix.h" + +namespace gtsam { + +/** + * A dummy factor that takes a linearized factor and inserts it into + * a nonlinear graph. This version uses exactly one type of variable. + */ +template +class LinearApproxFactor : public NonlinearFactor { + +public: + /** base type */ + typedef NonlinearFactor Base; + + /** shared pointer for convenience */ + typedef boost::shared_ptr > shared_ptr; + + /** typedefs for key vectors */ + typedef std::vector KeyVector; + +protected: + /** hold onto the factor itself */ + GaussianFactor::shared_ptr lin_factor_; + + /** linearization points for error calculation */ + Config lin_points_; + + /** keep keys for the factor */ + KeyVector nonlinearKeys_; + + /** + * use this for derived classes with keys that don't copy easily + */ + LinearApproxFactor(size_t dim, const Config& lin_points) + : Base(noiseModel::Unit::Create(dim)), lin_points_(lin_points) {} + +public: + + /** use this constructor when starting with nonlinear keys */ + LinearApproxFactor(GaussianFactor::shared_ptr lin_factor, const Config& 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); + } + } + + virtual ~LinearApproxFactor() {} + + /** Vector of errors, unwhitened ! */ + virtual Vector unwhitenedError(const Config& c) const { + // extract the points in the new config + VectorConfig delta; + BOOST_FOREACH(const Key& key, nonlinearKeys_) { + X newPt = c[key], linPt = lin_points_[key]; + Vector d = logmap(linPt, newPt); + delta.insert(key, d); + } + + return lin_factor_->unweighted_error(delta); + } + + /** + * linearize to a GaussianFactor + * Just returns a copy of the existing factor + * NOTE: copies to avoid actual factor getting destroyed + * during elimination + */ + virtual boost::shared_ptr + linearize(const Config& c) const { + Vector b = lin_factor_->get_b(); + 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))); + } + + return boost::shared_ptr( + new GaussianFactor(terms, b, model)); + } + + /** get access to nonlinear keys */ + KeyVector nonlinearKeys() const { return nonlinearKeys_; } + + /** override print function */ + virtual void print(const std::string& s="") const { + Base::print(s); + lin_factor_->print(); + } + + /** access to b vector of gaussian */ + Vector get_b() const { return lin_factor_->get_b(); } +}; + +} // \namespace gtsam diff --git a/slam/Makefile.am b/slam/Makefile.am index 23dec1188..1994fcccd 100644 --- a/slam/Makefile.am +++ b/slam/Makefile.am @@ -30,6 +30,9 @@ headers += BetweenFactor.h PriorFactor.h # General constraints headers += BetweenConstraint.h BoundingConstraint.h TransformConstraint.h +# Utility factors +headers += LinearApproxFactor.h + # 2D Pose SLAM sources += pose2SLAM.cpp Pose2SLAMOptimizer.cpp dataset.cpp check_PROGRAMS += tests/testPose2Factor tests/testPose2Config tests/testPose2SLAM tests/testPose2Prior diff --git a/tests/Makefile.am b/tests/Makefile.am index 5553dd82c..b5feb1655 100644 --- a/tests/Makefile.am +++ b/tests/Makefile.am @@ -11,7 +11,7 @@ check_PROGRAMS += testNonlinearEquality testNonlinearFactor testNonlinearFactorG check_PROGRAMS += testNonlinearOptimizer testSubgraphPreconditioner check_PROGRAMS += testSymbolicBayesNet testSymbolicFactorGraph testTupleConfig check_PROGRAMS += testNonlinearEqualityConstraint testBoundingConstraint -check_PROGRAMS += testTransformConstraint +check_PROGRAMS += testTransformConstraint testLinearApproxFactor if USE_LDL check_PROGRAMS += testConstraintOptimizer diff --git a/tests/testLinearApproxFactor.cpp b/tests/testLinearApproxFactor.cpp new file mode 100644 index 000000000..a322dd0e6 --- /dev/null +++ b/tests/testLinearApproxFactor.cpp @@ -0,0 +1,43 @@ +/* + * @file testLinearApproxFactor.cpp + * @brief tests for dummy factor that contains a linear factor + * @author Alex Cunningham + */ + +#include +#include +#include // allow assert_equal() for vectors +#include +#include +#include + +using namespace std; +using namespace gtsam; + +typedef LinearApproxFactor ApproxFactor; + +/* ************************************************************************* */ +TEST ( LinearApproxFactor, basic ) { + Symbol key1('x', 1); + Matrix A1 = eye(2); + Vector b = repeat(2, 1.2); + SharedDiagonal model = noiseModel::Unit::Create(2); + GaussianFactor::shared_ptr lin_factor(new GaussianFactor(key1, A1, b, model)); + planarSLAM::Config lin_points; + ApproxFactor f1(lin_factor, lin_points); + + EXPECT(f1.size() == 1); + ApproxFactor::KeyVector expKeyVec; + expKeyVec.push_back(planarSLAM::PointKey(key1.index())); + EXPECT(assert_equal(expKeyVec, f1.nonlinearKeys())); + + planarSLAM::Config config; // doesn't really need to have any data + GaussianFactor::shared_ptr actual = f1.linearize(config); + + // Check the linearization + CHECK(assert_equal(*lin_factor, *actual)); +} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +/* ************************************************************************* */