def __init__(self, pn_model):
self.vis = pn.Visualizer()
self.vis.create_window()
self.camera_representation = pn.LineSet()
self.camera_trace = pn.LineSet()
self.vis.add_geometry(pn_model)
self.vis.add_geometry(self.camera_representation)
self.vis.run()
return
def o3d_pca_geometry(vectors, center):
"""
Generates open3d lineset objects for the pca vectors created around the center of the 3D pointcloud.
"""
# separate each vector
vx = vectors[0]
vy = vectors[1]
vz = vectors[2]
# set 4 points, the end points of each vector and the centroid
points = [center, vx, vy, vz]
# set the lines to go from center to each point
lines = [
[0, 1],
[0, 2],
[0, 3],
]
#each vector is red, green blue are PCA1, PCA2 and PCA3 respectivley.
colors = [[1, 0, 0], [0, 1, 0], [0, 0, 1]]
#create open3d linesets.
line_set = o3d.LineSet()
line_set.points = o3d.Vector3dVector(points)
line_set.lines = o3d.Vector2iVector(lines)
line_set.colors = o3d.Vector3dVector(colors)
return line_set
def show_desktop_pcd(self, isshow=True):
self.desktop_3d_point = self.define_triangle()
# self.triangle_points_2d, triangle_depth = open3d_tools.convert_point3D_2D(self.desktop_3d_point,
# self.camera_conf.camera_intrinsic,
# self.camera_conf.depth_scale)
self.triangle_points_2d, triangle_depth2 = self.cam.convert_point3d_2d(
self.desktop_3d_point, self.camera_conf.depth_scale)
lines = [[0, 1], [1, 2], [2, 0]] # Right leg
colors = [[0, 0, 1] for i in range(len(lines))] # Default blue
# 定义三角形的三个角点
point_pcd = open3d.geometry.PointCloud() # 定义点云
point_pcd.points = open3d.Vector3dVector(self.desktop_3d_point)
point_pcd.transform(self.camera_conf.flip_transform)
# 定义三角形三条连接线
desktop_line_pcd = open3d.LineSet()
desktop_line_pcd.lines = open3d.Vector2iVector(lines)
desktop_line_pcd.colors = open3d.Vector3dVector(colors)
desktop_line_pcd.points = open3d.Vector3dVector(self.desktop_3d_point)
desktop_line_pcd.transform(self.camera_conf.flip_transform)
if isshow:
self.vis.add_geometry(desktop_line_pcd)
self.vis.add_geometry(point_pcd)
def camera_shape_create():
'''
生成相机的外形
Returns:
'''
triangle_points = 0.05 * np.array([[1, 1, 2], [1, -1, 2], [-1, -1, 2],[-1, 1,2],[0, 0, 0]], dtype=np.float32)
lines = [[0, 1], [1, 2], [2, 3],[0,3],[0,4],[1,4],[2,4],[3,4]] # Right leg
colors = [[0, 0, 1] for i in range(len(lines))] # Default blue
# 定义三角形的三个角点
point_pcd = open3d.geometry.PointCloud() # 定义点云
point_pcd.points = open3d.Vector3dVector(triangle_points)
# 定义三角形三条连接线
line_pcd = open3d.LineSet()
line_pcd.lines = open3d.Vector2iVector(lines)
line_pcd.colors = open3d.Vector3dVector(colors)
line_pcd.points = open3d.Vector3dVector(triangle_points)
open3d.write_line_set("../data/testDate/a.ply",line_pcd)
vis = open3d.Visualizer()
vis.create_window(window_name='Open3D_1', width=600, height=600, left=10, top=10, visible=True)
vis.get_render_option().point_size = 20
vis.add_geometry(line_pcd)
while True:
vis.update_geometry()
vis.poll_events()
vis.update_renderer()
cv2.waitKey(100)
def create_boundingbox(locs, dims, rots, color):
num_box = locs.shape[0]
boxes = []
linesets = []
origin = [0.5, 0.5, 0.5]
corners = box3d_visual_utils.corners_nd(dims)
corners = box3d_visual_utils.rotation_3d_in_axis(corners, rots, axis=2)
corners += locs.reshape([-1, 1, 3])
lines = [[0, 1], [0, 3], [2, 1], [2, 3], [4, 5], [4, 7], [6, 5], [6, 7],
[0, 4], [1, 5], [2, 6], [3, 7]]
colors = [color for i in range(len(lines))]
linesets = []
for i in range(num_box):
lineset = open3d.LineSet()
lineset.points = open3d.Vector3dVector(corners[i])
lineset.lines = open3d.Vector2iVector(lines)
lineset.colors = open3d.Vector3dVector(colors)
linesets.append(lineset)
return linesets
def draw_bbox(point_cloud):
x_max, y_max, z_max = np.max(point_cloud, axis=0)
x_min, y_min, z_min = np.min(point_cloud, axis=0)
bb1 = [x_max, y_min, z_max]
bb2 = [x_max, y_max, z_max]
bb3 = [x_max, y_max, z_min]
bb4 = [x_max, y_min, z_min]
bb5 = [x_min, y_min, z_max]
bb6 = [x_min, y_max, z_max]
bb7 = [x_min, y_max, z_min]
bb8 = [x_min, y_min, z_min]
bbox_v = np.array([bb1, bb2, bb3, bb4, bb5, bb6, bb7, bb8],
dtype=np.float32)
bbox_lines = [[0, 1], [1, 2], [2, 3], [3, 0], [4, 5], [5, 6], [6, 7],
[7, 4], [0, 4], [1, 5], [2, 6], [3, 7]] # 某条线由哪两个点构成
colors = [[0, 0, 1] for _ in range(len(bbox_lines))] # Default blue
# 构建bbox
bbox = open3d.LineSet()
bbox.lines = open3d.Vector2iVector(bbox_lines)
bbox.colors = open3d.Vector3dVector(colors)
bbox.points = open3d.Vector3dVector(bbox_v)
return bbox
def bounding_box(bound, color):
bound_x = bound[0]
bound_y = bound[1]
bound_z = bound[2]
point_1 = [bound_x, bound_y, bound_z]
point_2 = [-bound_x, bound_y, bound_z]
point_3 = [-bound_x, -bound_y, bound_z]
point_4 = [bound_x, -bound_y, bound_z]
point_5 = [bound_x, -bound_y, -bound_z]
point_6 = [bound_x, bound_y, -bound_z]
point_7 = [-bound_x, bound_y, -bound_z]
point_8 =[-bound_x, -bound_y, -bound_z]
points = [point_1, point_2, point_3, point_4,
point_5, point_6, point_7, point_8]
lines = [[0, 1], [1, 2], [2, 3], [3, 0],
[0, 5], [1, 6], [2, 7], [3, 4],
[4, 5], [5, 6], [6, 7], [7, 4]]
line_color = [color for i in range(len(lines))]
line_set = o3d.LineSet()
line_set.points = o3d.Vector3dVector(points)
line_set.lines = o3d.Vector2iVector(lines)
line_set.colors = o3d.Vector3dVector(line_color)
return line_set
def main():
# 读取点云数据并显示
init_points = np.loadtxt('./data/airplane_0001.txt',
delimiter=',').astype(np.float32)[:, 0:3]
pointcloud = open3d.PointCloud()
pointcloud.points = open3d.utility.Vector3dVector(init_points)
# open3d.visualization.draw_geometries([pointcloud],window_name='init_pointcloud')
# 用PCA分析点云主方向
w, v = PCA(init_points)
# print(v[:, 0:2])
point_cloud_vector = v[:, 0] # 点云主方向对应的向量
print('the main orientation of this pointcloud is: ', point_cloud_vector)
pca_v = np.array([[0, 0, 0], v[:, 0], v[:, 1], v[:, 2]], dtype=np.float32)
pca_lines = [[0, 1], [0, 2], [0, 3]]
colors = [[1, 0, 0], [0, 1, 0], [0, 0, 1]] # RGB
# 定义PCA的三条线
pca_line = open3d.LineSet()
pca_line.lines = open3d.Vector2iVector(pca_lines)
pca_line.colors = open3d.Vector3dVector(colors)
pca_line.points = open3d.Vector3dVector(pca_v)
# 显示PCA
# open3d.visualization.draw_geometries([pointcloud, pca_line],window_name='show_pca',height=500,width=600)
# 显示投影点云
# pca_v = np.hstack((v[:, 0:2], np.zeros((3,1))))
# pca_points = np.dot(init_points, pca_v)
# pointcloud.points = open3d.utility.Vector3dVector(pca_points)
# open3d.visualization.draw_geometries([pointcloud, pca_line],window_name='show_pca_2',height=500,width=600)
# 循环计算每个点的法向量
pointcloud_tree = open3d.geometry.KDTreeFlann(pointcloud) # 创建一个KDTree对象
points = np.array(pointcloud.points)
normals = []
search_radius = 0.1 # 领域半径
search_num = 100 # 球域内最大点的数量
start = time.time()
for i in range(points.shape[0]):
_, index, _ = pointcloud_tree.search_hybrid_vector_3d(
points[i, :], search_radius, search_num)
set = points[index, :]
_, set_v = PCA(set)
normals.append(set_v[:, 2])
print('use time:', time.time() - start)
normals = np.array(normals, dtype=np.float64)
pointcloud.normals = open3d.utility.Vector3dVector(normals)
open3d.visualization.draw_geometries([pointcloud],
window_name='show_normals',
height=500,
width=600)
def visualize_wireframe(annos):
"""visualize wireframe
"""
colormap = np.array(colormap_255) / 255
junctions = np.array([item['coordinate'] for item in annos['junctions']])
_, junction_pairs = np.where(np.array(annos['lineJunctionMatrix']))
junction_pairs = junction_pairs.reshape(-1, 2)
# extract hole lines
lines_holes = []
for semantic in annos['semantics']:
if semantic['type'] in ['window', 'door']:
for planeID in semantic['planeID']:
lines_holes.extend(
np.where(np.array(
annos['planeLineMatrix'][planeID]))[0].tolist())
lines_holes = np.unique(lines_holes)
# extract cuboid lines
cuboid_lines = []
for cuboid in annos['cuboids']:
for planeID in cuboid['planeID']:
cuboid_lineID = np.where(
np.array(annos['planeLineMatrix'][planeID]))[0].tolist()
cuboid_lines.extend(cuboid_lineID)
cuboid_lines = np.unique(cuboid_lines)
cuboid_lines = np.setdiff1d(cuboid_lines, lines_holes)
# visualize junctions
connected_junctions = junctions[np.unique(junction_pairs)]
connected_colors = np.repeat(colormap[0].reshape(1, 3),
len(connected_junctions),
axis=0)
junction_set = open3d.PointCloud()
junction_set.points = open3d.Vector3dVector(connected_junctions)
junction_set.colors = open3d.Vector3dVector(connected_colors)
# visualize line segments
line_colors = np.repeat(colormap[5].reshape(1, 3),
len(junction_pairs),
axis=0)
# color holes
if len(lines_holes) != 0:
line_colors[lines_holes] = colormap[6]
# color cuboids
if len(cuboid_lines) != 0:
line_colors[cuboid_lines] = colormap[2]
line_set = open3d.LineSet()
line_set.points = open3d.Vector3dVector(junctions)
line_set.lines = open3d.Vector2iVector(junction_pairs)
line_set.colors = open3d.Vector3dVector(line_colors)
open3d.draw_geometries([junction_set, line_set])
def plot_flow(x, t, phi, grid_size, t_sample):
"""
Plot the ground truth points, and the reconstructed patch by meshing the domain [0, 1]^2 and lifting the mesh
to R^3
:param x: The ground truth points we are trying to fit
:param t: The sample positions used to fit the mesh
:param phi: The fitted neural network
:param grid_size: The number of sample positions per axis in the meshgrid
:return: None
"""
# I'm doing the input here so you don't crash if you never use this function and don't have OpenGL
import open3d
with torch.no_grad():
mesh_samples = embed_3d(
torch.from_numpy(meshgrid_vertices(grid_size)).to(x), t[0, 2])
mesh_faces = meshgrid_face_indices(grid_size)
mesh_vertices = phi(mesh_samples)[:, 0:3]
recon_vertices = phi(t)[:, 0:3]
gt_color = np.array([0.1, 0.7, 0.1])
recon_color = np.array([0.7, 0.1, 0.1])
mesh_color = np.array([0.1, 0.1, 0.7])
curve_color = np.array([0.2, 0.2, 0.5])
pcloud_gt = open3d.PointCloud()
pcloud_gt.points = open3d.Vector3dVector(phi.invert(x).cpu().numpy())
pcloud_gt.paint_uniform_color(gt_color)
pcloud_recon = open3d.PointCloud()
pcloud_recon.points = open3d.Vector3dVector(
recon_vertices.cpu().numpy())
pcloud_recon.paint_uniform_color(recon_color)
mesh_recon = open3d.TriangleMesh()
mesh_recon.vertices = open3d.Vector3dVector(
mesh_vertices.cpu().numpy())
mesh_recon.triangles = open3d.Vector3iVector(mesh_faces)
mesh_recon.compute_vertex_normals()
mesh_recon.paint_uniform_color(mesh_color)
pc_initial = open3d.PointCloud()
pc_initial.points = open3d.Vector3dVector(t.cpu().numpy())
pc_initial.paint_uniform_color(curve_color)
flow_ode = open3d.LineSet()
# print(x.shape)
# print(t.shape)
flow = get_Lines(phi, t[::t_sample, :], 15)
flow_ode.points, flow_ode.lines = open3d.Vector3dVector(flow[0]), \
open3d.Vector2iVector(flow[1])
# flow_ode.colors = open3d.Vector3dVector(curve_color)
open3d.draw_geometries(
[pcloud_gt, pcloud_recon, mesh_recon, pc_initial, flow_ode])
def create_wire_box(edgelengths, color=[0.0, 0.0, 0.0]):
lineset = o3.LineSet()
x, y, z = edgelengths
lineset.points = o3.Vector3dVector([[0, 0, 0], [x, 0, 0], [0, y, 0],
[x, y, 0], [0, 0, z], [x, 0, z], [0, y, z], [x, y, z]])
lineset.lines = o3.Vector2iVector([[0, 1], [1, 3], [3, 2], [2, 0],
[0, 4], [1, 5], [3, 7], [2, 6],
[4, 5], [5, 7], [7, 6], [6, 4]])
lineset.colors = o3.Vector3dVector(np.tile(color, [len(lineset.lines), 1]))
return lineset
def main():
# 指定点云路径
cat_index = 0 # 物体编号，范围是0-39，即对应数据集中40个物体
root_dir = '/home/snow/ShenLan/Dtasets/ModelNet40-ply' # 数据集路径
cat = os.listdir(root_dir)
file_name = os.path.join(root_dir, cat[cat_index], 'train',
cat[cat_index] + '_0002.ply') # 默认使用第一个点云
# 加载原始点云
point_cloud_pynt = PyntCloud.from_file(file_name)
point_cloud_o3d = point_cloud_pynt.to_instance("open3d", mesh=False)
# o3d.visualization.draw_geometries([point_cloud_o3d]) # 显示原始点云
# 从点云中获取点，只对点进行处理
points = point_cloud_pynt.points
print('total points number is:', points.shape)
# 用PCA分析点云主方向
w, v = PCA(points, sort=False)
point_cloud_vector = v[:, 2] #点云主方向对应的向量
print('the main orientation of this pointcloud is: ', point_cloud_vector)
# TODO: 此处只显示了点云，还没有显示PCA
# 计算质心并绘制主方向
mesh_frame = o3d.geometry.create_mesh_coordinate_frame(size=2,
origin=[0, 0, 0])
center = np.mean(np.array(points), axis=0)
points_xl = np.vstack((center, center + point_cloud_vector))
colors = [[0, 0, 1] for i in range(len(points_xl))]
lines = [[0, 1]]
line_pcd = o3d.LineSet()
line_pcd.lines = o3d.utility.Vector2iVector(lines)
line_pcd.colors = o3d.utility.Vector3dVector(colors)
line_pcd.points = o3d.utility.Vector3dVector(points_xl)
o3d.visualization.draw_geometries([point_cloud_o3d, line_pcd, mesh_frame])
frame = o3d.geometry.create_mesh_coordinate_frame()
# # 循环计算每个点的法向量
pcd_tree = o3d.geometry.KDTreeFlann(point_cloud_o3d)
normals = []
# 作业2
# 屏蔽开始
points_np = np.array(points, dtype=np.float64)
for i, xi in enumerate(points_np):
nn, ids, _ = pcd_tree.search_knn_vector_3d(xi.reshape((3, 1)),
knn=20) # 1.negihbors
xi_neighbors = points_np[ids]
xw, xv = PCA(xi_neighbors, sort=False) # 2. PCA
normals.append(xv[:, 2]) # 3. normal
# 由于最近邻搜索是第二章的内容，所以此处允许直接调用open3d中的函数
# 屏蔽结束
normals = np.array(normals, dtype=np.float64)
# # TODO: 此处把法向量存放在了normals中
point_cloud_o3d.normals = o3d.utility.Vector3dVector(normals)
o3d.visualization.draw_geometries([point_cloud_o3d])
def create_line_set_bones(joints, joint_line):
# Draw the 24 bones (lines) connecting 25 joints
# The lines below is the kinematic tree that defines the connection between parent and child joints
colors = [[0, 0, 1] for i in range(24)] # Default blue
line_set = open3d.LineSet()
line_set.lines = open3d.Vector2iVector(joint_line)
line_set.colors = open3d.Vector3dVector(colors)
line_set.points = open3d.Vector3dVector(joints)
return line_set
def create_line_set_bones(joints):
# Draw the 24 bones (lines) connecting 25 joints
# The lines below is the kinematic tree that defines the connection between parent and child joints
lines = [[0, 1], [1, 20], [20, 2], [2, 3], # Spine
[20, 4], [4, 5], [5, 6], [6, 7], [7, 21], [7, 22], # Left arm and hand
[20, 8], [8, 9], [9, 10], [10, 11], [11, 23], [11, 24], # Right arm and hand
[0, 12], [12, 13], [13, 14], [14, 15], # Left leg
[0, 16], [16, 17], [17, 18], [18, 19]] # Right leg
colors = [[0, 0, 1] for i in range(24)] # Default blue
line_set = open3d.LineSet()
line_set.lines = open3d.Vector2iVector(lines)
line_set.colors = open3d.Vector3dVector(colors)
line_set.points = open3d.Vector3dVector(joints)
return line_set
def load_snapshot(sessionname):
cloud = o3.PointCloud()
trajectory = o3.LineSet()
with np.load(os.path.join(resultdir, sessionname, snapshotfile)) as data:
cloud.points = o3.Vector3dVector(data['points'])
cloud.colors = o3.Vector3dVector(
util.intensity2color(data['intensities'] / 255.0))
trajectory.points = o3.Vector3dVector(data['trajectory'])
lines = np.reshape(range(data['trajectory'].shape[0] - 1), [-1, 1]) \
+ [0, 1]
trajectory.lines = o3.Vector2iVector(lines)
trajectory.colors = o3.Vector3dVector(
np.tile([0.0, 0.5, 0.0], [lines.shape[0], 1]))
return cloud, trajectory
def o3d_trace_rays(cam_pose,
max_range=5.5,
fov_h_angle=0.34 * np.pi,
fov_v_angle=0.25 * np.pi,
angle_gap=0.01 * np.pi,
color=[0.5, 1, 0.5]):
ray_set = open3d.LineSet()
cam_forward = np.array([0, 0, 1])
cam_left = np.array([-1, 0, 0])
cam_up = np.array([0, -1, 0])
points = []
lines = []
points.append(cam_pose[:3, 3].tolist())
# prepare the list of view angles
fov_h_angle_list = np.arange(start=fov_h_angle / 2,
stop=-fov_h_angle / 2,
step=-angle_gap)
fov_h_angle_list = np.append(fov_h_angle_list, -fov_h_angle /
2) # ensure the last value is included
fov_v_angle_list = np.arange(start=fov_v_angle / 2,
stop=-fov_v_angle / 2,
step=-angle_gap)
fov_v_angle_list = np.append(fov_v_angle_list, -fov_v_angle /
2) # ensure the last value is included
for angle_v in fov_v_angle_list:
up_offset = cam_up * np.tan(angle_v)
for angle_h in fov_h_angle_list:
left_offset = cam_left * np.tan(angle_h)
direction_unit = cam_forward + up_offset + left_offset
direction_unit = direction_unit / np.linalg.norm(direction_unit)
direction_unit = np.dot(cam_pose[:3, :3], direction_unit)
point = cam_pose[:3, 3] + direction_unit * max_range
points.append(point.tolist())
for i in range(len(points) - 1):
line = [0, i + 1]
lines.append(line)
colors = [color for i in range(len(lines))]
# get vector camera_look_at
# set ray_set
ray_set.points = open3d.Vector3dVector(points)
ray_set.lines = open3d.Vector2iVector(lines)
ray_set.colors = open3d.Vector3dVector(colors)
return ray_set
def trimesh_to_open3d(src):
if isinstance(src, trimesh.Trimesh):
dst = open3d.TriangleMesh()
dst.vertices = open3d.Vector3dVector(src.vertices)
dst.triangles = open3d.Vector3iVector(src.faces)
vertex_colors = np.zeros((len(src.vertices), 3), dtype=float)
for face, face_color in zip(src.faces, src.visual.face_colors):
vertex_colors[face] += face_color[:3] / 255.0 # uint8 -> float
indices, counts = np.unique(src.faces.flatten(), return_counts=True)
vertex_colors[indices] /= counts[:, None]
dst.vertex_colors = open3d.Vector3dVector(vertex_colors)
dst.compute_vertex_normals()
elif isinstance(src, trimesh.PointCloud):
dst = open3d.PointCloud()
dst.points = open3d.Vector3dVector(src.vertices)
if src.colors:
colors = src.colors
colors = (colors[:, :3] / 255.0).astype(float)
dst.colors = open3d.Vector3dVector(colors)
elif isinstance(src, trimesh.scene.Camera):
dst = open3d.PinholeCameraIntrinsic(
width=src.resolution[0],
height=src.resolution[1],
fx=src.K[0, 0],
fy=src.K[1, 1],
cx=src.K[0, 2],
cy=src.K[1, 2],
)
elif isinstance(src, trimesh.path.Path3D):
lines = []
for entity in src.entities:
for i, j in zip(entity.points[:-1], entity.points[1:]):
lines.append((i, j))
lines = np.vstack(lines)
points = src.vertices
dst = open3d.LineSet()
dst.lines = open3d.Vector2iVector(lines)
dst.points = open3d.Vector3dVector(points)
elif isinstance(src, list):
dst = [trimesh_to_open3d(x) for x in src]
else:
raise ValueError("Unsupported type of src: {}".format(type(src)))
return dst
def triangle_pcd():
'''
定义三角形的点云
:return:
'''
triangle_points = np.array([[1, 0, 0], [0, 1, 0], [0, 0, 1]],
dtype=np.float32)
lines = [[0, 1], [1, 2], [2, 0]] # Right leg
colors = [[0, 0, 1] for i in range(len(lines))] # Default blue
# 定义三角形的三个角点
point_pcd = open3d.geometry.PointCloud() # 定义点云
point_pcd.points = open3d.Vector3dVector(triangle_points)
# 定义三角形三条连接线
line_pcd = open3d.LineSet()
line_pcd.lines = open3d.Vector2iVector(lines)
line_pcd.colors = open3d.Vector3dVector(colors)
line_pcd.points = open3d.Vector3dVector(triangle_points)
return line_pcd, point_pcd
def display_surface_normals(filename, size):
mesh = open3d.read_triangle_mesh(filename)
mesh.paint_uniform_color([0, 0, 0.353])
vertices = np.asarray(mesh.vertices)
triangles = vertices[np.array(mesh.triangles)]
# calculate normals
normal_lines = []
for tri in triangles:
u = tri[1] - tri[0]
v = tri[2] - tri[0]
normal = normalize(np.cross(u, v), size)
center = (tri[0] + tri[1] + tri[2]) / 3
normal_lines.append(center)
normal_lines.append(center + normal)
lines = [[i, i + 1] for i in range(0, len(normal_lines), 2)]
colors = [[1, 0, 0] for i in range(len(lines))]
line_set = open3d.LineSet()
line_set.points = open3d.Vector3dVector(normal_lines)
line_set.lines = open3d.Vector2iVector(lines)
line_set.colors = open3d.Vector3dVector(colors)
open3d.draw_geometries([mesh, line_set])
def drawGraph(p, G):
graph = open3d.LineSet()
graph.points = p_light.points
graph.lines = open3d.Vector2iVector(G.edges)
open3d.draw_geometries([graph])
#
#
# pcd = opend.read_point_cloud(path)
points = np.random.rand(20048, 3)
colors = np.random.rand(20048, 3)
# pcd2 = opend.PointCloud()
# pcd2.points = opend.Vector3dVector(points)
# pcd2.colors = opend.Vector3dVector(colors)
# opend.draw_geometries([pcd2])
# points = np.asarray(pcd.points)
start_time = time.time()
annotation = np.ones((points.shape[0], 1))
grid = Grid(points, colors, 0.05, 0.025,
annotation) #points,colors,cell_size,radius,annotations
print(time.time() - start_time)
print(grid.assigned)
pcd2 = opend.PointCloud()
pcd2.points = opend.Vector3dVector(
np.concatenate(grid.assigned) + np.array([1.0, 0.0, 0.0]))
pcd2.colors = opend.Vector3dVector(np.concatenate(grid.colors))
pcd3 = opend.PointCloud()
pcd3.points = opend.Vector3dVector(np.array(grid.corners))
line_set = opend.LineSet()
line_set.points = opend.Vector3dVector(np.array(grid.corners))
line_set.lines = opend.Vector2iVector(np.array(grid.grid_lines))
opend.draw_geometries([pcd, pcd2, pcd3, line_set])
def main():
parser = argparse.ArgumentParser(
description='DBSCAN segmentation algorithm')
parser.add_argument("--file",
dest="filename",
required=True,
help='Provide a .pcd file')
parser.add_argument("--eps",
dest="eps",
required=True,
type=float,
help='radius threshold',
default=1)
parser.add_argument("--minPts",
dest="minPoints",
required=True,
type=int,
help='clustering threshold points',
default=50)
parser.add_argument("--kitti", dest="useKitti", required=True)
parser.add_argument("--sklearn", dest="useSklearn", default="True")
args = parser.parse_args()
# Loading Data using open3D
pcd = op.read_point_cloud(args.filename) # Load data here
if (args.useSklearn == str(True)):
try:
import sklearn.neighbors
import dbscan_2 as dbscan
downsampleRatio = np.array(pcd.points).shape[0] / 1e4
except Exception as e:
print(e)
print(
"Please 'pip3 install scikit-learn, else set --sklearn False")
exit()
else:
import dbscan
downsampleRatio = np.array(pcd.points).shape[0] / 1e3
# Sample down the data if large input is given
pcd = op.uniform_down_sample(pcd, every_k_points=int(downsampleRatio))
# Converting pcd into python array
pcdArray = np.asarray(pcd.points)
ground_list = []
above_ground = []
if (args.useKitti == "True"):
# Ground plane segregation
for point in pcdArray:
if point[2] > -1.5: # considering points above -1.5 m of LiDAR as above the ground
above_ground.append(point)
else:
ground_list.append(point)
ground = np.asarray(ground_list)
above_ground = np.asarray(above_ground)
# Clustering algorithm using DBSCAN. Getting all the labels for segmentation
labels = dbscan.DBSCAN(above_ground, args.eps, args.minPoints)
labels = np.array(labels)
# Array to store the segments
pcdArrayList = []
# Color list (Randomly called to color clusters)
colorList = [[0, 1, 0], [0, 0, 1], [1, 1, 0], [1, 0, 1], [0, 1, 1],
[1, 0, 0]]
# Creating cluster cloud segments
for label in range(len(set(labels))):
newPcdArray = above_ground[np.where(labels == label)[0]]
# Converting the 3D points into open3D based structure
pcdPoints = op.Vector3dVector(newPcdArray)
newPcd = op.PointCloud()
newPcd.points = pcdPoints
color = colorList[np.random.randint(6)]
colorArr = np.full((newPcdArray.shape[0], 3), color)
newPcd.colors = op.Vector3dVector(colorArr)
pcdArrayList.append(newPcd)
# The labels have noises (-1), which is not a cluster
if newPcdArray.size:
# Get the extremities of the point cloud segment data
x_max = max(newPcdArray[:, 0])
x_min = min(newPcdArray[:, 0])
y_max = max(newPcdArray[:, 1])
y_min = min(newPcdArray[:, 1])
z_max = max(newPcdArray[:, 2])
z_min = min(newPcdArray[:, 2])
# Create a bounding box around the point cloud
cube = [[x_min, y_min, z_min], [x_max, y_min, z_min],
[x_min, y_max, z_min], [x_max, y_max, z_min],
[x_min, y_min, z_max], [x_max, y_min, z_max],
[x_min, y_max, z_max], [x_max, y_max, z_max]]
# The below creates relations to join cube points to form bounding boz
lines = [[0, 1], [0, 2], [1, 3], [2, 3], [4, 5], [4, 6], [5, 7],
[6, 7], [0, 4], [1, 5], [2, 6], [3, 7]]
# Creeate line object
line_set = op.LineSet()
# Convert list to open3D objects
line_set.points = op.Vector3dVector(cube)
line_set.lines = op.Vector2iVector(lines)
colors = [[0, 0, 0] for i in range(len(lines))]
line_set.colors = op.Vector3dVector(colors)
# Adding lines to the point cloud for visualization
pcdArrayList.extend([line_set])
if (args.useKitti == "True"):
# Now adding the ground plane back
newPcd = op.PointCloud()
newPcd.points = pcdPoints
newPcd.points = op.Vector3dVector(ground)
newPcd.paint_uniform_color([0.5, 0.5, 0.5])
# Add ground plan to the existing list
pcdArrayList.extend([newPcd])
# Display the results
op.draw_geometries(pcdArrayList)
def main():
points = np.genfromtxt('./data/airplane_0001.txt',
delimiter=',') # (10000,6)
points = pd.DataFrame(points[:, 0:3])
points.columns = ['x', 'y', 'z']
point_cloud_pynt = PyntCloud(points)
point_cloud_open3d = point_cloud_pynt.to_instance("open3d", mesh=False)
# open3d.visualization.draw_geometries([point_cloud_open3d]) # 显示原始点云
# 调用voxel滤波函数，实现滤波
voxel_resolution = 0.05
filtered_cloud = voxel_filter(np.array(point_cloud_pynt.points),
voxel_resolution)
print(
"Original points num：{num1}, After filtered points num: {num2}".format(
num1=np.array(point_cloud_pynt.points).shape[0],
num2=len(filtered_cloud)))
# point_cloud_open3d.points = open3d.utility.Vector3dVector(filtered_cloud)
#
# # 显示bbox和滤波后的点云
# bbox = draw_bbox(np.array(point_cloud_pynt.points))
# open3d.visualization.draw_geometries([point_cloud_open3d, bbox])
points = np.array(point_cloud_pynt.points) # shape:(10000, 3)
# 用PCA分析点云主方向
w, v = PCA(points)
pca_v = np.array([[0, 0, 0], v[:, 0], v[:, 1], v[:, 2]], dtype=np.float32)
pca_lines = [[0, 1], [0, 2], [0, 3]] # 某条线由哪两个点构成
colors = [[1, 0, 0], [0, 1, 0], [0, 0, 1]] # Default blue
# 定义PCA的三条线
pca_line = open3d.LineSet()
pca_line.lines = open3d.Vector2iVector(pca_lines)
pca_line.colors = open3d.Vector3dVector(colors)
pca_line.points = open3d.Vector3dVector(pca_v)
# 循环计算每个点的法向量
pcd_tree = open3d.geometry.KDTreeFlann(point_cloud_open3d) # 创建一个对象
normals = []
search_num = 20 # 搜索近邻点的数量
start = time.time()
for i in range(points.shape[0]):
_, index, _ = pcd_tree.search_knn_vector_3d(points[i, :], search_num)
set = points[index, :] # shape (20,3)
_, set_v = PCA(set)
normals.append(set_v[:, 2])
print('use time:', time.time() - start)
normals = np.array(normals, dtype=np.float64)
print(normals.shape)
point_cloud_open3d.normals = open3d.utility.Vector3dVector(normals)
bbox = draw_bbox(np.array(point_cloud_pynt.points))
open3d.visualization.draw_geometries([point_cloud_open3d, pca_line, bbox])
def animate_flow(x, t, phi, grid_size, t_sample, mesh0=None):
import open3d
with torch.no_grad():
if mesh0 == None:
mesh_samples = embed_3d(
torch.from_numpy(meshgrid_vertices(grid_size)).to(x), t[0, 2])
print(mesh_samples.shape)
mesh_faces = meshgrid_face_indices(grid_size)
mesh_vertices = phi(mesh_samples)[:, 0:3]
else:
mesh_samples = torch.from_numpy(mesh0[0]).to(x)
print("sample", mesh_samples.shape)
mesh_faces = mesh0[1][:, 0:3]
mesh_vertices = phi(mesh_samples)[:, 0:3]
recon_vertices = phi(t)[:, 0:3]
gt_color = np.array([0.1, 0.7, 0.1])
recon_color = np.array([0.7, 0.1, 0.1])
mesh_color = np.array([0.1, 0.1, 0.7])
curve_color = np.array([0.2, 0.2, 0.5])
pcloud_gt = open3d.PointCloud()
pcloud_gt.points = open3d.Vector3dVector(x.cpu().numpy())
pcloud_gt.paint_uniform_color(gt_color)
pcloud_recon = open3d.PointCloud()
pcloud_recon.points = open3d.Vector3dVector(
recon_vertices.cpu().numpy())
pcloud_recon.paint_uniform_color(recon_color)
mesh_recon = open3d.TriangleMesh()
mesh_recon.vertices = open3d.Vector3dVector(
mesh_vertices.cpu().numpy())
mesh_recon.triangles = open3d.Vector3iVector(mesh_faces)
mesh_recon.compute_vertex_normals()
mesh_recon.paint_uniform_color(mesh_color)
pc_initial = open3d.PointCloud()
pc_initial.points = open3d.Vector3dVector(t.cpu().numpy())
pc_initial.paint_uniform_color(curve_color)
flow_ode = open3d.LineSet()
# print(x.shape)
# print(t.shape)
flow = get_Lines(phi, t[::t_sample, :], 30)
print(len(flow[0]))
flow_ode.points, flow_ode.lines = open3d.Vector3dVector(flow[0]), \
open3d.Vector2iVector(flow[1])
# flow_ode.colors = open3d.Vector3dVector(curve_color)
# vis = open3d.Visualizer()
# vis.create_window()
# vis.remove_geometry(flow_ode)
# for geom in [pcloud_gt, pcloud_recon, mesh_recon, pc_initial, flow_ode]:
# vis.add_geometry(geometry=geom)
# # for i in range(10):
# # vis.remove_geometry(flow_ode)
#
# vis.remove_geometry(flow_ode)
# vis.update_geometry()
# vis.update_renderer()
# vis.poll_events()
store_frames = []
temp = mesh_samples
store_frames.append(temp)
for j in range(0, Nframes - 1):
print(j)
tj = ts[j]
tnext = ts[j + 1]
temp = phi.flow(tj, tnext, temp)[:, 0:3]
store_frames.append(temp)
def next_frame(vis):
# ctr = vis.get_view_control()
# ctr.rotate(10.0, 0.0)
# global i, ts
global i, ts, mesh0, Nframes
if i >= Nframes:
return True
print(i)
# if mesh0 == None:
# mesh_faces = meshgrid_face_indices(grid_size)
# else:
# mesh_faces =mesh0[1]
mesh_vertices = store_frames[i]
i += 1
mesh_recon.vertices = open3d.Vector3dVector(
mesh_vertices.cpu().numpy())
mesh_recon.triangles = open3d.Vector3iVector(mesh_faces)
mesh_recon.compute_vertex_normals()
mesh_recon.paint_uniform_color(mesh_color)
vis.update_geometry()
vis.update_renderer()
return False
def revert(vis):
global i
i = 0
return False
key_to_callback = {}
key_to_callback[ord(",")] = next_frame
key_to_callback[ord(".")] = revert
open3d.draw_geometries_with_key_callbacks(
[pcloud_gt, pcloud_recon, mesh_recon, pc_initial, flow_ode],
key_to_callback)
'predictions': t_predictions,
'probs': t_probs,
'pred_box': t_pred_box
}
# setup Visualizer ============================================================
if VISUALIZATION_LEVEL == 1:
print("Configure the viewpoint as you want and press [q]")
calib = dataset.get_calib(0)
cam_points_in_img_with_rgb = dataset.get_cam_points_in_image_with_rgb(
0, calib=calib)
vis = open3d.Visualizer()
vis.create_window()
pcd = open3d.PointCloud()
pcd.points = open3d.Vector3dVector(cam_points_in_img_with_rgb.xyz)
pcd.colors = open3d.Vector3dVector(cam_points_in_img_with_rgb.attr[:, 1:4])
line_set = open3d.LineSet()
graph_line_set = open3d.LineSet()
box_corners = np.array([[0, 0, 0]])
box_edges = np.array([[0, 0]])
line_set.points = open3d.Vector3dVector(box_corners)
line_set.lines = open3d.Vector2iVector(box_edges)
graph_line_set.points = open3d.Vector3dVector(box_corners)
graph_line_set.lines = open3d.Vector2iVector(box_edges)
vis.add_geometry(pcd)
vis.add_geometry(line_set)
vis.add_geometry(graph_line_set)
ctr = vis.get_view_control()
ctr.rotate(0.0, 3141.0, 0)
vis.run()
color_map = np.array([(211, 211, 211), (255, 0, 0), (255, 20, 147),
(65, 244, 101), (169, 244, 65), (65, 79, 244),
loc = localizer(target_mesh)
for cam_pcl in cam_pcl_list:
start_time = time.time()
loc.update_source(cam_pcl)
print start_time - time.time()
#%%
loc.update_source_to_target_transformation()
source = copy.deepcopy(loc.source_pcl)
source.transform(loc.source_to_target_transformation)
origines = [x[0] for x in loc.camera_coordinates_history()]
camera_trace = pn.LineSet()
camera_trace.points = pn.Vector3dVector(origines)
camera_trace.lines = pn.Vector2iVector([[i, i + 1]
for i in range(len(origines) - 1)])
pn.draw_geometries([target_mesh, source, camera_trace])
#%% SAVING COLOR IMAGE
for i, color_image in enumerate(cam_color_images):
plt.figure(1)
fig = plt.imshow(color_image)
fig.axes.get_xaxis().set_visible(False)
fig.axes.get_yaxis().set_visible(False)
plt.savefig("simulation/cam_color_images/%s" % i)
