Main Page · Modules · All Classes · Class Hierarchy
MASurfaceAnalyzer.cpp
1 /*
2  * This file is part of the AiBO+ project
3  *
4  * Copyright (C) 2005-2016 Csaba Kertész (csaba.kertesz@gmail.com)
5  *
6  * AiBO+ is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2 of the License, or
9  * (at your option) any later version.
10  *
11  * AiBO+ is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14  * GNU General Public License for more details.
15  *
16  * You should have received a copy of the GNU General Public License
17  * along with this program; if not, write to the Free Software
18  * Foundation, Inc., 59 Temple Street #330, Boston, MA 02111-1307, USA.
19  *
20  */
21 
22 #include "MASurfaceAnalyzer.hpp"
23 
24 #include "core/MAContainerStatistics.hpp"
25 #include "types/MABodyMotion.hpp"
26 #include "types/MARobotState.hpp"
27 #include "types/MATorso.hpp"
28 
29 #include <MCSampleStatistics.hpp>
30 
31 #include <libxtract.h>
32 
34  SampleSize(sample_size < 4 ? 4 : sample_size),
35  IrSampleSize(sample_size < 112 ? 112 : (sample_size+sample_size % 2)),
36  AccelXSamples(SampleSize, *new MCMedian<double>("Median")),
37  AccelYSamples(SampleSize, *new MCMedian<double>("Median")),
38  AccelZSamples(SampleSize, *new MCMedian<double>("Median")),
39  IrChestSamples(IrSampleSize, *new MCArithmeticMean<double>("Mean")),
40  PawLFSamples(SampleSize, *new MCArithmeticSum<double>("Sum")),
41  PawLHSamples(SampleSize, *new MCArithmeticSum<double>("Sum")),
42  PawRFSamples(SampleSize, *new MCArithmeticSum<double>("Sum")),
43  PawRHSamples(SampleSize, *new MCArithmeticSum<double>("Sum")),
44  LegLHJ1ForceSamples(SampleSize, *new MCArithmeticMean<double>("Mean")),
45  LegRHJ1ForceSamples(SampleSize, *new MCArithmeticMean<double>("Mean"))
46 {
53  AccelXSamples.AddStatistic(*new MCStandardDeviation<double>("StandardDeviation"));
54  AccelYSamples.AddStatistic(*new MCStandardDeviation<double>("StandardDeviation"));
55  AccelZSamples.AddStatistic(*new MCStandardDeviation<double>("StandardDeviation"));
56  IrChestSamples.AddStatistic(*new MCStandardDeviation<double>("StandardDeviation"));
65 }
66 
67 
69 {
70  AccelXRawSamples.push_back((double)robot_state.BodyMotion->AccelX.Degree);
71  AccelYRawSamples.push_back((double)robot_state.BodyMotion->AccelY.Degree);
72  AccelZRawSamples.push_back((double)robot_state.BodyMotion->AccelZ.Degree);
73  IrChestRawSamples.push_back((double)robot_state.Torso->ChestIR);
74  if ((int)IrChestRawSamples.size() > IrSampleSize)
75  {
76  IrChestRawSamples.erase(IrChestRawSamples.begin());
77  }
78  LegLHJ1ForceRawSamples.push_back((double)robot_state.LegLH->Force1);
79  LegRHJ1ForceRawSamples.push_back((double)robot_state.LegRH->Force1);
80  if ((int)AccelXRawSamples.size() > SampleSize)
81  {
84  AccelXRawSamples.erase(AccelXRawSamples.begin());
85  AccelYRawSamples.erase(AccelYRawSamples.begin());
86  AccelZRawSamples.erase(AccelZRawSamples.begin());
87  }
88  AccelXSamples << robot_state.BodyMotion->AccelX.Degree;
89  AccelYSamples << robot_state.BodyMotion->AccelY.Degree;
90  AccelZSamples << robot_state.BodyMotion->AccelZ.Degree;
91  IrChestSamples << (double)robot_state.Torso->ChestIR;
92  PawLFSamples << (double)robot_state.LegLF->Paw;
93  PawLHSamples << (double)robot_state.LegLH->Paw;
94  PawRFSamples << (double)robot_state.LegRF->Paw;
95  PawRHSamples << (double)robot_state.LegRH->Paw;
96  LegLHJ1ForceSamples << (double)robot_state.LegLH->Force1;
97  LegRHJ1ForceSamples << (double)robot_state.LegRH->Force1;
98 }
99 
100 
102 {
103  AccelXRawSamples.clear();
104  AccelYRawSamples.clear();
105  AccelZRawSamples.clear();
109  IrChestRawSamples.clear();
115  LegLHJ1ForceRawSamples.clear();
116  LegRHJ1ForceRawSamples.clear();
119 }
120 
121 
123 {
124  return AccelXSamples.IsValid();
125 }
126 
127 
129 {
130  if (!IsValid())
131  return MC::FloatList();
132 
133  MC::FloatList Features;
134  MC::FloatTable FFTComponents;
135  double UserData[4] = { 0.0f, 0.0f, 0.0f, 0.0f };
136  double Skewness = 0;
137  double RmsAmplitude = 0;
138 
139  // Accelerometer based features (x dimension)
140  Features.push_back((float)AccelXSamples.GetStatistic("Median")->GetResult());
141  MA_ANALYZER_ADD_FEATURE_NAME("AccelXMedian")
142  Features.push_back((float)AccelXSamples.GetStatistic("Maximum")->GetResult());
143  MA_ANALYZER_ADD_FEATURE_NAME("AccelXMax")
144  UserData[0] = AccelXSamples.GetStatistic("Mean")->GetResult();
145  UserData[1] = AccelXSamples.GetStatistic("StandardDeviation")->GetResult();
146  xtract_skewness((double*)&(AccelXRawSamples[0]), AccelXRawSamples.size(), UserData, &Skewness);
147  Features.push_back((float)Skewness);
148  MA_ANALYZER_ADD_FEATURE_NAME("AccelXSkewness")
149  xtract_rms_amplitude((double*)&(AccelXRawSamples[0]), AccelXRawSamples.size(), nullptr, &RmsAmplitude);
150  Features.push_back((float)RmsAmplitude);
151  MA_ANALYZER_ADD_FEATURE_NAME("AccelXRmsAmplitude")
152 
153  // Accelerometer based features (y dimension)
154  Features.push_back((float)AccelYSamples.GetStatistic("Median")->GetResult());
155  MA_ANALYZER_ADD_FEATURE_NAME("AccelYMedian")
156  Features.push_back((float)AccelYSamples.GetStatistic("Maximum")->GetResult());
157  MA_ANALYZER_ADD_FEATURE_NAME("AccelYMaximum")
158  UserData[0] = AccelYSamples.GetStatistic("Mean")->GetResult();
159  UserData[1] = AccelYSamples.GetStatistic("StandardDeviation")->GetResult();
160  xtract_skewness((double*)&(AccelYRawSamples[0]), AccelYRawSamples.size(), UserData, &Skewness);
161  Features.push_back((float)Skewness);
162  MA_ANALYZER_ADD_FEATURE_NAME("AccelYSkewness")
163  xtract_rms_amplitude((double*)&(AccelYRawSamples[0]), AccelYRawSamples.size(), nullptr, &RmsAmplitude);
164  Features.push_back((float)RmsAmplitude);
165  MA_ANALYZER_ADD_FEATURE_NAME("AccelYRmsAmplitude")
166 
167  // Accelerometer based features (z dimension)
169  Features.push_back((float)FFTComponents[0][0]);
170  MA_ANALYZER_ADD_FEATURE_NAME("AccelZFft0")
171  Features.push_back((float)FFTComponents[0][1]);
172  MA_ANALYZER_ADD_FEATURE_NAME("AccelZFft1")
173  Features.push_back((float)FFTComponents[0][2]);
174  MA_ANALYZER_ADD_FEATURE_NAME("AccelZFft2")
175  Features.push_back((float)FFTComponents[0][5]);
176  MA_ANALYZER_ADD_FEATURE_NAME("AccelZFft5")
177  Features.push_back((float)FFTComponents[0][8]);
178  MA_ANALYZER_ADD_FEATURE_NAME("AccelZFft8")
179  Features.push_back((float)FFTComponents[0][11]);
180  MA_ANALYZER_ADD_FEATURE_NAME("AccelZFft11")
181  Features.push_back((float)AccelZSamples.GetStatistic("Median")->GetResult());
182  MA_ANALYZER_ADD_FEATURE_NAME("AccelZMedian")
183  Features.push_back((float)AccelZSamples.GetStatistic("Maximum")->GetResult());
184  MA_ANALYZER_ADD_FEATURE_NAME("AccelZMaximum")
185  UserData[0] = AccelZSamples.GetStatistic("Mean")->GetResult();
186  UserData[1] = AccelZSamples.GetStatistic("StandardDeviation")->GetResult();
187  xtract_skewness((double*)&(AccelZRawSamples[0]), AccelZRawSamples.size(), UserData, &Skewness);
188  Features.push_back((float)Skewness);
189  MA_ANALYZER_ADD_FEATURE_NAME("AccelZSkewness")
190  xtract_rms_amplitude((double*)&(AccelZRawSamples[0]), AccelZRawSamples.size(), nullptr, &RmsAmplitude);
191  Features.push_back((float)RmsAmplitude);
192  MA_ANALYZER_ADD_FEATURE_NAME("AccelZRmsAmplitude")
193 
194  // Paw sensors based features
195  Features.push_back(PawLFSamples.GetStatistic("Sum")->GetResult() / PawLFSamples.GetSampleCount()*100.0);
196  MA_ANALYZER_ADD_FEATURE_NAME("PawLFSum")
197  Features.push_back(PawLHSamples.GetStatistic("Sum")->GetResult() / PawLHSamples.GetSampleCount()*100.0);
198  MA_ANALYZER_ADD_FEATURE_NAME("PawLHSum")
199  Features.push_back(PawRFSamples.GetStatistic("Sum")->GetResult() / PawRFSamples.GetSampleCount()*100.0);
200  MA_ANALYZER_ADD_FEATURE_NAME("PawRFSum")
201  Features.push_back(PawRHSamples.GetStatistic("Sum")->GetResult() / PawRHSamples.GetSampleCount()*100.0);
202  MA_ANALYZER_ADD_FEATURE_NAME("PawRHSum")
203 
204  // IR sensor features
206  for (int i = 0; i < 5; ++i)
207  {
208  Features.push_back(FFTComponents[0][i]);
209  }
210  MA_ANALYZER_ADD_FEATURE_NAME("IrChestFft0")
211  MA_ANALYZER_ADD_FEATURE_NAME("IrChestFft1")
212  MA_ANALYZER_ADD_FEATURE_NAME("IrChestFft2")
213  MA_ANALYZER_ADD_FEATURE_NAME("IrChestFft3")
214  MA_ANALYZER_ADD_FEATURE_NAME("IrChestFft4")
215  Features.push_back(MCCalculateVectorStatistic(FFTComponents[0], *new MCMaximum<float>));
216  MA_ANALYZER_ADD_FEATURE_NAME("IrChestFftMax")
217  Features.push_back((float)IrChestSamples.GetStatistic("Iqr")->GetResult());
218  MA_ANALYZER_ADD_FEATURE_NAME("IrChestIqr")
219  Features.push_back((float)IrChestSamples.GetStatistic("Maximum")->GetResult());
220  MA_ANALYZER_ADD_FEATURE_NAME("IrChestMax")
221  UserData[0] = IrChestSamples.GetStatistic("Mean")->GetResult();
222  UserData[1] = IrChestSamples.GetStatistic("StandardDeviation")->GetResult();
223  xtract_skewness((double*)&(IrChestRawSamples[0]), IrChestRawSamples.size(), UserData, &Skewness);
224  Features.push_back((float)Skewness);
225  MA_ANALYZER_ADD_FEATURE_NAME("IrChestSkewness")
226  xtract_rms_amplitude((double*)&(IrChestRawSamples[0]), IrChestRawSamples.size(), nullptr, &RmsAmplitude);
227  Features.push_back((float)RmsAmplitude);
228  MA_ANALYZER_ADD_FEATURE_NAME("IrChestRmsAmplitude")
229 
230  // LegLH1 force features
232  for (int i = 0; i < 3; ++i)
233  {
234  Features.push_back(FFTComponents[0][i]);
235  }
236  MA_ANALYZER_ADD_FEATURE_NAME("LegLHJ1ForceFft0")
237  MA_ANALYZER_ADD_FEATURE_NAME("LegLHJ1ForceFft1")
238  MA_ANALYZER_ADD_FEATURE_NAME("LegLHJ1ForceFft2")
239  Features.push_back(MCCalculateVectorStatistic(FFTComponents[0], *new MCMaximum<float>));
240  MA_ANALYZER_ADD_FEATURE_NAME("LegLHJ1ForceFftMax")
241  Features.push_back((float)LegLHJ1ForceSamples.GetStatistic("Iqr")->GetResult());
242  MA_ANALYZER_ADD_FEATURE_NAME("LegLHJ1ForceIqr")
243  Features.push_back((float)LegLHJ1ForceSamples.GetStatistic("Maximum")->GetResult());
244  MA_ANALYZER_ADD_FEATURE_NAME("LegLHJ1ForceMax")
245  UserData[0] = LegLHJ1ForceSamples.GetStatistic("Mean")->GetResult();
246  UserData[1] = LegLHJ1ForceSamples.GetStatistic("StandardDeviation")->GetResult();
247  xtract_skewness((double*)&(LegLHJ1ForceRawSamples[0]), LegLHJ1ForceRawSamples.size(), UserData, &Skewness);
248  Features.push_back((float)Skewness);
249  MA_ANALYZER_ADD_FEATURE_NAME("LegLHJ1ForceSkewness")
250  xtract_rms_amplitude((double*)&(LegLHJ1ForceRawSamples[0]), LegLHJ1ForceRawSamples.size(), nullptr, &RmsAmplitude);
251  Features.push_back((float)RmsAmplitude);
252  MA_ANALYZER_ADD_FEATURE_NAME("LegLHJ1ForceRmsAmplitude")
253 
254  // LegRH1 force features
256  for (int i = 0; i < 3; ++i)
257  {
258  Features.push_back(FFTComponents[0][i]);
259  }
260  MA_ANALYZER_ADD_FEATURE_NAME("LegRHJ1ForceFft0")
261  MA_ANALYZER_ADD_FEATURE_NAME("LegRHJ1ForceFft1")
262  MA_ANALYZER_ADD_FEATURE_NAME("LegRHJ1ForceFft2")
263  Features.push_back(MCCalculateVectorStatistic(FFTComponents[0], *new MCMaximum<float>));
264  MA_ANALYZER_ADD_FEATURE_NAME("LegRHJ1ForceFftMax")
265  Features.push_back((float)LegRHJ1ForceSamples.GetStatistic("Iqr")->GetResult());
266  MA_ANALYZER_ADD_FEATURE_NAME("LegRHJ1ForceIqr")
267  Features.push_back((float)LegRHJ1ForceSamples.GetStatistic("Maximum")->GetResult());
268  MA_ANALYZER_ADD_FEATURE_NAME("LegRHJ1ForceMax")
269  UserData[0] = LegRHJ1ForceSamples.GetStatistic("Mean")->GetResult();
270  UserData[1] = LegRHJ1ForceSamples.GetStatistic("StandardDeviation")->GetResult();
271  xtract_skewness((double*)&(LegRHJ1ForceRawSamples[0]), LegRHJ1ForceRawSamples.size(), UserData, &Skewness);
272  Features.push_back((float)Skewness);
273  MA_ANALYZER_ADD_FEATURE_NAME("LegRHJ1ForceSkewness")
274  xtract_rms_amplitude((double*)&(LegRHJ1ForceRawSamples[0]), LegRHJ1ForceRawSamples.size(), nullptr, &RmsAmplitude);
275  Features.push_back((float)RmsAmplitude);
276  MA_ANALYZER_ADD_FEATURE_NAME("LegRHJ1ForceRmsAmplitude")
277  MA_ANALYZER_FEATURE_NAMES_COMMIT(Features);
278  return Features;
279 }
Arithmetic sum statistic.
MCSamples< double > LegRHJ1ForceSamples
Force samples (RH/joint1)
int GetSampleCount() const
Get the sample count in the cache.
Definition: MCSamples.hpp:136
int IrSampleSize
Infrared sample size.
MC::DoubleList IrChestRawSamples
Infrared (chest) raw samples.
MCSamples< double > PawLFSamples
Paw (LF) samples.
MCSamples< double > AccelYSamples
Accelerometer (y dimension) samples.
virtual void AddSamples(const MARobotState &robot_state) override
Add new samples from the robot state.
Analyzer base class.
Definition: MAAnalyzer.hpp:67
float MCCalculateVectorStatistic(const std::vector< T > &vector, MCSampleStatistic< T > &statistic)
Calculate a statistic over a vector.
MC::DoubleList AccelXRawSamples
Accelerometer X raw samples.
MC::DoubleList LegLHJ1ForceRawSamples
Force raw samples (LH/joint1)
MCSamples< double > IrChestSamples
Infrared (chest) samples.
virtual MC::FloatList GetFeatureVector() override
Get a feature vector.
boost::scoped_ptr< MALeg > LegRH
LegRH.
MASurfaceAnalyzer(int sample_size)
Class constructor.
MC::FloatTable MAFFTComponentsFromContainer(const U &container)
Calculate FFT components over a container.
virtual bool IsValid() const override
Check if enough samples were added to the analyzer.
MCSamples< double > PawRFSamples
Paw (RF) samples.
Interquartile range statistic.
MC::DoubleList AccelYRawSamples
Accelerometer Y raw samples.
bool IsValid() const
Check if the sample cache is valid for calculations.
Definition: MCSamples.hpp:124
MCSampleStatistic< T > * GetStatistic(const std::string &statistic_name)
Get a statistic by name.
Definition: MCSamples.hpp:188
virtual void Reset() override
Reset the analyzer and drop all samples.
boost::scoped_ptr< MABodyMotion > BodyMotion
Body motion.
boost::scoped_ptr< MALeg > LegLF
LegLF.
boost::scoped_ptr< MALeg > LegRF
LegRF.
Maximum statistic.
MCSamples< double > AccelZSamples
Accelerometer (z dimension) samples.
int SampleSize
Sample size.
Standard deviation statistic.
MC::DoubleList AccelZRawSamples
Accelerometer Z raw samples.
MCSamples< double > PawLHSamples
Paw (LH) samples.
boost::scoped_ptr< MATorso > Torso
Torso.
boost::scoped_ptr< MALeg > LegLH
LegLH.
MCSamples< double > PawRHSamples
Paw (RH) samples.
Robot state.
MC::DoubleList LegRHJ1ForceRawSamples
Force raw samples (RH/joint1)
Median statistic.
MCSamples< double > LegLHJ1ForceSamples
Force samples (LH/joint1)
MCSamples< double > AccelXSamples
Accelerometer (x dimension) samples.
Arithmetic mean statistic.
void Reset()
Get the size of the sample cache.
Definition: MCSamples.hpp:227
void AddStatistic(MCSampleStatistic< T > &statistic)
Add a statistic calculation.
Definition: MCSamples.hpp:159