使用Open3D进行PCD拟合平面的Python代码示例

Posted 2023-05-11 西北逍遥

使用Open3D进行PCD拟合平面的Python代码示例

 

import open3d as o3d  
import numpy as np  
  
# 读取点云数据  
pcd = o3d.io.read_point_cloud("2023042501.pcd")  
  
# 创建PCD图  
pcd_graph = o3d.geometry.PointCloudGraph(pcd)  
  
# 选择要拟合的平面  
plane_center = o3d.geometry.Vector3d(0.0, 0.0, 0.0)  
plane_normal = o3d.geometry.Vector3d(1.0, 0.0, 0.0)  
plane_vertices = o3d.geometry.Vector3d(0.0, 0.0, 0.0)  
  
# 创建平面  
plane = o3d.geometry.Plane(plane_center, plane_normal)  
  
# 添加点云数据到平面  
plane_vertices = o3d.geometry.Vector3d(pcd.vertices[0], pcd.vertices[1], pcd.vertices[2])  
o3d.geometry.centroid(plane)  
plane.set_vertex_data_multi(plane_vertices)  
  
plane_vertices = o3d.geometry.Vector3d(pcd.vertices[0], pcd.vertices[1], pcd.vertices[2])  
o3d.geometry.center(plane)  
plane.set_vertex_data_multi(plane_vertices)  
  
plane_vertices = o3d.geometry.Vector3d(pcd.vertices[0], pcd.vertices[1], pcd.vertices[2])  
o3d.geometry.normal(plane)  
plane.set_vertex_data_multi(plane_vertices)  
  
# 显示结果  
o3d.visualization.draw_geometries([pcd_graph, plane])  
  
# 输出平面参数  
print("平面参数:", plane_center, plane_normal, plane_vertices)

 

 

 

#####################

QQ 3087438119

沿着两个平面的交点找到线

【中文标题】沿着两个平面的交点找到线

【英文标题】：Finding the line along the intersection of two planes

【发布时间】：2013-04-08 04:24:30

【问题描述】：

我试图在 3D 中绘制由两个平面的交点形成的线，但我无法理解数学，这已在 here 和 here 进行了解释。

我试图自己弄清楚，但最接近解决方案的是一个向量，它使用平面法线的叉积，指向与相交线相同的方向。我不知道如何在相交线上找到一个点，任何点都可以。我认为这种方法是死胡同。这是此尝试的屏幕截图：

我尝试使用this 问题中提到的解决方案，但它与原始解释有一个死链接，并且该等式对我不起作用（它有不平衡的括号，我试图在下面更正）。

var planeA = new THREE.Plane((new THREE.Vector3(0, 0, 1)).normalize(), 100);
var planeB = new THREE.Plane((new THREE.Vector3(1, 1, 1)).normalize(), -100);

var x1 = planeA.normal.x,
    y1 = planeA.normal.y,
    z1 = planeA.normal.z,
    d1 = planeA.constant;

var x2 = planeB.normal.x,
    y2 = planeB.normal.y,
    z2 = planeB.normal.z,
    d2 = planeB.constant;

var point1 = new THREE.Vector3();
point1.x = 0;
point1.z = (y2 / y1) * (d1 - d2) / (z2 - z1 * y2 / y1);
point1.y = (-z1 * point1.z - d1) / y1;

var point2 = new THREE.Vector3();
point2.x = 1;
point2.z = (y2 / y1) * (x1 * point2.x + d1) - (x2 * point2.x - d2) / (z2 - z1 * y2 / y1);
point2.y = (-z1 * point2.z - x1 * point2.x - d1) / y1;

console.log(point1, point2);

输出：

THREE.Vector3 x: -1, y: NaN, z: NaN, …
THREE.Vector3 x: 1, y: Infinity, z: -Infinity, …

预期输出：

x = 0 的交叉点，并且 同一直线上的另一个点 x = 1

如果有人能指出这应该如何工作的一个很好的解释，或者一个平面-平面相交算法的例子，我将不胜感激。

【问题讨论】：

如果你只让x = 0 在两个平面方程中，你可以找到对应于x 的y 和z。对 x = 1 重复此操作。

【参考方案1】：

当我遇到这样的问题时，我通常让一个符号代数包（在这种情况下是 Mathematica）来处理它。输入后

In[1]:= n1=x1,y1,z1;n2=x2,y2,z2;p=x,y,z;

In[2]:= Solve[n1.p==d1&&n2.p==d2,p]    

简化并代入 x=0 和 x=1，我得到

                d2 z1 - d1 z2        d2 y1 - d1 y2
