iLab Neuromorphic Robotics Toolkit  0.1
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
GaussianPDFImpl.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 
36 
37 // ######################################################################
38 template<class StateDef>
40  :
41  itsMean(Eigen::Matrix<double, StateDef::num_fields, 1>::Zero()),
42  itsCovariance(Eigen::Matrix<double, StateDef::num_fields, StateDef::num_fields>::Zero())
43 {
44  static_assert(std::is_base_of<nrt::StateDefinitionBase, StateDef>::value,
45  "GaussianPDFs must be created with State Definitions");
46 }
47 
48 // ######################################################################
49 template<class StateDef>
50 nrt::GaussianPDF<StateDef>::GaussianPDF(MeanType const & mean, CovarianceType const & covariance) :
51  itsMean(mean),
52  itsCovariance(covariance)
53 {
54  static_assert(std::is_base_of<nrt::StateDefinitionBase, StateDef>::value,
55  "GaussianPDFs must be created with State Definitions");
56 }
57 
58 // ######################################################################
59 template<class StateDef>
60 template<class Field>
62  typename std::enable_if
63  <
64  is_state_field<Field>(),
65  double &
66  >::type
67 {
68  static_assert(StateDef::template contains_field<Field>(),
69  "Trying to access mean element with a State Field "
70  "that is not part of the State Definition");
71  size_t const fieldIdx = boost::mpl::find<typename StateDef::fields, Field>::type::pos::value;
72  return itsMean(fieldIdx, 0);
73 }
74 
75 // ######################################################################
76 template<class StateDef>
77 template<class Field>
79  typename std::enable_if
80  <
81  is_state_field<Field>(),
82  double
83  >::type
84 {
85  static_assert(StateDef::template contains_field<Field>(),
86  "Trying to access mean element with a State Field "
87  "that is not part of the State Definition");
88  size_t const fieldIdx = boost::mpl::find<typename StateDef::fields, Field>::type::pos::value;
89  return itsMean(fieldIdx, 0);
90 }
91 
92 // ######################################################################
93 template<class StateDef>
94 template<class FieldGroup>
96  typename std::enable_if
97  <
98  is_state_field_group<FieldGroup>(),
99  Eigen::Block<MeanType, FieldGroup::num_fields, 1>
100  >::type
101 {
102  static_assert(StateDef::template contains_field_group<FieldGroup>(),
103  "Trying to access mean group with a State Field Group "
104  "that is not part of the State Definition");
105  typedef typename boost::mpl::at_c<typename FieldGroup::fields, 0>::type first_type;
106  size_t const fieldIdx = boost::mpl::find<typename StateDef::fields, first_type>::type::pos::value;
107  return itsMean.template block<FieldGroup::num_fields, 1>(fieldIdx, 0);
108 }
109 
110 // ######################################################################
111 template<class StateDef>
112 template<class FieldGroup>
113 auto nrt::GaussianPDF<StateDef>::mean() const ->
114  typename std::enable_if
115  <
116  is_state_field_group<FieldGroup>(),
117  Eigen::Block<MeanType const, FieldGroup::num_fields, 1>
118  >::type
119 {
120  static_assert(StateDef::template contains_field_group<FieldGroup>(),
121  "Trying to access mean group with a State Field Group "
122  "that is not part of the State Definition");
123  typedef typename boost::mpl::at_c<typename FieldGroup::fields, 0>::type first_type;
124  size_t const fieldIdx = boost::mpl::find<typename StateDef::fields, first_type>::type::pos::value;
125  return itsMean.template block<FieldGroup::num_fields, 1>(fieldIdx, 0);
126 }
127 
128 // ######################################################################
129 template<class StateDef>
130 template<typename Void>
132  typename std::enable_if
133  <
134  std::is_void<Void>::value,
135  MeanType &
136  >::type
137 {
138  return itsMean;
139 }
140 
141 // ######################################################################
142 template<class StateDef>
143 template<typename Void>
144  auto nrt::GaussianPDF<StateDef>::mean() const ->
145  typename std::enable_if
146  <
147  std::is_void<Void>::value,
148  MeanType
149  >::type
150 {
151  return itsMean;
152 }
153 
154 // ######################################################################
155 template<class StateDef>
156 template<class Field1, class Field2>
158  typename std::enable_if
159  <
160  are_state_fields<Field1, Field2>(),
161  double &
162  >::type
163 {
164  static_assert(StateDef::template contains_field<Field1>(),
165  "Trying to access covariance element with a State Field (Field1) "
166  "that is not part of the State Definition");
167 
168  static_assert(StateDef::template contains_field<Field2>(),
169  "Trying to access covariance element with a State Field (Field2) "
170  "that is not part of the State Definition");
171 
172  size_t const field1Idx = boost::mpl::find<typename StateDef::fields, Field1>::type::pos::value;
173  size_t const field2Idx = boost::mpl::find<typename StateDef::fields, Field2>::type::pos::value;
174  return itsCovariance(field1Idx, field2Idx);
175 }
176 
177 // ######################################################################
178 template<class StateDef>
179 template<class Field1, class Field2>
181  typename std::enable_if
182  <
183  are_state_fields<Field1, Field2>(),
184  double
185  >::type
186 {
187  static_assert(StateDef::template contains_field<Field1>(),
188  "Trying to access covariance element with a State Field (Field1) "
189  "that is not part of the State Definition");
190 
191  static_assert(StateDef::template contains_field<Field2>(),
192  "Trying to access covariance element with a State Field (Field2) "
193  "that is not part of the State Definition");
194 
195  size_t const field1Idx = boost::mpl::find<typename StateDef::fields, Field1>::type::pos::value;
196  size_t const field2Idx = boost::mpl::find<typename StateDef::fields, Field2>::type::pos::value;
197  return itsCovariance(field1Idx, field2Idx);
198 }
199 
200 // ######################################################################
201 template<class StateDef>
202 template<class Field>
204  typename std::enable_if
205  <
206  is_state_field<Field>(),
207  double&
208  >::type
209 {
210  static_assert(StateDef::template contains_field<Field>(),
211  "Trying to access covariance element with a State Field "
212  "that is not part of the State Definition");
213 
214  return covariance<Field, Field>();
215 }
216 
217 // ######################################################################
218 template<class StateDef>
219 template<class Field>
221  typename std::enable_if
222  <
223  is_state_field<Field>(),
224  double
225  >::type
226 {
227  static_assert(StateDef::template contains_field<Field>(),
228  "Trying to access covariance element with a State Field "
229  "that is not part of the State Definition");
230 
231  return covariance<Field, Field>();
232 }
233 
234 // ######################################################################
235 template<class StateDef>
236 template<class FieldGroup>
238  typename std::enable_if
239  <
240  is_state_field_group<FieldGroup>(),
241  Eigen::Block<CovarianceType, FieldGroup::num_fields, FieldGroup::num_fields>
242  >::type
243 {
244 
245  static_assert(StateDef::template contains_field_group<FieldGroup>(),
246  "Trying to access covariance block with a Field Group"
247  "that is not part of the State Definition");
248 
249  typedef typename boost::mpl::at_c<typename FieldGroup::fields, 0>::type first_type;
250  size_t const fieldIdx = boost::mpl::find<typename StateDef::fields, first_type>::type::pos::value;
251 
252  return itsCovariance.template block<FieldGroup::num_fields, FieldGroup::num_fields>(fieldIdx, fieldIdx);
253 }
254 
255 // ######################################################################
256 template<class StateDef>
257 template<class FieldGroup>
259  typename std::enable_if
260  <
261  is_state_field_group<FieldGroup>(),
262  Eigen::Block<CovarianceType const, FieldGroup::num_fields, FieldGroup::num_fields>
263  >::type
264 {
265 
266  static_assert(StateDef::template contains_field_group<FieldGroup>(),
267  "Trying to access covariance block with a Field Group"
268  "that is not part of the State Definition");
269 
270  typedef typename boost::mpl::at_c<typename FieldGroup::fields, 0>::type first_type;
271  size_t const fieldIdx = boost::mpl::find<typename StateDef::fields, first_type>::type::pos::value;
272 
273  return itsCovariance.template block<FieldGroup::num_fields, FieldGroup::num_fields>(fieldIdx, fieldIdx);
274 }
275 
276 // ######################################################################
277 template<class StateDef>
278 template<class FieldGroup1, class FieldGroup2>
280  typename std::enable_if
281  <
282  are_state_field_groups<FieldGroup1, FieldGroup2>(),
283  Eigen::Block<CovarianceType, FieldGroup1::num_fields, FieldGroup2::num_fields>
284  >::type
285 {
286  static_assert(StateDef::template contains_field_group<FieldGroup1>(),
287  "Trying to access covariance block with a Field Group (FieldGroup1)"
288  "that is not part of the State Definition");
289 
290  static_assert(StateDef::template contains_field_group<FieldGroup2>(),
291  "Trying to access covariance block with a Field Group (FieldGroup2)"
292  "that is not part of the State Definition");
293 
294  typedef typename boost::mpl::at_c<typename FieldGroup1::fields, 0>::type first_type_group1;
295  size_t const fieldIdx1 = boost::mpl::find<typename StateDef::fields, first_type_group1>::type::pos::value;
296 
297  typedef typename boost::mpl::at_c<typename FieldGroup2::fields, 0>::type first_type_group2;
298  size_t const fieldIdx2 = boost::mpl::find<typename StateDef::fields, first_type_group2>::type::pos::value;
299 
300  return itsCovariance.template block<FieldGroup1::num_fields, FieldGroup2::num_fields>(fieldIdx1, fieldIdx2);
301 }
302 
303 // ######################################################################
304 template<class StateDef>
305 template<class FieldGroup1, class FieldGroup2>
307  typename std::enable_if
308  <
309  are_state_field_groups<FieldGroup1, FieldGroup2>(),
310  Eigen::Block<CovarianceType const, FieldGroup1::num_fields, FieldGroup2::num_fields>
311  >::type
312 {
313  static_assert(StateDef::template contains_field_group<FieldGroup1>(),
314  "Trying to access covariance block with a Field Group (FieldGroup1)"
315  "that is not part of the State Definition");
316 
317  static_assert(StateDef::template contains_field_group<FieldGroup2>(),
318  "Trying to access covariance block with a Field Group (FieldGroup2)"
319  "that is not part of the State Definition");
320 
321  typedef typename boost::mpl::at_c<typename FieldGroup1::fields, 0>::type first_type_group1;
322  size_t const fieldIdx1 = boost::mpl::find<typename StateDef::fields, first_type_group1>::type::pos::value;
323 
324  typedef typename boost::mpl::at_c<typename FieldGroup2::fields, 0>::type first_type_group2;
325  size_t const fieldIdx2 = boost::mpl::find<typename StateDef::fields, first_type_group2>::type::pos::value;
326 
327  return itsCovariance.template block<FieldGroup1::num_fields, FieldGroup2::num_fields>(fieldIdx1, fieldIdx2);
328 }
329 
330 // ######################################################################
331 template<class StateDef>
332 template<typename Void>
334  typename std::enable_if
335  <
336  std::is_void<Void>::value,
337  CovarianceType &
338  >::type
339 {
340  return itsCovariance;
341 }
342 
343 // ######################################################################
344 template<class StateDef>
345 template<typename Void>
347  typename std::enable_if
348  <
349  std::is_void<Void>::value,
350  CovarianceType
351  >::type
352 {
353  return itsCovariance;
354 }
355 
356 // ######################################################################
357 template<class StateDef>
358 template<class Field1, class Field2>
361 {
362  // Inspired by: http://stats.stackexchange.com/a/9899
363 
364  static_assert(nrt::is_state_field<Field1>(), "Field1 is not a state field");
365  static_assert(nrt::is_state_field<Field2>(), "Field2 is not a state field");
366  static_assert(StateDef::template contains_field<Field1>(),
367  "Trying to access covariance element with a State Field (Field1) "
368  "that is not part of the State Definition");
369  static_assert(StateDef::template contains_field<Field2>(),
370  "Trying to access covariance element with a State Field (Field2) "
371  "that is not part of the State Definition");
372 
373  size_t const field1Idx = boost::mpl::find<typename StateDef::fields, Field1>::type::pos::value;
374  size_t const field2Idx = boost::mpl::find<typename StateDef::fields, Field2>::type::pos::value;
375 
376  Eigen::Vector2d mean;
377  mean << itsMean(field1Idx), itsMean(field2Idx);
378 
379  Eigen::Matrix2d covariance;
380  covariance << itsCovariance(field1Idx, field1Idx), itsCovariance(field1Idx, field2Idx),
381  itsCovariance(field2Idx, field1Idx), itsCovariance(field2Idx, field2Idx);
382 
383  Eigen::SelfAdjointEigenSolver<Eigen::Matrix2d> eigensolver(covariance);
384  double eigenvalue1 = eigensolver.eigenvalues()(0);
385  double eigenvalue2 = eigensolver.eigenvalues()(1);
386  Eigen::Vector2d eigenvector1 = eigensolver.eigenvectors().col(0);
387  Eigen::Vector2d eigenvector2 = eigensolver.eigenvectors().col(1);
388 
389  if(eigenvalue2 > eigenvalue1)
390  {
391  std::swap(eigenvalue1, eigenvalue2);
392  std::swap(eigenvector1, eigenvector2);
393  }
394 
395  double a = eigenvalue1;
396  double b = eigenvalue2;
397 
398  double alpha = atan2(eigenvector1(1), eigenvector1(0));
399 
400  std::vector<nrt::Point2D<double>> points;
401  for(size_t i=0; i < numpoints; ++i)
402  {
403  double theta = 2.0*M_PI / double(numpoints) * double(i);
404  double x = mean(0) + a * cos(theta) * cos(alpha) - b * sin(theta) * sin(alpha);
405  double y = mean(1) + a * cos(theta) * sin(alpha) + b * sin(theta) * cos(alpha);
406  points.push_back(nrt::Point2D<double>(x, y));
407  }
408  return nrt::Polygon<double>(points);
409 }