cubic_spline.cpp
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 
31 
32 #include <Eigen/Dense>
33 
34 namespace swri_geometry_util
35 {
36 
38  const std::vector<cv::Vec2d>& points,
39  double delta,
40  std::vector<std::vector<cv::Vec2d> >& splines)
41  {
42  if (delta <= 0)
43  {
44  return false;
45  }
46 
47  size_t num_points = points.size();
48 
49  splines.clear();
50  splines.resize(num_points - 1);
51 
52  // Accumulated distance along linear path.
53  std::vector<double> s = std::vector<double>(num_points);
54 
55  // Distance between consecutive points on path.
56  std::vector<double> ds = std::vector<double>(num_points);
57 
58  s[0] = 0;
59  for (size_t i = 1; i < num_points; i++)
60  {
61  double dx = points[i][0] - points[i - 1][0];
62  double dy = points[i][1] - points[i - 1][1];
63 
64  ds[i - 1] = std::sqrt(std::pow(dx, 2) + std::pow(dy, 2));
65  s[i] = s[i - 1] + ds[i - 1];
66  }
67 
68  // Normalize s to be [0:1]
69  double totalDistance = s[num_points - 1];
70  for (size_t i = 0; i < num_points; i++)
71  {
72  s[i] /= totalDistance;
73  }
74 
75  Eigen::MatrixX2d u(num_points, 2);
76  Eigen::MatrixXd A(num_points, num_points);
77  Eigen::MatrixX2d z(num_points, 2);
78 
79  // Set up the z0 equation.
80  u(0, 0) = 0;
81  u(0, 1) = 0;
82  A(0, 0) = 1;
83  for (size_t i = 1; i < num_points; i++)
84  {
85  A(0, i) = 0;
86  }
87 
88  // Set up the z1 to zn-1 equations.
89  for (size_t i = 1; i < num_points - 1; i++)
90  {
91  double hi = s[i + 1] - s[i];
92  double himl = s[i] - s[i - 1];
93 
94  u(i, 0) = 6.0 * ((points[i + 1][0] - points[i][0]) / hi - (points[i][0]
95  - points[i - 1][0]) / himl);
96  u(i, 1) = 6.0 * ((points[i + 1][1] - points[i][1]) / hi - (points[i][1]
97  - points[i - 1][1]) / himl);
98 
99  for (size_t j = 0; j < i - 1; j++)
100  {
101  A(i, j) = 0;
102  }
103  A(i, i - 1) = himl;
104  A(i, i) = 2 * (himl + hi);
105  A(i, i + 1) = hi;
106  for (size_t j = i + 2; j < num_points; j++)
107  {
108  A(i, j) = 0;
109  }
110  }
111 
112  // Set up the zn equation
113  u(num_points - 1, 0) = 0;
114  u(num_points - 1, 1) = 0;
115  for (size_t j = 0; j < num_points - 1; j++)
116  {
117  A(num_points - 1, j) = 0;
118  }
119  A(num_points - 1, num_points - 1) = 1;
120 
121  // Solve Az = u
122  z = A.colPivHouseholderQr().solve(u);
123 
124  for (size_t i = 0; i < num_points - 1; i++)
125  {
126  std::vector<cv::Vec2d>& spline = splines[i];
127 
128  spline.push_back(points[i]);
129 
130  double hi = s[i + 1] - s[i];
131  double act_dist = ds[i];
132  int32_t num_to_add = static_cast<int32_t> (act_dist / delta);
133  double normalizedDelta = hi / (num_to_add + 1);
134 
135  // Add the additional points between waypoints
136  for (int32_t j = 1; j <= num_to_add; j++)
137  {
138  // Calculate the (x,y) value of the interpolated point
139  double si = s[i] + j * normalizedDelta;
140  double xx = (z(i + 1, 0) * pow(si - s[i], 3) + z(i, 0) * pow(s[i + 1]
141  - si, 3)) / (6.0 * hi) + (points[i + 1][0] / hi - hi * z(i + 1, 0)
142  / 6.0) * (si - s[i]) + (points[i][0] / hi - hi * z(i, 0) / 6.0)
143  * (s[i + 1] - si);
144  double yy = (z(i + 1, 1) * pow(si - s[i], 3) + z(i, 1) * pow(s[i + 1]
145  - si, 3)) / (6.0 * hi) + (points[i + 1][1] / hi - hi * z(i + 1, 1)
146  / 6.0) * (si - s[i]) + (points[i][1] / hi - hi * z(i, 1) / 6.0)
147  * (s[i + 1] - si);
148 
149  // Add the point to the interpolated array
150  spline.push_back(cv::Vec2d(xx, yy));
151  }
152 
153  spline.push_back(points[i + 1]);
154  }
155 
156  return true;
157  }
158 
160  const std::vector<tf::Vector3>& points,
161  double delta,
162  std::vector<std::vector<tf::Vector3> >& splines)
163  {
164  std::vector<cv::Vec2d> cv_points(points.size());
165  for (size_t i = 0; i < points.size(); i++)
166  {
167  cv_points[i] = cv::Vec2d(points[i].x(), points[i].y());
168  }
169 
170  std::vector<std::vector<cv::Vec2d> > cv_splines;
171  bool result = CubicSplineInterpolation(cv_points, delta, cv_splines);
172 
173  splines.resize(cv_splines.size());
174  for (size_t i = 0; i < cv_splines.size(); i++)
175  {
176  splines[i].resize(cv_splines[i].size());
177  for (size_t j = 0; j < cv_splines[i].size(); j++)
178  {
179  splines[i][j] = tf::Vector3(cv_splines[i][j][0], cv_splines[i][j][1], 0);
180  }
181  }
182 
183  return result;
184  }
185 }
XmlRpcServer s
TFSIMD_FORCE_INLINE const tfScalar & y() const
bool CubicSplineInterpolation(const std::vector< cv::Vec2d > &points, double delta, std::vector< std::vector< cv::Vec2d > > &splines)
TFSIMD_FORCE_INLINE const tfScalar & x() const
TFSIMD_FORCE_INLINE const tfScalar & z() const


swri_geometry_util
Author(s): Marc Alban
autogenerated on Tue Apr 6 2021 02:50:29