questions about FastPlannerManager::planYaw() about fast-planner

[ZbyLGsc]

These are basic mathematics of B-spline.
For a 3-degree B-spline with N segments, there are N+3 control points, which answers your second questions.

As for the the first question, for a 3-degree B-spline the start and end state (0,1,2 order derivatives) are uniquely determined by the first and last 3 control points. The matrix state2pts is the relation between the state and control points.
You may check the paper "General matrix representations for b-splines" for the derivation.

[ZbyLGsc]

@WangChanglong17

[WangChanglong17]

I know that the start and end state are uniquely determined by the first and last 3 control points. So that yaw.block(seg_num, 0, 3, 1) = states2pts * start_yaw is an initialization? But after being optimized,
this initialization seems meaningless, because 1/6(yaw(0) + 4yaw(1) + yaw(2)) should be constrained to waypoints_(0) which is not equal to start_yaw. this is confusing.

[ZbyLGsc]

No, the first and last 3 points remain fixed throughout the optimization, so the boundary state will not be changed.

[ZbyLGsc]

Actually the first and last waypoint constraints take no effect.
In the code I do add them into the cost function, just for convenience of implementation.
But since the first and last 3 control points are already fixed, the optimization will not change them.
The associated cost of waypoints_(0) remains a constant term.

[WangChanglong17]

thanks very much for your reply. Now I understand. Great work!