Struct SmootherCostFunction

Struct Documentation

struct SmootherCostFunction

Cost function for path smoothing with multiple terms including curvature, smoothness, distance from original and obstacle avoidance.

Public Functions

inline SmootherCostFunction(const Eigen::Vector2d &original_pos, double next_to_last_length_ratio, bool reversing, const nav2_costmap_2d::Costmap2D *costmap, const std::shared_ptr<ceres::BiCubicInterpolator<ceres::Grid2D<u_char>>> &costmap_interpolator, const SmootherParams &params, double costmap_weight)

A constructor for nav2_constrained_smoother::SmootherCostFunction.

Parameters:

  • original_path – Original position of the path node

  • next_to_last_length_ratio – Ratio of next path segment compared to previous. Negative if one of them represents reversing motion.

  • reversing – Whether the path segment after this node represents reversing motion.

  • costmap – A costmap to get values for collision and obstacle avoidance

  • params – Optimization weights and parameters

  • costmap_weight – Costmap cost weight. Can be params.costmap_weight or params.cusp_costmap_weight

inline ceres::CostFunction *AutoDiff()
inline void setCostmapWeight(double costmap_weight)
inline double getCostmapWeight()
template<typename T>
inline bool operator()(const T *const pt, const T *const pt_next, const T *const pt_prev, T *pt_residual) const

Smoother cost function evaluation.

Parameters:

  • pt – X, Y coords of current point

  • pt_next – X, Y coords of next point

  • pt_prev – X, Y coords of previous point

  • pt_residual – array of output residuals (smoothing, curvature, distance, cost)

Returns:

if successful in computing values

Protected Functions

template<typename T>
inline void addSmoothingResidual(const double &weight, const Eigen::Matrix<T, 2, 1> &pt, const Eigen::Matrix<T, 2, 1> &pt_next, const Eigen::Matrix<T, 2, 1> &pt_prev, T &r) const

Cost function term for smooth paths.

Parameters:

  • weight – Weight to apply to function

  • pt – Point Xi for evaluation

  • pt_next – Point Xi+1 for calculating Xi’s cost

  • pt_prev – Point Xi-1 for calculating Xi’s cost

  • r – Residual (cost) of term

template<typename T>
inline void addCurvatureResidual(const double &weight, const Eigen::Matrix<T, 2, 1> &pt, const Eigen::Matrix<T, 2, 1> &pt_next, const Eigen::Matrix<T, 2, 1> &pt_prev, T &r) const

Cost function term for maximum curved paths.

Parameters:

  • weight – Weight to apply to function

  • pt – Point Xi for evaluation

  • pt_next – Point Xi+1 for calculating Xi’s cost

  • pt_prev – Point Xi-1 for calculating Xi’s cost

  • curvature_params – A struct to cache computations for the jacobian to use

  • r – Residual (cost) of term

template<typename T>
inline void addDistanceResidual(const double &weight, const Eigen::Matrix<T, 2, 1> &xi, const Eigen::Matrix<T, 2, 1> &xi_original, T &r) const

Cost function derivative term for steering away changes in pose.

Parameters:

  • weight – Weight to apply to function

  • xi – Point Xi for evaluation

  • xi_original – original point Xi for evaluation

  • r – Residual (cost) of term

template<typename T>
inline void addCostResidual(const double &weight, const Eigen::Matrix<T, 2, 1> &pt, const Eigen::Matrix<T, 2, 1> &pt_next, const Eigen::Matrix<T, 2, 1> &pt_prev, T &r) const

Cost function term for steering away from costs.

Parameters:

  • weight – Weight to apply to function

  • value – Point Xi’s cost’

  • params – computed values to reduce overhead

  • r – Residual (cost) of term

Protected Attributes

const Eigen::Vector2d original_pos_
double next_to_last_length_ratio_
bool reversing_
SmootherParams params_
double costmap_weight_
Eigen::Vector2d costmap_origin_
double costmap_resolution_
std::shared_ptr<ceres::BiCubicInterpolator<ceres::Grid2D<u_char>>> costmap_interpolator_