二维离散点的曲率计算

公式

曲率：


使用三个已知点的二次曲线的曲率作为估计曲率。
曲线参数方程为：

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);

参考: 计算离散点的曲率（附Python, MATLAB代码）.