diff --git a/.cproject b/.cproject index c685dd0d1..7f15e2ad7 100644 --- a/.cproject +++ b/.cproject @@ -1,4 +1,4 @@ - + @@ -298,22 +298,6 @@ - -make --j2 -install -true -true -true - - -make --j2 -check -true -true -true - make -k @@ -468,6 +452,7 @@ make + testBayesTree.run true false @@ -475,7 +460,6 @@ make - testSymbolicBayesNet.run true false @@ -483,6 +467,7 @@ make + testSymbolicFactorGraph.run true false @@ -738,6 +723,7 @@ make + testGraph.run true false @@ -751,6 +737,14 @@ true true + +make +-j2 +testTupleConfig.run +true +true +true + make -j2 @@ -775,6 +769,22 @@ true true + +make +-j2 +install +true +true +true + + +make +-j2 +check +true +true +true + diff --git a/cpp/LieConfig-inl.h b/cpp/LieConfig-inl.h index 2f9f36f23..7e19759e5 100644 --- a/cpp/LieConfig-inl.h +++ b/cpp/LieConfig-inl.h @@ -38,22 +38,30 @@ namespace gtsam { } template - const T& LieConfig::at(const Key& key) const { - const_iterator it = values_.find(key); - if (it == values_.end()) throw std::invalid_argument("invalid key: " + (string)key); + const T& LieConfig::at(const J& j) const { + const_iterator it = values_.find(j); + if (it == values_.end()) throw std::invalid_argument("invalid j: " + (string)j); else return it->second; } template - void LieConfig::insert(const Key& name, const T& val) { + void LieConfig::insert(const J& name, const T& val) { values_.insert(make_pair(name, val)); dim_ += dim(val); } template - void LieConfig::erase(const Key& key) { - iterator it = values_.find(key); - if (it == values_.end()) throw std::invalid_argument("invalid key: " + (string)key); + void LieConfig::erase(const J& j) { + size_t dim; // unused + erase(j, dim); + } + + template + void LieConfig::erase(const J& j, size_t& dim) { + iterator it = values_.find(j); + if (it == values_.end()) throw std::invalid_argument("invalid j: " + (string)j); + dim = gtsam::dim(it->second); + dim_ -= dim; values_.erase(it); } @@ -61,13 +69,13 @@ namespace gtsam { template LieConfig expmap(const LieConfig& c, const VectorConfig& delta) { LieConfig newConfig; - typedef pair::Key,T> Value; + typedef pair Value; BOOST_FOREACH(const Value& value, c) { - const typename LieConfig::Key& j = value.first; + const J& j = value.first; const T& pj = value.second; - string key = (string)j; - if (delta.contains(key)) { - const Vector& dj = delta[key]; + string jstr = (string)j; + if (delta.contains(jstr)) { + const Vector& dj = delta[jstr]; newConfig.insert(j, expmap(pj,dj)); } else newConfig.insert(j, pj); @@ -82,9 +90,9 @@ namespace gtsam { throw invalid_argument("Delta vector length does not match config dimensionality."); LieConfig newConfig; int delta_offset = 0; - typedef pair::Key,T> Value; + typedef pair Value; BOOST_FOREACH(const Value& value, c) { - const typename LieConfig::Key& j = value.first; + const J& j = value.first; const T& pj = value.second; int cur_dim = dim(pj); newConfig.insert(j,expmap(pj,sub(delta, delta_offset, delta_offset+cur_dim))); diff --git a/cpp/LieConfig.h b/cpp/LieConfig.h index 09bdd3937..3858204b8 100644 --- a/cpp/LieConfig.h +++ b/cpp/LieConfig.h @@ -48,8 +48,9 @@ namespace gtsam { /** * Typedefs */ - typedef J Key; - typedef std::map Values; + typedef J Key; + typedef T Value; + typedef std::map Values; typedef typename Values::iterator iterator; typedef typename Values::const_iterator const_iterator; @@ -71,17 +72,17 @@ namespace gtsam { /** Test whether configs are identical in keys and values */ bool equals(const LieConfig& expected, double tol=1e-9) const; - /** Retrieve a variable by key, throws std::invalid_argument if not found */ - const T& at(const Key& key) const; + /** Retrieve a variable by j, throws std::invalid_argument if not found */ + const T& at(const J& j) const; /** operator[] syntax for get */ - inline const T& operator[](const Key& key) const { return at(key);} + const T& operator[](const J& j) const { return at(j); } /** Check if a variable exists */ - bool exists(const Key& i) const {return values_.find(i)!=values_.end();} + bool exists(const J& i) const { return values_.find(i)!=values_.end(); } /** The number of variables in this config */ - int size() const { return values_.size(); } + size_t size() const { return values_.size(); } const_iterator begin() const { return values_.begin(); } const_iterator end() const { return values_.end(); } @@ -90,11 +91,16 @@ namespace gtsam { // imperative methods: - /** Add a variable with the given key */ - void insert(const Key& key, const T& val); + /** Add a variable with the given j */ + void insert(const J& j, const T& val); /** Remove a variable from the config */ - void erase(const Key& key) ; + void erase(const J& j); + + /** Remove a variable from the config while returning the dimensionality of + * the removed element (normally not needed by user code). + */ + void erase(const J& j, size_t& dim); /** Replace all keys and variables */ LieConfig& operator=(const LieConfig& rhs) { diff --git a/cpp/Makefile.am b/cpp/Makefile.am index 8490a3e0b..b3705283e 100644 --- a/cpp/Makefile.am +++ b/cpp/Makefile.am @@ -162,7 +162,7 @@ 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 testLieConfig +check_PROGRAMS += testPoint2 testRot2 testPose2 testPoint3 testRot3 testPose3 testCal3_S2 testLieConfig testTupleConfig testPoint2_SOURCES = testPoint2.cpp testRot2_SOURCES = testRot2.cpp testPose2_SOURCES = testPose2.cpp @@ -171,6 +171,7 @@ testRot3_SOURCES = testRot3.cpp testPose3_SOURCES = testPose3.cpp testCal3_S2_SOURCES = testCal3_S2.cpp testLieConfig_SOURCES = testLieConfig.cpp +testTupleConfig_SOURCES = testTupleConfig.cpp testPoint2_LDADD = libgtsam.la testRot2_LDADD = libgtsam.la @@ -180,6 +181,7 @@ testRot3_LDADD = libgtsam.la testPose3_LDADD = libgtsam.la testCal3_S2_LDADD = libgtsam.la testLieConfig_LDADD = libgtsam.la +testTupleConfig_LDADD = libgtsam.la noinst_PROGRAMS = timeRot3 timeRot3_SOURCES = timeRot3.cpp @@ -238,7 +240,7 @@ testSimpleCamera_SOURCES = testSimpleCamera.cpp testSimpleCamera_LDADD = libgtsam.la # Visual SLAM -sources += VSLAMConfig.cpp VSLAMGraph.cpp VSLAMFactor.cpp +sources += VSLAMGraph.cpp VSLAMFactor.cpp check_PROGRAMS += testVSLAMFactor testVSLAMGraph testVSLAMConfig testVSLAMFactor_SOURCES = testVSLAMFactor.cpp testVSLAMFactor_LDADD = libgtsam.la diff --git a/cpp/TupleConfig.h b/cpp/TupleConfig.h new file mode 100644 index 000000000..bd88b3c34 --- /dev/null +++ b/cpp/TupleConfig.h @@ -0,0 +1,134 @@ +/* + * TupleConfig.h + * + * Created on: Jan 13, 2010 + * Author: Richard Roberts and Manohar Paluri + */ + +#include + +#include "LieConfig-inl.h" + +#pragma once + +namespace gtsam { + + /** + * PairConfig: a config that holds two data types. + */ + template + class PairConfig : public Testable > { + public: +// typedef J1 Key1; +// typedef J2 Key2; +// typedef X1 Value1; +// typedef X2 Value2; + typedef LieConfig Config1; + typedef LieConfig Config2; + + private: + LieConfig config1_; + LieConfig config2_; + size_t size_; + size_t dim_; + + PairConfig(const LieConfig& config1, const LieConfig& config2) : + config1_(config1), config2_(config2), + size_(config1.size()+config2.size()), dim_(gtsam::dim(config1)+gtsam::dim(config2)) {} + + public: + + /** + * Default constructor creates an empty config. + */ + PairConfig(): size_(0), dim_(0) {} + + /** + * Copy constructor + */ + PairConfig(const PairConfig& c): + config1_(c.config1_), config2_(c.config2_), size_(c.size_), dim_(c.dim_) {} + + /** + * Print + */ + void print(const std::string& s = "") const { + std::cout << "TupleConfig " << s << ", size " << size_ << "\n"; + config1_.print(s + "Config1: "); + config2_.print(s + "Config2: "); + } + + /** + * Test for equality in keys and values + */ + bool equals(const PairConfig& c, double tol=1e-9) const { + return config1_.equals(c.config1_, tol) && config2_.equals(c.config2_, tol); } + + /** + * operator[] syntax to get a value by j, throws invalid_argument if + * value with specified j is not present. Will generate compile-time + * errors if j type does not match that on which the Config is templated. + */ + const X1& operator[](const J1& j) const { return config1_[j]; } + const X2& operator[](const J2& j) const { return config2_[j]; } + + /** + * size is the total number of variables in this config. + */ + size_t size() const { return size_; } + + /** + * dim is the dimensionality of the tangent space + */ + size_t dim() const { return dim_; } + + private: + template + void insert_helper(Config& config, const Key& j, const Value& value) { + config.insert(j, value); + size_ ++; + dim_ += gtsam::dim(value); + } + + template + void erase_helper(Config& config, const Key& j) { + size_t dim; + config.erase(j, dim); + dim_ -= dim; + size_ --; + } + + public: + /** + * expmap each element + */ + PairConfig expmap(const VectorConfig& delta) { + return PairConfig(gtsam::expmap(config1_, delta), gtsam::expmap(config2_, delta)); } + + /** + * Insert a variable with the given j + */ + void insert(const J1& j, const X1& value) { insert_helper(config1_, j, value); } + void insert(const J2& j, const X2& value) { insert_helper(config2_, j, value); } + + /** + * Remove the variable with the given j. Throws invalid_argument if the + * j is not present in the config. + */ + void erase(const J1& j) { erase_helper(config1_, j); } + void erase(const J2& j) { erase_helper(config2_, j); } + + /** + * Check if a variable exists + */ + bool exists(const J1& j) const { return config1_.exists(j); } + bool exists(const J2& j) const { return config2_.exists(j); } + + + }; + + template + inline PairConfig expmap(PairConfig c, const VectorConfig& delta) { return c.expmap(delta); } + + +} diff --git a/cpp/VSLAMConfig.cpp b/cpp/VSLAMConfig.cpp deleted file mode 100644 index 8fb4a32eb..000000000 --- a/cpp/VSLAMConfig.cpp +++ /dev/null @@ -1,44 +0,0 @@ -/** - * @file VSLAMConfig.cpp - * @brief The Config - * @author Alireza Fathi - * @author Carlos Nieto - */ - -#include - -#include -#include - -#include "VSLAMConfig.h" -#include "LieConfig-inl.h" - -using namespace std; - -namespace gtsam { - - /* ************************************************************************* */ - // Exponential map - VSLAMConfig expmap(const VSLAMConfig& x0, const VectorConfig & delta) { - VSLAMConfig x; - x.poses_ = expmap(x0.poses_, delta); - x.points_ = expmap(x0.points_, delta); - return x; - } - - /* ************************************************************************* */ - void VSLAMConfig::print(const std::string& s) const { - printf("%s:\n", s.c_str()); - poses_.print("Poses"); - points_.print("Points"); - } - - /* ************************************************************************* */ - bool VSLAMConfig::equals(const VSLAMConfig& c, double tol) const { - return poses_.equals(c.poses_, tol) && points_.equals(c.points_, tol); - } - -/* ************************************************************************* */ - -} // namespace gtsam - diff --git a/cpp/VSLAMConfig.h b/cpp/VSLAMConfig.h index 172dcbf4e..8d66e718b 100644 --- a/cpp/VSLAMConfig.h +++ b/cpp/VSLAMConfig.h @@ -5,85 +5,16 @@ * @author Carlos Nieto */ -#include -#include -#include -#include - -#include "Pose3Config.h" +#include "Pose3.h" +#include "Point3.h" +#include "TupleConfig.h" #pragma once namespace gtsam{ -/** - * Config that knows about points and poses - */ -class VSLAMConfig : Testable { - -public: - - typedef Symbol PoseKey; - typedef Symbol PointKey; - -private: - - LieConfig poses_; - LieConfig points_; - -public: - - /** - * default constructor - */ - VSLAMConfig() {} - - /** - * print - */ - void print(const std::string& s = "") const; - - /** - * check whether two configs are equal - */ - bool equals(const VSLAMConfig& c, double tol=1e-6) const; - - /** - * Get Poses or Points - */ - inline const Pose3& operator[](const PoseKey& key) const {return poses_[key];} - inline const Point3& operator[](const PointKey& key) const {return points_[key];} - - // (Awkwardly named) backwards compatibility: - - inline bool cameraPoseExists (const PoseKey& key) const {return poses_.exists(key);} - inline bool landmarkPointExists(const PointKey& key) const {return points_.exists(key);} - - inline Pose3 cameraPose (const PoseKey& key) const {return poses_[key];} - inline Point3 landmarkPoint(const PointKey& key) const {return points_[key];} - - inline size_t size() const {return points_.size() + poses_.size();} - inline size_t dim() const {return gtsam::dim(points_) + gtsam::dim(poses_);} - - // Imperative functions: - - inline void addCameraPose(const PoseKey& key, Pose3 cp) {poses_.insert(key,cp);} - inline void addLandmarkPoint(const PointKey& key, Point3 lp) {points_.insert(key,lp);} - - inline void removeCameraPose(const PoseKey& key) { poses_.erase(key);} - inline void removeLandmarkPose(const PointKey& key) { points_.erase(key);} - - inline void clear() {points_.clear(); poses_.clear();} - - friend VSLAMConfig expmap(const VSLAMConfig& c, const VectorConfig & delta); -}; - - -/** - * Exponential map: takes 6D vectors in VectorConfig - * and applies them to the poses in the VSLAMConfig. - * Needed for use in nonlinear optimization - */ -VSLAMConfig expmap(const VSLAMConfig& c, const VectorConfig & delta); -} // namespace gtsam + typedef Symbol VSLAMPoseKey; + typedef Symbol VSLAMPointKey; + typedef PairConfig VSLAMConfig; +} diff --git a/cpp/VSLAMFactor.h b/cpp/VSLAMFactor.h index 55986e14e..7dda532d7 100644 --- a/cpp/VSLAMFactor.h +++ b/cpp/VSLAMFactor.h @@ -15,8 +15,8 @@ namespace gtsam { - typedef NonlinearFactor2 VSLAMFactorBase; + typedef NonlinearFactor2 VSLAMFactorBase; /** * Non-linear factor for a constraint derived from a 2D measurement, @@ -81,8 +81,8 @@ namespace gtsam { friend class boost::serialization::access; template void serialize(Archive & ar, const unsigned int version) { - ar & BOOST_SERIALIZATION_NVP(key1_); - ar & BOOST_SERIALIZATION_NVP(key2_); + //ar & BOOST_SERIALIZATION_NVP(key1_); + //ar & BOOST_SERIALIZATION_NVP(key2_); ar & BOOST_SERIALIZATION_NVP(z_); ar & BOOST_SERIALIZATION_NVP(K_); } diff --git a/cpp/VSLAMGraph.cpp b/cpp/VSLAMGraph.cpp index ed85c9263..77b1a9691 100644 --- a/cpp/VSLAMGraph.cpp +++ b/cpp/VSLAMGraph.cpp @@ -28,12 +28,12 @@ bool compareLandmark(const std::string& key, const VSLAMConfig& feasible, const VSLAMConfig& input) { int j = atoi(key.substr(1, key.size() - 1).c_str()); - return feasible.landmarkPoint(j).equals(input.landmarkPoint(j)); + return feasible[VSLAMPointKey(j)].equals(input[VSLAMPointKey(j)]); } /* ************************************************************************* */ void VSLAMGraph::addLandmarkConstraint(int j, const gtsam::Point3& p) { - typedef NonlinearEquality NLE; + typedef NonlinearEquality NLE; boost::shared_ptr factor(new NLE(j, p)); push_back(factor); } @@ -43,12 +43,12 @@ bool compareCamera(const std::string& key, const VSLAMConfig& feasible, const VSLAMConfig& input) { int j = atoi(key.substr(1, key.size() - 1).c_str()); - return feasible.cameraPose(j).equals(input.cameraPose(j)); + return feasible[VSLAMPoseKey(j)].equals(input[VSLAMPoseKey(j)]); } /* ************************************************************************* */ void VSLAMGraph::addCameraConstraint(int j, const gtsam::Pose3& p) { - typedef NonlinearEquality NLE; + typedef NonlinearEquality NLE; boost::shared_ptr factor(new NLE(j,p)); push_back(factor); } diff --git a/cpp/testSQP.cpp b/cpp/testSQP.cpp index dcd64c00d..65ec0417f 100644 --- a/cpp/testSQP.cpp +++ b/cpp/testSQP.cpp @@ -493,9 +493,9 @@ TEST (SQP, stereo_truth ) { // create truth config boost::shared_ptr truthConfig(new VSLAMConfig); - truthConfig->addCameraPose(1, camera1.pose()); - truthConfig->addCameraPose(2, camera2.pose()); - truthConfig->addLandmarkPoint(1, landmark); + truthConfig->insert(1, camera1.pose()); + truthConfig->insert(2, camera2.pose()); + truthConfig->insert(1, landmark); // create graph shared_ptr graph(new VSLAMGraph()); @@ -560,15 +560,15 @@ TEST (SQP, stereo_truth_noisy ) { // create truth config boost::shared_ptr truthConfig(new VSLAMConfig); - truthConfig->addCameraPose(1, camera1.pose()); - truthConfig->addCameraPose(2, camera2.pose()); - truthConfig->addLandmarkPoint(1, landmark); + truthConfig->insert(1, camera1.pose()); + truthConfig->insert(2, camera2.pose()); + truthConfig->insert(1, landmark); // create config boost::shared_ptr noisyConfig(new VSLAMConfig); - noisyConfig->addCameraPose(1, camera1.pose()); - noisyConfig->addCameraPose(2, camera2.pose()); - noisyConfig->addLandmarkPoint(1, landmarkNoisy); + noisyConfig->insert(1, camera1.pose()); + noisyConfig->insert(2, camera2.pose()); + noisyConfig->insert(1, landmarkNoisy); // create graph shared_ptr graph(new VSLAMGraph()); @@ -629,8 +629,8 @@ namespace sqp_stereo { // binary constraint between landmarks /** g(x) = x-y = 0 */ Vector g(const VSLAMConfig& config, const list& keys) { - return config.landmarkPoint(getNum(keys.front())).vector() - - config.landmarkPoint(getNum(keys.back())).vector(); + return config[VSLAMPointKey(getNum(keys.front()))].vector() + - config[VSLAMPointKey(getNum(keys.back()))].vector(); } /** jacobian at l1 */ @@ -705,10 +705,10 @@ boost::shared_ptr stereoExampleTruthConfig() { // create config boost::shared_ptr truthConfig(new VSLAMConfig); - truthConfig->addCameraPose(1, camera1.pose()); - truthConfig->addCameraPose(2, camera2.pose()); - truthConfig->addLandmarkPoint(1, landmark1); - truthConfig->addLandmarkPoint(2, landmark2); // create two landmarks in same place + truthConfig->insert(1, camera1.pose()); + truthConfig->insert(2, camera2.pose()); + truthConfig->insert(1, landmark1); + truthConfig->insert(2, landmark2); // create two landmarks in same place return truthConfig; } @@ -763,10 +763,10 @@ TEST (SQP, stereo_sqp_noisy ) { // noisy config boost::shared_ptr initConfig(new VSLAMConfig); - initConfig->addCameraPose(1, pose1); - initConfig->addCameraPose(2, pose2); - initConfig->addLandmarkPoint(1, landmark1); - initConfig->addLandmarkPoint(2, landmark2); // create two landmarks in same place + initConfig->insert(1, pose1); + initConfig->insert(2, pose2); + initConfig->insert(1, landmark1); + initConfig->insert(2, landmark2); // create two landmarks in same place // create ordering Ordering ord; @@ -828,10 +828,10 @@ TEST (SQP, stereo_sqp_noisy_manualLagrange ) { // noisy config boost::shared_ptr initConfig(new VSLAMConfig); - initConfig->addCameraPose(1, pose1); - initConfig->addCameraPose(2, pose2); - initConfig->addLandmarkPoint(1, landmark1); - initConfig->addLandmarkPoint(2, landmark2); // create two landmarks in same place + initConfig->insert(1, pose1); + initConfig->insert(2, pose2); + initConfig->insert(1, landmark1); + initConfig->insert(2, landmark2); // create two landmarks in same place // create ordering with lagrange multiplier included Ordering ord; diff --git a/cpp/testTupleConfig.cpp b/cpp/testTupleConfig.cpp new file mode 100644 index 000000000..26cf94653 --- /dev/null +++ b/cpp/testTupleConfig.cpp @@ -0,0 +1,166 @@ +/* + * testTupleConfig.cpp + * + * Created on: Jan 13, 2010 + * Author: richard + */ + +#include +#include +#include +#include + +#include "TupleConfig.h" +#include "Vector.h" + +using namespace gtsam; +using namespace std; + +typedef Symbol PoseKey; +typedef Symbol PointKey; +typedef PairConfig Config; + +/* ************************************************************************* */ +TEST( PairConfig, insert_equals1 ) +{ + Pose2 x1(1,2,3), x2(6,7,8); + Point2 l1(4,5), l2(9,10); + + Config expected; + expected.insert(1, x1); + expected.insert(2, x2); + expected.insert(1, l1); + expected.insert(2, l2); + + Config actual; + actual.insert(1, x1); + actual.insert(2, x2); + actual.insert(1, l1); + actual.insert(2, l2); + + CHECK(assert_equal(expected,actual)); +} + +/* ************************************************************************* */ +TEST( PairConfig, insert_equals2 ) +{ + Pose2 x1(1,2,3), x2(6,7,8); + Point2 l1(4,5), l2(9,10); + + Config cfg1; + cfg1.insert(1, x1); + cfg1.insert(2, x2); + cfg1.insert(1, l1); + cfg1.insert(2, l2); + + Config cfg2; + cfg2.insert(1, x1); + cfg2.insert(2, x2); + cfg2.insert(1, l1); + + CHECK(!cfg1.equals(cfg2)); + + cfg2.insert(2, Point2(9,11)); + + CHECK(!cfg1.equals(cfg2)); +} + +///* ************************************************************************* */ +//TEST( PairConfig, insert_duplicate ) +//{ +// Pose2 x1(1,2,3), x2(6,7,8); +// Point2 l1(4,5), l2(9,10); +// +// Config cfg1; +// cfg1.insert(1, x1); +// cfg1.insert(2, x2); +// cfg1.insert(1, l1); +// cfg1.insert(2, l2); +// cfg1.insert(2, l1); +// +// CHECK(assert_equal(l2, cfg1[PointKey(2)])); +// CHECK(cfg1.size() == 4); +// CHECK(cfg1.dim() == 10); +//} + + +/* ************************************************************************* */ +TEST( PairConfig, size_dim ) +{ + Pose2 x1(1,2,3), x2(6,7,8); + Point2 l1(4,5), l2(9,10); + + Config cfg1; + cfg1.insert(1, x1); + cfg1.insert(2, x2); + cfg1.insert(1, l1); + cfg1.insert(2, l2); + + CHECK(cfg1.size() == 4); + CHECK(cfg1.dim() == 10); +} + +/* ************************************************************************* */ +TEST(PairConfig, at) +{ + Pose2 x1(1,2,3), x2(6,7,8); + Point2 l1(4,5), l2(9,10); + + Config cfg1; + cfg1.insert(1, x1); + cfg1.insert(2, x2); + cfg1.insert(1, l1); + cfg1.insert(2, l2); + + CHECK(assert_equal(x1, cfg1[PoseKey(1)])); + CHECK(assert_equal(x2, cfg1[PoseKey(2)])); + CHECK(assert_equal(l1, cfg1[PointKey(1)])); + CHECK(assert_equal(l2, cfg1[PointKey(2)])); + + bool caught = false; + try { + cfg1[PoseKey(3)]; + } catch(invalid_argument e) { + caught = true; + } + CHECK(caught); + + caught = false; + try { + cfg1[PointKey(3)]; + } catch(invalid_argument e) { + caught = true; + } + CHECK(caught); +} + +/* ************************************************************************* */ +TEST(PairConfig, expmap) +{ + Pose2 x1(1,2,3), x2(6,7,8); + Point2 l1(4,5), l2(9,10); + + Config cfg1; + cfg1.insert(1, x1); + cfg1.insert(2, x2); + cfg1.insert(1, l1); + cfg1.insert(2, l2); + + VectorConfig increment; + increment.insert("x1", Vector_(3, 1.0, 1.1, 1.2)); + increment.insert("x2", Vector_(3, 1.3, 1.4, 1.5)); + increment.insert("l1", Vector_(2, 1.0, 1.1)); + increment.insert("l2", Vector_(2, 1.3, 1.4)); + + Config expected; + expected.insert(1, expmap(x1, Vector_(3, 1.0, 1.1, 1.2))); + expected.insert(2, expmap(x2, Vector_(3, 1.3, 1.4, 1.5))); + expected.insert(1, Point2(5.0, 6.1)); + expected.insert(2, Point2(10.3, 11.4)); + + CHECK(assert_equal(expected, expmap(cfg1, increment))); +} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +/* ************************************************************************* */ diff --git a/cpp/testVSLAMConfig.cpp b/cpp/testVSLAMConfig.cpp index 0e6a4b60d..8ee7d951b 100644 --- a/cpp/testVSLAMConfig.cpp +++ b/cpp/testVSLAMConfig.cpp @@ -16,12 +16,12 @@ TEST( VSLAMConfig, update_with_large_delta) { // this test ensures that if the update for delta is larger than // the size of the config, it only updates existing variables VSLAMConfig init; - init.addCameraPose(1, Pose3()); - init.addLandmarkPoint(1, Point3(1.0, 2.0, 3.0)); + init.insert(1, Pose3()); + init.insert(1, Point3(1.0, 2.0, 3.0)); VSLAMConfig expected; - expected.addCameraPose(1, Pose3(Rot3(), Point3(0.1, 0.1, 0.1))); - expected.addLandmarkPoint(1, Point3(1.1, 2.1, 3.1)); + expected.insert(1, Pose3(Rot3(), Point3(0.1, 0.1, 0.1))); + expected.insert(1, Point3(1.1, 2.1, 3.1)); VectorConfig delta; delta.insert("x1", Vector_(6, 0.0, 0.0, 0.0, 0.1, 0.1, 0.1)); diff --git a/cpp/testVSLAMFactor.cpp b/cpp/testVSLAMFactor.cpp index c1e3bdd82..aefaa1f1b 100644 --- a/cpp/testVSLAMFactor.cpp +++ b/cpp/testVSLAMFactor.cpp @@ -36,8 +36,8 @@ TEST( VSLAMFactor, error ) // For the following configuration, the factor predicts 320,240 VSLAMConfig config; - Rot3 R;Point3 t1(0,0,-6); Pose3 x1(R,t1); config.addCameraPose(1, x1); - Point3 l1; config.addLandmarkPoint(1, l1); + Rot3 R;Point3 t1(0,0,-6); Pose3 x1(R,t1); config.insert(1, x1); + Point3 l1; config.insert(1, l1); CHECK(assert_equal(Point2(320.,240.),factor->predict(x1,l1))); // Which yields an error of 3^2/2 = 4.5 @@ -65,8 +65,8 @@ TEST( VSLAMFactor, error ) delta.insert("l1",Vector_(3, 1.,2.,3.)); VSLAMConfig actual_config = expmap(config, delta); VSLAMConfig expected_config; - Point3 t2(1,1,-5); Pose3 x2(R,t2); expected_config.addCameraPose(1, x2); - Point3 l2(1,2,3); expected_config.addLandmarkPoint(1, l2); + Point3 t2(1,1,-5); Pose3 x2(R,t2); expected_config.insert(1, x2); + Point3 l2(1,2,3); expected_config.insert(1, l2); CHECK(assert_equal(expected_config,actual_config,1e-9)); } diff --git a/cpp/testVSLAMGraph.cpp b/cpp/testVSLAMGraph.cpp index 72415e20a..52b0b93ed 100644 --- a/cpp/testVSLAMGraph.cpp +++ b/cpp/testVSLAMGraph.cpp @@ -76,12 +76,12 @@ TEST( VSLAMGraph, optimizeLM) // Create an initial configuration corresponding to the ground truth boost::shared_ptr initialEstimate(new VSLAMConfig); - initialEstimate->addCameraPose(1, camera1); - initialEstimate->addCameraPose(2, camera2); - initialEstimate->addLandmarkPoint(1, landmark1); - initialEstimate->addLandmarkPoint(2, landmark2); - initialEstimate->addLandmarkPoint(3, landmark3); - initialEstimate->addLandmarkPoint(4, landmark4); + initialEstimate->insert(1, camera1); + initialEstimate->insert(2, camera2); + initialEstimate->insert(1, landmark1); + initialEstimate->insert(2, landmark2); + initialEstimate->insert(3, landmark3); + initialEstimate->insert(4, landmark4); // Create an ordering of the variables list keys; @@ -119,12 +119,12 @@ TEST( VSLAMGraph, optimizeLM2) // Create an initial configuration corresponding to the ground truth boost::shared_ptr initialEstimate(new VSLAMConfig); - initialEstimate->addCameraPose(1, camera1); - initialEstimate->addCameraPose(2, camera2); - initialEstimate->addLandmarkPoint(1, landmark1); - initialEstimate->addLandmarkPoint(2, landmark2); - initialEstimate->addLandmarkPoint(3, landmark3); - initialEstimate->addLandmarkPoint(4, landmark4); + initialEstimate->insert(1, camera1); + initialEstimate->insert(2, camera2); + initialEstimate->insert(1, landmark1); + initialEstimate->insert(2, landmark2); + initialEstimate->insert(3, landmark3); + initialEstimate->insert(4, landmark4); // Create an ordering of the variables list keys; @@ -163,12 +163,12 @@ TEST( VSLAMGraph, CHECK_ORDERING) // Create an initial configuration corresponding to the ground truth boost::shared_ptr initialEstimate(new VSLAMConfig); - initialEstimate->addCameraPose(1, camera1); - initialEstimate->addCameraPose(2, camera2); - initialEstimate->addLandmarkPoint(1, landmark1); - initialEstimate->addLandmarkPoint(2, landmark2); - initialEstimate->addLandmarkPoint(3, landmark3); - initialEstimate->addLandmarkPoint(4, landmark4); + initialEstimate->insert(1, camera1); + initialEstimate->insert(2, camera2); + initialEstimate->insert(1, landmark1); + initialEstimate->insert(2, landmark2); + initialEstimate->insert(3, landmark3); + initialEstimate->insert(4, landmark4); shared_ptr ordering(new Ordering(graph->getOrdering()));