Ego planner code parsing Bspline_ Optimizer section (3)

1、 int BsplineOptimizer::earlyExit(void *func_data, const double *x, const double *g, const double fx, const double xnorm, const double gnorm, const double step, int n, int k, int ls)

// If force_stop_type_ Not for DONT_STOP Just go back to true, Otherwise return to false.

int BsplineOptimizer::earlyExit(void *func_data, const double *x, const double *g, const double fx, const double xnorm, const double gnorm, const double step, int n, int k, int ls)
  {
    BsplineOptimizer *opt = reinterpret_cast<BsplineOptimizer *>(func_data);
    // cout << "k=" << k << endl;
    // cout << "opt->flag_continue_to_optimize_=" << opt->flag_continue_to_optimize_ << endl;
    return (opt->force_stop_type_ == STOP_FOR_ERROR || opt->force_stop_type_ == STOP_FOR_REBOUND);
  }
  // If force_stop_type_ Not for DONT_STOP Just go back to true, Otherwise return to false.

2、double BsplineOptimizer::costFunctionRebound(void *func_data, const double *x, double *grad, const int n)

// call combineCostRebound(x,grad,cost,n) return cost cost .

double BsplineOptimizer::costFunctionRebound(void *func_data, const double *x, double *grad, const int n)
  {
    BsplineOptimizer *opt = reinterpret_cast<BsplineOptimizer *>(func_data);

    double cost;
    opt->combineCostRebound(x, grad, cost, n);

    opt->iter_num_ += 1;
    return cost;
  }
  // call combineCostRebound(x,grad,cost,n)  return cost.

3、double BsplineOptimizer::costFunctionRefine(void *func_data, const double *x, double *grad, const int n)

// call combineCostRefine(x,grad,cost,n) Return the weighted cost cost

double BsplineOptimizer::costFunctionRefine(void *func_data, const double *x, double *grad, const int n)
  {
    BsplineOptimizer *opt = reinterpret_cast<BsplineOptimizer *>(func_data);

    double cost;
    opt->combineCostRefine(x, grad, cost, n);

    opt->iter_num_ += 1;
    return cost;
  }
  // call combineCostRefine(x,grad,cost,n)  return cost

4、void BsplineOptimizer::calcDistanceCostRebound(const Eigen::MatrixXd &q, double &cost,

                                                 Eigen::MatrixXd &gradient, int iter_num, double smoothness_cost)

 // Calculate the collision penalty , In the paper Collision Penalty Partial formula (5)(6)(7)

 void BsplineOptimizer::calcDistanceCostRebound(const Eigen::MatrixXd &q, double &cost,
                                                 Eigen::MatrixXd &gradient, int iter_num, double smoothness_cost)
   // Calculate the collision penalty , In the paper Collision Penalty Partial formula (5)(6)(7)
  {
    cost = 0.0;// cost 
    int end_idx = q.cols() - order_;// Last index 
    double demarcation = cps_.clearance;// Safety distance of control point sf
    double a = 3 * demarcation, b = -3 * pow(demarcation, 2), c = pow(demarcation, 3);
    //a=3 Times the safe distance 
    //b=-3*sf The square of 
    //c=sf The square of 

    force_stop_type_ = DONT_STOP;// Stop reason 
    if (iter_num > 3 && smoothness_cost / (cps_.size - 2 * order_) < 0.1)
    // 0.1 is an experimental value that indicates the trajectory is smooth enough.
    //0.1 It's an experimental value , Indicates that the trajectory is smooth enough .
    {
      check_collision_and_rebound();// Check collision and corresponding trajectory pop-up 
    }

    /*** calculate distance cost and gradient ***/
    // Calculate distance cost and gradient 
    for (auto i = order_; i < end_idx; ++i)// Traverse the control point 
    {
      for (size_t j = 0; j < cps_.direction[i].size(); ++j)// Traverse all base points 
      {
        double dist = (cps_.points.col(i) - cps_.base_point[i][j]).dot(cps_.direction[i][j]);
        // distance =( The first i Control points - The first i The number corresponding to the first control point j BPS （ That is, from the control point to j Distance between obstacles ）)* The direction vector of this direction 
        double dist_err = cps_.clearance - dist,;
        // A safe distance from - distance , Corresponding to cij, namely cij=sf-dij
        Eigen::Vector3d dist_grad = cps_.direction[i][j];// The distance gradient is i The first control point corresponds to the j Direction of basis points 

        if (dist_err < 0)// If cij <0
        {
          /* do nothing */
        }
        else if (dist_err < demarcation)// If cij Less than the safe distance （0<cij<sf）
        {
          cost += pow(dist_err, 3);// The cost is cij The third power of 
          gradient.col(i) += -3.0 * dist_err * dist_err * dist_grad;
          // The gradient of this control point is = Gradient of control point +(-3*cij*cij* Gradient direction ), Corresponding to the second formula for calculating the gradient in the paper 
        }
        else// If cij>sf, The distance between the control point and the obstacle is less than the safe distance 
        {
          cost += a * dist_err * dist_err + b * dist_err + c;
          // cost = cost +a*cij*cij+b*cij+c, among ,a=3 Times sf,b=-3*sf The square of ,c=sf The square of 
          gradient.col(i) += -(2.0 * a * dist_err + b) * dist_grad;
          // The gradient of this control point is = Gradient of control point -(2.0 * a * cij + b) *  Gradient direction 
        }
      }
    }
  }
  // Calculate the collision penalty , In the paper Collision Penalty Partial formula (5)(6)(7)

 

 5、void BsplineOptimizer::calcFitnessCost(const Eigen::MatrixXd &q, double &cost, Eigen::MatrixXd &gradient)

