Minimized headers included in Lie.h, removed print from testLieConfig
parent
8d84078036
commit
33ba34a1f6
|
@ -5,9 +5,15 @@
|
||||||
* Author: richard
|
* Author: richard
|
||||||
*/
|
*/
|
||||||
|
|
||||||
|
#include "LieConfig.h"
|
||||||
|
|
||||||
#include <boost/foreach.hpp>
|
#include <boost/foreach.hpp>
|
||||||
#include <boost/tuple/tuple.hpp>
|
#include <boost/tuple/tuple.hpp>
|
||||||
#include "LieConfig.h"
|
#include <utility>
|
||||||
|
#include <iostream>
|
||||||
|
#include <stdexcept>
|
||||||
|
|
||||||
|
#include "VectorConfig.h"
|
||||||
|
|
||||||
using namespace std;
|
using namespace std;
|
||||||
|
|
||||||
|
@ -35,6 +41,26 @@ namespace gtsam {
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
template<class T>
|
||||||
|
const T& LieConfig<T>::get(const std::string& key) const {
|
||||||
|
iterator it = values_.find(key);
|
||||||
|
if (it == values_.end()) throw std::invalid_argument("invalid key");
|
||||||
|
else return it->second;
|
||||||
|
}
|
||||||
|
|
||||||
|
template<class T>
|
||||||
|
boost::optional<const T&> LieConfig<T>::gettry(const std::string& key) const {
|
||||||
|
const_iterator it = values_.find(key);
|
||||||
|
if (it == values_.end()) return boost::optional<const T&>();
|
||||||
|
else return it->second;
|
||||||
|
}
|
||||||
|
|
||||||
|
template<class T>
|
||||||
|
void LieConfig<T>::insert(const std::string& name, const T& val) {
|
||||||
|
values_.insert(make_pair(name, val));
|
||||||
|
dim_ += dim(val);
|
||||||
|
}
|
||||||
|
|
||||||
template<class T>
|
template<class T>
|
||||||
LieConfig<T> expmap(const LieConfig<T>& c, const VectorConfig& delta) {
|
LieConfig<T> expmap(const LieConfig<T>& c, const VectorConfig& delta) {
|
||||||
LieConfig<T> newConfig;
|
LieConfig<T> newConfig;
|
||||||
|
|
|
@ -11,18 +11,13 @@
|
||||||
|
|
||||||
|
|
||||||
#include <map>
|
#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 "Vector.h"
|
||||||
|
#include "Testable.h"
|
||||||
#include "Lie.h"
|
#include "Lie.h"
|
||||||
|
|
||||||
|
namespace boost { template<class T> class optional; }
|
||||||
|
namespace gtsam { class VectorConfig; }
|
||||||
|
|
||||||
namespace gtsam {
|
namespace gtsam {
|
||||||
|
|
||||||
|
@ -57,11 +52,7 @@ namespace gtsam {
|
||||||
virtual ~LieConfig() {}
|
virtual ~LieConfig() {}
|
||||||
|
|
||||||
/** Retrieve a variable by key, throws std::invalid_argument if not found */
|
/** Retrieve a variable by key, throws std::invalid_argument if not found */
|
||||||
const T& get(const std::string& key) const {
|
const T& get(const std::string& key) const;
|
||||||
iterator it = values_.find(key);
|
|
||||||
if (it == values_.end()) throw std::invalid_argument("invalid key");
|
|
||||||
else return it->second;
|
|
||||||
}
|
|
||||||
|
|
||||||
/** operator[] syntax for get */
|
/** operator[] syntax for get */
|
||||||
inline const T& operator[](const std::string& name) const {
|
inline const T& operator[](const std::string& name) const {
|
||||||
|
@ -69,17 +60,10 @@ namespace gtsam {
|
||||||
}
|
}
|
||||||
|
|
||||||
/** Retrieve a variable by key, returns nothing if not found */
|
/** Retrieve a variable by key, returns nothing if not found */
|
||||||
boost::optional<const T&> gettry(const std::string& key) const {
|
boost::optional<const T&> gettry(const 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 */
|
/** Add a variable with the given key */
|
||||||
void insert(const std::string& name, const T& val) {
|
void insert(const std::string& name, const T& val);
|
||||||
values_.insert(make_pair(name, val));
|
|
||||||
dim_ += dim(val);
|
|
||||||
}
|
|
||||||
|
|
||||||
/** Replace all keys and variables */
|
/** Replace all keys and variables */
|
||||||
LieConfig& operator=(const LieConfig& rhs) {
|
LieConfig& operator=(const LieConfig& rhs) {
|
||||||
|
|
|
@ -109,12 +109,12 @@ TEST(LieConfig, expmap_d)
|
||||||
LieConfig<Vector> config0;
|
LieConfig<Vector> config0;
|
||||||
config0.insert("v1", Vector_(3, 1.0, 2.0, 3.0));
|
config0.insert("v1", Vector_(3, 1.0, 2.0, 3.0));
|
||||||
config0.insert("v2", Vector_(3, 5.0, 6.0, 7.0));
|
config0.insert("v2", Vector_(3, 5.0, 6.0, 7.0));
|
||||||
config0.print("config0");
|
//config0.print("config0");
|
||||||
|
|
||||||
LieConfig<Pose2> poseconfig;
|
LieConfig<Pose2> poseconfig;
|
||||||
poseconfig.insert("p1", Pose2(1,2,3));
|
poseconfig.insert("p1", Pose2(1,2,3));
|
||||||
poseconfig.insert("p2", Pose2(0.3, 0.4, 0.5));
|
poseconfig.insert("p2", Pose2(0.3, 0.4, 0.5));
|
||||||
poseconfig.print("poseconfig");
|
//poseconfig.print("poseconfig");
|
||||||
}
|
}
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
|
|
Loading…
Reference in New Issue