From 41735ff9d76fa06b258d671cf36b590e6c9e9c8e Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 1 Jul 2010 19:57:20 +0000 Subject: [PATCH] Added some comments with Alex --- cpp/LieConfig-inl.h | 20 +++++++++----------- cpp/LieConfig.h | 17 +++++++++++++---- 2 files changed, 22 insertions(+), 15 deletions(-) diff --git a/cpp/LieConfig-inl.h b/cpp/LieConfig-inl.h index 6bea2d98b..2398d0762 100644 --- a/cpp/LieConfig-inl.h +++ b/cpp/LieConfig-inl.h @@ -2,7 +2,7 @@ * LieConfig.cpp * * Created on: Jan 8, 2010 - * Author: richard + * @Author: Richard Roberts */ #pragma once @@ -57,8 +57,7 @@ namespace gtsam { template size_t LieConfig::dim() const { size_t n = 0; - typedef pair Value; - BOOST_FOREACH(const Value& value, values_) + BOOST_FOREACH(const typename Values::value_type& value, values_) n += gtsam::dim(value.second); return n; } @@ -66,8 +65,7 @@ namespace gtsam { template VectorConfig LieConfig::zero() const { VectorConfig z; - typedef pair Value; - BOOST_FOREACH(const Value& value, values_) + BOOST_FOREACH(const typename Values::value_type& value, values_) z.insert(value.first,gtsam::zero(gtsam::dim(value.second))); return z; } @@ -122,8 +120,8 @@ namespace gtsam { template LieConfig expmap(const LieConfig& c, const VectorConfig& delta) { LieConfig newConfig; - typedef pair Value; - BOOST_FOREACH(const Value& value, c) { + typedef pair KeyValue; + BOOST_FOREACH(const KeyValue& value, c) { const J& j = value.first; const T& pj = value.second; Symbol jkey = (Symbol)j; @@ -145,8 +143,8 @@ namespace gtsam { } LieConfig newConfig; int delta_offset = 0; - typedef pair Value; - BOOST_FOREACH(const Value& value, c) { + typedef pair KeyValue; + BOOST_FOREACH(const KeyValue& value, c) { const J& j = value.first; const T& pj = value.second; int cur_dim = dim(pj); @@ -161,8 +159,8 @@ namespace gtsam { template VectorConfig logmap(const LieConfig& c0, const LieConfig& cp) { VectorConfig delta; - typedef pair Value; - BOOST_FOREACH(const Value& value, cp) { + typedef pair KeyValue; + BOOST_FOREACH(const KeyValue& value, cp) { if(c0.exists(value.first)) delta.insert(value.first, logmap(c0[value.first], value.second)); diff --git a/cpp/LieConfig.h b/cpp/LieConfig.h index acc9bdde7..69a3bbde3 100644 --- a/cpp/LieConfig.h +++ b/cpp/LieConfig.h @@ -2,9 +2,15 @@ * LieConfig.h * * Created on: Jan 5, 2010 - * Author: Richard Roberts + * @Author: Richard Roberts * * 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 @@ -25,17 +31,20 @@ namespace gtsam { /** * 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 LieConfig : public Testable > { + class LieConfig : public Testable > { public: /** * Typedefs */ - typedef J Key; - typedef T Value; + typedef J Key; + typedef T Value; typedef std::map Values; typedef typename Values::iterator iterator; typedef typename Values::const_iterator const_iterator;