135 lines
3.6 KiB
C++
135 lines
3.6 KiB
C++
/*
|
|
* TupleConfig.h
|
|
*
|
|
* Created on: Jan 13, 2010
|
|
* Author: Richard Roberts and Manohar Paluri
|
|
*/
|
|
|
|
#include <iostream>
|
|
|
|
#include "LieConfig-inl.h"
|
|
|
|
#pragma once
|
|
|
|
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:
|
|
// typedef J1 Key1;
|
|
// typedef J2 Key2;
|
|
// typedef X1 Value1;
|
|
// typedef X2 Value2;
|
|
typedef LieConfig<J1, X1> Config1;
|
|
typedef LieConfig<J2, X2> Config2;
|
|
|
|
private:
|
|
LieConfig<J1, X1> config1_;
|
|
LieConfig<J2, X2> config2_;
|
|
size_t size_;
|
|
size_t dim_;
|
|
|
|
PairConfig(const LieConfig<J1,X1>& config1, const LieConfig<J2,X2>& 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<J1, X1, J2, X2>& 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<J1, X1, J2, X2>& 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<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) {
|
|
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<class J1, class X1, class J2, class X2>
|
|
inline PairConfig<J1,X1,J2,X2> expmap(PairConfig<J1,X1,J2,X2> c, const VectorConfig& delta) { return c.expmap(delta); }
|
|
|
|
|
|
}
|