三点法求曲率，使用C++实现

// An highlighted block
try
{   //取点 P1、P2、P3是位置信息
    P1 = PT1.pose.position;
    P2 = PT2.pose.position;
    P3 = PT3.pose.position;
    
    //计算曲率部分，使用正弦定理 a/sinA = 2R
    if(P1.x == P2.x == P3.x)//三点横坐标相同，即共线，直接标记曲率为0
    {
        curvity = 0;
    }
    else
    {
        double dis1,dis2,dis3;
        double cosA,sinA,dis;
        dis1 = sqrt((P1.x - P2.x)*(P1.x - P2.x) + (P1.y - P2.y)*(P1.y - P2.y));
        dis2 = sqrt((P1.x - P3.x)*(P1.x - P3.x) + (P1.y - P3.y)*(P1.y - P3.y));
        dis3 = sqrt((P2.x - P3.x)*(P2.x - P3.x) + (P2.y - P3.y)*(P2.y - P3.y));
        dis = dis1*dis1 + dis3*dis3 - dis2*dis2;
        cosA = dis/(2*dis1*dis3);//余弦定理求角度
        sinA = sqrt(1 - cosA*cosA);//求正弦
        curvity = 0.5*dis2/sinA;//正弦定理求外接圆半径
        curvity = 1/curvity;//半径的倒数是曲率，半径越小曲率越大
    }
    cout<<"curvity = "<<curvity<<"\n";
}
catch(tf::TransformException &ex)//处理意外情况
{
    ROS_ERROR("%s",ex.what());
    cout<<"三点取点失败\n";
}

参考博客
[1]: https://blog.csdn.net/Amych_/article/details/80209950