公式
曲率:
使用三个已知点的二次曲线的曲率作为估计曲率。
曲线参数方程为:
6个未知数,三个点里有6个已知分量,列六个方程,解出这 (a1,a2,a3), (b1,b2,b3)即可。
这里使用两段矢量的长度来作为取值范围:
这里我们希望参数方程中的t满足如下条件:
则有:
写成矩阵形式:
和
简写为:
可以使用求矩阵逆的方式求解线性方程:
有了(a1,a2,a3)和(b1,b2,b3)就有了曲线的解析方程,接下来就和解析求曲率一样了,先算变量导数:
然后就是最终的曲率
这样,任意给定三个点,都可以估计出这三个点是比较【弯的】还是比较【直的】,直的曲率小,van的曲率大
Eigen::Vector3d x;//(x(i - 1), x(i), x(i + 1));
Eigen::Vector3d y;//(y(i - 1), y(i), y(i + 1));
double dxn = x(i) -x(i - 1);
double dyn = y(i) -y(i - 1);
double dxp = x(i+1) -x(i);
double dyp = x(i+1) -x(i);
double dn = std::sqrt(dxn * dxn + dyn * dyn);
double dp = std::sqrt(dxp * dxp + dyp * dyp);
Eigen::Matrix3d M_matrix;
M_matrix << 1, -dn, dn * dn,
1, 0, 0,
1, dp, dp * dp;
Eigen::Vector3d aa = M_matrix.inverse() * x;
Eigen::Vector3d bb = M_matrix.inverse() * y;
curvature = 2 * (aa(2) * bb(1) - aa(1) * bb(2)) / std::pow((aa(1) * aa(1) + bb(1) * bb(1)), 3 / 2);