116 lines
3.0 KiB
C++
116 lines
3.0 KiB
C++
/*
|
|
* LieConfig.h
|
|
*
|
|
* Created on: Jan 5, 2010
|
|
* Author: Richard Roberts
|
|
*
|
|
* A templated config for Lie-group elements
|
|
*/
|
|
|
|
#pragma once
|
|
|
|
|
|
#include <map>
|
|
#include <string>
|
|
#include <boost/foreach.hpp>
|
|
#include <utility>
|
|
#include <iostream>
|
|
#include <stdexcept>
|
|
#include <boost/optional.hpp>
|
|
|
|
#include "Testable.h"
|
|
#include "VectorConfig.h"
|
|
#include "Vector.h"
|
|
#include "Lie.h"
|
|
|
|
|
|
namespace gtsam {
|
|
|
|
template<class T> class LieConfig;
|
|
|
|
/** Dimensionality of the tangent space */
|
|
template<class T>
|
|
size_t dim(const LieConfig<T>& c);
|
|
|
|
/** Add a delta config */
|
|
template<class T>
|
|
LieConfig<T> expmap(const LieConfig<T>& c, const VectorConfig& delta);
|
|
|
|
/** Add a delta vector, uses iterator order */
|
|
template<class T>
|
|
LieConfig<T> expmap(const LieConfig<T>& c, const Vector& delta);
|
|
|
|
|
|
template<class T>
|
|
class LieConfig : public Testable<LieConfig<T> > {
|
|
private:
|
|
std::map<std::string, T> values_;
|
|
size_t dim_;
|
|
|
|
public:
|
|
typedef typename std::map<std::string, T>::const_iterator iterator;
|
|
typedef iterator const_iterator;
|
|
|
|
LieConfig() : dim_(0) {}
|
|
LieConfig(const LieConfig& config) :
|
|
values_(config.values_), dim_(dim(config)) {}
|
|
virtual ~LieConfig() {}
|
|
|
|
/** Retrieve a variable by key, throws std::invalid_argument if not found */
|
|
const T& get(std::string& key) const {
|
|
iterator it = values_.find(key);
|
|
if (it == values_.end()) throw std::invalid_argument("invalid key");
|
|
else return it->second;
|
|
}
|
|
|
|
/** Retrieve a variable by key, returns nothing if not found */
|
|
boost::optional<const T&> gettry(std::string& key) const {
|
|
const_iterator it = values_.find(key);
|
|
if (it == values_.end()) return boost::optional<const T&>();
|
|
else return it->second;
|
|
}
|
|
|
|
/** Add a variable with the given key */
|
|
void insert(const std::string& name, const T& val) {
|
|
values_.insert(make_pair(name, val));
|
|
dim_ += dim(val);
|
|
}
|
|
|
|
/** Replace all keys and variables */
|
|
LieConfig& operator=(const LieConfig& rhs) {
|
|
values_ = rhs.values_;
|
|
dim_ = dim(rhs);
|
|
return (*this);
|
|
}
|
|
|
|
/** Remove all variables from the config */
|
|
void clear() {
|
|
values_.clear();
|
|
dim_ = 0;
|
|
}
|
|
|
|
/** The number of variables in this config */
|
|
int size() { return values_.size(); }
|
|
|
|
/** Test whether configs are identical in keys and values */
|
|
bool equals(const LieConfig& expected, double tol=1e-9) const;
|
|
|
|
void print(const std::string &s) const;
|
|
|
|
const_iterator begin() const { return values_.begin(); }
|
|
const_iterator end() const { return values_.end(); }
|
|
iterator begin() { return values_.begin(); }
|
|
iterator end() { return values_.end(); }
|
|
|
|
friend LieConfig<T> expmap<T>(const LieConfig<T>& c, const VectorConfig& delta);
|
|
friend LieConfig<T> expmap<T>(const LieConfig<T>& c, const Vector& delta);
|
|
friend size_t dim<T>(const LieConfig<T>& c);
|
|
|
|
};
|
|
|
|
/** Dimensionality of the tangent space */
|
|
template<class T>
|
|
size_t dim(const LieConfig<T>& c) { return c.dim_; }
|
|
}
|
|
|