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

exmap that takes Vector

release/4.3a0
Frank Dellaert 2009-12-11 22:43:34 +00:00
parent 137291b2c9
commit a1e90af90f
3 changed files with 35 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
cpp/VectorConfig.cpp
View File

@ -88,12 +88,11 @@ VectorConfig VectorConfig::operator-(const VectorConfig& b) const {
} }
/* ************************************************************************* */ /* ************************************************************************* */
VectorConfig VectorConfig::exmap(const VectorConfig & delta) const VectorConfig VectorConfig::exmap(const VectorConfig& delta) const
{ {
VectorConfig newConfig; VectorConfig newConfig;
for (const_iterator it = values.begin(); it!=values.end(); it++) { string j; Vector vj;
string j = it->first; FOREACH_PAIR(j, vj, values) {
const Vector &vj = it->second;
if (delta.contains(j)) { if (delta.contains(j)) {
const Vector& dj = delta[j]; const Vector& dj = delta[j];
check_size(j,vj,dj); check_size(j,vj,dj);
@ -105,6 +104,21 @@ VectorConfig VectorConfig::exmap(const VectorConfig & delta) const
return newConfig; return newConfig;
} }
/* ************************************************************************* */
VectorConfig VectorConfig::exmap(const Vector& delta) const
{
VectorConfig newConfig;
size_t i = 0;
string j; Vector vj;
FOREACH_PAIR(j, vj, values) {
size_t mj = vj.size();
Vector dj = sub(delta, i, i+mj);
newConfig.insert(j, vj + dj);
i += mj;
}
return newConfig;
}
/* ************************************************************************* */ /* ************************************************************************* */
const Vector& VectorConfig::get(const std::string& name) const { const Vector& VectorConfig::get(const std::string& name) const {
const_iterator it = values.find(name); const_iterator it = values.find(name);

8
cpp/VectorConfig.h
View File

@ -46,7 +46,13 @@ namespace gtsam {
* Add a delta config, needed for use in NonlinearOptimizer * Add a delta config, needed for use in NonlinearOptimizer
* For VectorConfig, this is just addition. * For VectorConfig, this is just addition.
*/ */
VectorConfig exmap(const VectorConfig & delta) const; VectorConfig exmap(const VectorConfig& delta) const;
/**
* Add a delta vector (not a config)
* Will use the ordering that map uses to loop over vectors
*/
VectorConfig exmap(const Vector& delta) const;
const_iterator begin() const {return values.begin();} const_iterator begin() const {return values.begin();}
const_iterator end() const {return values.end();} const_iterator end() const {return values.end();}

12
cpp/testVectorConfig.cpp
View File

@ -31,7 +31,7 @@ TEST( VectorConfig, equals1 )
expected.insert("a",v); expected.insert("a",v);
VectorConfig actual; VectorConfig actual;
actual.insert("a",v); actual.insert("a",v);
CHECK(actual.equals(expected)); CHECK(assert_equal(expected,actual));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -68,6 +68,14 @@ TEST( VectorConfig, contains)
CHECK(!fg.contains("gholi")); CHECK(!fg.contains("gholi"));
} }
/* ************************************************************************* */
TEST( VectorConfig, exmap)
{
VectorConfig c = createConfig();
Vector v = Vector_(6, 0.0,-1.0, 0.0, 0.0, 1.5, 0.0); // l1, x1, x2
CHECK(assert_equal(c.exmap(c),c.exmap(v)));
}
/* ************************************************************************* */ /* ************************************************************************* */
TEST( VectorConfig, plus) TEST( VectorConfig, plus)
{ {
@ -85,7 +93,7 @@ TEST( VectorConfig, plus)
// functional // functional
VectorConfig actual = fg.exmap(delta); VectorConfig actual = fg.exmap(delta);
CHECK(actual.equals(expected)); CHECK(assert_equal(expected,actual));
} }
/* ************************************************************************* */ /* ************************************************************************* */