【PCL】两平行面间的距离求解并可视化(附代码)

1. 解决方法

• 最近在解决两个平面的问题，求两个平行平面的距离，知道两个点和两个发方向。

• 用到的事高中知识

• 大概思路如下图
  

• 代码中求解AC 距离那块我手算的，没用代码解，中间量多，懒得写。知道方法就行。

• 手动创建两个平面和发下以及点，求距离并显示。

/*
*	已知道两平行平面的法线，以及两平面中的一个点
*   求两平面的距离
*/

#include <iostream> 
#include <math.h>
#include <string>
#include <vector>
#include <pcl/point_types.h>
#include <pcl/visualization/cloud_viewer.h>

int main(int argc, char * argv[]){
    // 定义L1 L2 两个面的法向量相反
    pcl::Normal NL1(0.5, 0.5, pow(0.5,0.5));
    pcl::Normal NL2(-0.5, -0.5, -pow(0.5,0.5));

    // 定义平面内的两个点 A ，B
    pcl::PointXYZRGB A,B;
    A.x = 1.0;
    A.y = 0.0;
    A.z = 0.0;
    A.r = 255;
    A.g = 255;
    A.b = 255;

    B.x = -1.0;
    B.y = 0.0;
    B.z = 0.0;
    B.r = 255;
    B.g = 255;
    B.b = 255;

    // 定义点C 过点A 在L2 平面的垂线交点为C
    pcl::PointXYZRGB C;
    // double x,y,z;
    // H 是AC 间的距离，也就是最后求的未知数
    // 方程如下，手动接得H = 1 或 0，取1
    // x = A.x + H * a
    // y = A.y + H * b
    // z = A.z + H * c
    // |AB|^2 = |AC|^2 + |BC|^2
    double H = 1; //用上面方法手算得到1
    C.x = A.x - H * NL1.normal_x;
    C.y = A.y - H * NL1.normal_y;
    C.z = A.z - H * NL1.normal_z;
    C.r = 255;
    C.g = 255;
    C.b = 255;

    // 生成两个平面内的点用于显示
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr L1(new pcl::PointCloud<pcl::PointXYZRGB>);
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr L2(new pcl::PointCloud<pcl::PointXYZRGB>);

    L1->push_back(A);
    L2->push_back(B);
    for (size_t i = 1; i < 50; i++)
    {
        pcl::PointXYZRGB P1;
        P1.x = A.x + 0.05 * i;
        P1.r = 255;
        P1.g = 0;
        P1.b = 0;
        for (size_t j = 1; j < 50; j++)
        {
            P1.y = A.y + 0.05 * j;
            P1.z = (0.5 - 0.5*P1.x - 0.5*P1.y)  / pow(0.5,0.5);
            L1->push_back(P1);
        }
    }
    for (int i = -30; i < 50; i++)
    {
        pcl::PointXYZRGB P2;
        P2.x = B.x + 0.05 * i;
        P2.r = 0;
        P2.g = 255;
        P2.b = 0;
        for (int j = -30; j < 50; j++)
        {
            P2.y = B.y + 0.05 * j;
            P2.z = (-0.5 - 0.5*P2.x - 0.5*P2.y)  / pow(0.5,0.5);
            L2->push_back(P2);
        }
    }
    
    cout<<"Virwe"<<endl;
    pcl::visualization::PCLVisualizer viewer;
    viewer.setBackgroundColor(0,0,0);
    viewer.addPointCloud(L1,"L1");
    viewer.addPointCloud(L2,"L2");

    viewer.addLine(A,B,0.0f,0.0f,1.0f,"AB");
    viewer.addLine(B,C,0.0f,0.0f,1.0f,"BC");
    viewer.addLine(C,A,0.0f,0.0f,1.0f,"CA");
    viewer.spin();

}

2. 【更新】带参数的求解

• 给出任意的 A B 和N，即可求出。
• 求解过程

• 代码如下
  可自行更改 NL1 A B 的定义的值

/*
*	已知道两平行平面的法线，以及两平面中的一个点
*   求两平面的距离
*/

