iLab Neuromorphic Robotics Toolkit  0.1
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
GaussianPDF.H
Go to the documentation of this file.
1 /*! @file
2  @author Randolph Voorhies (voorhies at usc dot edu)
3  @copyright GNU Public License (GPL v3)
4  @section License
5  @verbatim
6  // ////////////////////////////////////////////////////////////////////////
7  // The iLab Neuromorphic Robotics Toolkit (NRT) //
8  // Copyright 2010-2012 by the University of Southern California (USC) //
9  // and the iLab at USC. //
10  // //
11  // iLab - University of Southern California //
12  // Hedco Neurociences Building, Room HNB-10 //
13  // Los Angeles, Ca 90089-2520 - USA //
14  // //
15  // See http://ilab.usc.edu for information about this project. //
16  // ////////////////////////////////////////////////////////////////////////
17  // This file is part of The iLab Neuromorphic Robotics Toolkit. //
18  // //
19  // The iLab Neuromorphic Robotics Toolkit is free software: you can //
20  // redistribute it and/or modify it under the terms of the GNU General //
21  // Public License as published by the Free Software Foundation, either //
22  // version 3 of the License, or (at your option) any later version. //
23  // //
24  // The iLab Neuromorphic Robotics Toolkit is distributed in the hope //
25  // that it will be useful, but WITHOUT ANY WARRANTY; without even the //
26  // implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR //
27  // PURPOSE. See the GNU General Public License for more details. //
28  // //
29  // You should have received a copy of the GNU General Public License //
30  // along with The iLab Neuromorphic Robotics Toolkit. If not, see //
31  // <http://www.gnu.org/licenses/>. //
32  // ////////////////////////////////////////////////////////////////////////
33  @endverbatim */
34 
35 #ifndef INCLUDE_NRT_PROBABILISTIC_TYPES_GAUSSIANPDF_H
36 #define INCLUDE_NRT_PROBABILISTIC_TYPES_GAUSSIANPDF_H
37 
38 #include <nrt/Eigen/Eigen.H>
39 NRT_BEGIN_UNCHECKED_INCLUDES
40 #include <Eigen/Eigenvalues>
41 NRT_END_UNCHECKED_INCLUDES
44 #include <nrt/External/cereal/cereal.hpp>
45 
46 namespace nrt
47 {
48  //! A Gaussian representation of a given StateDefinition
49  /*! A GaussianPDF represents the set of variables in a StateDefinition with a mean vector and a covariance matrix
50  \tparam StateDef The StateDefinition which defines the fields of the mean and covariance
51 
52 @code
53 NRT_STATE_FIELD_GROUP(Position2D,(x)(y));
54 NRT_STATE_FIELD_GROUP(Velocity2D,(x)(y));
55 nrt::GaussianPDF<nrt::StateDefinition<Position2D, Velocity2D>> mygaussian;
56 
57 // Access a single field of the mean
58 mygaussian.mean<Position2D::x>() = 1.1;
59 
60 // Access a field group of the mean
61 mygaussian.mean<Velocity2D>() = Eigen::Vector2d(3.3, 4.4);
62 
63 // Access the entire mean
64 mygaussian.mean<>() = Eigen::Vector4d(1.1, 2.2, 3.3, 4.4);
65 
66 // Access the covariance between two fields
67 mygaussian.covariance<Position2D::x, Velocity2D::y>() = 0.00001;
68 
69 // Access the variance of a field
70 mygaussian.covariance<Position2D::x>() = 0.00001;
71 
72 // Access the covariance block of a field
73 mygaussian.covariance<Position2D>() = Eigen::Matrix2d(0.01, 0.02, 0.02, 0.03);
74 
75 // Accessthe entire covariance matrix
76 mygaussian.covariance<>() = Eigen::Matrix4d::Identity() * 0.01;
77 
78 @endcode
79 
80  \warning When editing covariance values manually, you must ensure that you maintain the symmetry of the matrix!
81  For example, if you assign mygaussian.covariance<Position2D::x, Position2D::y>() = 2.2, you must also
82  assign mygaussian.covariance<Position2D::y, Position2D::x>() = 2.2
83 
84  One shortcut is to just set the upper triangular part, and then make it symmetric, e.g.
85  mygaussian.covariance<>() = mygaussian.covariance<>().selfadjointView<Eigen::Upper>();
86 
87  \ingroup probabalistic
88  \see probabalistic
89  */
90  template<class StateDef>
92  {
93 
94  public:
95  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
96 
97  //! The statically sized vector type for the mean
98  typedef Eigen::Matrix<double, StateDef::num_fields, 1> MeanType;
99 
100  //! The statically sized matrix type for the covariance
101  typedef Eigen::Matrix<double, StateDef::num_fields, StateDef::num_fields> CovarianceType;
102 
103  private:
104  static const int dimensions = StateDef::num_fields;
105 
106  MeanType itsMean;
107  CovarianceType itsCovariance;
108 
109  protected:
110  friend class cereal::access;
111 
112  //! Serialization
113  template<class Archive> void serialize(Archive & ar)
114  {
115  ar( itsMean, itsCovariance );
116  }
117 
118  public:
119 
120  //! Default constructor
121  /*! The internal means and covariances are all set to 0 */
122  GaussianPDF();
123 
124  //! Construct a Gaussian with a mean and covariance
126 
127  //! Access the mean of a single field
128  /*! \tparam Field The single field to access, e.g. Position2D::x
129  \returns A reference to the scalar field
130 
131 @code
132 NRT_STATE_FIELD_GROUP(Position2D,(x)(y));
133 NRT_STATE_FIELD_GROUP(Velocity2D,(x)(y));
134 nrt::GaussianPDF<nrt::StateDefinition<Position2D, Velocity2D>> mypdf;
135 
136 mypdf.mean<Velocity2D::x>() = 1;
137 double velocity_x = mypdf.mean<Velocity2D::x>();
138 @endcode
139  */
140  template<class Field>
141  typename std::enable_if
142  <
143  is_state_field<Field>(),
144  double &
145  >::type
146  mean();
147 
148  //! Access the mean of a single field (const version)
149  template<class Field>
150  typename std::enable_if
151  <
152  is_state_field<Field>(),
153  double
154  >::type
155  mean() const;
156 
157  //! Access the mean of an entire field group
158  /*! \tparam FieldGroup The field group to access, e.g. Position2D
159  \returns An Eigen::Block, which acts like a reference to the vector of the field group.
160 
161 @code
162 NRT_STATE_FIELD_GROUP(Position2D,(x)(y));
163 NRT_STATE_FIELD_GROUP(Velocity2D,(x)(y));
164 nrt::GaussianPDF<nrt::StateDefinition<Position2D, Velocity2D>> mypdf;
165 
166 mypdf.mean<Velocity2D>() << 1, 2;
167 Eigen::Vector2d velocity = mypdf.mean<Velocity2D>();
168 @endcode
169  */
170  template<class FieldGroup>
171  typename std::enable_if
172  <
173  is_state_field_group<FieldGroup>(),
174  Eigen::Block<MeanType, FieldGroup::num_fields, 1>
175  >::type
176  mean();
177 
178  //! Access the mean of an entire field group (const version)
179  template<class FieldGroup>
180  typename std::enable_if
181  <
182  is_state_field_group<FieldGroup>(),
183  Eigen::Block<MeanType const, FieldGroup::num_fields, 1>
184  >::type
185  mean() const;
186 
187  //! Access the entire mean vector
188  /*! \tparam Void Leave this template parameter empty to specify you want the entire mean vector.
189  \returns A reference to the entire mean vector.
190 
191 @code
192 NRT_STATE_FIELD_GROUP(Position2D,(x)(y));
193 NRT_STATE_FIELD_GROUP(Velocity2D,(x)(y));
194 nrt::GaussianPDF<nrt::StateDefinition<Position2D, Velocity2D>> mypdf;
195 
196 mypdf.mean<>() << 1, 2, 3, 4;
197 Eigen::Vector4d mean = mypdf.mean<>();
198 @endcode
199  */
200  template<typename Void = void>
201  typename std::enable_if
202  <
203  std::is_void<Void>::value,
204  MeanType &
205  >::type
206  mean();
207 
208  //! Access the entire mean vector (const version)
209  template<typename Void = void>
210  typename std::enable_if
211  <
212  std::is_void<Void>::value,
213  MeanType
214  >::type
215  mean() const;
216 
217 
218  //! Access the covariance between two state fields
219  /*! \tparam Field1 The first state field
220  \tparam Field2 The second state field
221  \returns A reference to the covariance between Field1 and Field2
222 
223 @code
224 NRT_STATE_FIELD_GROUP(Position2D,(x)(y));
225 NRT_STATE_FIELD_GROUP(Velocity2D,(x)(y));
226 nrt::GaussianPDF<nrt::StateDefinition<Position2D, Velocity2D>> mypdf;
227 
228 mypdf.covariance<Position2D::x, Velocity2D::y>() = 3.3;
229 mypdf.covariance<Velocity2D::y, Position2D::x>() = 3.3;
230 @endcode
231 
232  */
233  template<class Field1, class Field2>
234  typename std::enable_if
235  <
236  are_state_fields<Field1, Field2>(),
237  double &
238  >::type
239  covariance();
240 
241  //! Access the covariance between two state fields (const version)
242  template<class Field1, class Field2>
243  typename std::enable_if
244  <
245  are_state_fields<Field1, Field2>(),
246  double
247  >::type
248  covariance() const;
249 
250  //! Access the variance of a single state field
251  /*! \tparam Field The state field
252  \returns A reference to the variance of Field
253 
254 @code
255 NRT_STATE_FIELD_GROUP(Position2D,(x)(y));
256 NRT_STATE_FIELD_GROUP(Velocity2D,(x)(y));
257 nrt::GaussianPDF<nrt::StateDefinition<Position2D, Velocity2D>> mypdf;
258 
259 mypdf.covariance<Position2D::y>() = 10;
260 assert(mypdf.covariance<Position2D::y, Position2D::y>() == 10);
261 @endcode
262  */
263  template<class Field>
264  typename std::enable_if
265  <
266  is_state_field<Field>(),
267  double&
268  >::type
269  covariance();
270 
271  //! Access the variance of a single state field (const version)
272  template<class Field>
273  typename std::enable_if
274  <
275  is_state_field<Field>(),
276  double
277  >::type
278  covariance() const;
279 
280  //! Access the covariance sub-matrix of a field group
281  /*! \tparam Field The state field
282  \returns An Eigen::Block, which acts like a reference to the specified covariance sub-matrix
283 
284 @code
285 NRT_STATE_FIELD_GROUP(Position2D,(x)(y));
286 NRT_STATE_FIELD_GROUP(Velocity2D,(x)(y));
287 nrt::GaussianPDF<nrt::StateDefinition<Position2D, Velocity2D>> mypdf;
288 
289 Eigen::Matrix2d pos_variance = mypdf.covariance<Position2D>();
290 @endcode
291 
292  */
293  template<class FieldGroup>
294  typename std::enable_if
295  <
296  is_state_field_group<FieldGroup>(),
297  Eigen::Block<CovarianceType, FieldGroup::num_fields, FieldGroup::num_fields>
298  >::type
299  covariance();
300 
301  //! Access the covariance sub-matrix of a field group (const version)
302  template<class FieldGroup>
303  typename std::enable_if
304  <
305  is_state_field_group<FieldGroup>(),
306  Eigen::Block<CovarianceType const, FieldGroup::num_fields, FieldGroup::num_fields>
307  >::type
308  covariance() const;
309 
310  //! Access the covariance sub-matrix between two field groups
311  /*!
312  \tparam FieldGroup1 The first State Field Group
313  \tparam FieldGroup2 The second State Field Group
314  \returns The covariance matrix between FieldGroup1 and FieldGroup2
315 
316 @code
317 NRT_STATE_FIELD_GROUP(Position2D,(x)(y));
318 NRT_STATE_FIELD_GROUP(Velocity2D,(x)(y));
319 nrt::GaussianPDF<nrt::StateDefinition<Position2D, Velocity2D>> mypdf;
320 
321 Eigen::Matrix4d covariance = mypdf.covariance<Position2D, Velocity2D>();
322 @endcode
323 
324  */
325  template<class FieldGroup1, class FieldGroup2>
326  typename std::enable_if
327  <
328  are_state_field_groups<FieldGroup1, FieldGroup2>(),
329  Eigen::Block<CovarianceType, FieldGroup1::num_fields, FieldGroup2::num_fields>
330  >::type
331  covariance();
332 
333  //! Access the covariance sub-matrix between two field groups (const version)
334  template<class FieldGroup1, class FieldGroup2>
335  typename std::enable_if
336  <
337  are_state_field_groups<FieldGroup1, FieldGroup2>(),
338  Eigen::Block<CovarianceType const, FieldGroup1::num_fields, FieldGroup2::num_fields>
339  >::type
340  covariance() const;
341 
342  //! Access the entire covariance matrix
343  /*! \tparam Void Leave this template parameter empty to specify you want the entire covariance matrix.
344  \returns A reference to the entire covariance matrix.
345 
346 @code
347 NRT_STATE_FIELD_GROUP(Position2D,(x)(y));
348 NRT_STATE_FIELD_GROUP(Velocity2D,(x)(y));
349 nrt::GaussianPDF<nrt::StateDefinition<Position2D, Velocity2D>> mypdf;
350 
351 Eigen::Matrix4d covariance = mypdf.covariance<>();
352 @endcode
353  */
354  template<typename Void = void>
355  typename std::enable_if
356  <
357  std::is_void<Void>::value,
359  >::type
360  covariance();
361 
362  //! Access the entire covariance matrix (const version)
363  template<typename Void = void>
364  typename std::enable_if
365  <
366  std::is_void<Void>::value,
368  >::type
369  covariance() const;
370 
371  //! Generate an ellipse showing 1 standard deviation around the covariance of the two given fields
372  template<class Field1, class Field2>
373  nrt::Polygon<double> covarianceEllipse(size_t numpoints = 30) const;
374  };
375 }
376 
378 
379 #endif // INCLUDE_NRT_PROBABILISTIC_TYPES_GAUSSIANPDF_H