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

Added some comments with Alex

release/4.3a0
Frank Dellaert 2010-07-01 19:57:20 +00:00
parent 1547597ba4
commit 41735ff9d7
2 changed files with 22 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
cpp/LieConfig-inl.h
View File

@ -2,7 +2,7 @@
* LieConfig.cpp * LieConfig.cpp
* *
* Created on: Jan 8, 2010 * Created on: Jan 8, 2010
* Author: richard * @Author: Richard Roberts
*/ */
#pragma once #pragma once
@ -57,8 +57,7 @@ namespace gtsam {
template<class J, class T> template<class J, class T>
size_t LieConfig<J,T>::dim() const { size_t LieConfig<J,T>::dim() const {
size_t n = 0; size_t n = 0;
typedef pair<J,T> Value; BOOST_FOREACH(const typename Values::value_type& value, values_)
BOOST_FOREACH(const Value& value, values_)
n += gtsam::dim(value.second); n += gtsam::dim(value.second);
return n; return n;
} }
@ -66,8 +65,7 @@ namespace gtsam {
template<class J, class T> template<class J, class T>
VectorConfig LieConfig<J,T>::zero() const { VectorConfig LieConfig<J,T>::zero() const {
VectorConfig z; VectorConfig z;
typedef pair<J,T> Value; BOOST_FOREACH(const typename Values::value_type& value, values_)
BOOST_FOREACH(const Value& value, values_)
z.insert(value.first,gtsam::zero(gtsam::dim(value.second))); z.insert(value.first,gtsam::zero(gtsam::dim(value.second)));
return z; return z;
} }
@ -122,8 +120,8 @@ namespace gtsam {
template<class J, class T> template<class J, class T>
LieConfig<J,T> expmap(const LieConfig<J,T>& c, const VectorConfig& delta) { LieConfig<J,T> expmap(const LieConfig<J,T>& c, const VectorConfig& delta) {
LieConfig<J,T> newConfig; LieConfig<J,T> newConfig;
typedef pair<J,T> Value; typedef pair<J,T> KeyValue;
BOOST_FOREACH(const Value& value, c) { BOOST_FOREACH(const KeyValue& value, c) {
const J& j = value.first; const J& j = value.first;
const T& pj = value.second; const T& pj = value.second;
Symbol jkey = (Symbol)j; Symbol jkey = (Symbol)j;
@ -145,8 +143,8 @@ namespace gtsam {
} }
LieConfig<J,T> newConfig; LieConfig<J,T> newConfig;
int delta_offset = 0; int delta_offset = 0;
typedef pair<J,T> Value; typedef pair<J,T> KeyValue;
BOOST_FOREACH(const Value& value, c) { BOOST_FOREACH(const KeyValue& value, c) {
const J& j = value.first; const J& j = value.first;
const T& pj = value.second; const T& pj = value.second;
int cur_dim = dim(pj); int cur_dim = dim(pj);
@ -161,8 +159,8 @@ namespace gtsam {
template<class J, class T> template<class J, class T>
VectorConfig logmap(const LieConfig<J,T>& c0, const LieConfig<J,T>& cp) { VectorConfig logmap(const LieConfig<J,T>& c0, const LieConfig<J,T>& cp) {
VectorConfig delta; VectorConfig delta;
typedef pair<J,T> Value; typedef pair<J,T> KeyValue;
BOOST_FOREACH(const Value& value, cp) { BOOST_FOREACH(const KeyValue& value, cp) {
if(c0.exists(value.first)) if(c0.exists(value.first))
delta.insert(value.first, delta.insert(value.first,
logmap(c0[value.first], value.second)); logmap(c0[value.first], value.second));

17
cpp/LieConfig.h
View File

@ -2,9 +2,15 @@
* LieConfig.h * LieConfig.h
* *
* Created on: Jan 5, 2010 * Created on: Jan 5, 2010
* Author: Richard Roberts * @Author: Richard Roberts
* *
* A templated config for Lie-group elements * A templated config for Lie-group elements
*
* Detailed story:
* A configuration is a map from keys to values. It is used to specify the value of a bunch
* of variables in a factor graph. A LieConfig is a configuration which can hold variables that
* are elements of Lie groups, not just vectors. It then, as a whole, implements a aggregate type
* which is also a Lie group, and hence supports operations dim, exmap, and logmap.
*/ */
#pragma once #pragma once
@ -25,17 +31,20 @@ namespace gtsam {
/** /**
* Lie type configuration * Lie type configuration
* Takes two template types
* J: a type to look up values in the configuration (need to be sortable)
* T: the type of values being stored in the configuration
*/ */
template<class J, class T> template<class J, class T>
class LieConfig : public Testable<LieConfig<J,T> > { class LieConfig : public Testable<LieConfig<J, T> > {
public: public:
/** /**
* Typedefs * Typedefs
*/ */
typedef J Key; typedef J Key;
typedef T Value; typedef T Value;
typedef std::map<J, T> Values; typedef std::map<J, T> Values;
typedef typename Values::iterator iterator; typedef typename Values::iterator iterator;
typedef typename Values::const_iterator const_iterator; typedef typename Values::const_iterator const_iterator;