计算曲率半径 C++实现

double cal_curvature_radius(const Eigen::Vector2d& p0,
                            const Eigen::Vector2d& p1,
                            const Eigen::Vector2d& p2) {
    double aa = (p1 - p2).squaredNorm();
    double bb = (p2 - p0).squaredNorm();
    double cc = (p0 - p1).squaredNorm();
    double a = std::sqrt(aa);
    double b = std::sqrt(bb);
    double c = std::sqrt(cc);
    double cos_b = (aa + cc - bb) / (2 * a * c);
    if (std::abs(cos_b) >= 1.0) {
        return 65504.0;  // 直线，返回一个很大值即可
    }
    double sin_b = std::sqrt(1 - cos_b * cos_b);
    return 0.5 * b / sin_b;
}