iLab Neuromorphic Robotics Toolkit  0.1
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
ParticlePDFImpl.H
Go to the documentation of this file.
1 /*! @file
2  @author Lior Elazary
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 template <class PDFDef, class WeightDef>
38  itsElemMem(), elem(new PDFDef(itsElemMem))
39 {
40  itsElemMem.setZero();
41 }
42 
43 // ######################################################################
44 template <class PDFDef, class WeightDef>
46  itsParticles(n),
47  itsElemMem(), elem(new PDFDef(itsElemMem))
48 {
49  itsElemMem.setZero();
50 }
51 
52 
53 // ######################################################################
54 template <class PDFDef, class WeightDef>
56  itsParticles(gauss.size()*2+1),
57  itsElemMem(), elem(new PDFDef(itsElemMem))
58 {
59 
60  double k = 0;
61  double alpha = 1e-3;
62  int numStates = gauss.size();
63 
64  double lambda = alpha*alpha*(numStates+k)-numStates;
65  double gamma = sqrt(numStates + lambda);
66 
67  ////We use the chol decomposition for stability instead of the sqrt
68  Eigen::MatrixXd A = gauss.itsSigma;
69  Eigen::MatrixXd sChol = A.llt().matrixU();
70 
71  Eigen::MatrixXd sigmaLoc=sChol.transpose()*gamma;
72 
73  //Add the mean
74  itsParticles.at(0).data = gauss.itsMu;
75 
76  //Add the + sigma locations
77  for(int i=0; i<numStates; i++)
78  itsParticles.at(i+1).data =
79  gauss.itsMu + sigmaLoc.block(0, i, numStates, 1);
80 
81  //Add the - sigma locations
82  for(int i=0; i<numStates; i++)
83  itsParticles.at(i+numStates).data =
84  gauss.itsMu - sigmaLoc.block(0, i, numStates, 1);
85 
86 
87 }
88 
89 // ######################################################################
90 template <class PDFDef, class WeightDef>
92 {
93  itsParticles = pdf.itsParticles;
94  itsElemMem = pdf.itsElemMem;
95  elem = std::shared_ptr<PDFDef>(new PDFDef(itsElemMem));
96 }
97 
98 // ######################################################################
99 template <class PDFDef, class WeightDef>
101 {
102 
103 }
104 
105 //// ######################################################################
106 template <class PDFDef, class WeightDef>
107 double & nrt::ParticlePDF<PDFDef,WeightDef>::at(int const idx, double & addr)
108 {
109  return itsParticles.at(idx).data(elem->indexof(addr));
110 }
111 
112 //// ######################################################################
113 template <class PDFDef, class WeightDef>
114 double const nrt::ParticlePDF<PDFDef,WeightDef>::at(int const idx, double & addr) const
115 {
116  return itsParticles[idx].data(elem->indexof(addr));
117 }
118 
119 //// ######################################################################
120 template <class PDFDef, class WeightDef>
121 WeightDef & nrt::ParticlePDF<PDFDef,WeightDef>::at(int const idx)
122 {
123  return itsParticles.at(idx).weight;
124 }
125 
126 //// ######################################################################
127 template <class PDFDef, class WeightDef>
128 WeightDef const nrt::ParticlePDF<PDFDef,WeightDef>::at(int const idx) const
129 {
130  return itsParticles[idx].weight;
131 }
132 
133 // ######################################################################
134 template <class PDFDef, class WeightDef>
136 {
137  return PDFDef::size;
138 }
139 
140 // ######################################################################
141 template <class PDFDef, class WeightDef>
143 {
144  return itsParticles.size();
145 }
146 
147 //// ######################################################################
148 template <class PDFDef, class WeightDef>
150 {
151  itsParticles = pdf.itsParticles;
152  itsElemMem = pdf.itsElemMem;
153  elem = std::shared_ptr<PDFDef>(new PDFDef(itsElemMem));
154  return *this;
155 
156 }