00001 // ***************************************************************************** 00002 // 00003 // Copyright (c) 2014, Southwest Research Institute® (SwRI®) 00004 // All rights reserved. 00005 // 00006 // Redistribution and use in source and binary forms, with or without 00007 // modification, are permitted provided that the following conditions are met: 00008 // * Redistributions of source code must retain the above copyright 00009 // notice, this list of conditions and the following disclaimer. 00010 // * Redistributions in binary form must reproduce the above copyright 00011 // notice, this list of conditions and the following disclaimer in the 00012 // documentation and/or other materials provided with the distribution. 00013 // * Neither the name of Southwest Research Institute® (SwRI®) nor the 00014 // names of its contributors may be used to endorse or promote products 00015 // derived from this software without specific prior written permission. 00016 // 00017 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 00018 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 00019 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 00020 // ARE DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY 00021 // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 00022 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00023 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 00024 // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 00025 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 00026 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 00027 // 00028 // ***************************************************************************** 00029 00030 #ifndef MATH_UTIL_STAT_BUFFER_H_ 00031 #define MATH_UTIL_STAT_BUFFER_H_ 00032 00033 #ifndef PI 00034 #define PI 3.14159265358979 00035 #endif 00036 00037 #include <cmath> 00038 #include <algorithm> 00039 00040 #include <swri_math_util/generic_ring_buffer.h> 00041 00042 namespace swri_math_util 00043 { 00044 template <class T> 00045 class StatBuffer: public GenRingBuffer<T> 00046 { 00047 public: 00048 void modifyBufferSize(int NumElements) // moved from private 04/02/2008 JJC 00049 { 00050 this->realloc_mem(NumElements); 00051 } 00052 00053 StatBuffer() 00054 { 00055 this->modifyBufferSize(30); 00056 } 00057 00058 explicit StatBuffer(int NumElements) 00059 { 00060 this->modifyBufferSize(NumElements); 00061 } 00062 00063 ~StatBuffer() 00064 { 00065 this->modifyBufferSize(0); 00066 } 00067 00068 bool UpdateStats() 00069 { 00070 return this->computeStats(); 00071 } 00072 00073 bool UpdateDiffStats() 00074 { 00075 return this->computeDiffStats(); 00076 } 00077 00078 T reportDiffMean() 00079 { 00080 return RetainedDiffStats.mean; 00081 } 00082 00083 T reportDiffMedian() 00084 { 00085 return RetainedDiffStats.median; 00086 } 00087 00088 T reportDiffMin() 00089 { 00090 return RetainedDiffStats.min; 00091 } 00092 00093 T reportDiffMax() 00094 { 00095 return RetainedDiffStats.max; 00096 } 00097 00098 T reportMean() 00099 { 00100 return RetainedStats.mean; 00101 } 00102 00103 // Computes the mean of the last NumToAvg items in the buffer 00104 T reportPartialMean(int NumToAvg) 00105 { 00106 return this->computeMean(NumToAvg); 00107 } 00108 00109 T reportMedian() 00110 { 00111 return RetainedStats.median; 00112 } 00113 00114 T reportMin() 00115 { 00116 return RetainedStats.min; 00117 } 00118 00119 T reportMax() 00120 { 00121 return RetainedStats.max; 00122 } 00123 T reportStd() 00124 { 00125 return RetainedStats.std; 00126 } 00127 00128 T reportVar() 00129 { 00130 return RetainedStats.variance; 00131 } 00132 T reportRetainedStats() 00133 { 00134 return RetainedStats; 00135 } 00136 00137 private: 00138 typedef struct 00139 { 00140 T mean; 00141 T min; 00142 T max; 00143 T median; 00144 T std; 00145 T variance; 00146 } StatPack; 00147 00148 StatPack RetainedStats; 00149 StatPack RetainedDiffStats; 00150 00151 T computeMean(int NumToAvg) 00152 { 00153 int NumElems = this->size(); 00154 if (NumElems <= 0) return (T)(0.0); 00155 T CurVal = *this->getTail(0); 00156 T sum = 0; 00157 NumToAvg = std::min(NumToAvg, NumElems); 00158 for (int i = 0; i < NumToAvg; ++i) 00159 { 00160 CurVal = *this->getTail(i); 00161 sum += CurVal; 00162 } 00163 T mean = sum/((T)NumToAvg); 00164 return mean; 00165 } 00166 00167 bool computeStats() 00168 { 00169 int NumElems = this->size(); 00170 if (NumElems <= 0) return false; 00171 00172 T sum = 0; 00173 T &min = RetainedStats.min; 00174 T &max = RetainedStats.max; 00175 T &mean = RetainedStats.mean; 00176 T &median = RetainedStats.median; 00177 T &std = RetainedStats.std; 00178 T &var = RetainedStats.variance; 00179 00180 T CurVal = *this->get(0); 00181 sum += CurVal; 00182 min = CurVal; 00183 max = CurVal; 00184 mean = CurVal; 00185 median = CurVal; 00186 std = 0; 00187 var = std*std; 00188 00189 // compute mean, min and max 00190 for (int i = 1; i < NumElems; i++) 00191 { 00192 CurVal = *this->get(i); 00193 sum += CurVal; 00194 if (CurVal > max) max = CurVal; 00195 else if (CurVal < min) min = CurVal; 00196 } 00197 mean = sum/((T)NumElems); 00198 sum = 0; 00199 00200 // compute 00201 if (NumElems > 1) 00202 { 00203 T *vec1 = new T[NumElems]; // for median calculation 00204 for (int i = 0; i < NumElems; i++) 00205 { 00206 CurVal = *this->get(i); 00207 sum += (CurVal-mean)*(CurVal-mean); 00208 vec1[i] = CurVal; // for median calculation 00209 } 00210 std=(T)sqrt(static_cast<double>(sum/(NumElems-1))); 00211 var = std*std; 00212 00213 // Compute Median 00214 std::sort(vec1, vec1+NumElems); // first sort the data 00215 if (NumElems % 2 == 0) 00216 { 00217 median = (vec1[NumElems/2-1] + vec1[NumElems/2])/2; 00218 } 00219 else 00220 { 00221 median = vec1[NumElems/2]; 00222 } 00223 if (NumElems <= 1) 00224 { 00225 delete vec1; 00226 } 00227 else 00228 { 00229 delete [] vec1; 00230 } 00231 } 00232 00233 return true; 00234 } 00235 00236 bool computeDiffStats() 00237 { 00238 int NumElems = this->size(); 00239 if (NumElems <= 1) return false; 00240 00241 T sum = 0; 00242 T &min = RetainedDiffStats.min; 00243 T &max = RetainedDiffStats.max; 00244 T &mean = RetainedDiffStats.mean; 00245 T &median = RetainedDiffStats.median; 00246 00247 T *vec1 = new T[NumElems]; 00248 00249 T CurVal1 = *this->get(0); 00250 T CurVal2 = *this->get(1); 00251 T CVDiff = CurVal2-CurVal1; 00252 00253 vec1[0] = CVDiff; 00254 00255 sum += CVDiff; 00256 min = CVDiff; 00257 max = CVDiff; 00258 mean = CVDiff; 00259 median = CVDiff; 00260 for (int i = 1; i < NumElems-1; i++) 00261 { 00262 CurVal1 = *this->get(i); 00263 CurVal2 = *this->get(i+1); 00264 CVDiff = CurVal2-CurVal1; 00265 vec1[i] = CVDiff; 00266 sum += CVDiff; 00267 if (CVDiff > max) max = CVDiff; 00268 else if (CVDiff < min) min = CVDiff; 00269 } 00270 mean = sum/((T)NumElems); 00271 00272 NumElems--; // we put in one fewer than NumElems into the vector 00273 std::sort(vec1, vec1+NumElems); // first sort the data 00274 if (NumElems % 2 == 0) 00275 { 00276 median = (vec1[NumElems/2-1] + vec1[NumElems/2])/2; 00277 } 00278 else 00279 { 00280 median = vec1[NumElems/2]; 00281 } 00282 00283 delete [] vec1; 00284 00285 return true; 00286 } 00287 }; 00288 } 00289 00290 00291 #endif // MATH_UTIL_STAT_BUFFER_H_