common.hpp
Go to the documentation of this file.
00001 /*
00002  * common.hpp
00003  *
00004  *  Created on: Apr 11, 2013
00005  *      Author: jorge
00006  */
00007 
00008 #ifndef COMMON_HPP_
00009 #define COMMON_HPP_
00010 
00011 
00012 #include <tf/tf.h>
00013 
00014 #include <geometry_msgs/Pose.h>
00015 #include <geometry_msgs/PoseStamped.h>
00016 
00017 
00018 namespace mtk
00019 {
00020 
00021 // Template functions
00022 
00023 template <typename T> std::string nb2str(T x)
00024 {
00025   return static_cast<std::ostringstream*>( &(std::ostringstream() << x) )->str();
00026 }
00027 
00028 template <typename T> T median(const std::vector<T>& v) {
00029   // Return the median element of a vector
00030   std::vector<T> c = v;  // clone, as nth_element will short half of the vector
00031   std::nth_element(c.begin(), c.begin() + c.size()/2, c.end());
00032   return c[c.size()/2];
00033 };
00034 
00035 template <typename T> T mean(const std::vector<T>& v)
00036 {
00037   T acc = 0;
00038   for (unsigned int i = 0; i < v.size(); i++)
00039     acc += v[i];
00040   return acc/v.size();
00041 };
00042 
00043 template <typename T> T variance(const std::vector<T>& v)
00044 {
00045   T acc = 0;
00046   T avg = mean(v);
00047   for (unsigned int i = 0; i < v.size(); i++)
00048     acc += std::pow(v[i] - avg, 2);
00049   return acc/(v.size() - 1);
00050 };
00051 
00052 template <typename T> T std_dev(const std::vector<T>& v)
00053 {
00054   return std::sqrt(variance(v));
00055 }
00056 
00057 
00058 template <typename T> T sign(T x)
00059 {
00060   return x > 0.0 ? +1.0 : x < 0.0 ? -1.0 : 0.0;
00061 }
00062 
00063 // Other functions
00064 
00065 void tf2pose(const tf::Transform& tf, geometry_msgs::Pose& pose);
00066 void tf2pose(const tf::StampedTransform& tf, geometry_msgs::PoseStamped& pose);
00067 
00068 void pose2tf(const geometry_msgs::Pose& pose, tf::Transform& tf);
00069 void pose2tf(const geometry_msgs::PoseStamped& pose, tf::StampedTransform& tf);
00070 
00071 const char* point2str(const geometry_msgs::Point& point);
00072 const char* pose2str(const geometry_msgs::Pose& pose);
00073 const char* pose2str(const geometry_msgs::PoseStamped& pose);
00074 
00075 
00076 } /* namespace mtk */
00077 
00078 #endif /* COMMON_HPP_ */


yocs_math_toolkit
Author(s): Jorge Santos
autogenerated on Thu Jun 6 2019 21:47:25