空间中平面方程求解及点到平面的距离

1.空间中平面方程的一般形式为：   

Ax+By+Cz+D=0 (参数,A,B,C,D是描述平面空间特征的常数)  

已知空间中3个点的坐标(x1,y1,z1),(x2,y2,z2),(x3,y3,z3)，求解平面方程。

解法1.根据已知的3个点，建立3个联合方程组，进行消元；
2.根据克莱姆法则，

求解的结果中，(A,B,C)表示平面的法向量。下面给出OpenCV中具体代码实现：
 

//根据3个点,计算空间平面的方程
//Ax+By+Cz+D=0
//输入参数:point3fArray---空间中3个点的坐标,大小为3;输入点>3时,只取前3个点
//输出参数A,B,C,D
//返回值:true---计算成功;false----计算失败
bool GetPanelEquation(vector<cv::Point3f> &point3fArray, float &A, float &B, float &C, float &D)
{
    if( point3fArray.size() <3 )
    {
        cerr<<"GetPanelEquation(...)函数中输入点的数量小于3."<<endl;
        return false;
    }

    A = point3fArray[0].y*(point3fArray[1].z-point3fArray[2].z) +
        point3fArray[1].y*(point3fArray[2].z-point3fArray[0].z) +
        point3fArray[2].y*(point3fArray[0].z-point3fArray[1].z);

    B = point3fArray[0].z*(point3fArray[1].x-point3fArray[2].x) +
        point3fArray[1].z*(point3fArray[2].x-point3fArray[0].x) +
        point3fArray[2].z*(point3fArray[0].x-point3fArray[1].x);

    C = point3fArray[0].x*(point3fArray[1].y-point3fArray[2].y) +
        point3fArray[1].x*(point3fArray[2].y-point3fArray[0].y) +
        point3fArray[2].x*(point3fArray[0].y-point3fArray[1].y);

    D = -point3fArray[0].x*(point3fArray[1].y*point3fArray[2].z - point3fArray[2].y*point3fArray[1].z) -
         point3fArray[1].x*(point3fArray[2].y*point3fArray[0].z - point3fArray[0].y*point3fArray[2].z) -
         point3fArray[2].x*(point3fArray[0].y*point3fArray[1].z - point3fArray[1].y*point3fArray[0].z);


    return true;

//    //以下方式计算结果和上面一致
//    float a,b,c,d;
//    a = (point3fArray[1].y - point3fArray[0].y)*(point3fArray[2].z - point3fArray[0].z) -
//        (point3fArray[1].z - point3fArray[0].z)*(point3fArray[2].y - point3fArray[0].y);
//
//    b = (point3fArray[1].z - point3fArray[0].z)*(point3fArray[2].x - point3fArray[0].x) -
//        (point3fArray[1].x - point3fArray[0].x)*(point3fArray[2].z - point3fArray[0].z);
//
//    c = (point3fArray[1].x - point3fArray[0].x)*(point3fArray[2].y - point3fArray[0].y) -
//        (point3fArray[1].y - point3fArray[0].y)*(point3fArray[2].x - point3fArray[0].x);
//
//    d = 0 - (a * point3fArray[0].x + b*point3fArray[0].y + c*point3fArray[0].z);
//
//
//    return true;
}

2.计算空间点到空间平面距离

float point2panelDistance(cv::Point3f &pt, float A, float B, float C, float D)
{
    float distance = -10000;

    float temp = sqrt(A * A + B * B + C * C);

    if( temp <= 0.001)
    {
        cerr<<"point2panelDistance(...)函数中,平面方程参数A,B,C为0."<<endl;
        return distance;
    }

    distance = 1.0*abs(A*pt.x+B*pt.y+C*pt.z+D) / temp;

    return distance;
}