// Calculate the cost of adaptation

 

 void BsplineOptimizer::calcFitnessCost(const Eigen::MatrixXd &q, double &cost, Eigen::MatrixXd &gradient)
  // Calculate the cost of adaptation 
  {

    cost = 0.0;// The cost is 0

    int end_idx = q.cols() - order_;

    // def: f = |x*v|^2/a^2 + |x×v|^2/b^2
    double a2 = 25, b2 = 1;
    for (auto i = order_ - 1; i < end_idx + 1; ++i)
    {
      Eigen::Vector3d x = (q.col(i - 1) + 4 * q.col(i) + q.col(i + 1)) / 6.0 - ref_pts_[i - 1];
      Eigen::Vector3d v = (ref_pts_[i] - ref_pts_[i - 2]).normalized();

      double xdotv = x.dot(v);
      Eigen::Vector3d xcrossv = x.cross(v);

      double f = pow((xdotv), 2) / a2 + pow(xcrossv.norm(), 2) / b2;
      cost += f;

      Eigen::Matrix3d m;
      m << 0, -v(2), v(1), v(2), 0, -v(0), -v(1), v(0), 0;
      Eigen::Vector3d df_dx = 2 * xdotv / a2 * v + 2 / b2 * m * xcrossv;

      gradient.col(i - 1) += df_dx / 6;
      gradient.col(i) += 4 * df_dx / 6;
      gradient.col(i + 1) += df_dx / 6;
    }
  }

6、void BsplineOptimizer::calcSmoothnessCost(const Eigen::MatrixXd &q, double &cost,

                                            Eigen::MatrixXd &gradient, bool falg_use_jerk /* = true*/)

// Calculate the smoothing cost , In the paper Smoothness Penalty Partial formula (4)

void BsplineOptimizer::calcSmoothnessCost(const Eigen::MatrixXd &q, double &cost,
                                            Eigen::MatrixXd &gradient, bool falg_use_jerk /* = true*/)
  // Calculate the smoothing cost , In the paper Smoothness Penalty Partial formula (4)
  {

    cost = 0.0;

    if (falg_use_jerk)// Speed up 
    {
      Eigen::Vector3d jerk, temp_j;

      for (int i = 0; i < q.cols() - 3; i++)//i=0 To Nc-2
      {
        /* evaluate jerk */
        jerk = q.col(i + 3) - 3 * q.col(i + 2) + 3 * q.col(i + 1) - q.col(i);
        // Calculate the acceleration 
        cost += jerk.squaredNorm();// Add up 
        temp_j = 2.0 * jerk;
        /* jerk gradient */// Add velocity gradient 
        gradient.col(i + 0) += -temp_j;
        gradient.col(i + 1) += 3.0 * temp_j;
        gradient.col(i + 2) += -3.0 * temp_j;
        gradient.col(i + 3) += temp_j;
      }
    }
    else
    {
      Eigen::Vector3d acc, temp_acc;// The acceleration 

      for (int i = 0; i < q.cols() - 2; i++)//i=0 To Nc-1
      {
        /* evaluate acc */
        acc = q.col(i + 2) - 2 * q.col(i + 1) + q.col(i);
        cost += acc.squaredNorm();// Add up 
        temp_acc = 2.0 * acc;
        /* acc gradient */// Acceleration gradient 
        gradient.col(i + 0) += temp_acc;
        gradient.col(i + 1) += -2.0 * temp_acc;
        gradient.col(i + 2) += temp_acc;
      }
    }
  }

7、  void BsplineOptimizer::calcFeasibilityCost(const Eigen::MatrixXd &q, double &cost,

                                             Eigen::MatrixXd &gradient)

 // Calculate the feasibility cost , In the paper Feasibility Penalty Partial formula (8)(9)(10)

 // The feasibility is ensured by limiting the higher derivative of the trajectory in each dimension

 