Out[5]= y -> -------------, z -> ----------------, 
                y2 z1 - y1 z2       -(y2 z1) + y1 z2

            d2 z1 - x2 z1 - d1 z2 + x1 z2
>    y -> -----------------------------, 
                    y2 z1 - y1 z2

            d2 y1 - x2 y1 + (-d1 + x1) y2
>      z -> -----------------------------
                  -(y2 z1) + y1 z2

【讨论】：

这听起来像是解决此类问题的好工具，但我不熟悉符号代数包。说实话，当你为我做的时候，我什至很难理解输出：p 我检查了 Octave，并决定学习该工具需要花费与研究解决方案一样多的时间以老式的方式解决我的问题。不过，我很想在未来更多地探索 Octave，也许在参加线性代数在线课程时。我希望可汗有教训！

感谢您向我展示了一个解决未来几何问题的好工具。我是 3D 编程的新手，我可以看到在我的未来会有更多这样的问题。

@DanRoss 除非 Octave 有我不知道的功能，否则您需要更像 Sage 的东西。

另外，您可能想了解射影几何，以减少必须处理的退化情况的数量（例如，线平行于 x 轴）。

是的，案例。 article 建议选择平面法线叉积中绝对值最大的轴。我随意选择了x轴。你称之为投影几何，我将不得不谷歌。

【参考方案2】：

先决条件

---

回想一下，要表示一条线，我们需要一个描述其方向的向量和该线所经过的点。这称为参数化形式：

line_point(t) = t * (point_2 - point_1) + point_1

其中point_1 和point_2 是直线经过的任意点，t 是一个标量，它参数化我们的直线。现在，如果我们将任意t 放入上面的等式中，我们就可以在线上找到任意一点line_point(t)。

注意：术语(point_2 - point_1) 什么都不是，只是描述我们线的方向的向量，术语point_1 什么都不是，而是我们的线经过的一个点（当然point_2) 也可以使用。

算法

---

通过以下方式找到相交线的方向direction 平面法线的叉积，即direction = cross(normal_1, normal_2)。

选择任何平面，例如第一个，然后找到任意 2 个不同的点 在这架飞机上：point_1 和 point_2。如果我们假设平面方程 是 a1 * x + b1 * y + c1 * z + d1 = 0 的形式，然后找到 2 我们可以做到以下几点：

y1 = 1
z1 = 0
x1 = -(b1 + d1) / a1

y2 = 0
z2 = 1
x2 = -(c1 + d1) / a1

point_1 = (x1, y1, z1) 和 point_2 = (x2, y2, z2) 的位置。

现在我们有 2 个点，我们可以构造 参数化的 第一个平面上位于的线的表示： line_point(t) = t * (point_2 - point_1) + point_1，其中line_point(t) 描述这条线上的任何点，t 只是一个输入标量 （通常称为参数）。

找到直线的交点intersection_point line_point(t) 和 second 平面 a2 * x + b2 * y + c2 * z + d2 = 0 通过使用 标准line-plane intersection algorithm（注意 代数形式部分，因为这是实现线平面所需的全部内容 交叉路口，如果您还没有这样做的话）。

我们的相交线现在找到了，可以在 像往常一样的参数化形式：intersection_line_point(s) = s * direction + intersection_point，其中intersection_line_point(s) 描述此交线上的任意点，s 是参数。

注意：我没有在任何地方读过这个算法，我只是根据我对线性代数的知识从头顶设计出来的。这并不意味着它不起作用，但这个算法可能可以进一步优化。

调理

---

当 2 个法线向量 normal_1 和 normal_2 几乎共线时，这个问题会变得极端 ill-conditioned。从几何上讲，这意味着这两个平面几乎平行彼此，并且以可接受的精度确定相交线在有限的情况下变得不可能精度算术，在这种情况下是浮点算术。

【讨论】：

好吧，我到了第 2 步，发现一个问题：point_1（红球）和point_2（绿球）不在第一个平面上，它们在它的两侧pic。很可能我没有正确理解您的解释，3D 数学对我来说是一门新学科，而且我在学校没有做太多线性代数。我终于找到了一个关于交集算法的好article，似乎我已经从它的“直接线性方程”解决方案中得到了一个workingscript。

你显然做错了。再一次，在第 2 步中，我们要选择这 2 个中的 任何平原（因为它根本不重要）并找到位于该平面上的 任何线。要确定一条线，我们所要做的就是简单地取 2 个任意点 和那个平面（同样，确切的点并不重要，重要的是它们在那个平面上）。所以我们选择了一个简单的选择来做很少的计算，当计算这两个点时——我们的线（位于所选平面上）可以完全由步骤 3 中的line_point(t) 方程确定。

【参考方案3】：

