interpolation_1d.h
Go to the documentation of this file.
1 // *****************************************************************************
2 //
3 // Copyright (c) 2015, Southwest Research Institute® (SwRI®)
4 // All rights reserved.
5 //
6 // Redistribution and use in source and binary forms, with or without
7 // modification, are permitted provided that the following conditions are met:
8 // * Redistributions of source code must retain the above copyright
9 // notice, this list of conditions and the following disclaimer.
10 // * Redistributions in binary form must reproduce the above copyright
11 // notice, this list of conditions and the following disclaimer in the
12 // documentation and/or other materials provided with the distribution.
13 // * Neither the name of Southwest Research Institute® (SwRI®) nor the
14 // names of its contributors may be used to endorse or promote products
15 // derived from this software without specific prior written permission.
16 //
17 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20 // ARE DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY
21 // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
22 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
23 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
24 // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
25 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
26 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
27 //
28 // *****************************************************************************
29 #ifndef MATH_UTIL_INTERPOLATION_1D_H_
30 #define MATH_UTIL_INTERPOLATION_1D_H_
31 
32 #include <string>
33 #include <vector>
34 
35 #include <ros/node_handle.h>
36 
37 namespace swri_math_util
38 {
40 {
41  public:
45  };
46 
48 
49  bool appendPoint(double x, double y);
50  size_t numPoints() const;
51  std::pair<double, double> getPoint(size_t index) const;
52  void removePoint(size_t index);
53 
54  void clear();
55 
57  std::string interpolationTypeString() const;
59 
60  bool readFromParameter(
61  const ros::NodeHandle &node_handle,
62  const std::string &param_name,
63  bool error_if_missing);
64 
65  bool readFromParameter(
66  XmlRpc::XmlRpcValue &curve_param,
67  const std::string &param_name);
68 
69  double minX() const;
70  double maxX() const;
71 
72  double eval(double x) const;
73 
74  private:
76  std::vector<double> x_;
77  std::vector<double> y_;
78 };
79 
80 inline
81 double Interpolation1D::eval(double x) const
82 {
83  if (x_.size() == 0) {
84  // If we have no points, we just return a sensible default value.
85  return 0.0;
86  } else if (x_.size() == 1) {
87  // If we have a single point, we can't do any interpolation so we
88  // just return the single output value.
89  return y_[0];
90  } else if (x <= x_.front()) {
91  // Clamp the output to the first output if we are below the
92  // domain.
93  return y_.front();
94  } else if (x >= x_.back()) {
95  // Clamp the output to the last output if we are above the domain.
96  return y_.back();
97  }
98 
99  // If we pass the general special cases, we have at least two points
100  // that bound the evaluation point. First, we find the index of the
101  // point below the evaluation point, and then perform the
102  // interpolation.
103 
104  // We are searching for the interval where x_i <= x <= x_{i+1}. The
105  // first interval has index 0, the last has index x_.size()-2.
106  size_t i_min = 0;
107  size_t i_max = x_.size()-2;
108  size_t i_mid = 0;
109 
110  while (i_min <= i_max) {
111  i_mid = i_min + (i_max-i_min)/2;
112 
113  if (x_[i_mid] <= x && x_[i_mid+1] >= x) {
114  // This is the interval that contains our point, so stop searching.
115  break;
116  } else if (x < x_[i_mid]) {
117  // The desired interval must be below this one.
118  i_max = i_mid - 1;
119  } else {
120  i_min = i_mid + 1;
121  }
122  }
123  // The desired interval is at i_mid.
124 
125  if (interp_type_ == ZERO_ORDER_HOLD) {
126  return y_[i_mid];
127  } else if (interp_type_ == LINEAR) {
128  size_t i0 = i_mid;
129  size_t i1 = i_mid+1;
130  double s = (x - x_[i0]) / (x_[i1] - x_[i0]);
131  return (1.0-s)*y_[i0] + s*y_[i1];
132  } else {
133  // We should always have a valid interpolation type, but just in
134  // case we print out an error and use a zero order hold.
135  ROS_ERROR("Invalid interpolation type: %d", interp_type_);
136  return y_[i_mid];
137  }
138 }
139 } // namespace swri_math_util
140 #endif // MATH_UTIL_INTERPOLATION_1D_H_
141 
142 
bool appendPoint(double x, double y)
XmlRpcServer s
std::string interpolationTypeString() const
void setInterpolationType(InterpolationType type)
std::pair< double, double > getPoint(size_t index) const
double eval(double x) const
#define ROS_ERROR(...)
bool readFromParameter(const ros::NodeHandle &node_handle, const std::string &param_name, bool error_if_missing)
InterpolationType interpolationType()


swri_math_util
Author(s): Marc Alban
autogenerated on Sat Jan 21 2023 03:13:11