adding nlls term file

smac_planner/include/smac_planner/upsampler_cost_function_nlls.hpp

@@ -0,0 +1,336 @@
+// Copyright (c) 2020, Samsung Research America
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License. Reserved.
+
+#ifndef SMAC_PLANNER__COST_FUNCTION_NLLS_HPP_
+#define SMAC_PLANNER__COST_FUNCTION_NLLS_HPP_
+
+#include <cmath>
+#include <vector>
+#include <iostream>
+#include <unordered_map>
+#include <memory>
+#include <queue>
+#include <utility>
+
+#include "ceres/ceres.h"
+#include "Eigen/Core"
+#include "smac_planner/types.hpp"
+#include "smac_planner/minimal_costmap.hpp"
+#include "smac_planner/options.hpp"
+
+#define EPSILON 0.0001  
+
+namespace smac_planner
+{
+
+// TODO reduce code duplication. there's very litle change here.
+
+// upsampler params TODO
+
+// curvature minimization ? TODO
+
+/**
+ * @struct smac_planner::UpsamplerConstrainedCostFunction
+ * @brief Cost function for path upsampling with multiple terms using NLLS
+ * including curvature, smoothness, collision, and avoid obstacles.
+ */
+class UpsamplerConstrainedCostFunction : public ceres::SizedCostFunction<1, 1, 1> 
+{
+public:
+  /**
+   * @brief A constructor for smac_planner::UpsamplerConstrainedCostFunction
+   * @param num_points Number of path points to consider
+   */
+  UpsamplerConstrainedCostFunction(
+    const std::vector<Eigen::Vector2d> & path,
+    const SmootherParams & params,
+    const int & upsample_ratio,
+    const int & i)
+  : _path(path),
+    _params(params),
+    _upsample_ratio(upsample_ratio),
+    index(i)
+  {
+  }
+
+  /**
+   * @struct CurvatureComputations
+   * @brief Cache common computations between the curvature terms to minimize recomputations
+   */
+  struct CurvatureComputations 
+  {
+    /**
+     * @brief A constructor for smac_planner::CurvatureComputations
+     */
+    CurvatureComputations() {
+      valid = true;
+    }
+
+    bool valid;
+    /**
+     * @brief Check if result is valid for penalty
+     * @return is valid (non-nan, non-inf, and turning angle > max)
+     */
+    bool isValid() {
+      return valid;
+    }
+
+    Eigen::Vector2d delta_xi{0,0};
+    Eigen::Vector2d delta_xi_p{0,0};
+    double delta_xi_norm{0};
+    double delta_xi_p_norm{0};
+    double delta_phi_i{0};
+    double turning_rad{0};
+    double ki_minus_kmax{0};
+  };
+
+  /**
+   * @brief Smoother cost function evaluation
+   * @param parameters X,Y pairs of points 
+   * @param cost total cost of path
+   * @param gradient of path at each X,Y pair from cost function derived analytically
+   * @return if successful in computing values
+   */
+
+  bool Evaluate(
+    double const * const * parameters,
+    double * residuals,
+    double ** jacobians) const override
+  {
+    Eigen::Vector2d xi = Eigen::Vector2d(parameters[0][0], parameters[1][0]);
+    Eigen::Vector2d xi_p1 = _path.at(index + 1);
+    Eigen::Vector2d xi_m1 = _path.at(index - 1);
+    CurvatureComputations curvature_params;
+    double grad_x_raw = 0, grad_y_raw = 0, cost_raw = 0;
+
+    // compute cost
+    addSmoothingResidual(15000, xi, xi_p1, xi_m1, cost_raw);  // TODO params
+    addCurvatureResidual(60.0, xi, xi_p1, xi_m1, curvature_params, cost_raw);
+
+    residuals[0] = 0;
+    residuals[0] = cost_raw;  // objective function value x
+
+    if (jacobians != NULL && jacobians[0] != NULL) {
+      addSmoothingJacobian(15000, xi, xi_p1, xi_m1, grad_x_raw, grad_y_raw);
+      addCurvatureJacobian(60.0, xi, xi_p1, xi_m1, curvature_params, grad_x_raw, grad_y_raw);
+
+      jacobians[0][0] = 0;
+      jacobians[1][0] = 0;
+      jacobians[0][0] = grad_x_raw;  // x derivative
+      jacobians[1][0] = grad_y_raw;  // y derivative
+      jacobians[0][1] = 0.0;
+      jacobians[1][1] = 0.0;
+    }
+
+    return true;
+  }
+
+protected:
+  /**
+   * @brief Cost function term for smooth paths
+   * @param weight Weight to apply to function
+   * @param pt Point Xi for evaluation
+   * @param pt Point Xi+1 for calculating Xi's cost
+   * @param pt Point Xi-1 for calculating Xi's cost
+   * @param r Residual (cost) of term
+   */
+  inline void addSmoothingResidual(
+    const double & weight,
+    const Eigen::Vector2d & pt,
+    const Eigen::Vector2d & pt_p,
+    const Eigen::Vector2d & pt_m,
+    double & r) const
+  {
+    r += weight * (
+      pt_p.dot(pt_p)
+      - 4 * pt_p.dot(pt)
+      + 2 * pt_p.dot(pt_m)
+      + 4 * pt.dot(pt)
+      - 4 * pt.dot(pt_m)
+      + pt_m.dot(pt_m));  // objective function value
+  }
+
+  /**
+   * @brief Cost function derivative term for smooth paths
+   * @param weight Weight to apply to function
+   * @param pt Point Xi for evaluation
+   * @param pt Point Xi+1 for calculating Xi's cost
+   * @param pt Point Xi-1 for calculating Xi's cost
+   * @param j0 Gradient of X term
+   * @param j1 Gradient of Y term
+   */
+  inline void addSmoothingJacobian(
+    const double & weight,
+    const Eigen::Vector2d & pt,
+    const Eigen::Vector2d & pt_p,
+    const Eigen::Vector2d & pt_m,
+    double & j0,
+    double & j1) const
+  {
+    j0 += weight *
+      (- 4 * pt_m[0] + 8 * pt[0] - 4 * pt_p[0]);  // xi x component of partial-derivative
+    j1 += weight *
+      (- 4 * pt_m[1] + 8 * pt[1] - 4 * pt_p[1]);  // xi y component of partial-derivative
+  }
+
+  /**
+   * @brief Get path curvature information
+   * @param pt Point Xi for evaluation
+   * @param pt Point Xi+1 for calculating Xi's cost
+   * @param pt Point Xi-1 for calculating Xi's cost
+   * @param curvature_params A struct to cache computations for the jacobian to use
+   */
+  inline void getCurvatureParams(
+    const Eigen::Vector2d & pt,
+    const Eigen::Vector2d & pt_p,
+    const Eigen::Vector2d & pt_m,
+    CurvatureComputations & curvature_params) const
+  {
+    curvature_params.valid = true;
+    curvature_params.delta_xi = Eigen::Vector2d(pt[0] - pt_m[0], pt[1] - pt_m[1]);
+    curvature_params.delta_xi_p = Eigen::Vector2d(pt_p[0] - pt[0], pt_p[1] - pt[1]); 
+    curvature_params.delta_xi_norm = curvature_params.delta_xi.norm();
+    curvature_params.delta_xi_p_norm = curvature_params.delta_xi_p.norm();
+
+    if (curvature_params.delta_xi_norm < EPSILON || curvature_params.delta_xi_p_norm < EPSILON ||
+      std::isnan(curvature_params.delta_xi_p_norm) || std::isnan(curvature_params.delta_xi_norm) ||
+      std::isinf(curvature_params.delta_xi_p_norm) || std::isinf(curvature_params.delta_xi_norm)) {
+      // ensure we have non-nan values returned
+      curvature_params.valid = false;
+      return;
+    }
+
+    const double & delta_xi_by_xi_p =
+      curvature_params.delta_xi_norm * curvature_params.delta_xi_p_norm;
+    double projection =
+      curvature_params.delta_xi.dot(curvature_params.delta_xi_p) / delta_xi_by_xi_p;
+    if (fabs(1 - projection) < EPSILON || fabs(projection + 1) < EPSILON) {
+      projection = 1.0;
+    }
+
+    curvature_params.delta_phi_i = std::acos(projection);
+    curvature_params.turning_rad = curvature_params.delta_phi_i / curvature_params.delta_xi_norm;
+
+    curvature_params.ki_minus_kmax = curvature_params.turning_rad - _upsample_ratio * _params.max_curvature;
+    // TODO is use of upsample_ratio correct here? small number?
+    // TODO can remove the subtraction with a lower weight value, does have direction issue, maybe just tuning?
+
+    if (curvature_params.ki_minus_kmax <= EPSILON) {
+      // Quadratic penalty need not apply  
+      curvature_params.valid = false;
+    }
+  }
+
+  /**
+   * @brief Cost function term for maximum curved paths
+   * @param weight Weight to apply to function
+   * @param pt Point Xi for evaluation
+   * @param pt Point Xi+1 for calculating Xi's cost
+   * @param pt Point Xi-1 for calculating Xi's cost
+   * @param curvature_params A struct to cache computations for the jacobian to use
+   * @param r Residual (cost) of term
+   */
+  inline void addCurvatureResidual(
+    const double & weight,
+    const Eigen::Vector2d & pt,
+    const Eigen::Vector2d & pt_p,
+    const Eigen::Vector2d & pt_m,
+    CurvatureComputations & curvature_params,
+    double & r) const
+  {
+    getCurvatureParams(pt, pt_p, pt_m, curvature_params);
+
+    if (!curvature_params.isValid()) { 
+      return;
+    }
+
+    r += weight *
+      curvature_params.ki_minus_kmax * curvature_params.ki_minus_kmax;  // objective function value
+  }
+
+  /**
+   * @brief Cost function derivative term for maximum curvature paths
+   * @param weight Weight to apply to function
+   * @param pt Point Xi for evaluation
+   * @param pt Point Xi+1 for calculating Xi's cost
+   * @param pt Point Xi-1 for calculating Xi's cost
+   * @param curvature_params A struct with cached values to speed up Jacobian computation
+   * @param j0 Gradient of X term
+   * @param j1 Gradient of Y term
+   */
+  inline void addCurvatureJacobian(
+    const double & weight,
+    const Eigen::Vector2d & pt,
+    const Eigen::Vector2d & pt_p,
+    const Eigen::Vector2d & pt_m,
+    CurvatureComputations & curvature_params,
+    double & j0,
+    double & j1) const
+  {
+    if (!curvature_params.isValid()) {
+      return;
+    }
+
+    const double & partial_delta_phi_i_wrt_cost_delta_phi_i =
+      -1 / std::sqrt(1 - std::pow(std::cos(curvature_params.delta_phi_i), 2));
+    const Eigen::Vector2d ones = Eigen::Vector2d(1.0, 1.0);
+    auto neg_pt_plus = -1 * pt_p;
+    Eigen::Vector2d p1 = normalizedOrthogonalComplement(
+      pt, neg_pt_plus, curvature_params.delta_xi_norm, curvature_params.delta_xi_p_norm);
+    Eigen::Vector2d p2 = normalizedOrthogonalComplement(
+      neg_pt_plus, pt, curvature_params.delta_xi_p_norm, curvature_params.delta_xi_norm);
+
+    const double & u = 2 * curvature_params.ki_minus_kmax;
+    const double & common_prefix =
+      (1 / curvature_params.delta_xi_norm) * partial_delta_phi_i_wrt_cost_delta_phi_i;
+    const double & common_suffix = curvature_params.delta_phi_i /
+      (curvature_params.delta_xi_norm * curvature_params.delta_xi_norm);
+    const Eigen::Vector2d & d_delta_xi_d_xi = curvature_params.delta_xi / curvature_params.delta_xi_norm;
+
+    const Eigen::Vector2d jacobian =  u * (common_prefix * (-p1 - p2) - (common_suffix * d_delta_xi_d_xi));
+    const Eigen::Vector2d jacobian_im1 = u * (common_prefix * p2 + (common_suffix * d_delta_xi_d_xi));
+    const Eigen::Vector2d jacobian_ip1 = u * (common_prefix * p1);
+    // j0 += weight * jacobian[0]; // TODO try with the prior xi-1 and xi+1s
+    // j1 += weight * jacobian[1];
+    j0 += weight *
+      (jacobian_im1[0] + 2 * jacobian[0] + jacobian_ip1[0]);  // xi x component of partial-derivative  // TODO I think better without this?
+    j1 += weight *
+      (jacobian_im1[1] + 2 * jacobian[1] + jacobian_ip1[1]);  // xi y component of partial-derivative  // Maybe reflects issue in not using xi-1/+1's
+  }
+  /**
+   * @brief Computing the normalized orthogonal component of 2 vectors
+   * @param a Vector
+   * @param b Vector
+   * @param norm a Vector's norm
+   * @param norm b Vector's norm
+   * @return Normalized vector of orthogonal components
+   */
+  inline Eigen::Vector2d normalizedOrthogonalComplement(
+    const Eigen::Vector2d & a,
+    const Eigen::Vector2d & b,
+    const double & a_norm,
+    const double & b_norm) const
+  {
+    return (a - (a.dot(b) * b / b.squaredNorm())) / (a_norm * b_norm);
+  }
+
+  SmootherParams _params;
+  int _upsample_ratio;
+  std::vector<Eigen::Vector2d> _path;
+  int index;
+};
+
+}  // namespace smac_planner
+
+#endif  // SMAC_PLANNER__COST_FUNCTION_NLLS_HPP_