nav2_constrained_smoother

平滑cost: template<typename T> inline void addSmoothingResidual( const double & weight, const Eigen::Matrix<T, 2, 1> & pt, const Eigen::Matrix<T, 2, 1> ......