### 1.2 Define objective function of optimization for each segment
$$
cost =
\sum_{i=1}^{n}
\Big(
\int\limits_{0}^{t_i} (f_i''')^2(t) dt
+\int\limits_{0}^{t_i} (g_i''')^2(t) dt
\Big)
$$
### 1.3 Convert the cost function to QP formulation
QP formulation:
$$
\frac{1}{2} \cdot x^T \cdot H \cdot x + f^T \cdot x
\\
s.t. LB \leq x \leq UB
\\
A_{eq}x = b_{eq}
\\
Ax \leq b
$$
## 2 Constraints
### 2.1 Joint smoothness constraints
This constraint is to make the spline joint smooth. Let's assume two segment $seg_k$ and $seg_{k+1}$ are connected, and the accumulated s of segment $seg_k$ is $s_k$. The we can get the constraint euqation as
$$
f_k(s_k) = f_{k+1} (s_0)
$$
Similarly we can get the equality constraints for
$$
f'_k(s_k) = f'_{k+1} (s_0)
\\
f''_k(s_k) = f''_{k+1} (s_0)
\\
f'''_k(s_k) = f'''_{k+1} (s_0)
\\
g_k(s_k) = g_{k+1} (s_0)
\\
g'_k(s_k) = g'_{k+1} (s_0)
\\
g''_k(s_k) = g''_{k+1} (s_0)
\\
g'''_k(s_k) = g'''_{k+1} (s_0)
$$
### 2.2 Sampled points for boundary constraint
Evenly sample m points along the path, and check predefined boundary at those points.
