Python 在open3d中创建箭头

我正在使用Open3D可视化一些点云。我想添加在特定点开始和结束的箭头。箭头将显示我正在处理的一些事情。然而，我还没有找到一个简单的方法来添加这些箭头

我注意到有一个创建笛卡尔坐标系的函数，它使用箭头。因此，可以将箭头添加到三维可视化中

将open3d导入为o3d
#创建笛卡尔坐标
FOR=o3d.geometry.TriangleMesh.create_坐标系(
大小=10，原点=[0,0,0]）
#想象
o3d.可视化.绘制几何体（[FOR]）

在Open3D中找不到创建箭头的简单方法让我很沮丧，经过一段时间的努力，我找到了一个解决方案

将open3d导入为o3d
将numpy作为np导入
def绘图几何图形（PCD）：
"""
绘制几何图形
Args：
-PCD（）：[pcd1，pcd2，…]
"""
o3d.可视化.绘制几何图形（PCD）
def get_o3d_用于（原点=[0,0,0]，大小=10）：
""" 
为创建可添加到open3d点云的
"""
网格\u框架=o3d.geometry.TriangleMesh.create\u坐标\u框架(
尺寸=尺寸）
网格\帧平移（原点）
返回（网格/框架）
def矢量_幅值（vec）：
"""
计算向量的大小。
Args：
-vec（）：
"""
震级=np.sqrt（np.sum（向量**2））
返回（幅度）
def计算箭头（vec）的旋转角度：
"""
计算从矢量vec到目标所需的旋转
原始的z轴矢量。第一次旋转即
在z轴上计算。这将在
然后，在y轴上旋转。
返回所需的z轴和y轴的旋转角度
将向量向量向量设置为与z轴相同的方向
原版的
Args：
-vec（）：
"""
#在的z轴上旋转
gamma=np.arctan（vec[1]/vec[0]）
Rz=np.数组（[[np.cos（gamma），-np.sin（gamma），0]，
[np.sin（gamma），np.cos（gamma），0]，
[0,0,1]])
#旋转矢量以计算下一个旋转
vec=Rz。T@vec.reshape(-1,1)
向量=向量重塑（-1）
#在的y轴上旋转
beta=np.arctan（vec[0]/vec[2]）
Ry=np.array（[[np.cos（beta），0，np.sin（beta）]，
[0,1,0],
[-np.sin（beta），0，np.cos（beta）]）
返回（Rz，Ry）
def创建箭头（比例=10）：
"""
在中为Open3D创建箭头
"""
圆锥体高度=刻度*0.2
气缸高度=刻度*0.8
圆锥体半径=刻度/10
圆柱半径=刻度/20
网格框架=o3d.geometry.TriangleMesh.create箭头（圆锥体半径=1，
锥体高度=锥体高度，
圆柱半径=0.5，
气缸高度=气缸高度）
返回（网格/框架）
def get_箭头（原点=[0,0,0]，终点=无，向量=无）：
"""
创建从原点到终点的箭头，
或从原点开始从矢量向量创建箭头。
Args：
-end（）：端点。[x，y，z]
-vec（）：向量[i，j，k]
"""
比例=10
Ry=Rz=np.眼（3）
T=np.数组（[[1,0,0,0]，[0,1,0,0]，[0,0,1,0]，[0,0,0,1]]
T[：3，-1]=原点
如果结束不是无：
向量=np.数组（结束）-np.数组（原点）
elif vec不是无：
向量=np.数组（向量）
如果end不是None或vec不是None：
标度=矢量大小（vec）
Rz，Ry=计算箭头（vec）的旋转角度
网格=创建箭头（比例）
#创建箭头
旋转网格（Ry，center=np.array（[0,0,0]））
旋转网格（Rz，中心=np.数组（[0,0,0]））
网格。平移（原点）
返回（网格）
#创建笛卡尔坐标系
FOR=get_o3d_FOR（）
#创建从点（5,5,5）到点（10,10,10）的箭头
#箭头=获取箭头（[5,5,5]，[10,10,10]）
#从（5,5,5）开始创建表示矢量向量的箭头
#arrow=get_arrow（[5,5,5]，vec=[5,5,5]）
#在与z轴相同的位置创建箭头
arrow=get_arrow（）
#画一切
绘制几何图形（[FOR，箭头]）

如本文所述，我们可以计算2个（单位）向量的对齐操作之间的矩阵，如下式所示： 其中“r”定义为p和q的叉积。 “[r]x”由一个斜对称矩阵定义，可以找到

因此，我们可以使用此方法创建所需的箭头或圆柱体：只需使用上一个矩阵作为目标的旋转，并对齐中心。

演示可以通过以下方式实现（基于open3d 0.9.0）：

---

最初我对你的答案投了赞成票，但我注意到你现在至少有一个错误。当您尝试在Z轴方向绘制箭头时，如arrow=get_arrow（end=[0,0,1]），它会输出NaN，因为vec[1]/vec[0]实际上是0除以0。这是一个创建箭头网格的工厂函数，但它不采用起点/终点，因此需要相应地对其进行转换。看见

import numpy as np 
import open3d as o3d 


def get_cross_prod_mat(pVec_Arr):
    # pVec_Arr shape (3)
    qCross_prod_mat = np.array([
        [0, -pVec_Arr[2], pVec_Arr[1]], 
        [pVec_Arr[2], 0, -pVec_Arr[0]],
        [-pVec_Arr[1], pVec_Arr[0], 0],
    ])
    return qCross_prod_mat


def caculate_align_mat(pVec_Arr):
    scale = np.linalg.norm(pVec_Arr)
    pVec_Arr = pVec_Arr/ scale
    # must ensure pVec_Arr is also a unit vec. 
    z_unit_Arr = np.array([0,0,1])
    z_mat = get_cross_prod_mat(z_unit_Arr)

    z_c_vec = np.matmul(z_mat, pVec_Arr)
    z_c_vec_mat = get_cross_prod_mat(z_c_vec)

    if np.dot(z_unit_Arr, pVec_Arr) == -1:
        qTrans_Mat = -np.eye(3, 3)
    elif np.dot(z_unit_Arr, pVec_Arr) == 1:   
        qTrans_Mat = np.eye(3, 3)
    else:
        qTrans_Mat = np.eye(3, 3) + z_c_vec_mat + np.matmul(z_c_vec_mat,
                                                    z_c_vec_mat)/(1 + np.dot(z_unit_Arr, pVec_Arr))

    qTrans_Mat *= scale
    return qTrans_Mat



if __name__ == "__main__":
    z_unit_Arr = np.array([0,0,1])
    begin = [1, 0, 0]
    end = [1.6, 0.4, 0.8]
    vec_Arr = np.array(end) - np.array(begin)
    vec_len = np.linalg.norm(vec_Arr)


    mesh_frame = o3d.geometry.TriangleMesh.create_coordinate_frame(size=0.6, origin=[0, 0, 0])
    

    mesh_arrow = o3d.geometry.TriangleMesh.create_arrow(
        cone_height= 0.2 * vec_len, 
        cone_radius= 0.06 * vec_len, 
        cylinder_height= 0.8 * vec_len,
        cylinder_radius=  0.04 * vec_len
        )
    mesh_arrow.paint_uniform_color([1,0,1])
    mesh_arrow.compute_vertex_normals()

    mesh_sphere_begin = o3d.geometry.TriangleMesh.create_sphere(radius=0.1, resolution= 20)
    mesh_sphere_begin.translate(begin)
    mesh_sphere_begin.paint_uniform_color([0,1,1])
    mesh_sphere_begin.compute_vertex_normals()

    mesh_sphere_end = o3d.geometry.TriangleMesh.create_sphere(radius=0.1, resolution= 20)
    mesh_sphere_end.translate(end)
    mesh_sphere_end.paint_uniform_color([0,1,1])
    mesh_sphere_end.compute_vertex_normals()

    # mesh_arrow,
    o3d.visualization.draw_geometries(
        geometry_list= [mesh_frame, mesh_arrow, mesh_sphere_begin, mesh_sphere_end],
        window_name= "before", width= 800, height= 600
    )

    rot_mat = caculate_align_mat(vec_Arr)
    mesh_arrow.rotate(rot_mat, center = False)
    o3d.visualization.draw_geometries(
        geometry_list= [mesh_frame, mesh_arrow, mesh_sphere_begin, mesh_sphere_end],
        window_name= "after rotate", width= 800, height= 600
    )
    mesh_arrow.translate(np.array(begin)) # 0.5*(np.array(end) - np.array(begin))
    o3d.visualization.draw_geometries(
        geometry_list= [mesh_frame, mesh_arrow, mesh_sphere_begin, mesh_sphere_end],
        window_name= "after translate", width= 800, height= 600
    )