让three.js为你解决这个问题很容易。

如果你用矩阵表示法来表达你的问题

m * x = v

那么x的解是

x = inverse( m ) * v

我们将为 m 使用 4x4 矩阵，因为 three.js 为 Matrix4 类提供了一个 inverse() 方法。

var x1 = 0,
    y1 = 0,
    z1 = 1,
    d1 = 100;

var x2 = 1,
    y2 = 1,
    z2 = 1,
    d2 = -100;

var c = 0; // the desired value for the x-coordinate

var v = new THREE.Vector4( d1, d2, c, 1 );

var m = new THREE.Matrix4( x1, y1, z1, 0, 
                           x2, y2, z2, 0,
                           1,  0,  0,  0,
                           0,  0,  0,  1
                         );

var minv = new THREE.Matrix4().getInverse( m );

v.applyMatrix4( minv );

console.log( v );

根据需要，v 的 x 分量将等于 c，y 和 z 分量将包含您要查找的值。 w 分量是不相关的。

现在，重复 c 的下一个值，c = 1。

three.js r.58

【讨论】：

执行这段代码返回 x: 100, y: -100, z: 0, w: 1 ，所以x不等于c。当我玩这个时，它似乎总是在 x 和 y 中返回 d1 和 d2 中的内容以及 z 中的 c 中的内容，并且无论我如何更改平面创建的向量，都不会改变。这是如何被接受的答案？这是我尝试过的一个 jsfiddle：jsfiddle.net/0vzbjeco

【参考方案4】：

这是http://geomalgorithms.com/a05-_intersect-1.html 中描述的平面-平面相交解决方案的实现。本质上，您首先使用平面法线的叉积来找到两个平面中线的方向。其次，您在平面的隐式方程上使用一些代数（P . n + d = 0，其中 P 是平面上的某个点，n 是法线，d 是平面常数）来求解位于平面上的点平面的交点，并且也在 x=0、y=0 或 z=0 平面之一上。解决方案是由一个点和一个向量描述的线。我使用的是three.js 79版

/*

Algorithm taken from http://geomalgorithms.com/a05-_intersect-1.html. See the
section 'Intersection of 2 Planes' and specifically the subsection
(A) Direct Linear Equation

*/
function intersectPlanes(p1, p2) 

  // the cross product gives us the direction of the line at the intersection
  // of the two planes, and gives us an easy way to check if the two planes
  // are parallel - the cross product will have zero magnitude
  var direction = new THREE.Vector3().crossVectors(p1.normal, p2.normal)
  var magnitude = direction.distanceTo(new THREE.Vector3(0, 0, 0))
  if (magnitude === 0) 
    return null
  

  // now find a point on the intersection. We use the 'Direct Linear Equation'
  // method described in the linked page, and we choose which coordinate
  // to set as zero by seeing which has the largest absolute value in the
  // directional vector

  var X = Math.abs(direction.x)
  var Y = Math.abs(direction.y)
  var Z = Math.abs(direction.z)

  var point

  if (Z >= X && Z >= Y) 
    point = solveIntersectingPoint('z', 'x', 'y', p1, p2)
   else if (Y >= Z && Y >= X)
    point = solveIntersectingPoint('y', 'z', 'x', p1, p2)
   else 
    point = solveIntersectingPoint('x', 'y', 'z', p1, p2)
  

  return [point, direction]



/*

This method helps finding a point on the intersection between two planes.
Depending on the orientation of the planes, the problem could solve for the
zero point on either the x, y or z axis

*/
function solveIntersectingPoint(zeroCoord, A, B, p1, p2)
    var a1 = p1.normal[A]
    var b1 = p1.normal[B]
    var d1 = p1.constant

    var a2 = p2.normal[A]
    var b2 = p2.normal[B]
    var d2 = p2.constant

    var A0 = ((b2 * d1) - (b1 * d2)) / ((a1 * b2 - a2 * b1))
    var B0 = ((a1 * d2) - (a2 * d1)) / ((a1 * b2 - a2 * b1))

    var point = new THREE.Vector3()
    point[zeroCoord] = 0
    point[A] = A0
    point[B] = B0

    return point



var planeA = new THREE.Plane((new THREE.Vector3(0, 0, 1)).normalize(), 100)
var planeB = new THREE.Plane((new THREE.Vector3(1, 1, 1)).normalize(), -100)

var [point, direction] = intersectPlanes(planeA, planeB)

【讨论】：

此代码无效！我将它发布在一个 jsfiddle 中，并沿着返回的交叉点绘制平面和一个矢量，它完全在其他地方：jsfiddle.net/ksfoLp1d/6这是如何获得 7 次投票的？