00001 // ***************************************************************************** 00002 // 00003 // Copyright (c) 2015, 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 #ifndef MATH_UTIL_INTERPOLATION_1D_H_ 00030 #define MATH_UTIL_INTERPOLATION_1D_H_ 00031 00032 #include <string> 00033 #include <vector> 00034 00035 #include <ros/node_handle.h> 00036 00037 namespace swri_math_util 00038 { 00039 class Interpolation1D 00040 { 00041 public: 00042 enum InterpolationType { 00043 ZERO_ORDER_HOLD, 00044 LINEAR 00045 }; 00046 00047 Interpolation1D(); 00048 00049 bool appendPoint(double x, double y); 00050 size_t numPoints() const; 00051 std::pair<double, double> getPoint(size_t index) const; 00052 void removePoint(size_t index); 00053 00054 void clear(); 00055 00056 InterpolationType interpolationType(); 00057 std::string interpolationTypeString() const; 00058 void setInterpolationType(InterpolationType type); 00059 00060 bool readFromParameter( 00061 const ros::NodeHandle &node_handle, 00062 const std::string ¶m_name, 00063 bool error_if_missing); 00064 00065 bool readFromParameter( 00066 XmlRpc::XmlRpcValue &curve_param, 00067 const std::string ¶m_name); 00068 00069 double minX() const; 00070 double maxX() const; 00071 00072 double eval(double x) const; 00073 00074 private: 00075 InterpolationType interp_type_; 00076 std::vector<double> x_; 00077 std::vector<double> y_; 00078 }; 00079 00080 inline 00081 double Interpolation1D::eval(double x) const 00082 { 00083 if (x_.size() == 0) { 00084 // If we have no points, we just return a sensible default value. 00085 return 0.0; 00086 } else if (x_.size() == 1) { 00087 // If we have a single point, we can't do any interpolation so we 00088 // just return the single output value. 00089 return y_[0]; 00090 } else if (x <= x_.front()) { 00091 // Clamp the output to the first output if we are below the 00092 // domain. 00093 return y_.front(); 00094 } else if (x >= x_.back()) { 00095 // Clamp the output to the last output if we are above the domain. 00096 return y_.back(); 00097 } 00098 00099 // If we pass the general special cases, we have at least two points 00100 // that bound the evaluation point. First, we find the index of the 00101 // point below the evaluation point, and then perform the 00102 // interpolation. 00103 00104 // We are searching for the interval where x_i <= x <= x_{i+1}. The 00105 // first interval has index 0, the last has index x_.size()-2. 00106 size_t i_min = 0; 00107 size_t i_max = x_.size()-2; 00108 size_t i_mid = 0; 00109 00110 while (i_min <= i_max) { 00111 i_mid = i_min + (i_max-i_min)/2; 00112 00113 if (x_[i_mid] <= x && x_[i_mid+1] >= x) { 00114 // This is the interval that contains our point, so stop searching. 00115 break; 00116 } else if (x < x_[i_mid]) { 00117 // The desired interval must be below this one. 00118 i_max = i_mid - 1; 00119 } else { 00120 i_min = i_mid + 1; 00121 } 00122 } 00123 // The desired interval is at i_mid. 00124 00125 if (interp_type_ == ZERO_ORDER_HOLD) { 00126 return y_[i_mid]; 00127 } else if (interp_type_ == LINEAR) { 00128 size_t i0 = i_mid; 00129 size_t i1 = i_mid+1; 00130 double s = (x - x_[i0]) / (x_[i1] - x_[i0]); 00131 return (1.0-s)*y_[i0] + s*y_[i1]; 00132 } else { 00133 // We should always have a valid interpolation type, but just in 00134 // case we print out an error and use a zero order hold. 00135 ROS_ERROR("Invalid interpolation type: %d", interp_type_); 00136 return y_[i_mid]; 00137 } 00138 } 00139 } // namespace swri_math_util 00140 #endif // MATH_UTIL_INTERPOLATION_1D_H_ 00141 00142