三维空间两条直线的最短距离、最近点及C++算法实现
2016-12-18 15:46
1566 查看
未经许可请勿转载
在双目视觉立体空间重建中,会根据两个相机中的物体图像坐标,求取给定三维坐标系的三维坐标,而可以根据物体
图像坐标、相机内参、给定坐标系的相机外参,求取相机光轴线的方程,从而实现立体重建,内外参、直线方程请执行
搜索学习,本文主要是解决在已知空间两直线求最短距离及最近点。如有疑问或错误,请与评论区指出,谢谢。
一、理论:
已知空间中两线段,如果它们无限变粗,判断是否相交。(主要讨论不在同一平面的情况)
线段AB,线段CD
问题的关键是求出这两条任意直线之间的最短距离,以及在这个距离上的两线最接近点坐标,判断该点是否在线段AB
和线段CD上。
首先将直线方程化为对称式,得到其方向向量n1=(a1,b1,c1),n2=(a2,b2,c2).
再将两向量叉乘得到其公垂向量N=(x,y,z),在两直线上分别选取点A,B(任意),得到向量AB,
求向量AB在向量N方向的投影即为两异面直线间的距离了(就是最短距离了)。
最短距离的求法:d=|向量N*向量AB|/|向量N|(上面是两向量的数量积,下面是取模)。
设交点为C,D,带入公垂线N的对称式中,又因为C,D两点分别满足一开始的直线方程,所以得到关于C(或D)的两个
连等方程,分别解出来就好了!
二、方程式
直线1:(平面式)
面e:A_1e*x+B_1e*y+C_1e*z+D_1e=0;
面f:A_1f*x+B_1f*y+C_1f*z+D_1f=0;
直线2:(平面式)
面e:A_2e*x+B_2e*y+C_2e*z+D_2e=0;
面f:A_2f*x+B_2f*y+C_2f*z+D_2f=0;
直线1的方向向量:(direction vector)
DV_1=(A_1e,B_1e,C_1e)X(A_1f,B_1f,C_1f)
=(B_1e*C_1f-B_1f*C_1e, C_1f*A_1e-A_1e*C_1f,A_1e*B_1f-A_1f*B_1e);
DV_2=(B_2e*C_2f-B_2f*C_2e, C_2f*A_2e-A_2e*C_2f,A_2e*B_2f-A_2f*B_2e);
在直线1、2上任取一点R1,R2
领Z_R1=1,Z_R2=1;
直线1带入Z_R 1有:
面e:A_1e*x+B_1e*y+C_1e*Z_R1+D_1e=0;
面f:A_1f*x+B_1f*y+C_1f*Z_R1+D_1f=0;
即:
P_R1=(X_R1,Y_R1,Z_R1);
同理:
P_R2=(X_R2,Y_R2,Z_R2);
两条直线的公垂线:(public vertical vector)
DV_PVV=(DV_1) X( DV_2);
在两条直线任意选的点的方向向量:
DV_RP=(P_R1-P_R2);
任选直线的方向向量与公垂线向量的数量积:即此直线在公垂线上的投影(d=|n*AB|/|N|):
当DIS小于某个给定阈值时,求最近点。
直线1的对称性方程:
令公垂线与直线1交于点m,交线与点n;
有
直线2:
并且把m、n带入公垂线方程,有:
把用公垂线表示的点m和n,分别带入直线1和2的对称性方程,消除点m和n, 有:
直线1:
直线2:
上述两式相减:
求出t_1m,t_2n,则m和n
最近点:
#include<opencv2/core/core.hpp>
using namespace cv;
Point3d two_space_linear_collision_get_nearest_point(double A_0e, double B_0e, double C_0e, double D_0e, double A_0f, double B_0f, double C_0f, double D_0f,
double A_1e, double B_1e, double C_1e, double D_1e, double A_1f, double B_1f, double C_1f, double D_1f)
{
//直线0的A面的D,A,B,C
double line0_flatE_d = D_0e;
Mat line0_flatE = (Mat_<double>(1, 3) << A_0e, B_0e, C_0e);
//直线0的B面的D,A,B,C
double line0_flatF_d = D_0f;
Mat line0_flatF = (Mat_<double>(1, 3) << A_0f, B_0f, C_0f);
//直线1的A面的D,A,B,C
double line1_flatE_d = D_1e;
Mat line1_flatE = (Mat_<double>(1, 3) << A_1e, B_1e, C_1e);
//直线1的B面的D,A,B,C
double line1_flatF_d = D_1f;
Mat line1_flatF = (Mat_<double>(1, 3) << A_1f, B_1f, C_1f);
//两条直线0,1的direction vector(raws = 1, cols = 3)
Mat direction_line0 = line0_flatE.cross(line0_flatF);
Mat direction_line1 = line1_flatE.cross(line1_flatF);
//line0的randpoint
Point3d randPoint0(0.0, 0.0, 0.0);
randPoint0.z = 800;
Mat randPoint0_in = (Mat_<double>(2, 2) << line0_flatE.at<double>(0, 0), line0_flatE.at<double>(0, 1),
line0_flatF.at<double>(0, 0), line0_flatF.at<double>(0, 1));
Mat randPoint0_out = randPoint0_in.inv()*(Mat_<double>(2, 1) << -line0_flatE_d - randPoint0.z*line0_flatE.at<double>(0, 2),
-line0_flatF_d - randPoint0.z*line0_flatF.at<double>(0, 2));
randPoint0.x = randPoint0_out.at<double>(0, 0);
randPoint0.y = randPoint0_out.at<double>(1, 0);
//line1上randpoint
Point3d randPoint1(0.0, 0.0, 0.0);
randPoint1.z = 800;
Mat randPoint1_in = (Mat_<double>(2, 2) << line1_flatE.at<double>(0, 0), line1_flatE.at<double>(0, 1),
line1_flatF.at<double>(0, 0), line1_flatF.at<double>(0, 1));
Mat randPoint1_out = randPoint1_in.inv()*(Mat_<double>(2, 1) << -line1_flatE_d - randPoint1.z*line1_flatE.at<double>(0, 2),
-line1_flatF_d - randPoint1.z*line1_flatF.at<double>(0, 2));
randPoint1.x = randPoint1_out.at<double>(0, 0);
randPoint1.y = randPoint1_out.at<double>(1, 0);
//Public vertical vector(rows=1,cols=3)
Mat publicVerticalRay = direction_line0.cross(direction_line1);
//任选点的方向向量
Mat randRay = (Mat_<double>(3, 1) << randPoint0.x - randPoint1.x, randPoint0.y - randPoint1.y, randPoint0.z - randPoint1.z);
Mat cross_rand_mult = publicVerticalRay*randRay;
//公垂线的模
double public_vertical_vector_length = sqrtf(publicVerticalRay.at<double>(0, 0)*publicVerticalRay.at<double>(0, 0)
+ publicVerticalRay.at<double>(0, 1)*publicVerticalRay.at<double>(0, 1)
+ publicVerticalRay.at<double>(0, 2)*publicVerticalRay.at<double>(0, 2));
//两直线的最小距离d=|公向量*任选向量|/|公向量|
double minDistance = std::abs(cross_rand_mult.at<double>(0, 0)) / public_vertical_vector_length;
Point3d point_3d(0.0,0.0,0.0);
if (minDistance<50)
{
Mat two_line_t_in = (Mat_<double>(3, 3) << publicVerticalRay.at<double>(0, 0), -direction_line0.at<double>(0, 0), direction_line1.at<double>(0, 0),
publicVerticalRay.at<double>(0, 1), -direction_line0.at<double>(0, 1), direction_line1.at<double>(0, 1),
publicVerticalRay.at<double>(0, 2), -direction_line0.at<double>(0, 2), direction_line1.at<double>(0, 2));
Mat two_line_t = two_line_t_in.inv()*randRay;
Point3d node0, node1;
node0.x = two_line_t.at<double>(1, 0)*direction_line0.at<double>(0, 0) + randPoint0.x;
node0.y = two_line_t.at<double>(1, 0)*direction_line0.at<double>(0, 1) + randPoint0.y;
node0.z = two_line_t.at<double>(1, 0)*direction_line0.at<double>(0, 2) + randPoint0.z;
node1.x = two_line_t.at<double>(2, 0)*direction_line1.at<double>(0, 0) + randPoint1.x;
node1.y = two_line_t.at<double>(2, 0)*direction_line1.at<double>(0, 1) + randPoint1.y;
node1.z = two_line_t.at<double>(2, 0)*direction_line1.at<double>(0, 2) + randPoint1.z;
point_3d = (node0 + node1) / 2;
}
else
{
point_3d = Point3d(0.0,0.0,0.0);
}
return point_3d;
}
Have a good day~
注:编写比较匆忙,如有偏差请谅解,未经许可请勿转载
在双目视觉立体空间重建中,会根据两个相机中的物体图像坐标,求取给定三维坐标系的三维坐标,而可以根据物体
图像坐标、相机内参、给定坐标系的相机外参,求取相机光轴线的方程,从而实现立体重建,内外参、直线方程请执行
搜索学习,本文主要是解决在已知空间两直线求最短距离及最近点。如有疑问或错误,请与评论区指出,谢谢。
一、理论:
已知空间中两线段,如果它们无限变粗,判断是否相交。(主要讨论不在同一平面的情况)
线段AB,线段CD
问题的关键是求出这两条任意直线之间的最短距离,以及在这个距离上的两线最接近点坐标,判断该点是否在线段AB
和线段CD上。
首先将直线方程化为对称式,得到其方向向量n1=(a1,b1,c1),n2=(a2,b2,c2).
再将两向量叉乘得到其公垂向量N=(x,y,z),在两直线上分别选取点A,B(任意),得到向量AB,
求向量AB在向量N方向的投影即为两异面直线间的距离了(就是最短距离了)。
最短距离的求法:d=|向量N*向量AB|/|向量N|(上面是两向量的数量积,下面是取模)。
设交点为C,D,带入公垂线N的对称式中,又因为C,D两点分别满足一开始的直线方程,所以得到关于C(或D)的两个
连等方程,分别解出来就好了!
二、方程式
直线1:(平面式)
面e:A_1e*x+B_1e*y+C_1e*z+D_1e=0;
面f:A_1f*x+B_1f*y+C_1f*z+D_1f=0;
直线2:(平面式)
面e:A_2e*x+B_2e*y+C_2e*z+D_2e=0;
面f:A_2f*x+B_2f*y+C_2f*z+D_2f=0;
直线1的方向向量:(direction vector)
DV_1=(A_1e,B_1e,C_1e)X(A_1f,B_1f,C_1f)
=(B_1e*C_1f-B_1f*C_1e, C_1f*A_1e-A_1e*C_1f,A_1e*B_1f-A_1f*B_1e);
DV_2=(B_2e*C_2f-B_2f*C_2e, C_2f*A_2e-A_2e*C_2f,A_2e*B_2f-A_2f*B_2e);
在直线1、2上任取一点R1,R2
领Z_R1=1,Z_R2=1;
直线1带入Z_R 1有:
面e:A_1e*x+B_1e*y+C_1e*Z_R1+D_1e=0;
面f:A_1f*x+B_1f*y+C_1f*Z_R1+D_1f=0;
即:
P_R1=(X_R1,Y_R1,Z_R1);
同理:
P_R2=(X_R2,Y_R2,Z_R2);
两条直线的公垂线:(public vertical vector)
DV_PVV=(DV_1) X( DV_2);
在两条直线任意选的点的方向向量:
DV_RP=(P_R1-P_R2);
任选直线的方向向量与公垂线向量的数量积:即此直线在公垂线上的投影(d=|n*AB|/|N|):
当DIS小于某个给定阈值时,求最近点。
直线1的对称性方程:
令公垂线与直线1交于点m,交线与点n;
有
直线2:
并且把m、n带入公垂线方程,有:
把用公垂线表示的点m和n,分别带入直线1和2的对称性方程,消除点m和n, 有:
直线1:
直线2:
上述两式相减:
求出t_1m,t_2n,则m和n
最近点:
#include<opencv2/core/core.hpp>
using namespace cv;
Point3d two_space_linear_collision_get_nearest_point(double A_0e, double B_0e, double C_0e, double D_0e, double A_0f, double B_0f, double C_0f, double D_0f,
double A_1e, double B_1e, double C_1e, double D_1e, double A_1f, double B_1f, double C_1f, double D_1f)
{
//直线0的A面的D,A,B,C
double line0_flatE_d = D_0e;
Mat line0_flatE = (Mat_<double>(1, 3) << A_0e, B_0e, C_0e);
//直线0的B面的D,A,B,C
double line0_flatF_d = D_0f;
Mat line0_flatF = (Mat_<double>(1, 3) << A_0f, B_0f, C_0f);
//直线1的A面的D,A,B,C
double line1_flatE_d = D_1e;
Mat line1_flatE = (Mat_<double>(1, 3) << A_1e, B_1e, C_1e);
//直线1的B面的D,A,B,C
double line1_flatF_d = D_1f;
Mat line1_flatF = (Mat_<double>(1, 3) << A_1f, B_1f, C_1f);
//两条直线0,1的direction vector(raws = 1, cols = 3)
Mat direction_line0 = line0_flatE.cross(line0_flatF);
Mat direction_line1 = line1_flatE.cross(line1_flatF);
//line0的randpoint
Point3d randPoint0(0.0, 0.0, 0.0);
randPoint0.z = 800;
Mat randPoint0_in = (Mat_<double>(2, 2) << line0_flatE.at<double>(0, 0), line0_flatE.at<double>(0, 1),
line0_flatF.at<double>(0, 0), line0_flatF.at<double>(0, 1));
Mat randPoint0_out = randPoint0_in.inv()*(Mat_<double>(2, 1) << -line0_flatE_d - randPoint0.z*line0_flatE.at<double>(0, 2),
-line0_flatF_d - randPoint0.z*line0_flatF.at<double>(0, 2));
randPoint0.x = randPoint0_out.at<double>(0, 0);
randPoint0.y = randPoint0_out.at<double>(1, 0);
//line1上randpoint
Point3d randPoint1(0.0, 0.0, 0.0);
randPoint1.z = 800;
Mat randPoint1_in = (Mat_<double>(2, 2) << line1_flatE.at<double>(0, 0), line1_flatE.at<double>(0, 1),
line1_flatF.at<double>(0, 0), line1_flatF.at<double>(0, 1));
Mat randPoint1_out = randPoint1_in.inv()*(Mat_<double>(2, 1) << -line1_flatE_d - randPoint1.z*line1_flatE.at<double>(0, 2),
-line1_flatF_d - randPoint1.z*line1_flatF.at<double>(0, 2));
randPoint1.x = randPoint1_out.at<double>(0, 0);
randPoint1.y = randPoint1_out.at<double>(1, 0);
//Public vertical vector(rows=1,cols=3)
Mat publicVerticalRay = direction_line0.cross(direction_line1);
//任选点的方向向量
Mat randRay = (Mat_<double>(3, 1) << randPoint0.x - randPoint1.x, randPoint0.y - randPoint1.y, randPoint0.z - randPoint1.z);
Mat cross_rand_mult = publicVerticalRay*randRay;
//公垂线的模
double public_vertical_vector_length = sqrtf(publicVerticalRay.at<double>(0, 0)*publicVerticalRay.at<double>(0, 0)
+ publicVerticalRay.at<double>(0, 1)*publicVerticalRay.at<double>(0, 1)
+ publicVerticalRay.at<double>(0, 2)*publicVerticalRay.at<double>(0, 2));
//两直线的最小距离d=|公向量*任选向量|/|公向量|
double minDistance = std::abs(cross_rand_mult.at<double>(0, 0)) / public_vertical_vector_length;
Point3d point_3d(0.0,0.0,0.0);
if (minDistance<50)
{
Mat two_line_t_in = (Mat_<double>(3, 3) << publicVerticalRay.at<double>(0, 0), -direction_line0.at<double>(0, 0), direction_line1.at<double>(0, 0),
publicVerticalRay.at<double>(0, 1), -direction_line0.at<double>(0, 1), direction_line1.at<double>(0, 1),
publicVerticalRay.at<double>(0, 2), -direction_line0.at<double>(0, 2), direction_line1.at<double>(0, 2));
Mat two_line_t = two_line_t_in.inv()*randRay;
Point3d node0, node1;
node0.x = two_line_t.at<double>(1, 0)*direction_line0.at<double>(0, 0) + randPoint0.x;
node0.y = two_line_t.at<double>(1, 0)*direction_line0.at<double>(0, 1) + randPoint0.y;
node0.z = two_line_t.at<double>(1, 0)*direction_line0.at<double>(0, 2) + randPoint0.z;
node1.x = two_line_t.at<double>(2, 0)*direction_line1.at<double>(0, 0) + randPoint1.x;
node1.y = two_line_t.at<double>(2, 0)*direction_line1.at<double>(0, 1) + randPoint1.y;
node1.z = two_line_t.at<double>(2, 0)*direction_line1.at<double>(0, 2) + randPoint1.z;
point_3d = (node0 + node1) / 2;
}
else
{
point_3d = Point3d(0.0,0.0,0.0);
}
return point_3d;
}
Have a good day~
注:编写比较匆忙,如有偏差请谅解,未经许可请勿转载
相关文章推荐
- 已知三维空间两条直线,如何计算两条直线距离最近的位置的中点
- 三维空间碰撞问题;空间中两直线的最短距离及最近点
- 三维空间碰撞问题;空间中两直线的最短距离及最近点
- 三维空间碰撞问题;空间中两直线的最短距离及最近点
- 空间两条直线段的最短距离及最近点计算
- 三维空间两直线/线段最短距离、线段计算算法
- 三维空间异面直线的距离及最近点
- 求空间内两条直线的最近距离以及最近点的坐标(C++)
- 三维空间两直线/线段最短距离、线段计算算法 【转】
- C++实现三维空间中点到点、点到直线、点到平面的距离计算
- hdoj4741 求空间中不平行的两条直线的最短距离及最短线段与两直线的交点
- 计算几何模板补充(三维空间体积、平面、直线、向量相关计算。附上hdu4741,求异面直线的最短距离与交点)
- 点到直线和点到线段的最短距离
- 蜂窝小区最短距离实现 (数学归纳法+广度优先算法)
- 最短编译距离(Minimum Edit Distance)算法及java实现
- 求两条直线的交点,运用面向对象的思想编程实现C++源码
- Python实现求两个字符串的最短编辑距离
- 求空间中线段上到已知直线距离最近的点
- 点(x3,y3)到经过点(x1,y1)和点(x2,y2)的直线的最短距离
- c++递归实现关于无向图中任意两点的最短距离