optimizeTEB 的流程就是循环几次下列操作（多次图优化）：

1. teb_.autoResize() 在外循环里
2. buildGraph函数;
3. optimizeGraph 内循环就是g2o的optimize函数的参数
4. computeCurrentCost函数（目前的图优化的花费，只在最后一次外循环时进行一次）
5. clearGraph函数;

bool TebOptimalPlanner::optimizeTEB( int  iterations_innerloop, 
	   int  iterations_outerloop,  bool compute_cost_afterwards,
     double  obst_cost_scale,    double viapoint_cost_scale, 
     bool  alternative_time_cost )
{
  if (cfg_->optim.optimization_activate == false) 
      return false;

  bool success = false;
  optimized_ = false;
  // ROS源码里是 1
  double weight_multiplier = 2.0;
  // TODO: we introduced the non-fast mode with the support of dynamic obstacles
  // ( which leads to better results in terms of x-y-t homotopy planning )
  //  但此模式尚未完全测试
  // 目前保持legacy fast mode为默认
  bool fast_mode = !cfg_->obstacles.include_dynamic_obstacles; // false
  
  for(int i=0; i<iterations_outerloop; ++i)
  {
    if (cfg_->trajectory.teb_autosize)   // 设置为 true
    {
      teb_.autoResize(cfg_->trajectory.dt_ref, cfg_->trajectory.dt_hysteresis, 
        cfg_->trajectory.min_samples,  cfg_->trajectory.max_samples, fast_mode);
    }
    success = buildGraph(weight_multiplier);
    if (!success) 
    {
        clearGraph();
        return false;
    }
    success = optimizeGraph(iterations_innerloop, false);
    if (!success) 
    {
        clearGraph();
        return false;
    }
    optimized_ = true;
    // compute cost vec only in the last iteration
    if (compute_cost_afterwards &&  i==iterations_outerloop-1) 
      computeCurrentCost(obst_cost_scale, viapoint_cost_scale, alternative_time_cost);
      
    clearGraph();
    // 根据目前配置，变成 4 了
    weight_multiplier *= cfg_->optim.weight_adapt_factor;
  }
  return true;
}

optimization_activate

1. 外部循环通过调用autoResize()来根据时间分辨率调整轨迹，应根据CPU的控制速率要求来定

2. 内部循环调用optimizeGraph()进行优化，内部循环的时间经过实验2-6次足够

3. 最后调用computeCurrentCost()计算轨迹花费，将图中所有边的error的平方和构成cost

autoResize

一般不要用快速模式，也就是要进行100次外循环

// iterate through all TEB states and add/remove states
void TimedElasticBand::autoResize(double dt_ref, double dt_hysteresis, 
            int min_samples,  int max_samples,  bool fast_mode)
{
  bool modified = true;
  // 确保不会get stuck in some oscillation, 因此最大100此循环
  for (int rep = 0; rep < 100 && modified; ++rep) 
  {
    modified = false;
    // TimeDiff connects Point(i) with Point(i+1)
    // TimeDiff   return timediff_vec_.at(index)->dt();
    for(int i=0; i < sizeTimeDiffs(); ++i) 
    {
      if(TimeDiff(i) > dt_ref + dt_hysteresis && sizeTimeDiffs()<max_samples)
      {
        //ROS_DEBUG("teb_local_planner: autoResize() inserting new bandpoint i=%u, #TimeDiffs=%lu",i,sizeTimeDiffs()   );
        double newtime = 0.5 * TimeDiff(i);
        TimeDiff(i) = newtime;
        // 时间差太大，插入一个元素
        insertPose(i+1, PoseSE2::average(Pose(i),Pose(i+1)) );
        insertTimeDiff(i+1, newtime);

        modified = true;
      }
      // only remove samples if size is larger than min_samples.
      else if(TimeDiff(i) < dt_ref - dt_hysteresis && sizeTimeDiffs()>min_samples)
      {
        //ROS_DEBUG("teb_local_planner: autoResize() deleting bandpoint i=%u, #TimeDiffs=%lu",i,sizeTimeDiffs()   );
        if(i < ((int)sizeTimeDiffs()-1))
        {
          // 时间差太小，合并 i 到 i+1
          TimeDiff(i+1) = TimeDiff(i+1) + TimeDiff(i);
          deleteTimeDiff(i);
          deletePose(i+1);
        }
        else
        { // last motion should be adjusted, shift time to the interval before
          // 合并 i 到 i-1
          TimeDiff(i-1) += TimeDiff(i);
          deleteTimeDiff(i);
          deletePose(i);
        }

        modified = true;
      }
    }
    // 如果是快速模式则结束，也就是rep<100不起作用
    if (fast_mode)    break;
  }
}

这里其实是动态对initTrajectoryToGoal中建立的弹性带进行调整，不能让位姿点和deltT太密集，也不能太稀疏。min_samples和max_samples就是一组限制条件，dt_ref 和 dt_hysteresis是另一组