void BsplineOptimizer::calcFeasibilityCost(const Eigen::MatrixXd &q, double &cost,
                                             Eigen::MatrixXd &gradient)
  // Calculate the feasibility cost , In the paper Feasibility Penalty Partial formula (8)(9)(10)
  // The feasibility is ensured by limiting the higher derivative of the trajectory in each dimension 
  {

    //#define SECOND_DERIVATIVE_CONTINOUS, The second derivative is continuous 

#ifdef SECOND_DERIVATIVE_CONTINOUS

    cost = 0.0;
    double demarcation = 1.0; // 1m/s, 1m/s/s   Safe range 
    double ar = 3 * demarcation, br = -3 * pow(demarcation, 2), cr = pow(demarcation, 3);
    double al = ar, bl = -br, cl = cr;

    /* abbreviation */
    double ts, ts_inv2, ts_inv3;
    ts = bspline_interval_;//ts by b Spline interval 
    ts_inv2 = 1 / ts / ts;// The acceleration 
    ts_inv3 = 1 / ts / ts / ts;// Speed up 

    /* velocity feasibility */
    // Speed feasibility 
    for (int i = 0; i < q.cols() - 1; i++)
    {
      Eigen::Vector3d vi = (q.col(i + 1) - q.col(i)) / ts;
      // The speed is the last control point - Divide the previous control point by time ts

      for (int j = 0; j < 3; j++)
      {
        if (vi(j) > max_vel_ + demarcation)// If the speed is greater than the maximum speed + Safe speed 1m/s
        {
          double diff = vi(j) - max_vel_;// The difference is the current speed - Maximum speed 
          cost += (ar * diff * diff + br * diff + cr) * ts_inv3; // cost 
          // multiply ts_inv3 to make vel and acc has similar magnitude
          // multiply ts_inv3, send vel and acc With similar amplitude 

          double grad = (2.0 * ar * diff + br) / ts * ts_inv3;// Calculate the gradient 
          gradient(j, i + 0) += -grad;
          gradient(j, i + 1) += grad;
        }
        else if (vi(j) > max_vel_)// If the speed is greater than the maximum speed 
        {
          double diff = vi(j) - max_vel_;
          cost += pow(diff, 3) * ts_inv3;
          ;

          double grad = 3 * diff * diff / ts * ts_inv3;
          ;
          gradient(j, i + 0) += -grad;
          gradient(j, i + 1) += grad;
        }
        else if (vi(j) < -(max_vel_ + demarcation))
        {
          double diff = vi(j) + max_vel_;
          cost += (al * diff * diff + bl * diff + cl) * ts_inv3;

          double grad = (2.0 * al * diff + bl) / ts * ts_inv3;
          gradient(j, i + 0) += -grad;
          gradient(j, i + 1) += grad;
        }
        else if (vi(j) < -max_vel_)
        {
          double diff = vi(j) + max_vel_;
          cost += -pow(diff, 3) * ts_inv3;

          double grad = -3 * diff * diff / ts * ts_inv3;
          gradient(j, i + 0) += -grad;
          gradient(j, i + 1) += grad;
        }
        else
        {
          /* nothing happened */
        }
      }
    }

    /* acceleration feasibility */
    // Acceleration feasibility 
    for (int i = 0; i < q.cols() - 2; i++)
    {
      Eigen::Vector3d ai = (q.col(i + 2) - 2 * q.col(i + 1) + q.col(i)) * ts_inv2;
      // The acceleration =( The third control point -2* Second control point + Current control point ）* 1 / ts / ts

      for (int j = 0; j < 3; j++)
      {
        if (ai(j) > max_acc_ + demarcation)// angular velocity > Maximum acceleration + Safe acceleration 1m/s
        {
          double diff = ai(j) - max_acc_;
          cost += ar * diff * diff + br * diff + cr;// Corresponding cost 

          double grad = (2.0 * ar * diff + br) * ts_inv2;
          gradient(j, i + 0) += grad;
          gradient(j, i + 1) += -2 * grad;
          gradient(j, i + 2) += grad;
        }
        else if (ai(j) > max_acc_)
        {
          double diff = ai(j) - max_acc_;
          cost += pow(diff, 3);

          double grad = 3 * diff * diff * ts_inv2;
          gradient(j, i + 0) += grad;
          gradient(j, i + 1) += -2 * grad;
          gradient(j, i + 2) += grad;
        }
        else if (ai(j) < -(max_acc_ + demarcation))
        {
          double diff = ai(j) + max_acc_;
          cost += al * diff * diff + bl * diff + cl;

          double grad = (2.0 * al * diff + bl) * ts_inv2;
          gradient(j, i + 0) += grad;
          gradient(j, i + 1) += -2 * grad;
          gradient(j, i + 2) += grad;
        }
        else if (ai(j) < -max_acc_)
        {
          double diff = ai(j) + max_acc_;
          cost += -pow(diff, 3);

          double grad = -3 * diff * diff * ts_inv2;
          gradient(j, i + 0) += grad;
          gradient(j, i + 1) += -2 * grad;
          gradient(j, i + 2) += grad;
        }
        else
        {
          /* nothing happened */
        }
      }
    }

#else

    cost = 0.0;
    /* abbreviation */
    double ts, /*vm2, am2, */ ts_inv2;
    // vm2 = max_vel_ * max_vel_;
    // am2 = max_acc_ * max_acc_;

    ts = bspline_interval_;
    ts_inv2 = 1 / ts / ts;

    /* velocity feasibility */// Speed feasibility 
    for (int i = 0; i < q.cols() - 1; i++)//i=1 To Nc, In the paper Feasibility Penalty Partial formula (8)
    {
      Eigen::Vector3d vi = (q.col(i + 1) - q.col(i)) / ts;// Corresponding to the formula in the paper (2)

      //cout << "temp_v * vi=" ;
      for (int j = 0; j < 3; j++)//xyz Calculate the speed on the three axes respectively 
      {
        if (vi(j) > max_vel_)// Speed on each axis > Maximum speed 
        {
          // cout << "fuck VEL" << endl;
          // cout << vi(j) << endl;
          cost += pow(vi(j) - max_vel_, 2) * ts_inv2;// Cumulative slave i=1 To Nc, Calculation WvF(Vi)
           // multiply ts_inv3 to make vel and acc has similar magnitude
           // * ts_inv2 send vel and acc With similar amplitude , That is, multiply by a weight Wv
          
          gradient(j, i + 0) += -2 * (vi(j) - max_vel_) / ts * ts_inv2;
          gradient(j, i + 1) += 2 * (vi(j) - max_vel_) / ts * ts_inv2;
        }
        else if (vi(j) < -max_vel_)
        {
          cost += pow(vi(j) + max_vel_, 2) * ts_inv2;

          gradient(j, i + 0) += -2 * (vi(j) + max_vel_) / ts * ts_inv2;
          gradient(j, i + 1) += 2 * (vi(j) + max_vel_) / ts * ts_inv2;
        }
        else
        {
          /* code */
        }
      }
    }

    /* acceleration feasibility */
    // Acceleration feasibility 
    for (int i = 0; i < q.cols() - 2; i++)//i=1 To Nc-1, In the paper Feasibility Penalty Partial formula (8)
    {
      Eigen::Vector3d ai = (q.col(i + 2) - 2 * q.col(i + 1) + q.col(i)) * ts_inv2;// Calculation speed 

      //cout << "temp_a * ai=" ;
      for (int j = 0; j < 3; j++)// Calculation xyz Acceleration on each axis 
      {
        if (ai(j) > max_acc_)// The acceleration > Maximum acceleration 
        {
          // cout << "fuck ACC" << endl;
          // cout << ai(j) << endl;
          cost += pow(ai(j) - max_acc_, 2);// Cumulative slave i=1 To Nc-1, Calculation WaF(Ai)

          gradient(j, i + 0) += 2 * (ai(j) - max_acc_) * ts_inv2;
          gradient(j, i + 1) += -4 * (ai(j) - max_acc_) * ts_inv2;
          gradient(j, i + 2) += 2 * (ai(j) - max_acc_) * ts_inv2;
        }
        else if (ai(j) < -max_acc_)
        {
          cost += pow(ai(j) + max_acc_, 2);

          gradient(j, i + 0) += 2 * (ai(j) + max_acc_) * ts_inv2;
          gradient(j, i + 1) += -4 * (ai(j) + max_acc_) * ts_inv2;
          gradient(j, i + 2) += 2 * (ai(j) + max_acc_) * ts_inv2;
        }
        else
        {
          /* code */
        }
      }
      //cout << endl;
    }

#endif
  }

