时间约束

EdgeTimeOptimal类太简单了，误差函数 _error[0] = timediff->dt();，优化 $\Delta T_i$，那么对其求偏导，显然只有一个矩阵，一个元素 1 ： _jacobianOplusXi( 0 , 0 ) = 1;

kinematics 约束

两个误差方程，一个是 non-holonomic约束， ：

初始配置$x_i$和方向$d_i,d_{i+1}$之间的角度$\theta _i$ 必须等于配置$x_{i+1}$和方向$d_i,d_{i+1}$之间的夹角，即$\theta _i = \theta_{i+1}$

根据二维叉积 A×B=|A| |B|⋅sinα 得到

所以代码中的目标函数写成

$$|(cos{\theta_1}+cos{\theta_2})(y_2-y_1) - (sin{\theta_1}+sin{\theta_2})(x_2-x_1) |$$

另一个是 positive-drive-direction约束

$$-(x_2-x_1)cos{\theta_1} -(y_2-y_1)sin{\theta_1}$$

两个configure，所以两个雅格比，维度明显是 2x3，两个误差方程分别对(x, y, angle)求偏导，源码里的求导很简单，还不如速度约束的求导复杂，就不写过程了。

值得注意的是绝对值的求导结果会用sign函数表示。