UtilFunctions.h
Go to the documentation of this file.
00001 //=================================================================================================
00002 // Copyright (c) 2011, Stefan Kohlbrecher, TU Darmstadt
00003 // All rights reserved.
00004 
00005 // Redistribution and use in source and binary forms, with or without
00006 // modification, are permitted provided that the following conditions are met:
00007 //     * Redistributions of source code must retain the above copyright
00008 //       notice, this list of conditions and the following disclaimer.
00009 //     * Redistributions in binary form must reproduce the above copyright
00010 //       notice, this list of conditions and the following disclaimer in the
00011 //       documentation and/or other materials provided with the distribution.
00012 //     * Neither the name of the Simulation, Systems Optimization and Robotics
00013 //       group, TU Darmstadt nor the names of its contributors may be used to
00014 //       endorse or promote products derived from this software without
00015 //       specific prior written permission.
00016 
00017 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00018 // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00019 // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00020 // DISCLAIMED. IN NO EVENT SHALL THE 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 #ifndef utilfunctions_h__
00030 #define utilfunctions_h__
00031 
00032 #include <cmath>
00033 
00034 namespace util{
00035 
00036 static inline float normalize_angle_pos(float angle)
00037 {
00038   return fmod(fmod(angle, 2.0f*M_PI) + 2.0f*M_PI, 2.0f*M_PI);
00039 }
00040 
00041 static inline float normalize_angle(float angle)
00042 {
00043   float a = normalize_angle_pos(angle);
00044   if (a > M_PI){
00045     a -= 2.0f*M_PI;
00046   }
00047   return a;
00048 }
00049 
00050 static inline float sqr(float val)
00051 {
00052   return val*val;
00053 }
00054 
00055 static inline int sign(int x)
00056 {
00057   return x > 0 ? 1 : -1;
00058 }
00059 
00060 template<typename T>
00061 static T toDeg(const T radVal)
00062 {
00063   return radVal * static_cast<T>(180.0 / M_PI);
00064 }
00065 
00066 template<typename T>
00067 static T toRad(const T degVal)
00068 {
00069   return degVal * static_cast<T>(M_PI / 180.0);
00070 }
00071 
00072 static bool poseDifferenceLargerThan(const Eigen::Vector3f& pose1, const Eigen::Vector3f& pose2, float distanceDiffThresh, float angleDiffThresh)
00073 {
00074   //check distance
00075   if ( ( (pose1.head<2>() - pose2.head<2>()).norm() ) > distanceDiffThresh){
00076     return true;
00077   }
00078 
00079   float angleDiff = (pose1.z() - pose2.z());
00080 
00081   if (angleDiff > M_PI) {
00082     angleDiff -= M_PI * 2.0f;
00083   } else if (angleDiff < -M_PI) {
00084     angleDiff += M_PI * 2.0f;
00085   }
00086 
00087   if (abs(angleDiff) > angleDiffThresh){
00088     return true;
00089   }
00090   return false;
00091 }
00092 
00093 }
00094 
00095 #endif


hector_mapping
Author(s): Stefan Kohlbrecher
autogenerated on Wed Aug 26 2015 11:45:03