8、 bool BsplineOptimizer::check_collision_and_rebound(void)

  // Judge whether it is necessary to check obstacles and eject tracks from obstacles , And calculation Pij Code out

bool BsplineOptimizer::check_collision_and_rebound(void)
  // Judge whether it is necessary to check obstacles and eject tracks from obstacles , And calculation Pij The output code is consistent 
  {

    int end_idx = cps_.size - order_;

    /*** Check and segment the initial trajectory according to obstacles ***/
    // Check and segment the initial trajectory according to the obstacles 
    int in_id, out_id;
    vector<std::pair<int, int>> segment_ids;
    bool flag_new_obs_valid = false;
    int i_end = end_idx - (end_idx - order_) / 3;
    for (int i = order_ - 1; i <= i_end; ++i)
    {

      bool occ = grid_map_->getInflateOccupancy(cps_.points.col(i));

      /*** check if the new collision will be valid ***/
      // Check whether the new obstacle is effective 
      if (occ)
      {
        for (size_t k = 0; k < cps_.direction[i].size(); ++k)
        {
          cout.precision(2);
          if ((cps_.points.col(i) - cps_.base_point[i][k]).dot(cps_.direction[i][k]) < 1 * grid_map_->getResolution()) // current point is outside all the collision_points.
          {
            occ = false; // Not really takes effect, just for better hunman understanding.
            break;
          }
        }
      }

      if (occ)
      {
        flag_new_obs_valid = true;

        int j;
        for (j = i - 1; j >= 0; --j)
        {
          occ = grid_map_->getInflateOccupancy(cps_.points.col(j));
          if (!occ)
          {
            in_id = j;
            break;
          }
        }
        if (j < 0) // fail to get the obs free point, Can't get obs Idle point 
        {
          ROS_ERROR("ERROR! the drone is in obstacle. This should not happen.");
          in_id = 0;
        }

        for (j = i + 1; j < cps_.size; ++j)
        {
          occ = grid_map_->getInflateOccupancy(cps_.points.col(j));

          if (!occ)
          {
            out_id = j;
            break;
          }
        }
        if (j >= cps_.size) // fail to get the obs free point
        {
          ROS_WARN("WARN! terminal point of the current trajectory is in obstacle, skip this planning.");

          force_stop_type_ = STOP_FOR_ERROR;
          return false;
        }

        i = j + 1;

        segment_ids.push_back(std::pair<int, int>(in_id, out_id));
      }
    }

    if (flag_new_obs_valid)
    {
      vector<vector<Eigen::Vector3d>> a_star_pathes;
      for (size_t i = 0; i < segment_ids.size(); ++i)
      {
        /*** a star search ***/
        Eigen::Vector3d in(cps_.points.col(segment_ids[i].first)), out(cps_.points.col(segment_ids[i].second));
        if (a_star_->AstarSearch(/*(in-out).norm()/10+0.05*/ 0.1, in, out))
        {
          a_star_pathes.push_back(a_star_->getPath());
        }
        else
        {
          ROS_ERROR("a star error");
          segment_ids.erase(segment_ids.begin() + i);
          i--;
        }
      }

      /*** Assign parameters to each segment ***/
      for (size_t i = 0; i < segment_ids.size(); ++i)
      {
        // step 1
        for (int j = segment_ids[i].first; j <= segment_ids[i].second; ++j)
          cps_.flag_temp[j] = false;

        // step 2
        int got_intersection_id = -1;
        for (int j = segment_ids[i].first + 1; j < segment_ids[i].second; ++j)
        {
          Eigen::Vector3d ctrl_pts_law(cps_.points.col(j + 1) - cps_.points.col(j - 1)), intersection_point;
          int Astar_id = a_star_pathes[i].size() / 2, last_Astar_id; // Let "Astar_id = id_of_the_most_far_away_Astar_point" will be better, but it needs more computation
          double val = (a_star_pathes[i][Astar_id] - cps_.points.col(j)).dot(ctrl_pts_law), last_val = val;
          while (Astar_id >= 0 && Astar_id < (int)a_star_pathes[i].size())
          {
            last_Astar_id = Astar_id;

            if (val >= 0)
              --Astar_id;
            else
              ++Astar_id;

            val = (a_star_pathes[i][Astar_id] - cps_.points.col(j)).dot(ctrl_pts_law);

            // cout << val << endl;

            if (val * last_val <= 0 && (abs(val) > 0 || abs(last_val) > 0)) // val = last_val = 0.0 is not allowed
            {
              intersection_point =
                  a_star_pathes[i][Astar_id] +
                  ((a_star_pathes[i][Astar_id] - a_star_pathes[i][last_Astar_id]) *
                   (ctrl_pts_law.dot(cps_.points.col(j) - a_star_pathes[i][Astar_id]) / ctrl_pts_law.dot(a_star_pathes[i][Astar_id] - a_star_pathes[i][last_Astar_id])) // = t
                  );

              got_intersection_id = j;
              break;
            }
          }

          if (got_intersection_id >= 0)
          {
            cps_.flag_temp[j] = true;
            double length = (intersection_point - cps_.points.col(j)).norm();
            if (length > 1e-5)
            {
              for (double a = length; a >= 0.0; a -= grid_map_->getResolution())
              {
                bool occ = grid_map_->getInflateOccupancy((a / length) * intersection_point + (1 - a / length) * cps_.points.col(j));

                if (occ || a < grid_map_->getResolution())
                {
                  if (occ)
                    a += grid_map_->getResolution();
                  cps_.base_point[j].push_back((a / length) * intersection_point + (1 - a / length) * cps_.points.col(j));
                  cps_.direction[j].push_back((intersection_point - cps_.points.col(j)).normalized());
                  break;
                }
              }
            }
            else
            {
              got_intersection_id = -1;
            }
          }
        }

        //step 3
        if (got_intersection_id >= 0)
        {
          for (int j = got_intersection_id + 1; j <= segment_ids[i].second; ++j)
            if (!cps_.flag_temp[j])
            {
              cps_.base_point[j].push_back(cps_.base_point[j - 1].back());
              cps_.direction[j].push_back(cps_.direction[j - 1].back());
            }

          for (int j = got_intersection_id - 1; j >= segment_ids[i].first; --j)
            if (!cps_.flag_temp[j])
            {
              cps_.base_point[j].push_back(cps_.base_point[j + 1].back());
              cps_.direction[j].push_back(cps_.direction[j + 1].back());
            }
        }
        else
          ROS_WARN("Failed to generate direction. It doesn't matter.");
      }

      force_stop_type_ = STOP_FOR_REBOUND;
      return true;
    }

    return false;
  }

