所有约束的信息矩阵都是对角矩阵

• 速度

Eigen::Matrix<double,2,2> information;
information(0,0) = cfg_->optim.weight_max_vel_x;
information(1,1) = cfg_->optim.weight_max_vel_theta;
information(0,1) = 0.0;
information(1,0) = 0.0;

• 加速度

  Eigen::Matrix<double,2,2> information;
  information.fill(0);
  information(0,0) = cfg_->optim.weight_acc_lim_x;
  information(1,1) = cfg_->optim.weight_acc_lim_theta;

• 时间

  Eigen::Matrix<double,1,1> information;
  information.fill(cfg_->optim.weight_optimaltime);

• 最短距离

Eigen::Matrix<double,1,1> information;
information.fill(cfg_->optim.weight_shortest_path);

• 优先转弯方向，二元边

  // create edge for satisfiying kinematic constraints
  Eigen::Matrix<double,1,1> information_rotdir;
  information_rotdir.fill(cfg_->optim.weight_prefer_rotdir);

• 障碍

Eigen::Matrix<double,1,1> information;
information.fill(cfg_->optim.weight_obstacle * weight_multiplier);

• 动态障碍 AddEdgesDynamicObstacles

Eigen::Matrix<double,2,2> information;
information(0,0) = cfg_->optim.weight_dynamic_obstacle * weight_multiplier;
information(1,1) = cfg_->optim.weight_dynamic_obstacle_inflation;
information(0,1) = information(1,0) = 0;