interpolation_1d.h
Go to the documentation of this file.
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 &param_name,
00063     bool error_if_missing);
00064 
00065   bool readFromParameter(
00066     XmlRpc::XmlRpcValue &curve_param,
00067     const std::string &param_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 


swri_math_util
Author(s): Marc Alban
autogenerated on Thu Jun 6 2019 20:34:41