9、bool BsplineOptimizer::BsplineOptimizeTrajRebound(Eigen::MatrixXd &optimal_points, double ts)

  //B Spline optimization trajectory , Push the track out of the obstacle , Get the optimal collision free trajectory

 // Set the time interval t, call rebound_optimize() Push the track out of the obstacle , Get the optimal collision free trajectory , And assign its control point to optimal_points.

bool BsplineOptimizer::BsplineOptimizeTrajRebound(Eigen::MatrixXd &optimal_points, double ts)
  //B Spline optimization trajectory , Push the track out of the obstacle , Get the optimal collision free trajectory 
  {
    setBsplineInterval(ts);// Set up B Spline interval 

    bool flag_success = rebound_optimize();

    optimal_points = cps_.points;

    return flag_success;
    // Set the time interval ts
    // call rebound_optimize() Push the track out of the obstacle 
    // Get the optimal collision free trajectory , And assign its control point to optimal_points.
  }

10、 bool BsplineOptimizer::BsplineOptimizeTrajRefine(const Eigen::MatrixXd &init_points, const double ts, Eigen::MatrixXd &optimal_points)

  //B Spline optimization trajectory , Redistribute time , To the optimal dynamic feasible trajectory

// Set the initial control point init_points、 The time interval ts, call refine_optimize() Redistribute time , Get the optimal dynamic feasible trajectory , And assign its control point to optimal_points.

