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 test_benchmark():
vector_size = int(2e6)
x = np.random.randint(10, size=(vector_size, 3)).astype(np.float64)
print("\nopen3d.Vector3dVector:", x.shape)
start_time = time.time()
y = open3d.Vector3dVector(x)
print("open3d -> numpy: %.6fs" % (time.time() - start_time))
start_time = time.time()
z = np.asarray(y)
print("numpy -> open3d: %.6fs" % (time.time() - start_time))
np.testing.assert_allclose(x, z)
print("\nopen3d.Vector3iVector:", x.shape)
x = np.random.randint(10, size=(vector_size, 3)).astype(np.int32)
start_time = time.time()
y = open3d.Vector3iVector(x)
print("open3d -> numpy: %.6fs" % (time.time() - start_time))
start_time = time.time()
z = np.asarray(y)
print("numpy -> open3d: %.6fs" % (time.time() - start_time))
np.testing.assert_allclose(x, z)
print("\nopen3d.Vector2iVector:", x.shape)
x = np.random.randint(10, size=(vector_size, 2)).astype(np.int32)
start_time = time.time()
y = open3d.Vector2iVector(x)
print("open3d -> numpy: %.6fs" % (time.time() - start_time))
start_time = time.time()
z = np.asarray(y)
print("numpy -> open3d: %.6fs" % (time.time() - start_time))
np.testing.assert_allclose(x, z)
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 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 markersCallback(self, markers_msg):
stamp = markers_msg.header.stamp
confidence = []
marker_pts = []
bundle_pts = []
for marker in markers_msg.markers:
if(marker.id in self.marker_centers.keys()):
q = marker.pose.pose.orientation
pt = marker.pose.pose.position
trans_mat = tf.transformations.quaternion_matrix([q.x,q.y,q.z,q.w])
trans_mat[:3,3] = [pt.x, pt.y, pt.z]
corners = self.tag_corners.dot(trans_mat.T)
#marker_poses[marker.id] = trans_mat
#confidence.append(marker.confidence)
marker_pts.append([pt.x, pt.y, pt.z])
bundle_pts.append(list(self.marker_centers[marker.id]))
marker_pts.extend(corners[:,:3])
bundle_pts.extend(list(self.marker_corners[marker.id]))
pts_src = open3d.PointCloud()
pts_src.points = open3d.Vector3dVector(marker_pts)
pts_tgt = open3d.PointCloud()
pts_tgt.points = open3d.Vector3dVector(bundle_pts)
corres = open3d.Vector2iVector(np.tile(np.arange(len(marker_pts)),[2,1]).T)
ransac_res = open3d.registration_ransac_based_on_correspondence(pts_src, pts_tgt, corres, self.max_corres_dist)
trans_ransac = ransac_res.transformation
self.tf_broadcaster.sendTransform(trans_ransac[:3,3],
tf.transformations.quaternion_from_matrix(trans_ransac),
marker.header.stamp,
marker.header.frame_id,
self.frame_id,
)
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 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 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 update_camera_coordinates(self, camera_coordinates):
self.camera_representation.points = pn.Vector3dVector(
camera_coordinates)
self.camera_representation.lines = pn.Vector2iVector([[0, 1], [0, 2],
[0, 3]])
self.vis.add_geometry(self.camera_representation)
self.vis.update_geometry()
self.vis.poll_events()
self.vis.update_renderer()
return
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 refine_registration(source, target, corrs):
start = time.time()
p2p = o3d.TransformationEstimationPointToPoint()
result = p2p.compute_transformation(source, target,
o3d.Vector2iVector(corrs))
elapsed_time = time.time() - start
return result, elapsed_time
def update_trace(self, camera_coordinates_history):
origines = [x[0] for x in camera_coordinates_history]
self.camera_trace.points = pn.Vector3dVector(origines)
self.camera_trace.lines = \
pn.Vector2iVector([[i,i+1] for i in range(len(origines)-1)])
self.vis.add_geometry(self.camera_trace)
self.vis.update_geometry()
self.vis.poll_events()
self.vis.update_renderer()
return
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 transformation_matrix(source_points, target_points):
"""
Compute the transformation matrix between to set of matching points.
Parameters:
source_points (numpy.arrays): the source points array
target_points (numpy.arrays): the target points array matching the
source
Returns:
transformation_matrix (numpy.arrays) : The 4D transformation matrix from source to target
"""
assert (len(source_points) >= 3
and len(target_points) == len(source_points))
source = pn.PointCloud()
source.points = pn.Vector3dVector(source_points)
target = pn.PointCloud()
target.points = pn.Vector3dVector(target_points)
corr = np.array(2 * [range(len(source_points))]).T
p2p = pn.TransformationEstimationPointToPoint()
trans = p2p.compute_transformation(source, target, pn.Vector2iVector(corr))
return trans
def draw_lidar_label(points, labels):
boxes = [utils.compute_box3d(x) for x in labels]
lines = [[0, 1], [1, 2], [2, 3], [3, 0], [0, 4], [1, 5], [2, 6], [3, 7],
[4, 5], [5, 6], [6, 7], [7, 4]]
colors = [[1, 0, 0] for i in range(len(lines))]
pcd = o3d.PointCloud()
pcd.points = o3d.Vector3dVector(points)
vis = o3d.visualization.Visualizer()
vis.create_window()
vis.add_geometry(pcd)
for i, box in enumerate(boxes):
if labels[i].cls == 'DontCare':
continue
lineset = o3d.geometry.LineSet()
lineset.points = o3d.Vector3dVector(box)
lineset.lines = o3d.Vector2iVector(lines)
lineset.colors = o3d.Vector3dVector(colors)
vis.add_geometry(lineset)
vis.run()
vis.destroy_window()
# u maps to the x-axis
# v maps to the y-axis
# n maps to the z-axis
M_eye = np.eye(4)
eye_t = np.array([0., 0., 0])
M_eye[:3, 3] = eye_t
coord = open3d.create_mesh_coordinate_frame(0.4)
coord1 = open3d.create_mesh_coordinate_frame(0.3)
coord2 = open3d.create_mesh_coordinate_frame(0.3)
mesh_sphere = open3d.geometry.create_mesh_sphere(radius=0.1)
# open3d.draw_geometries([coord, coord1, coord2, mesh_sphere])#, coord2, coord3])
line_set = open3d.geometry.LineSet()
line_set.lines = open3d.Vector2iVector([[0, 1]])
line_set.points = open3d.Vector3dVector([eye_t, eye_t])
vis = open3d.visualization.Visualizer()
vis.create_window()
vis.add_geometry(line_set)
vis.add_geometry(coord)
vis.add_geometry(coord1)
# vis.add_geometry(coord2)
vis.add_geometry(mesh_sphere)
I4 = np.eye(4)
R_y_90 = euler2mat(*np.deg2rad([0, 90, 0]))
start_t = np.array([3.0, 2.0, 0.])
start_M = np.eye(4)
def drawGraph(p, G):
graph = open3d.LineSet()
graph.points = p_light.points
graph.lines = open3d.Vector2iVector(G.edges)
open3d.draw_geometries([graph])
def run_test(input_array):
open3d_array = open3d.Vector2iVector(input_array)
output_array = np.asarray(open3d_array)
np.testing.assert_allclose(input_array, output_array)
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)
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),
(65, 181, 244), (229, 244, 66)],
dtype=np.float32)
color_map = color_map / 255.0
gt_color_map = {
'Pedestrian': (0, 255, 255),
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)
