图优化 optimizeGraph

// clear_after 调用时写死为false
bool TebOptimalPlanner::optimizeGraph(int no_iterations, bool clear_after)
{
  if (!teb_.isInit() || teb_.sizePoses() < cfg_->trajectory.min_samples)
  {
    ROS_WARN("optimizeGraph(): TEB is empty or has too less elements. Skipping optimization.");
    if (clear_after)
        clearGraph();
    return false; 
  }
  // 头文件定义 boost::shared_ptr<g2o::SparseOptimizer> optimizer_;
  // 开始图优化
  optimizer_->setVerbose(cfg_->optim.optimization_verbose);
  optimizer_->initializeOptimization();
  int iter = optimizer_->optimize(no_iterations);  // 优化结束
  
  //   可以保存 海森矩阵
   /*  g2o::OptimizationAlgorithmLevenberg lm = 
      dynamic_cast<g2o::OptimizationAlgorithmLevenberg*>(optimizer_->solver());
   lm->solver()->saveHessian("~/Matlab/Hessian.txt");*/
  if (!iter)
  {
     ROS_ERROR("optimizeGraph(): Optimization failed! iter=%i", iter);
     return false;
  }
  if (clear_after)
     clearGraph();  
    
  return true;
}

再回到optimizeTEB，剩余部分是这样的:

success = optimizeGraph(iterations_innerloop, false);
if (!success) 
{
    clearGraph();
    return false;
}
optimized_ = true;
// 只在最后一次外循环时 compute cost vec
if (compute_cost_afterwards && i==iterations_outerloop-1) 
  computeCurrentCost(obst_cost_scale, viapoint_cost_scale, alternative_time_cost);

clearGraph();
weight_multiplier *= cfg_->optim.weight_adapt_factor;
// iterations_outerloop 结束

computeCurrentCost

void TebOptimalPlanner::computeCurrentCost( double obst_cost_scale, 
      double viapoint_cost_scale,   bool alternative_time_cost)
{ 
  // check if graph is empty/exist, important if function is called 
  //  between buildGraph and optimizeGraph/clearGraph
  bool graph_exist_flag(false);
  if (optimizer_->edges().empty() && optimizer_->vertices().empty())
  {
    // here the graph is build again, for time efficiency make sure to call this function 
    // between buildGraph and Optimize (deleted), but it depends on the application
    buildGraph(); 
    optimizer_->initializeOptimization();
  }
  else
      graph_exist_flag = true;

  optimizer_->computeInitialGuess();
  //  cost_ 在 TebOptimalPlanner构造函数里 初始化为 HUGE_VAL
  cost_ = 0;  
  if (alternative_time_cost)
  {
    cost_ += teb_.getSumOfAllTimeDiffs();
    /*  we use SumOfAllTimeDiffs() here, because edge cost depends on 
    number of samples, which is not always the same for similar TEBs,
    since we are using an AutoResize Function with hysteresis  */
  }
  // now we need pointers to all edges -> calculate error for each edge-type
  // since we aren't storing edge pointers, we need to check every edge
  for (std::vector<g2o::OptimizableGraph::Edge*>::const_iterator it = 
    optimizer_->activeEdges().begin();  it!= optimizer_->activeEdges().end(); it++)
  {
    double cur_cost = (*it)->chi2();

    if (dynamic_cast<EdgeObstacle*>(*it) != nullptr
        || dynamic_cast<EdgeInflatedObstacle*>(*it) != nullptr
        || dynamic_cast<EdgeDynamicObstacle*>(*it) != nullptr)
    {
      cur_cost *= obst_cost_scale;
    }
    else if (dynamic_cast<EdgeViaPoint*>(*it) != nullptr)
        cur_cost *= viapoint_cost_scale;
    else if (dynamic_cast<EdgeTimeOptimal*>(*it) != nullptr && alternative_time_cost)
    {
      // skip these edges if alternative_time_cost is active
      continue; 
    }
    cost_ += cur_cost;
  }
  // delete temporary created graph
  if (!graph_exist_flag) 
    clearGraph();
}

相应的有个TebOptimalPlanner::getCurrentCost()返回每次图优化计算的代价，而它又用于HomotopyClassPlanner::computeCurrentCost 和 HomotopyClassPlanner::selectBestTeb()

• obst_cost_scale Specify extra scaling for obstacle costs (only used if compute_cost_afterwards is true)

• viapoint_cost_scale Specify extra scaling for via-point costs (only used if compute_cost_afterwards is true)

• alternative_time_cost Replace the cost for the time optimal objective by the actual (weighted) transition time (only used if compute_cost_afterwards is true)

总结

optimizeTEB包括两个循环:

1. 通过调用TimedElasticBand::autoResize()，外循环resizes the trajectory

2. optimizeGraph()是内循环部分，调用solver(使用g2o的sparse Levenberg-Marquardt方法) 遍历一定次数的optimization calls

3. 内外循环次数的比例defines the contraction behavior and convergence rate of the trajectory optimization. 2~6个内循环足够了

• TebOptPlannerContainer tebs_; : Container that stores multiple local teb planners (for alternative equivalence classes) and their corresponding costs

The number of outer loop iterations should be determined by considering the maximum CPU time required to match the control rate.

Optionally, the cost vector can be calculated by specifyingcompute_cost_afterwards, the cost vector can be accessed afterwards using getCurrentCost(). 目前使用TebOptimalPlanner不会计算代价，使用HomotopyClassPlanner会，第一次计算时，代价是一个很大很大的数，第二次减小很多，之后逐渐减小到1左右或者更小