bool BsplineOptimizer::BsplineOptimizeTrajRefine(const Eigen::MatrixXd &init_points, const double ts, Eigen::MatrixXd &optimal_points)
  //B Spline optimization trajectory , Redistribute time , To the optimal dynamic feasible trajectory 
  {

    setControlPoints(init_points);
    setBsplineInterval(ts);

    bool flag_success = refine_optimize();

    optimal_points = cps_.points;

    return flag_success;
    // Set the initial control point init_points、 The time interval ts
    // call refine_optimize() Redistribute time 
    // Get the optimal dynamic feasible trajectory , And assign its control point to optimal_points
  }

11、bool BsplineOptimizer::rebound_optimize()

 // To optimize , Push the track out of the obstacle

// Use L-BFGS Methods to optimize the objective function , Get smooth 、 No collision 、 Dynamic feasible trajectory

  bool BsplineOptimizer::rebound_optimize()
  // To optimize , Push the track out of the obstacle 
  // Use L-BFGS Methods to optimize the objective function , Get smooth 、 No collision 、 Dynamic feasible trajectory 
  {
    iter_num_ = 0;
    int start_id = order_;
    int end_id = this->cps_.size - order_;
    variable_num_ = 3 * (end_id - start_id);
    double final_cost;

    ros::Time t0 = ros::Time::now(), t1, t2;
    int restart_nums = 0, rebound_times = 0;
    ;
    bool flag_force_return, flag_occ, success;
    new_lambda2_ = lambda2_;
    constexpr int MAX_RESART_NUMS_SET = 3;
    do
    {
      /* ---------- prepare ---------- */
      min_cost_ = std::numeric_limits<double>::max();
      iter_num_ = 0;
      flag_force_return = false;
      flag_occ = false;
      success = false;

      double q[variable_num_];
      memcpy(q, cps_.points.data() + 3 * start_id, variable_num_ * sizeof(q[0]));

      lbfgs::lbfgs_parameter_t lbfgs_params;
      lbfgs::lbfgs_load_default_parameters(&lbfgs_params);
      lbfgs_params.mem_size = 16;
      lbfgs_params.max_iterations = 200;
      lbfgs_params.g_epsilon = 0.01;

      /* ---------- optimize ---------- */
      t1 = ros::Time::now();
      int result = lbfgs::lbfgs_optimize(variable_num_, q, &final_cost, BsplineOptimizer::costFunctionRebound, NULL, BsplineOptimizer::earlyExit, this, &lbfgs_params);
      t2 = ros::Time::now();
      double time_ms = (t2 - t1).toSec() * 1000;
      double total_time_ms = (t2 - t0).toSec() * 1000;

      /* ---------- success temporary, check collision again ---------- */
      if (result == lbfgs::LBFGS_CONVERGENCE ||
          result == lbfgs::LBFGSERR_MAXIMUMITERATION ||
          result == lbfgs::LBFGS_ALREADY_MINIMIZED ||
          result == lbfgs::LBFGS_STOP)
      {
        //ROS_WARN("Solver error in planning!, return = %s", lbfgs::lbfgs_strerror(result));
        flag_force_return = false;

        UniformBspline traj = UniformBspline(cps_.points, 3, bspline_interval_);
        double tm, tmp;
        traj.getTimeSpan(tm, tmp);
        double t_step = (tmp - tm) / ((traj.evaluateDeBoorT(tmp) - traj.evaluateDeBoorT(tm)).norm() / grid_map_->getResolution());
        for (double t = tm; t < tmp * 2 / 3; t += t_step) // Only check the closest 2/3 partition of the whole trajectory.
        {
          flag_occ = grid_map_->getInflateOccupancy(traj.evaluateDeBoorT(t));
          if (flag_occ)
          {
            //cout << "hit_obs, t=" << t << " P=" << traj.evaluateDeBoorT(t).transpose() << endl;

            if (t <= bspline_interval_) // First 3 control points in obstacles!
            {
              cout << cps_.points.col(1).transpose() << "\n"
                   << cps_.points.col(2).transpose() << "\n"
                   << cps_.points.col(3).transpose() << "\n"
                   << cps_.points.col(4).transpose() << endl;
              ROS_WARN("First 3 control points in obstacles! return false, t=%f", t);
              return false;
            }

            break;
          }
        }

        if (!flag_occ)
        {
          printf("\033[32miter(+1)=%d,time(ms)=%5.3f,total_t(ms)=%5.3f,cost=%5.3f\n\033[0m", iter_num_, time_ms, total_time_ms, final_cost);
          success = true;
        }
        else // restart
        {
          restart_nums++;
          initControlPoints(cps_.points, false);
          new_lambda2_ *= 2;

          printf("\033[32miter(+1)=%d,time(ms)=%5.3f,keep optimizing\n\033[0m", iter_num_, time_ms);
        }
      }
      else if (result == lbfgs::LBFGSERR_CANCELED)
      {
        flag_force_return = true;
        rebound_times++;
        cout << "iter=" << iter_num_ << ",time(ms)=" << time_ms << ",rebound." << endl;
      }
      else
      {
        ROS_WARN("Solver error. Return = %d, %s. Skip this planning.", result, lbfgs::lbfgs_strerror(result));
        // while (ros::ok());
      }

    } while ((flag_occ && restart_nums < MAX_RESART_NUMS_SET) ||
             (flag_force_return && force_stop_type_ == STOP_FOR_REBOUND && rebound_times <= 20));

    return success;
  }

