diff --git a/.cproject b/.cproject index b1aba5b3c..bba3082a5 100644 --- a/.cproject +++ b/.cproject @@ -5,15 +5,15 @@ - - - - - - - - - + + + + + + + + + @@ -297,415 +297,425 @@ - - - make - install - true - true - true - - - make - check - true - true - true - - - make - -k - check - true - false - true - - - make - testSimpleCamera.run - true - true - true - - - make - -f local.mk - testCal3_S2.run - true - true - true - - - make - - testVSLAMFactor.run - true - true - true - - - make - testCalibratedCamera.run - true - true - true - - - make - - testGaussianConditional.run - true - true - true - - - make - testPose2.run - true - true - true - - - make - testRot3.run - true - true - true - - - make - - testNonlinearOptimizer.run - true - true - true - - - make - testGaussianFactor.run - true - true - true - - - make - testGaussianFactorGraph.run - true - true - true - - - make - - testNonlinearFactorGraph.run - true - true - true - - - make - testPose3.run - true - true - true - - - make - - testVectorConfig.run - true - true - true - - - make - - testPoint2.run - true - true - true - - - make - testNonlinearFactor.run - true - true - true - - - make - timeGaussianFactor.run - true - true - true - - - make - timeGaussianFactorGraph.run - true - true - true - - - make - testGaussianBayesNet.run - true - true - true - - - make - - testBayesTree.run - true - false - true - - - make - testSymbolicBayesNet.run - true - false - true - - - make - - testSymbolicFactorGraph.run - true - false - true - - - make - testVector.run - true - true - true - - - make - testMatrix.run - true - true - true - - - make - - testVSLAMGraph.run - true - true - true - - - make - - testNonlinearEquality.run - true - true - true - - - make - testSQP.run - true - true - true - - - make - - testNonlinearConstraint.run - true - true - true - - - make - testSQPOptimizer.run - true - true - true - - - make - testVSLAMConfig.run - true - true - true - - - make - testControlConfig.run - true - true - true - - - make - testControlPoint.run - true - true - true - - - make - - testControlGraph.run - true - true - true - - - make - testOrdering.run - true - true - true - - - make - - testPose2Constraint.run - true - true - true - - - make - testRot2.run - true - true - true - - - make - testGaussianBayesTree.run - true - true - true - - - make - - testPose3Factor.run - true - true - true - - - make - - testUrbanGraph.run - true - true - true - - - make - - testUrbanConfig.run - true - true - true - - - make - - testUrbanMeasurement.run - true - true - true - - - make - testUrbanOdometry.run - true - true - true - - - make - - testIterative.run - true - true - true - - - make - - testGaussianISAM2.run - true - true - true - - - make - testSubgraphPreconditioner.run - true - true - true - - - make - testBayesNetPreconditioner.run - true - true - true - - - make - - testPose2Factor.run - true - true - true - - - make - - timeRot3.run - true - true - true - - - make - - install - true - true - true - - - make - - install - true - true - true - - - make - - clean - true - true - true - - - make - - check - true - true - true - - - - + + +make + +install +true +true +true + + +make + +check +true +true +true + + +make +-k +check +true +false +true + + +make + +testSimpleCamera.run +true +true +true + + +make +-f local.mk +testCal3_S2.run +true +true +true + + +make +testVSLAMFactor.run +true +true +true + + +make + +testCalibratedCamera.run +true +true +true + + +make +testGaussianConditional.run +true +true +true + + +make + +testPose2.run +true +true +true + + +make + +testRot3.run +true +true +true + + +make +testNonlinearOptimizer.run +true +true +true + + +make + +testGaussianFactor.run +true +true +true + + +make + +testGaussianFactorGraph.run +true +true +true + + +make +testNonlinearFactorGraph.run +true +true +true + + +make + +testPose3.run +true +true +true + + +make +testVectorConfig.run +true +true +true + + +make +testPoint2.run +true +true +true + + +make + +testNonlinearFactor.run +true +true +true + + +make + +timeGaussianFactor.run +true +true +true + + +make + +timeGaussianFactorGraph.run +true +true +true + + +make + +testGaussianBayesNet.run +true +true +true + + +make +testBayesTree.run +true +false +true + + +make + +testSymbolicBayesNet.run +true +false +true + + +make +testSymbolicFactorGraph.run +true +false +true + + +make + +testVector.run +true +true +true + + +make + +testMatrix.run +true +true +true + + +make +testVSLAMGraph.run +true +true +true + + +make +testNonlinearEquality.run +true +true +true + + +make + +testSQP.run +true +true +true + + +make +testNonlinearConstraint.run +true +true +true + + +make + +testSQPOptimizer.run +true +true +true + + +make + +testVSLAMConfig.run +true +true +true + + +make + +testControlConfig.run +true +true +true + + +make + +testControlPoint.run +true +true +true + + +make +testControlGraph.run +true +true +true + + +make + +testOrdering.run +true +true +true + + +make +testPose2Constraint.run +true +true +true + + +make + +testRot2.run +true +true +true + + +make + +testGaussianBayesTree.run +true +true +true + + +make +testPose3Factor.run +true +true +true + + +make +testUrbanGraph.run +true +true +true + + +make +testUrbanConfig.run +true +true +true + + +make +testUrbanMeasurement.run +true +true +true + + +make + +testUrbanOdometry.run +true +true +true + + +make +testIterative.run +true +true +true + + +make +testGaussianISAM2.run +true +true +true + + +make + +testSubgraphPreconditioner.run +true +true +true + + +make + +testBayesNetPreconditioner.run +true +true +true + + +make +testPose2Factor.run +true +true +true + + +make +timeRot3.run +true +true +true + + +make +install +true +true +true + + +make +-j2 +testLieConfig.run +true +true +true + + +make +install +true +true +true + + +make +clean +true +true +true + + +make +check +true +true +true + + + + diff --git a/cpp/Lie.h b/cpp/Lie.h index c6d5cd9d9..965bcf7dc 100644 --- a/cpp/Lie.h +++ b/cpp/Lie.h @@ -8,10 +8,8 @@ #ifndef LIE_H_ #define LIE_H_ +#include #include "Vector.h" -#include "VectorConfig.h" -#include "Matrix.h" - namespace gtsam { @@ -61,6 +59,25 @@ namespace gtsam { Vector logmap(const T& lp) const; }; + + /** Call print on the object */ + template + inline void print_(const T& object, const char *s = "") { + object.print(s); + } + + /** Call equal on the object */ + template + inline bool equal(const T& obj1, const T& obj2, double tol) { + return obj1.equals(obj2, tol); + } + + /** Call equal on the object without tolerance (use default tolerance) */ + template + inline bool equal(const T& obj1, const T& obj2) { + return obj1.equal(obj2); + } + } diff --git a/cpp/LieConfig-inl.h b/cpp/LieConfig-inl.h new file mode 100644 index 000000000..c4913c77a --- /dev/null +++ b/cpp/LieConfig-inl.h @@ -0,0 +1,68 @@ +/* + * LieConfig.cpp + * + * Created on: Jan 8, 2010 + * Author: richard + */ + +#include +#include +#include "LieConfig.h" + +using namespace std; + +#define FOREACH_PAIR( KEY, VAL, COL) BOOST_FOREACH (boost::tie(KEY,VAL),COL) + +namespace gtsam { + + template + void LieConfig::print(const std::string &s) const { + std::cout << "LieConfig " << s << ", size " << values_.size() << "\n"; + std::pair v; + BOOST_FOREACH(v, values_) + ::gtsam::print(v.second, v.first + ": "); + } + + template + bool LieConfig::equals(const LieConfig& expected, double tol) const { + if (values_.size() != expected.values_.size()) return false; + std::pair v; + BOOST_FOREACH(v, values_) { + boost::optional expval = expected.gettry(v.first); + if(!expval || !::gtsam::equal(v.second, *expval, tol)) + return false; + } + return true; + } + + template + LieConfig expmap(const LieConfig& c, const VectorConfig& delta) { + LieConfig newConfig; + std::string j; T pj; + FOREACH_PAIR(j, pj, c.values_) { + if (delta.contains(j)) { + const Vector& dj = delta[j]; + //check_size(j,vj,dj); + newConfig.insert(j, expmap(pj,dj)); + } else + newConfig.insert(j, pj); + } + return newConfig; + } + + template + LieConfig expmap(const LieConfig& c, const Vector& delta) { + LieConfig newConfig; + std::pair value; + int delta_offset = 0; + BOOST_FOREACH(value, c) { + int cur_dim = dim(value.second); + newConfig.insert(value.first, + expmap(value.second, + sub(delta, delta_offset, delta_offset+cur_dim))); + delta_offset += cur_dim; + } + return newConfig; + } + +} diff --git a/cpp/LieConfig.h b/cpp/LieConfig.h new file mode 100644 index 000000000..3583a3135 --- /dev/null +++ b/cpp/LieConfig.h @@ -0,0 +1,115 @@ +/* + * LieConfig.h + * + * Created on: Jan 5, 2010 + * Author: Richard Roberts + * + * A templated config for Lie-group elements + */ + +#pragma once + + +#include +#include +#include +#include +#include +#include +#include + +#include "Testable.h" +#include "VectorConfig.h" +#include "Vector.h" +#include "Lie.h" + + +namespace gtsam { + + template class LieConfig; + + /** Dimensionality of the tangent space */ + template + size_t dim(const LieConfig& c); + + /** Add a delta config */ + template + LieConfig expmap(const LieConfig& c, const VectorConfig& delta); + + /** Add a delta vector, uses iterator order */ + template + LieConfig expmap(const LieConfig& c, const Vector& delta); + + + template + class LieConfig : public Testable > { + private: + std::map values_; + size_t dim_; + + public: + typedef typename std::map::const_iterator iterator; + typedef iterator const_iterator; + + LieConfig() : dim_(0) {} + LieConfig(const LieConfig& config) : + values_(config.values_), dim_(dim(config)) {} + virtual ~LieConfig() {} + + /** Retrieve a variable by key, throws std::invalid_argument if not found */ + const T& get(std::string& key) const { + iterator it = values_.find(key); + if (it == values_.end()) throw std::invalid_argument("invalid key"); + else return it->second; + } + + /** Retrieve a variable by key, returns nothing if not found */ + boost::optional gettry(std::string& key) const { + const_iterator it = values_.find(key); + if (it == values_.end()) return boost::optional(); + else return it->second; + } + + /** Add a variable with the given key */ + void insert(const std::string& name, const T& val) { + values_.insert(make_pair(name, val)); + dim_ += dim(val); + } + + /** Replace all keys and variables */ + LieConfig& operator=(const LieConfig& rhs) { + values_ = rhs.values_; + dim_ = dim(rhs); + return (*this); + } + + /** Remove all variables from the config */ + void clear() { + values_.clear(); + dim_ = 0; + } + + /** The number of variables in this config */ + int size() { return values_.size(); } + + /** Test whether configs are identical in keys and values */ + bool equals(const LieConfig& expected, double tol=1e-9) const; + + void print(const std::string &s) const; + + const_iterator begin() const { return values_.begin(); } + const_iterator end() const { return values_.end(); } + iterator begin() { return values_.begin(); } + iterator end() { return values_.end(); } + + friend LieConfig expmap(const LieConfig& c, const VectorConfig& delta); + friend LieConfig expmap(const LieConfig& c, const Vector& delta); + friend size_t dim(const LieConfig& c); + + }; + + /** Dimensionality of the tangent space */ + template + size_t dim(const LieConfig& c) { return c.dim_; } +} + diff --git a/cpp/Makefile.am b/cpp/Makefile.am index e907b0325..58fdacdc0 100644 --- a/cpp/Makefile.am +++ b/cpp/Makefile.am @@ -159,14 +159,15 @@ testSQPOptimizer_LDADD = libgtsam.la # geometry headers += Lie.h Lie-inl.h sources += Point2.cpp Rot2.cpp Pose2.cpp Point3.cpp Rot3.cpp Pose3.cpp Cal3_S2.cpp -check_PROGRAMS += testPoint2 testRot2 testPose2 testPoint3 testRot3 testPose3 testCal3_S2 +check_PROGRAMS += testPoint2 testRot2 testPose2 testPoint3 testRot3 testPose3 testCal3_S2 testLieConfig testPoint2_SOURCES = testPoint2.cpp testRot2_SOURCES = testRot2.cpp testPose2_SOURCES = testPose2.cpp testPoint3_SOURCES = testPoint3.cpp testRot3_SOURCES = testRot3.cpp testPose3_SOURCES = testPose3.cpp -testCal3_S2_SOURCES = testCal3_S2.cpp +testCal3_S2_SOURCES = testCal3_S2.cpp +testLieConfig_SOURCES = testLieConfig.cpp testPoint2_LDADD = libgtsam.la testRot2_LDADD = libgtsam.la @@ -175,6 +176,7 @@ testPoint3_LDADD = libgtsam.la testRot3_LDADD = libgtsam.la testPose3_LDADD = libgtsam.la testCal3_S2_LDADD = libgtsam.la +testLieConfig_LDADD = libgtsam.la noinst_PROGRAMS = timeRot3 timeRot3_SOURCES = timeRot3.cpp diff --git a/cpp/Point2.h b/cpp/Point2.h index 55431c08b..931dc2dbf 100644 --- a/cpp/Point2.h +++ b/cpp/Point2.h @@ -8,6 +8,7 @@ #include #include "Vector.h" +#include "Matrix.h" #include "Testable.h" #include "Lie.h" diff --git a/cpp/Pose2.h b/cpp/Pose2.h index 6e07cd21e..9fff2e885 100644 --- a/cpp/Pose2.h +++ b/cpp/Pose2.h @@ -81,6 +81,8 @@ namespace gtsam { /** Return the x,y,theta of this pose */ inline Vector logmap(const Pose2& p) { return Vector_(3, p.x(), p.y(), p.theta()); } + /** print using member print function, currently used by LieConfig */ + inline void print(const Pose2& obj, const std::string& str = "") { obj.print(str); } /** Return point coordinates in pose coordinate frame */ inline Point2 transform_to(const Pose2& pose, const Point2& point) { diff --git a/cpp/Vector.h b/cpp/Vector.h index df7657cbb..21dc2e00e 100644 --- a/cpp/Vector.h +++ b/cpp/Vector.h @@ -19,6 +19,8 @@ typedef boost::numeric::ublas::vector Vector; #endif +#include "Lie.h" + namespace gtsam { /** @@ -83,7 +85,7 @@ bool zero(const Vector& v); /** * dimensionality == size */ -inline size_t dim(const Vector& v) { return v.size();} +inline size_t dim(const Vector& v) { return v.size(); } /** * print with optional string @@ -107,6 +109,20 @@ bool greaterThanOrEqual(const Vector& v1, const Vector& v2); */ bool equal_with_abs_tol(const Vector& vec1, const Vector& vec2, double tol=1e-9); +/** + * Override of equal in Lie.h + */ +inline bool equal(const Vector& vec1, const Vector& vec2, double tol) { + return equal_with_abs_tol(vec1, vec2, tol); +} + +/** + * Override of equal in Lie.h + */ +inline bool equal(const Vector& vec1, const Vector& vec2) { + return equal_with_abs_tol(vec1, vec2); +} + /** * Same, prints if error * @param vec1 Vector @@ -193,6 +209,21 @@ Vector concatVectors(size_t nrVectors, ...); */ Vector rand_vector_norm(size_t dim, double mean = 0, double sigma = 1); +/** + * Exponential map, just returns the vector itself + */ +template<> inline Vector expmap(const Vector& v) { return v; } + +/** + * Compose, adds vectors + */ +inline Vector compose(const Vector& v0, const Vector& v1) { return v0+v1; } + +/** + * Inverse, negates the vector + */ +inline Vector inverse(const Vector& v) { return -v; } + } // namespace gtsam static boost::minstd_rand generator(42u); diff --git a/cpp/testLieConfig.cpp b/cpp/testLieConfig.cpp new file mode 100644 index 000000000..91d1017a2 --- /dev/null +++ b/cpp/testLieConfig.cpp @@ -0,0 +1,122 @@ +/* + * testLieConfig.cpp + * + * Created on: Jan 5, 2010 + * Author: richard + */ + +#include +#include +#include + +#include "LieConfig-inl.h" +#include "Vector.h" + +using namespace gtsam; +using namespace std; + +/* ************************************************************************* */ +TEST( LieConfig, equals1 ) +{ + LieConfig expected; + Vector v = Vector_(3, 5.0, 6.0, 7.0); + expected.insert("a",v); + LieConfig actual; + actual.insert("a",v); + CHECK(assert_equal(expected,actual)); +} + +/* ************************************************************************* */ +TEST( LieConfig, equals2 ) +{ + LieConfig cfg1, cfg2; + Vector v1 = Vector_(3, 5.0, 6.0, 7.0); + Vector v2 = Vector_(3, 5.0, 6.0, 8.0); + cfg1.insert("x", v1); + cfg2.insert("x", v2); + CHECK(!cfg1.equals(cfg2)); + CHECK(!cfg2.equals(cfg1)); +} + +/* ************************************************************************* */ +TEST( LieConfig, equals_nan ) +{ + LieConfig cfg1, cfg2; + Vector v1 = Vector_(3, 5.0, 6.0, 7.0); + Vector v2 = Vector_(3, 0.0/0.0, 0.0/0.0, 0.0/0.0); + cfg1.insert("x", v1); + cfg2.insert("x", v2); + CHECK(!cfg1.equals(cfg2)); + CHECK(!cfg2.equals(cfg1)); +} + +/* ************************************************************************* */ +TEST(LieConfig, expmap_a) +{ + LieConfig config0; + config0.insert("v1", Vector_(3, 1.0, 2.0, 3.0)); + config0.insert("v2", Vector_(3, 5.0, 6.0, 7.0)); + + VectorConfig increment; + increment.insert("v1", Vector_(3, 1.0, 1.1, 1.2)); + increment.insert("v2", Vector_(3, 1.3, 1.4, 1.5)); + + LieConfig expected; + expected.insert("v1", Vector_(3, 2.0, 3.1, 4.2)); + expected.insert("v2", Vector_(3, 6.3, 7.4, 8.5)); + + CHECK(assert_equal(expected, expmap(config0, increment))); +} + +/* ************************************************************************* */ +TEST(LieConfig, expmap_b) +{ + LieConfig config0; + config0.insert("v1", Vector_(3, 1.0, 2.0, 3.0)); + config0.insert("v2", Vector_(3, 5.0, 6.0, 7.0)); + + VectorConfig increment; + increment.insert("v2", Vector_(3, 1.3, 1.4, 1.5)); + + LieConfig expected; + expected.insert("v1", Vector_(3, 1.0, 2.0, 3.0)); + expected.insert("v2", Vector_(3, 6.3, 7.4, 8.5)); + + CHECK(assert_equal(expected, expmap(config0, increment))); +} + +/* ************************************************************************* */ +TEST(LieConfig, expmap_c) +{ + LieConfig config0; + config0.insert("v1", Vector_(3, 1.0, 2.0, 3.0)); + config0.insert("v2", Vector_(3, 5.0, 6.0, 7.0)); + + Vector increment = Vector_(6, + 1.0, 1.1, 1.2, + 1.3, 1.4, 1.5); + + LieConfig expected; + expected.insert("v1", Vector_(3, 2.0, 3.1, 4.2)); + expected.insert("v2", Vector_(3, 6.3, 7.4, 8.5)); + + CHECK(assert_equal(expected, expmap(config0, increment))); +} + +/* ************************************************************************* */ +TEST(LieConfig, expmap_d) +{ + LieConfig config0; + config0.insert("v1", Vector_(3, 1.0, 2.0, 3.0)); + config0.insert("v2", Vector_(3, 5.0, 6.0, 7.0)); + config0.print("config0"); + + LieConfig poseconfig; + poseconfig.insert("p1", Pose2(1,2,3)); + poseconfig.insert("p2", Pose2(0.3, 0.4, 0.5)); + poseconfig.print("poseconfig"); +} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +/* ************************************************************************* */