Changed PairConfig interface so that the internal configs need to be accessed with first() and second(). This is in anticipation of switching PairConfig to TupleConfig2.
Alex Cunningham
parent
2068477e32
commit
5c1c8ee76f
|
|
@ -5,6 +5,8 @@
|
|||
* Author: richard
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "LieConfig-inl.h"
|
||||
|
||||
#include "TupleConfig.h"
|
||||
|
|
@ -21,16 +23,16 @@ namespace gtsam {
|
|||
template<class J1, class X1, class J2, class X2>
|
||||
void PairConfig<J1,X1,J2,X2>::print(const std::string& s) const {
|
||||
std::cout << "TupleConfig " << s << ", size " << size_ << "\n";
|
||||
first.print(s + "Config1: ");
|
||||
second.print(s + "Config2: ");
|
||||
first().print(s + "Config1: ");
|
||||
second().print(s + "Config2: ");
|
||||
}
|
||||
|
||||
template<class J1, class X1, class J2, class X2>
|
||||
void PairConfig<J1,X1,J2,X2>::insert(const PairConfig& config) {
|
||||
for (typename Config1::const_iterator it = config.first.begin(); it!=config.first.end(); it++) {
|
||||
for (typename Config1::const_iterator it = config.first().begin(); it!=config.first().end(); it++) {
|
||||
insert(it->first, it->second);
|
||||
}
|
||||
for (typename Config2::const_iterator it = config.second.begin(); it!=config.second.end(); it++) {
|
||||
for (typename Config2::const_iterator it = config.second().begin(); it!=config.second().end(); it++) {
|
||||
insert(it->first, it->second);
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -12,136 +12,7 @@
|
|||
|
||||
namespace gtsam {
|
||||
|
||||
/**
|
||||
* PairConfig: a config that holds two data types.
|
||||
*/
|
||||
template<class J1, class X1, class J2, class X2>
|
||||
class PairConfig : public Testable<PairConfig<J1, X1, J2, X2> > {
|
||||
|
||||
public:
|
||||
|
||||
// publicly available types
|
||||
typedef LieConfig<J1, X1> Config1;
|
||||
typedef LieConfig<J2, X2> Config2;
|
||||
|
||||
// Two configs in the pair as in std:pair
|
||||
LieConfig<J1, X1> first;
|
||||
LieConfig<J2, X2> second;
|
||||
|
||||
private:
|
||||
|
||||
size_t size_;
|
||||
size_t dim_;
|
||||
|
||||
PairConfig(const LieConfig<J1,X1>& config1, const LieConfig<J2,X2>& config2) :
|
||||
first(config1), second(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<J1, X1, J2, X2>& c):
|
||||
first(c.first), second(c.second), size_(c.size_), dim_(c.dim_) {}
|
||||
|
||||
/**
|
||||
* Print
|
||||
*/
|
||||
void print(const std::string& s = "") const;
|
||||
|
||||
/**
|
||||
* Test for equality in keys and values
|
||||
*/
|
||||
bool equals(const PairConfig<J1, X1, J2, X2>& c, double tol=1e-9) const {
|
||||
return first.equals(c.first, tol) && second.equals(c.second, 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 first[j]; }
|
||||
const X2& operator[](const J2& j) const { return second[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<class Config, class Key, class Value>
|
||||
void insert_helper(Config& config, const Key& j, const Value& value) {
|
||||
config.insert(j, value);
|
||||
size_ ++;
|
||||
dim_ += gtsam::dim(value);
|
||||
}
|
||||
|
||||
template<class Config, class Key>
|
||||
void erase_helper(Config& config, const Key& j) {
|
||||
size_t dim;
|
||||
config.erase(j, dim);
|
||||
dim_ -= dim;
|
||||
size_ --;
|
||||
}
|
||||
|
||||
public:
|
||||
/**
|
||||
* expmap each element
|
||||
*/
|
||||
PairConfig<J1,X1,J2,X2> expmap(const VectorConfig& delta) const {
|
||||
return PairConfig(gtsam::expmap(first, delta), gtsam::expmap(second, delta)); }
|
||||
|
||||
/**
|
||||
* logmap each element
|
||||
*/
|
||||
VectorConfig logmap(const PairConfig<J1,X1,J2,X2>& cp) const {
|
||||
VectorConfig ret(gtsam::logmap(first, cp.first));
|
||||
ret.insert(gtsam::logmap(second, cp.second));
|
||||
return ret;
|
||||
}
|
||||
|
||||
/**
|
||||
* Insert a variable with the given j
|
||||
*/
|
||||
void insert(const J1& j, const X1& value) { insert_helper(first, j, value); }
|
||||
void insert(const J2& j, const X2& value) { insert_helper(second, j, value); }
|
||||
|
||||
void insert(const PairConfig& config);
|
||||
|
||||
/**
|
||||
* 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(first, j); }
|
||||
void erase(const J2& j) { erase_helper(second, j); }
|
||||
|
||||
/**
|
||||
* Check if a variable exists
|
||||
*/
|
||||
bool exists(const J1& j) const { return first.exists(j); }
|
||||
bool exists(const J2& j) const { return second.exists(j); }
|
||||
|
||||
|
||||
};
|
||||
|
||||
template<class J1, class X1, class J2, class X2>
|
||||
inline PairConfig<J1,X1,J2,X2> expmap(const PairConfig<J1,X1,J2,X2> c, const VectorConfig& delta) { return c.expmap(delta); }
|
||||
|
||||
template<class J1, class X1, class J2, class X2>
|
||||
inline VectorConfig logmap(const PairConfig<J1,X1,J2,X2> c0, const PairConfig<J1,X1,J2,X2>& cp) { return c0.logmap(cp); }
|
||||
|
||||
/**
|
||||
/**
|
||||
* Tuple configs to handle subconfigs of LieConfigs
|
||||
*
|
||||
* This uses a recursive structure of config pairs to form a lisp-like
|
||||
|
|
@ -319,6 +190,8 @@ namespace gtsam {
|
|||
TupleConfig2() {}
|
||||
TupleConfig2(const TupleConfig2<Config1, Config2>& config) :
|
||||
TupleConfig<Config1, TupleConfigEnd<Config2> >(config) {}
|
||||
TupleConfig2(const Config1& cfg1, const Config2& cfg2) :
|
||||
TupleConfig<Config1, TupleConfigEnd<Config2> >(cfg1, TupleConfigEnd<Config2>(cfg2)) {}
|
||||
};
|
||||
|
||||
template<class Config1, class Config2, class Config3>
|
||||
|
|
@ -326,6 +199,8 @@ namespace gtsam {
|
|||
TupleConfig3() {}
|
||||
TupleConfig3(const TupleConfig3<Config1, Config2, Config3>& config) :
|
||||
TupleConfig<Config1, TupleConfig<Config2, TupleConfigEnd<Config3> > >(config) {}
|
||||
TupleConfig3(const Config1& cfg1, const Config2& cfg2, const Config3& cfg3) :
|
||||
TupleConfig<Config1, TupleConfig<Config2, TupleConfigEnd<Config3> > >(cfg1, TupleConfig<Config2, TupleConfigEnd<Config3> >(cfg2, TupleConfigEnd<Config3>(cfg3))) {}
|
||||
};
|
||||
|
||||
template<class Config1, class Config2, class Config3, class Config4>
|
||||
|
|
@ -348,4 +223,153 @@ namespace gtsam {
|
|||
TupleConfig6(const TupleConfig6<Config1, Config2, Config3, Config4, Config5, Config6>& config) :
|
||||
TupleConfig<Config1, TupleConfig<Config2, TupleConfig<Config3, TupleConfig<Config4, TupleConfig<Config5, TupleConfigEnd<Config6> > > > > >(config) {}
|
||||
};
|
||||
|
||||
/**
|
||||
* PairConfig: an alias for a pair of configs using TupleConfig2
|
||||
* STILL IN TESTING - will soon replace PairConfig
|
||||
*/
|
||||
// template<class J1, class X1, class J2, class X2>
|
||||
// struct PairConfig : TupleConfig2<LieConfig<J1, X1>, LieConfig<J2, X2> > {
|
||||
// PairConfig() {}
|
||||
// PairConfig(const PairConfig<J1, X1, J2, X2>& config) :
|
||||
// TupleConfig2<LieConfig<J1, X1>, LieConfig<J2, X2> >(config) {}
|
||||
// PairConfig(const LieConfig<J1, X1>& cfg1,const LieConfig<J2, X2>& cfg2) :
|
||||
// TupleConfig2<LieConfig<J1, X1>, LieConfig<J2, X2> >(cfg1, cfg2) {}
|
||||
// };
|
||||
|
||||
/**
|
||||
* PairConfig: a config that holds two data types.
|
||||
*/
|
||||
template<class J1, class X1, class J2, class X2>
|
||||
class PairConfig : public Testable<PairConfig<J1, X1, J2, X2> > {
|
||||
|
||||
public:
|
||||
|
||||
// publicly available types
|
||||
typedef LieConfig<J1, X1> Config1;
|
||||
typedef LieConfig<J2, X2> Config2;
|
||||
|
||||
protected:
|
||||
|
||||
// Two configs in the pair as in std:pair
|
||||
LieConfig<J1, X1> first_;
|
||||
LieConfig<J2, X2> second_;
|
||||
|
||||
private:
|
||||
|
||||
size_t size_;
|
||||
size_t dim_;
|
||||
|
||||
PairConfig(const LieConfig<J1,X1>& config1, const LieConfig<J2,X2>& config2) :
|
||||
first_(config1), second_(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<J1, X1, J2, X2>& c):
|
||||
first_(c.first_), second_(c.second_), size_(c.size_), dim_(c.dim_) {}
|
||||
|
||||
/**
|
||||
* Print
|
||||
*/
|
||||
void print(const std::string& s = "") const;
|
||||
|
||||
/**
|
||||
* Test for equality in keys and values
|
||||
*/
|
||||
bool equals(const PairConfig<J1, X1, J2, X2>& c, double tol=1e-9) const {
|
||||
return first_.equals(c.first_, tol) && second_.equals(c.second_, tol); }
|
||||
|
||||
/** Direct access functions */
|
||||
inline const Config1& first() const { return first_; }
|
||||
inline const Config2& second() const { return second_; }
|
||||
|
||||
/**
|
||||
* 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 first_[j]; }
|
||||
const X2& operator[](const J2& j) const { return second_[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<class Config, class Key, class Value>
|
||||
void insert_helper(Config& config, const Key& j, const Value& value) {
|
||||
config.insert(j, value);
|
||||
size_ ++;
|
||||
dim_ += gtsam::dim(value);
|
||||
}
|
||||
|
||||
template<class Config, class Key>
|
||||
void erase_helper(Config& config, const Key& j) {
|
||||
size_t dim;
|
||||
config.erase(j, dim);
|
||||
dim_ -= dim;
|
||||
size_ --;
|
||||
}
|
||||
|
||||
public:
|
||||
/**
|
||||
* expmap each element
|
||||
*/
|
||||
PairConfig<J1,X1,J2,X2> expmap(const VectorConfig& delta) const {
|
||||
return PairConfig(gtsam::expmap(first_, delta), gtsam::expmap(second_, delta)); }
|
||||
|
||||
/**
|
||||
* logmap each element
|
||||
*/
|
||||
VectorConfig logmap(const PairConfig<J1,X1,J2,X2>& cp) const {
|
||||
VectorConfig ret(gtsam::logmap(first_, cp.first_));
|
||||
ret.insert(gtsam::logmap(second_, cp.second_));
|
||||
return ret;
|
||||
}
|
||||
|
||||
/**
|
||||
* Insert a variable with the given j
|
||||
*/
|
||||
void insert(const J1& j, const X1& value) { insert_helper(first_, j, value); }
|
||||
void insert(const J2& j, const X2& value) { insert_helper(second_, j, value); }
|
||||
|
||||
void insert(const PairConfig& config);
|
||||
|
||||
/**
|
||||
* 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(first_, j); }
|
||||
void erase(const J2& j) { erase_helper(second_, j); }
|
||||
|
||||
/**
|
||||
* Check if a variable exists
|
||||
*/
|
||||
bool exists(const J1& j) const { return first_.exists(j); }
|
||||
bool exists(const J2& j) const { return second_.exists(j); }
|
||||
|
||||
|
||||
};
|
||||
|
||||
template<class J1, class X1, class J2, class X2>
|
||||
inline PairConfig<J1,X1,J2,X2> expmap(const PairConfig<J1,X1,J2,X2> c, const VectorConfig& delta) { return c.expmap(delta); }
|
||||
|
||||
template<class J1, class X1, class J2, class X2>
|
||||
inline VectorConfig logmap(const PairConfig<J1,X1,J2,X2> c0, const PairConfig<J1,X1,J2,X2>& cp) { return c0.logmap(cp); }
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -328,6 +328,14 @@ TEST(TupleConfig, typedefs)
|
|||
TupleConfig6<PoseConfig, PointConfig, LamConfig, Point3Config, Pose3Config, Point3Config2> cfg5;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( TupleConfig, constructor_insert )
|
||||
{
|
||||
PoseConfig cfg1;
|
||||
PointConfig cfg2;
|
||||
LamConfig cfg3;
|
||||
TupleConfig3<PoseConfig, PointConfig, LamConfig> config(cfg1, cfg2, cfg3);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
|
||||
|
|
|
|||
Loading…
Reference in New Issue