12、bool BsplineOptimizer::refine_optimize()

 // To optimize the

 // Use L-BFGS Methods to optimize the objective function , After getting reallocated time , smooth 、 Good fitting 、 Dynamic feasible trajectory .

 bool BsplineOptimizer::refine_optimize()
  // To optimize the 
  // Use L-BFGS Methods to optimize the objective function 
  // After getting reallocated time , smooth 、 Good fitting 、 Dynamic feasible trajectory .
  {
    iter_num_ = 0;
    int start_id = order_;
    int end_id = this->cps_.points.cols() - order_;
    variable_num_ = 3 * (end_id - start_id);

    double q[variable_num_];
    double final_cost;

    memcpy(q, cps_.points.data() + 3 * start_id, variable_num_ * sizeof(q[0]));

    double origin_lambda4 = lambda4_;
    bool flag_safe = true;
    int iter_count = 0;
    do
    {
      lbfgs::lbfgs_parameter_t lbfgs_params;
      lbfgs::lbfgs_load_default_parameters(&lbfgs_params);
      lbfgs_params.mem_size = 16;
      lbfgs_params.max_iterations = 200;
      lbfgs_params.g_epsilon = 0.001;

      int result = lbfgs::lbfgs_optimize(variable_num_, q, &final_cost, BsplineOptimizer::costFunctionRefine, NULL, NULL, this, &lbfgs_params);
      if (result == lbfgs::LBFGS_CONVERGENCE ||
          result == lbfgs::LBFGSERR_MAXIMUMITERATION ||
          result == lbfgs::LBFGS_ALREADY_MINIMIZED ||
          result == lbfgs::LBFGS_STOP)
      {
        //pass
      }
      else
      {
        ROS_ERROR("Solver error in refining!, return = %d, %s", result, lbfgs::lbfgs_strerror(result));
      }

      UniformBspline traj = UniformBspline(cps_.points, 3, bspline_interval_);
      double tm, tmp;
      traj.getTimeSpan(tm, tmp);
      double t_step = (tmp - tm) / ((traj.evaluateDeBoorT(tmp) - traj.evaluateDeBoorT(tm)).norm() / grid_map_->getResolution()); // Step size is defined as the maximum size that can passes throgth every gird.
      for (double t = tm; t < tmp * 2 / 3; t += t_step)
      {
        if (grid_map_->getInflateOccupancy(traj.evaluateDeBoorT(t)))
        {
          // cout << "Refined traj hit_obs, t=" << t << " P=" << traj.evaluateDeBoorT(t).transpose() << endl;

          Eigen::MatrixXd ref_pts(ref_pts_.size(), 3);
          for (size_t i = 0; i < ref_pts_.size(); i++)
          {
            ref_pts.row(i) = ref_pts_[i].transpose();
          }

          flag_safe = false;
          break;
        }
      }

      if (!flag_safe)
        lambda4_ *= 2;

      iter_count++;
    } while (!flag_safe && iter_count <= 0);

    lambda4_ = origin_lambda4;

    //cout << "iter_num_=" << iter_num_ << endl;

    return flag_safe;
  }

