stat_vector_3d.cpp
Go to the documentation of this file.
00001 /*
00002  * Three dimensional statistic vector for use with the
00003  * for ROS Node which interfaces with a wiimote control unit.
00004  * Copyright (c) 2016, Intel Corporation.
00005  *
00006  * This program is free software; you can redistribute it and/or modify it
00007  * under the terms and conditions of the GNU General Public License,
00008  * version 2, as published by the Free Software Foundation.
00009  *
00010  * This program is distributed in the hope it will be useful, but WITHOUT
00011  * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
00012  * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
00013  * more details.
00014  */
00015 
00016 /*
00017  * Initial C++ implementation by
00018  *   Mark Horn <mark.d.horn@intel.com>
00019  *
00020  * Revisions:
00021  *
00022  */
00023 
00024 #include "wiimote/stat_vector_3d.h"
00025 
00026 #include "ros/ros.h"
00027 
00028 #include <numeric>
00029 #include <functional>
00030 #include <algorithm>
00031 #include <math.h>
00032 
00033 
00034 StatVector3d::StatVector3d()
00035 {
00036   count_ = 0;
00037 }
00038 
00039 StatVector3d::StatVector3d(int x, int y, int z)
00040 {
00041   count_ = 0;
00042 
00043   addData(x, y, z);
00044 }
00045 
00046 void StatVector3d::clear()
00047 {
00048   x_.clear();
00049   y_.clear();
00050   z_.clear();
00051 
00052   count_ = 0;
00053 }
00054 
00055 int StatVector3d::size()
00056 {
00057   return count_;
00058 }
00059 
00060 void StatVector3d::addData(int x, int y, int z)
00061 {
00062   ++count_;
00063 
00064   x_.push_back(x);
00065   y_.push_back(y);
00066   z_.push_back(z);
00067 }
00068 
00069 TVectorDouble StatVector3d::getMeanRaw()
00070 {
00071   TVectorDouble result;
00072 
00073   if (x_.size() < 1)
00074   {
00075     ROS_ERROR("StatVector3d:: Not enough data points for calculations!");
00076     ros::Exception("Not enough data points for calculations");
00077 
00078     return result;
00079   }
00080 
00081   double x_sum = std::accumulate(std::begin(x_), std::end(x_), 0.0);
00082   result.push_back(x_sum / x_.size());
00083 
00084   double y_sum = std::accumulate(std::begin(y_), std::end(y_), 0.0);
00085   result.push_back(y_sum / y_.size());
00086 
00087   double z_sum = std::accumulate(std::begin(z_), std::end(z_), 0.0);
00088   result.push_back(z_sum / z_.size());
00089 
00090   return result;
00091 }
00092 
00093 TVectorDouble StatVector3d::getMeanScaled(double scale)
00094 {
00095   TVectorDouble mean = getMeanRaw();
00096 
00097   std::transform(mean.begin(), mean.end(), mean.begin(),
00098                      std::bind1st(std::multiplies<double>(), scale));
00099 
00100   return mean;
00101 }
00102 
00103 TVectorDouble StatVector3d::getVarianceRaw()
00104 {
00105   TVectorDouble result;
00106 
00107   TVectorDouble mean = getMeanRaw();
00108 
00109   if (x_.size() < 2)
00110   {
00111     ROS_ERROR("StatVector3d:: Not enough data points for calculations!");
00112     ros::Exception("Not enough data points for calculations");
00113 
00114     return result;
00115   }
00116 
00117   double accum = 0.0;
00118   std::for_each(std::begin(x_), std::end(x_), [&](const double d)  // NOLINT(build/c++11)
00119   {
00120     accum += (d - mean.at(0)) * (d - mean.at(0));
00121   });  // NOLINT(whitespace/braces)
00122 
00123   result.push_back(accum / (x_.size()-1));
00124 
00125   accum = 0.0;
00126   std::for_each(std::begin(y_), std::end(y_), [&](const double d)  // NOLINT(build/c++11)
00127   {
00128     accum += (d - mean.at(1)) * (d - mean.at(1));
00129   });  // NOLINT(whitespace/braces)
00130 
00131   result.push_back(accum / (y_.size()-1));
00132 
00133   accum = 0.0;
00134   std::for_each(std::begin(z_), std::end(z_), [&](const double d)  // NOLINT(build/c++11)
00135   {
00136     accum += (d - mean.at(2)) * (d - mean.at(2));
00137   });  // NOLINT(whitespace/braces)
00138 
00139   result.push_back(accum / (z_.size()-1));
00140 
00141   return result;
00142 }
00143 
00144 TVectorDouble StatVector3d::getVarianceScaled(double scale)
00145 {
00146   TVectorDouble variance = getVarianceRaw();
00147 
00148   std::transform(variance.begin(), variance.end(), variance.begin(),
00149                      std::bind1st(std::multiplies<double>(), scale));
00150 
00151   return variance;
00152 }
00153 
00154 TVectorDouble StatVector3d::getStandardDeviationRaw()
00155 {
00156   TVectorDouble result;
00157 
00158   TVectorDouble variance = getVarianceRaw();
00159 
00160   result.push_back(sqrt(variance.at(0)));
00161   result.push_back(sqrt(variance.at(1)));
00162   result.push_back(sqrt(variance.at(2)));
00163 
00164   return result;
00165 }
00166 
00167 TVectorDouble StatVector3d::getStandardDeviationScaled(double scale)
00168 {
00169   TVectorDouble stddev = getStandardDeviationRaw();
00170 
00171   std::transform(stddev.begin(), stddev.end(), stddev.begin(),
00172                      std::bind1st(std::multiplies<double>(), scale));
00173 
00174   return stddev;
00175 }


wiimote
Author(s): Andreas Paepcke, Melonee Wise, Mark Horn
autogenerated on Sat Jun 8 2019 20:54:41