#include <iostream> 
#include <math.h>
#include <string>
#include <vector>
#include <pcl/point_types.h>
#include <pcl/visualization/cloud_viewer.h>

int main(int argc, char * argv[]){
    // 定义L1 L2 两个面的法向量相反
    pcl::Normal NL1(0.5, 0.5, pow(0.5,0.5));
    pcl::Normal NL2(-0.5, -0.5, -pow(0.5,0.5));

    // 定义平面内的两个点 A ，B
    pcl::PointXYZRGB A(1.0, 0.0, 0.0, 255, 255, 255);
    pcl::PointXYZRGB B(-1.0, 0.3, 0.2, 255, 255, 255);


    // 定义点C 过点A 在L2 平面的垂线交点为C
    // H 是AC 间的距离，也就是最后求的未知数
    // 方程如下，手动接得H = 1 或 0，取1
    // x = A.x + H * a
    // y = A.y + H * b
    // z = A.z + H * c
    // |AB|^2 = |AC|^2 + |BC|^2
    pcl::PointXYZRGB C;
    double x1,x2,x3,y1,y2,y3,z1,z2,z3;
    double a,b,c;
    double H;
    x1 = A.x; y1 =A.y; z1 =A.z;
    x2 = B.x; y2 =B.y; z2 =B.z;
    a = NL1.normal_x; b = NL1.normal_y; c = NL1.normal_z;
    double X, Y, Z;
    X = x1-x2; Y = y1-y2; Z = z1-z2;
    
    double E = 2 * (pow(a,2) + pow(b,2) + pow(c,2));
    double F = -2 * (X*a + Y*b + Z*c);
    double alfa2 = pow((x1-x2), 2) + pow((y1-y2), 2) + pow((z1-z2), 2);
    double G = pow(X,2) + pow(Y,2) + pow(Z,2) - alfa2;

    double Htemp = pow((pow(F,2) - 4*E*G), 0.5);
    H = 0.5 * (Htemp - F) / E;
    x3 = x1 - H * a;
    y3 = y1 - H * b;
    z3 = z1 - H * c;
    C.x = x3;
    C.y = y3;
    C.z = z3;
    C.r = 255;
    C.g = 255;
    C.b = 255;

    // 生成两个平面内的点用于显示
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr L1(new pcl::PointCloud<pcl::PointXYZRGB>);
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr L2(new pcl::PointCloud<pcl::PointXYZRGB>);

    L1->push_back(A);
    L2->push_back(B);
    double L1_D = A.x*a + A.y*b + A.z*c;
    for (size_t i = 1; i < 50; i++)
    {
        pcl::PointXYZRGB P1;
        P1.x = A.x + 0.05 * i;
        P1.r = 255;
        P1.g = 0;
        P1.b = 0;
        for (size_t j = 1; j < 50; j++)
        {
            P1.y = A.y + 0.05 * j;
            if (c == 0.0)
                P1.z = A.z;
            else
                P1.z = (L1_D - a*P1.x - b*P1.y) / c;
            L1->push_back(P1);
        }
    }
    double L2_D = -B.x*a - B.y*b - B.z*c;
    for (int i = -30; i < 50; i++)
    {
        pcl::PointXYZRGB P2;
        P2.x = B.x + 0.05 * i;
        P2.r = 0;
        P2.g = 255;
        P2.b = 0;
        for (int j = -30; j < 50; j++)
        {
            P2.y = B.y + 0.05 * j;
            if (c == 0.0)
                P2.z = B.z;
            else
                P2.z = (-L2_D - a*P2.x - b*P2.y) / c;
            L2->push_back(P2);
        }
    }
    
    cout<<"Virwe"<<endl;
    pcl::visualization::PCLVisualizer viewer;
    viewer.setBackgroundColor(0,0,0);
    viewer.addPointCloud(L1,"L1");
    viewer.addPointCloud(L2,"L2");

    viewer.addLine(A,B,0.0f,0.0f,1.0f,"AB");
    viewer.addLine(B,C,0.0f,0.0f,1.0f,"BC");
    viewer.addLine(C,A,0.0f,0.0f,1.0f,"CA");
    viewer.spin();

}