13、void BsplineOptimizer::combineCostRebound(const double *x, double *grad, double &f_combine, const int n)

 // Find the smooth term of the trajectory 、 Collision items 、 Weighted sum of feasible terms in dynamics f_combine And the direction of the gradient grad

void BsplineOptimizer::combineCostRebound(const double *x, double *grad, double &f_combine, const int n)
  // Find the smooth term of the trajectory 、 Collision items 、 Weighted sum of feasible terms in dynamics f_combine And the direction of the gradient grad
  {

    memcpy(cps_.points.data() + 3 * order_, x, n * sizeof(x[0]));

    /* ---------- evaluate cost and gradient ---------- */
    double f_smoothness, f_distance, f_feasibility;

    Eigen::MatrixXd g_smoothness = Eigen::MatrixXd::Zero(3, cps_.size);
    Eigen::MatrixXd g_distance = Eigen::MatrixXd::Zero(3, cps_.size);
    Eigen::MatrixXd g_feasibility = Eigen::MatrixXd::Zero(3, cps_.size);

    calcSmoothnessCost(cps_.points, f_smoothness, g_smoothness);
    calcDistanceCostRebound(cps_.points, f_distance, g_distance, iter_num_, f_smoothness);
    calcFeasibilityCost(cps_.points, f_feasibility, g_feasibility);

    f_combine = lambda1_ * f_smoothness + new_lambda2_ * f_distance + lambda3_ * f_feasibility;
    //printf("origin %f %f %f %f\n", f_smoothness, f_distance, f_feasibility, f_combine);

    Eigen::MatrixXd grad_3D = lambda1_ * g_smoothness + new_lambda2_ * g_distance + lambda3_ * g_feasibility;
    memcpy(grad, grad_3D.data() + 3 * order_, n * sizeof(grad[0]));
  }

14、 void BsplineOptimizer::combineCostRefine(const double *x, double *grad, double &f_combine, const int n)

  // Obtain the trajectory after extending the allocated time Smooth term of 、 Fitting term 、 Weighted sum of feasible terms in dynamics f_combine And the direction of the gradient grad

  void BsplineOptimizer::combineCostRefine(const double *x, double *grad, double &f_combine, const int n)
  // The smooth term of the trajectory after extending the allocated time is obtained 、 Fitting term 、 Weighted sum of feasible terms in dynamics f_combine And the direction of the gradient grad
  {

    memcpy(cps_.points.data() + 3 * order_, x, n * sizeof(x[0]));

    /* ---------- evaluate cost and gradient ---------- */
    double f_smoothness, f_fitness, f_feasibility;

    Eigen::MatrixXd g_smoothness = Eigen::MatrixXd::Zero(3, cps_.points.cols());
    Eigen::MatrixXd g_fitness = Eigen::MatrixXd::Zero(3, cps_.points.cols());
    Eigen::MatrixXd g_feasibility = Eigen::MatrixXd::Zero(3, cps_.points.cols());

    //time_satrt = ros::Time::now();

    calcSmoothnessCost(cps_.points, f_smoothness, g_smoothness);
    calcFitnessCost(cps_.points, f_fitness, g_fitness);
    calcFeasibilityCost(cps_.points, f_feasibility, g_feasibility);

    /* ---------- convert to solver format...---------- */
    f_combine = lambda1_ * f_smoothness + lambda4_ * f_fitness + lambda3_ * f_feasibility;
    // printf("origin %f %f %f %f\n", f_smoothness, f_fitness, f_feasibility, f_combine);

    Eigen::MatrixXd grad_3D = lambda1_ * g_smoothness + lambda4_ * g_fitness + lambda3_ * g_feasibility;
    memcpy(grad, grad_3D.data() + 3 * order_, n * sizeof(grad[0]));
  }

} // namespace ego_planner

ah It's not easy to finish it ！！