iLab Neuromorphic Robotics Toolkit  0.1
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
test-State.C
Go to the documentation of this file.
1 /*! @file
2  @author Laurent Itti <itti@usc.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 #include <nrt/Probabilistic/Types/StateDef.H>
36 #include <nrt/Probabilistic/Types/GaussianPDF.H>
37 #include <nrt/Probabilistic/Types/ParticlePDF.H>
38 
39 struct Pos3D
40 {
41  Pos3D(double storage[]) : x(storage[0]), y(storage[1]), z(storage[2])
42  { std::cout << "Pos3D addresses: " << &x << ' ' << &y << ' ' << &z <<std::endl; }
43 
44  double & x;
45  double & y;
46  double & z;
47 
48  static size_t const size = 3;
49 };
50 
51 std::ostream & operator<<(std::ostream & os, Pos3D const & pos)
52 { os << "Pos3D(" << pos.x << ", " << pos.y << ", " << pos.z << ')'; return os; }
53 
54 struct Vel3D
55 {
56  Vel3D(double storage[]) : vx(storage[0]), vy(storage[1]), vz(storage[2])
57  { std::cout << "Vel3D addresses: " << &vx << ' ' << &vy << ' ' << &vz <<std::endl; }
58 
59  double & vx;
60  double & vy;
61  double & vz;
62 
63  static size_t const size = 3;
64 };
65 
66 std::ostream & operator<<(std::ostream & os, Vel3D const & vel)
67 { os << "Vel3D(" << vel.vx << ", " << vel.vy << ", " << vel.vz << ')'; return os; }
68 
69 int main(int argc, char const * argv[])
70 {
71 
72  // ########## Simple scenario:
73  typedef nrt::StateDef<Pos3D, Vel3D> myState;
74  myState::datatype data;
75  myState state(data);
76 
77  state.get<Pos3D>().x = 1.0;
78  state.get<Pos3D>().y = 2.0;
79  state.get<Pos3D>().z = 3.0;
80 
81  state.get<Vel3D>().vx = 4.0;
82  state.get<Vel3D>().vy = 5.0;
83  state.get<Vel3D>().vz = 6.0;
84 
85 
86  std::cout << "Pos3D only: " << state.get<Pos3D>() << std::endl;
87 
88  std::cout << "Full state: " << state << std::endl;
89 
90  // ########## Composition works:
91  typedef nrt::StateDef<Pos3D, Vel3D> MachineState;
92  typedef nrt::StateDef<MachineState, Pos3D, Vel3D> FactoryState;
93  typedef nrt::StateDef<Pos3D, FactoryState, Vel3D> ComplicatedState;
94 
95  FactoryState::datatype factorydata;
96  FactoryState s(factorydata);
97 
98  std::cout << "FactoryState size = " << FactoryState::size << std::endl;
99 
100  ComplicatedState::datatype csdata;
101  ComplicatedState cs(csdata);
102 
103  std::cout << "ComplicatedState size = " << ComplicatedState::size << std::endl;
104 
105  cs.get<Vel3D>().vy = 123.0;
106  cs.get<FactoryState>().get<Pos3D>().x = 456.0;
107  cs.get<FactoryState>().get<MachineState>().get<Pos3D>().z = 789.0;
108 
109  std::cout << "ComplicatedState: " << cs << std::endl;
110 
111  size_t idx = cs.indexof(cs.get<FactoryState>().get<MachineState>().get<Vel3D>().vy);
112 
113  // modify our raw data directly:
114  csdata[idx] = 42.0;
115 
116  // should be reflected in our State:
117  std::cout << "index test: index = " << idx << ", value = " << cs[idx] << std::endl;
118  std::cout << "index test: value = " << cs.get<FactoryState>().get<MachineState>().get<Vel3D>().vy << std::endl;
119 
120  // Say we want to get the address of the FactoryState; then we want to work from there
121  FactoryState & baseref = cs.get<FactoryState>();
122  size_t baseidx = cs.index<FactoryState>();
123  size_t offset = baseref.indexof(baseref.get<Vel3D>().vx);
124  std::cout << "baseidx = " << baseidx << ", offset = " << offset <<std::endl;
125 
126  cs[baseidx + offset] = 27.0;
127 
128  std::cout << "ComplicatedState: " << cs << std::endl;
129 
130  nrt::ParticlePDF<MachineState,double> particles(10);
131 
132  nrt::GaussianPDF<MachineState> gauss;
133  gauss.mean(gauss.elem->get<Pos3D>().x) = 10;
134  gauss.mean(gauss.elem->get<Pos3D>().y) = 20;
135  gauss.mean(gauss.elem->get<Pos3D>().z) = 30;
136 
137  gauss.covariance(gauss.elem->get<Pos3D>().x) = 4*4;
138  gauss.covariance(gauss.elem->get<Pos3D>().y) = 4*4;
139  gauss.covariance(gauss.elem->get<Pos3D>().z) = 4*4;
140 
141 
142  printf("Set Particle\n");
143  particles.at(1, particles.elem->get<Pos3D>().x) = 10;
144  particles.at(1) = 4.5;
145  printf("Show Particle\n");
146 
147  printf("Pos 3d %f %f %f %f\n",
148  particles.at(1, particles.elem->get<Pos3D>().x),
149  particles.at(1, particles.elem->get<Pos3D>().y),
150  particles.at(1, particles.elem->get<Pos3D>().z),
151  particles.at(1));
152 
153 
154  nrt::ParticlePDF<MachineState,double> samples(gauss);
155 
156  printf("Samples\n");
157  for(uint i=0; i<samples.size(); i++)
158  {
159  printf("%i\n", i);
160  printf("Pos 3d (%f %f %f) %f\n",
161  samples.at(i, samples.elem->get<Pos3D>().x),
162  samples.at(i, samples.elem->get<Pos3D>().y),
163  samples.at(i, samples.elem->get<Pos3D>().z),
164  samples.at(i));
165  }
166 
167 
168 
169  return 0;
170 }
171 
172