障碍约束定义的误差函数表示机器人到障碍的最小距离。 $ \min \textrm{penaltyBelow}( dist2point ) \cdot weight $.
1 | const VertexPose* bandpt = static_cast<const VertexPose*>(_vertices[0]); |
obstacle_cost_exponent
和min_obstacle_dist
的设置,一般都会进入if情况。 注意 max cost (before weighting) is the same as the straight line version and that all other costs are below the straight line (for positive exponent), so it may be necessary to increase weight_obstacle and/or the inflation_weight when using larger exponents.