cubic_spline.cpp
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 
00030 #include <swri_geometry_util/cubic_spline.h>
00031 
00032 #include <Eigen/Dense>
00033 
00034 namespace swri_geometry_util
00035 {
00036 
00037   bool CubicSplineInterpolation(
00038     const std::vector<cv::Vec2d>& points,
00039     double delta,
00040     std::vector<std::vector<cv::Vec2d> >& splines)
00041   {
00042     if (delta <= 0)
00043     {
00044       return false;
00045     }
00046 
00047     size_t num_points = points.size();
00048 
00049     splines.clear();
00050     splines.resize(num_points - 1);
00051 
00052     // Accumulated distance along linear path.
00053     std::vector<double> s = std::vector<double>(num_points);
00054 
00055     // Distance between consecutive points on path.
00056     std::vector<double> ds = std::vector<double>(num_points);
00057 
00058     s[0] = 0;
00059     for (size_t i = 1; i < num_points; i++)
00060     {
00061       double dx = points[i][0] - points[i - 1][0];
00062       double dy = points[i][1] - points[i - 1][1];
00063 
00064       ds[i - 1] = std::sqrt(std::pow(dx, 2) + std::pow(dy, 2));
00065       s[i] = s[i - 1] + ds[i - 1];
00066     }
00067 
00068     // Normalize s to be [0:1]
00069     double totalDistance = s[num_points - 1];
00070     for (size_t i = 0; i < num_points; i++)
00071     {
00072       s[i] /= totalDistance;
00073     }
00074 
00075     Eigen::MatrixX2d u(num_points, 2);
00076     Eigen::MatrixXd A(num_points, num_points);
00077     Eigen::MatrixX2d z(num_points, 2);
00078 
00079     // Set up the z0 equation.
00080     u(0, 0) = 0;
00081     u(0, 1) = 0;
00082     A(0, 0) = 1;
00083     for (size_t i = 1; i < num_points; i++)
00084     {
00085       A(0, i) = 0;
00086     }
00087 
00088     // Set up the z1 to zn-1 equations.
00089     for (size_t i = 1; i < num_points - 1; i++)
00090     {
00091       double hi = s[i + 1] - s[i];
00092       double himl = s[i] - s[i - 1];
00093 
00094       u(i, 0) = 6.0 * ((points[i + 1][0] - points[i][0]) / hi - (points[i][0]
00095           - points[i - 1][0]) / himl);
00096       u(i, 1) = 6.0 * ((points[i + 1][1] - points[i][1]) / hi - (points[i][1]
00097           - points[i - 1][1]) / himl);
00098 
00099       for (size_t j = 0; j < i - 1; j++)
00100       {
00101         A(i, j) = 0;
00102       }
00103       A(i, i - 1) = himl;
00104       A(i, i) = 2 * (himl + hi);
00105       A(i, i + 1) = hi;
00106       for (size_t j = i + 2; j < num_points; j++)
00107       {
00108         A(i, j) = 0;
00109       }
00110     }
00111 
00112     // Set up the zn equation
00113     u(num_points - 1, 0) = 0;
00114     u(num_points - 1, 1) = 0;
00115     for (size_t j = 0; j < num_points - 1; j++)
00116     {
00117       A(num_points - 1, j) = 0;
00118     }
00119     A(num_points - 1, num_points - 1) = 1;
00120 
00121     // Solve Az = u
00122     z = A.colPivHouseholderQr().solve(u);
00123 
00124     for (size_t i = 0; i < num_points - 1; i++)
00125     {
00126       std::vector<cv::Vec2d>& spline = splines[i];
00127 
00128       spline.push_back(points[i]);
00129 
00130       double hi = s[i + 1] - s[i];
00131       double act_dist = ds[i];
00132       int32_t num_to_add = static_cast<int32_t> (act_dist / delta);
00133       double normalizedDelta = hi / (num_to_add + 1);
00134 
00135       // Add the additional points between waypoints
00136       for (int32_t j = 1; j <= num_to_add; j++)
00137       {
00138         // Calculate the (x,y) value of the interpolated point
00139         double si = s[i] + j * normalizedDelta;
00140         double xx = (z(i + 1, 0) * pow(si - s[i], 3) + z(i, 0) * pow(s[i + 1]
00141             - si, 3)) / (6.0 * hi) + (points[i + 1][0] / hi - hi * z(i + 1, 0)
00142             / 6.0) * (si - s[i]) + (points[i][0] / hi - hi * z(i, 0) / 6.0)
00143             * (s[i + 1] - si);
00144         double yy = (z(i + 1, 1) * pow(si - s[i], 3) + z(i, 1) * pow(s[i + 1]
00145             - si, 3)) / (6.0 * hi) + (points[i + 1][1] / hi - hi * z(i + 1, 1)
00146             / 6.0) * (si - s[i]) + (points[i][1] / hi - hi * z(i, 1) / 6.0)
00147             * (s[i + 1] - si);
00148 
00149         // Add the point to the interpolated array
00150         spline.push_back(cv::Vec2d(xx, yy));
00151       }
00152 
00153       spline.push_back(points[i + 1]);
00154     }
00155 
00156     return true;
00157   }
00158 
00159   bool CubicSplineInterpolation(
00160     const std::vector<tf::Vector3>& points,
00161     double delta,
00162     std::vector<std::vector<tf::Vector3> >& splines)
00163   {
00164     std::vector<cv::Vec2d> cv_points(points.size());
00165     for (size_t i = 0; i < points.size(); i++)
00166     {
00167       cv_points[i] = cv::Vec2d(points[i].x(), points[i].y());
00168     }
00169 
00170     std::vector<std::vector<cv::Vec2d> > cv_splines;
00171     bool result = CubicSplineInterpolation(cv_points, delta, cv_splines);
00172 
00173     splines.resize(cv_splines.size());
00174     for (size_t i = 0; i < cv_splines.size(); i++)
00175     {
00176       splines[i].resize(cv_splines[i].size());
00177       for (size_t j = 0; j < cv_splines[i].size(); j++)
00178       {
00179         splines[i][j] = tf::Vector3(cv_splines[i][j][0], cv_splines[i][j][1], 0);
00180       }
00181     }
00182 
00183     return result;
00184   }
00185 }


swri_geometry_util
Author(s): Marc Alban
autogenerated on Tue Oct 3 2017 03:19:17