12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Added insertSub() to tupleconfig to allow the insertion of a subtype of config at once.

release/4.3a0
Alex Cunningham 2010-03-17 16:24:22 +00:00
parent 1f6990635c
commit 0a48b45b12
2 changed files with 32 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
cpp/TupleConfig.h
View File

@ -67,6 +67,11 @@ namespace gtsam {
second_.insert(config.second_);
}
// insert a subconfig
template<class Cfg>
void insertSub(const Cfg& config) { second_.insertSub(config); }
void insertSub(const Config1& config) { first_.insert(config); }
// erase an element by key
template<class Key>
void erase(const Key& j) { second_.erase(j); }
@ -151,6 +156,9 @@ namespace gtsam {
// insert function for whole configs
void insert(const TupleConfigEnd<Config>& config) {first_.insert(config.first_); }
// insert function for sub configs
void insertSub(const Config& config) {first_.insert(config); }
const Value1& operator[](const Key1& j) const { return first_[j]; }
const Config& config() const { return first_; }

33
cpp/testTupleConfig.cpp
View File

@ -457,18 +457,33 @@ TEST(TupleConfig, insert_config_typedef) {
}
/* ************************************************************************* */
#include "NonlinearFactorGraph-inl.h"
TEST( TupleConfig, graphs_and_factors )
{
typedef TupleConfig3<PoseConfig, PointConfig, LamConfig> ConfigC;
typedef NonlinearFactorGraph<ConfigC> GraphC;
typedef NonlinearFactor1<ConfigC, PoseKey, Pose2> FactorC;
TEST(TupleConfig, partial_insert) {
TupleConfig3<PoseConfig, PointConfig, LamConfig> init, expected;
// test creation
GraphC graph;
ConfigC config;
PoseKey x1(1), x2(2);
PointKey l1(1), l2(2);
LamKey L1(1), L2(2);
Pose2 pose1(1.0, 2.0, 0.3), pose2(3.0, 4.0, 5.0);
Point2 point1(2.0, 3.0), point2(5.0, 6.0);
Vector lam1 = Vector_(1, 2.3), lam2 = Vector_(1, 4.5);
init.insert(x1, pose1);
init.insert(l1, point1);
init.insert(L1, lam1);
PoseConfig cfg1;
cfg1.insert(x2, pose2);
init.insertSub(cfg1);
expected.insert(x1, pose1);
expected.insert(l1, point1);
expected.insert(L1, lam1);
expected.insert(x2, pose2);
CHECK(assert_equal(expected, init));
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */