文章目录
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();
}