def open3dVis(self, depth, normal=None, color=None):
"""Visualize 3d map points from eigen depth
Args:
depth: depth map
normal: normal map
color: rgb image
"""
points = get3dpoints(depth, color)
points = np.asarray(points)
pcd = PointCloud()
pcd.points = Vector3dVector(points)
if color is not None:
color = np.reshape(color, [-1, 3])
pcd.colors = Vector3dVector(color / 255.)
if normal is not None:
normal = np.reshape(normal, [-1, 3])
else:
_, normal, _ = self.compute_local_planes(points[:, 0],
points[:, 1], points[:,
2])
normal = np.reshape(normal, [-1, 3])
pcd.normals = Vector3dVector(normal)
draw_geometries([pcd])
def estimateNormals(self, points):
pcd = PointCloud()
pcd.points = Vector3dVector(points)
estimate_normals(pcd,
search_param=KDTreeSearchParamHybrid(radius=0.1,
max_nn=20))
return np.asarray(pcd.normals)
def pc2pcd(pc):
"""
Args:
pc: shape(3,N)
"""
assert pc.shape[0] == 3
pcd = PointCloud()
pcd.points = Vector3dVector(pc.T)
return pcd
def show_pointcloud(points):
"""
using open3d to show point cloud
Input:
points: a list of points in points cloud with shape (m, 3)
"""
assert np.asanyarray(
points).shape[1] == 3 # make sure the shape is correct
pc_cropped = PointCloud()
pc_cropped.points = Vector3dVector(np.asanyarray(points))
draw_geometries([pc_cropped])
def pc2color_pcd(pc, color):
from open3d import PointCloud, Vector3dVector
color = np.array(color).reshape(-1, 3)
assert pc.shape[0] == 3
if color.shape[0] == 1:
color = np.repeat(color, pc.shape[1], axis=0)
color = color.copy().astype(np.float32)
color /= 255.0
pcd = PointCloud()
pcd.points = Vector3dVector(pc.T)
pcd.colors = Vector3dVector(color[:, ::-1])
return pcd
def main(radius, name):
cloud = epc.compress((epc[:, -2] > radius) & (radius >= epc[:, -1]), 0)
pcd = PointCloud()
pcd.points = Vector3dVector(cloud[:, :3])
pcd.colors = Vector3dVector(cloud[:, 3:6])
write_point_cloud(name, pcd)
print(
pcd,
cloud[:, -1].max(),
cloud[:, -3].sum(),
cloud[:, -4].max(),
cloud[:, -5].sum(),
)
return None
def merge_visualise(self, pc1, pc2):
"""Concatenate all 3d points in pc1 and pc2
Args:
pc1: point cloud 1
pc2: point cloud 2
"""
pcd = PointCloud()
points = np.concatenate((pc1['points'], pc2['points']), axis=0)
colors = np.concatenate((pc1['colors'], pc2['colors']), axis=0)
pcd.points = Vector3dVector(np.asarray(points))
pcd.colors = Vector3dVector(np.asarray(colors) / 255.)
draw_geometries([pcd])
def construct_pointcloud(points):
"""
construct a point cloud object in open3d
Input:
points: a list of points in points cloud with shape (m, 3)
where m is the number of points
Output:
pc: a point cloud object of PointCloud class from open3d, with points
"""
pc = PointCloud()
pc.points = Vector3dVector(np.asanyarray(points))
return pc
def open3dVis(self, data):
"""Visualize through open3d
Args:
data: dict containing, input, depth, normal
"""
pcd = PointCloud()
depth = data['depth'][0, 0]
xyz, _ = self.get_point_cloud(depth, data['image'][0])
if 'normal' in data.keys():
normals = np.transpose(data['normal'][0], (1, 2, 0))
normals = normals.reshape((-1, 3))
else:
_, normals, _ = self.__compute_local_planes(
xyz[:, 0], xyz[:, 1], xyz[:, 2])
normals = normals.reshape((-1, 3))
color = np.transpose(data['image'][0], [1, 2, 0]).reshape((-1, 3))
pcd.points = Vector3dVector(xyz)
pcd.colors = Vector3dVector(color / 255.)
pcd.normals = Vector3dVector(normals)
draw_geometries([pcd])
def point_plane_residual(x: np.ndarray, base_transform: np.ndarray,
pcd_source: o3d.PointCloud,
pcd_target: o3d.PointCloud,
kdtree_target: o3d.KDTreeFlann, max_correspond_dist):
assert pcd_target.has_normals()
num_points = len(pcd_source.points)
points_source = np.asarray(pcd_source.points)
nearest_points = np.empty((num_points, 3), dtype=float)
nearest_normals = np.empty((num_points, 3), dtype=float)
points_target = np.asarray(pcd_target.points)
normals_target = np.asarray(pcd_target.normals)
for i in range(num_points):
k, idx, _ = kdtree_target.search_hybrid_vector_3d(
pcd_source.points[i], max_correspond_dist, 1)
if k == 1:
nearest_points[i, :] = points_target[idx[0], :]
nearest_normals[i, :] = normals_target[idx[0], :]
else:
nearest_points[i, :] = points_source[i, :]
nearest_normals[i, :] = np.array(
[0, 0, 0], dtype=float) # cause a zero residual
angle = x[0]
t1 = x[1]
t2 = x[2]
transform = rotation_matrix(angle, rotation_axis)
transform[:3, 3] = t1 * translation_axis1 + t2 * translation_axis2
transform = np.dot(transform, base_transform)
position_residuals = np.dot(transform[:3, :3],
points_source.T) + transform[:3, 3].reshape(
3, 1) - nearest_points.T
residuals = position_residuals * nearest_normals.T
residuals = np.sum(residuals, axis=0)
return residuals
def cicp(pcd_source: o3d.PointCloud, pcd_target: o3d.PointCloud,
base_transform: np.ndarray, max_correspond_dist_coarse,
max_correspond_dist_fine):
kdtree = o3d.KDTreeFlann(pcd_target)
if not pcd_target.has_normals():
o3d.estimate_normals(pcd_target,
search_param=o3d.KDTreeSearchParamHybrid(
radius=0.1, max_nn=30))
x0 = np.array([0, 0, 0], dtype=float)
result = least_squares(point_plane_residual,
x0,
jac='2-point',
method='trf',
loss='soft_l1',
args=(base_transform, pcd_source, pcd_target,
kdtree, max_correspond_dist_coarse))
x0 = result.x
result = least_squares(point_plane_residual,
x0,
jac='2-point',
method='trf',
loss='soft_l1',
args=(base_transform, pcd_source, pcd_target,
kdtree, max_correspond_dist_fine))
x = result.x
angle = x[0]
t1 = x[1]
t2 = x[2]
transform = rotation_matrix(angle, rotation_axis)
transform[:3, 3] = t1 * translation_axis1 + t2 * translation_axis2
transform = np.dot(transform, base_transform)
return transform, result
def open_3d_vis(args, output_dir):
"""
http://www.open3d.org/docs/tutorial/Basic/visualization.html
-- Mouse view control --
Left button + drag : Rotate.
Ctrl + left button + drag : Translate.
Wheel : Zoom in/out.
-- Keyboard view control --
[/] : Increase/decrease field of view.
R : Reset view point.
Ctrl/Cmd + C : Copy current view status into the clipboard. (A nice view has been saved as utilites/view.json
Ctrl/Cmd + V : Paste view status from clipboard.
-- General control --
Q, Esc : Exit window.
H : Print help message.
P, PrtScn : Take a screen capture.
D : Take a depth capture.
O : Take a capture of current rendering settings.
"""
json_dir = os.path.join(
output_dir, 'json_' + args.list_flag + '_trans') + '_iou' + str(1.0)
args.test_dir = json_dir
args.gt_dir = args.dataset_dir + 'car_poses'
args.res_file = os.path.join(output_dir,
'json_' + args.list_flag + '_res.txt')
args.simType = None
car_models = load_car_models(os.path.join(args.dataset_dir, 'car_models'))
det_3d_metric = Detect3DEval(args)
evalImgs = det_3d_metric.evaluate()
image_id = evalImgs['image_id']
print(image_id)
# We only draw the most loose constraint here
gtMatches = evalImgs['gtMatches'][0]
dtScores = evalImgs['dtScores']
mesh_car_all = []
# We also save road surface
xmin, xmax, ymin, ymax, zmin, zmax = np.inf, -np.inf, np.inf, -np.inf, np.inf, -np.inf
for car in det_3d_metric._gts[image_id]:
car_model = car_models[car['car_id']]
R = euler_angles_to_rotation_matrix(car['pose'][:3])
vertices = np.matmul(R, car_model['vertices'].T) + np.asarray(
car['pose'][3:])[:, None]
xmin, xmax, ymin, ymax, zmin, zmax = update_road_surface(
xmin, xmax, ymin, ymax, zmin, zmax, vertices)
mesh_car = TriangleMesh()
mesh_car.vertices = Vector3dVector(vertices.T)
mesh_car.triangles = Vector3iVector(car_model['faces'] - 1)
# Computing normal
mesh_car.compute_vertex_normals()
# we render the GT car in BLUE
mesh_car.paint_uniform_color([0, 0, 1])
mesh_car_all.append(mesh_car)
for i, car in enumerate(det_3d_metric._dts[image_id]):
if dtScores[i] > args.dtScores:
car_model = car_models[car['car_id']]
R = euler_angles_to_rotation_matrix(car['pose'][:3])
vertices = np.matmul(R, car_model['vertices'].T) + np.asarray(
car['pose'][3:])[:, None]
mesh_car = TriangleMesh()
mesh_car.vertices = Vector3dVector(vertices.T)
mesh_car.triangles = Vector3iVector(car_model['faces'] - 1)
# Computing normal
mesh_car.compute_vertex_normals()
if car['id'] in gtMatches:
# if it is a true positive, we render it as green
mesh_car.paint_uniform_color([0, 1, 0])
else:
# else we render it as red
mesh_car.paint_uniform_color([1, 0, 0])
mesh_car_all.append(mesh_car)
# Draw the road surface here
# x = np.linspace(xmin, xmax, 100)
# every 0.1 meter
xyz = get_road_surface_xyz(xmin, xmax, ymin, ymax, zmin, zmax)
# Pass xyz to Open3D.PointCloud and visualize
pcd_road = PointCloud()
pcd_road.points = Vector3dVector(xyz)
pcd_road.paint_uniform_color([0, 0, 1])
mesh_car_all.append(pcd_road)
# draw mesh frame
mesh_frame = create_mesh_coordinate_frame(size=3, origin=[0, 0, 0])
mesh_car_all.append(mesh_frame)
draw_geometries(mesh_car_all)
det_3d_metric.accumulate()
det_3d_metric.summarize()
def pcd_array(cloud):
pcd = PointCloud()
pcd.points = Vector3dVector(cloud[:, :3])
pcd.colors = Vector3dVector(cloud[:, 3:])
return pcd
points = np.loadtxt(r'../Data/pole2.txt', skiprows=1, delimiter=';')[:, :3]
#MyRANSAC.Model = CylinderModel((0,0), (0.05, 0.3)) #Theta and Phi, Min Max radius of the cylinders
#PlaneModel()
from Runner import Search
from geometry import *
Result = Search(1, points, CylinderModel(0.05, (0.02, 0.05), (0, 0)))
Result = Search(1, points, PlaneModel(0.05))
resultPoints = Result[1][0]
#resultPoints = np.vstack((Result[1][0], Result[1][1]))
# For visualization:
pcd = PointCloud()
pcd.points = Vector3dVector(Result[2])
pcd2 = PointCloud()
pcd2.points = Vector3dVector(resultPoints)
pcd2.paint_uniform_color([0.1, 0.1, 0.1])
estimate_normals(pcd2,
search_param=KDTreeSearchParamHybrid(radius=0.1, max_nn=20))
draw_geometries([pcd2])
#########
pcd = PointCloud()
pcd.points = Vector3dVector(points)
draw_geometries([pcd])
from numpy import load
from open3d import PointCloud, Vector3dVector, write_point_cloud
from sys import argv
if len(argv) > 3:
name_epc = argv[1]
radius = float(argv[2])
name_output = argv[3]
else:
name_epc = input('input npy: ')
radius = float(input('radius: '))
name_output = input('output: ')
epc = load(name_epc)
cloud = epc[:, :-2].compress((epc[:, -2] > radius) & (radius >= epc[:, -1]), 0)
pcd = PointCloud()
pcd.points = Vector3dVector(cloud[:, :3])
pcd.colors = Vector3dVector(cloud[:, 3:])
write_point_cloud(name_output, pcd)
print(pcd)
def convert_NA2PC(mesh_as_NA):
output = PointCloud()
output.points = Vector3dVector(mesh_as_NA)
return output
def export_scene_pointcloud(nusc: NuScenes,
out_path: str,
scene_token: str,
scene_name,
trafo_in_origin_BOOL: bool,
write_ascii_BOOL: bool,
channel: str = 'LIDAR_TOP',
min_dist: float = 3.0,
max_dist: float = 30.0,
verbose: bool = True) -> None:
"""
Export fused point clouds of a scene to a Wavefront OBJ file.
This point-cloud can be viewed in your favorite 3D rendering tool, e.g. Meshlab or Maya.
:param nusc: NuScenes instance.
:param out_path: Output path to write the point-cloud to.
:param scene_token: Unique identifier of scene to render.
:param channel: Channel to render.
:param min_dist: Minimum distance to ego vehicle below which points are dropped.
:param max_dist: Maximum distance to ego vehicle above which points are dropped.
:param verbose: Whether to print messages to stdout.
"""
# Check inputs.
valid_channels = [
'LIDAR_TOP', 'RADAR_FRONT', 'RADAR_FRONT_RIGHT', 'RADAR_FRONT_LEFT',
'RADAR_BACK_LEFT', 'RADAR_BACK_RIGHT'
]
camera_channels = [
'CAM_FRONT_LEFT', 'CAM_FRONT', 'CAM_FRONT_RIGHT', 'CAM_BACK_LEFT',
'CAM_BACK', 'CAM_BACK_RIGHT'
]
assert channel in valid_channels, 'Input channel {} not valid.'.format(
channel)
# Get records from DB.
scene_rec = nusc.get('scene', scene_token)
start_sample_rec = nusc.get('sample', scene_rec['first_sample_token'])
sd_rec = nusc.get('sample_data', start_sample_rec['data'][channel])
# Make list of frames
cur_sd_rec = sd_rec
sd_tokens = []
while cur_sd_rec['next'] != '':
cur_sd_rec = nusc.get('sample_data', cur_sd_rec['next'])
sd_tokens.append(cur_sd_rec['token'])
ego_pose_info = []
zeitstample = 0
for sd_token in tqdm(sd_tokens):
zeitstample = zeitstample + 1
if verbose:
print('Processing {}'.format(sd_rec['filename']))
sc_rec = nusc.get('sample_data', sd_token)
sample_rec = nusc.get('sample', sc_rec['sample_token'])
# lidar_token = sd_rec['token'] # only for the start_sample_rec = nusc.get('sample', scene_rec['first_sample_token'])
lidar_rec = nusc.get('sample_data', sd_token)
pc = LidarPointCloud.from_file(
osp.join(nusc.dataroot, lidar_rec['filename']))
# Points live in their own reference frame. So they need to be transformed (DELETED: via global to the image plane.
# First step: transform the point cloud to the ego vehicle frame for the timestamp of the sweep.
cs_record = nusc.get('calibrated_sensor',
lidar_rec['calibrated_sensor_token'])
pc.rotate(Quaternion(cs_record['rotation']).rotation_matrix)
pc.translate(np.array(cs_record['translation']))
# Optional Filter by distance to remove the ego vehicle.
dists_origin = np.sqrt(np.sum(pc.points[:3, :]**2, axis=0))
keep = np.logical_and(min_dist <= dists_origin,
dists_origin <= max_dist)
pc.points = pc.points[:, keep]
# coloring = coloring[:, keep]
if verbose:
print('Distance filter: Keeping %d of %d points...' %
(keep.sum(), len(keep)))
# Second step: transform to the global frame.
poserecord = nusc.get('ego_pose', lidar_rec['ego_pose_token'])
if trafo_in_origin_BOOL == True:
pc.rotate(Quaternion(poserecord['rotation']).rotation_matrix)
pc.translate(np.array(poserecord['translation']))
ego_pose_info.append(
"Ego-Pose-Info for time-stample %i sd_token(%s) :\n" %
(zeitstample, sd_token))
ego_pose_info.append(" Rotation : %s \n" %
np.array2string(np.array(poserecord['rotation'])))
ego_pose_info.append(
" Translation : %s \n" %
np.array2string(np.array(poserecord['translation'])))
pc_nparray = np.asarray(pc.points)
# print(pc_nparray.T.shape(1))
xyzi = pc_nparray.T
xyz = np.zeros((xyzi.shape[0], 3))
xyz[:, 0] = np.reshape(pc_nparray[0], -1)
xyz[:, 1] = np.reshape(pc_nparray[1], -1)
xyz[:, 2] = np.reshape(pc_nparray[2], -1)
# xyzi[:, 3] = np.reshape(pc_nparray[3], -1)
intensity_from_velodyne = xyzi[:, 3] / 255
# print(np.amax(intensity_from_velodyne))
# print(intensity_from_velodyne.shape)
intensity = np.zeros((intensity_from_velodyne.shape[0], 3))
intensity[:, 0] = np.reshape(intensity_from_velodyne[0], -1)
intensity[:, 1] = np.reshape(intensity_from_velodyne[0], -1)
intensity[:, 2] = np.reshape(intensity_from_velodyne[0], -1)
PointCloud_open3d = PointCloud()
PointCloud_open3d.colors = Vector3dVector(intensity)
# xyzi = np.zeros((xyzi0.shape[0], 3))
# xyzi[:, 0] = np.reshape(pc_nparray[0], -1)
# xyzi[:, 1] = np.reshape(pc_nparray[1], -1)
# xyzi[:, 2] = np.reshape(pc_nparray[2], -1)
# print(xyzi.shape)
PointCloud_open3d.points = Vector3dVector(xyz)
write_point_cloud("%s-%i-%s.pcd" % (out_path, zeitstample, sd_token),
PointCloud_open3d,
write_ascii=write_ascii_BOOL)
# Write ego-pose.
f = open(args.out_dir + "/ego_pose_info.txt", 'w+')
f.write("ego_pose_info for scene %s \n (with token %s) \n" %
(scene_name, scene_token))
f.write(' '.join(ego_pose_info))
f.close()
return current_r, current_t
# To get the now_pc and next_pc, there are two ways:
#(1) you can load from RGBD image by
from open3d import Image as IMG
from open3d import create_point_cloud_from_rgbd_image, create_rgbd_image_from_color_and_depth, read_pinhole_camera_intrinsic
color_image = np.array(Image.open("{0}/rgb/{1}".format(data_root, rgb_name)))
depth_im = np.array(Image.open("{0}/depth/{1}".format(data_root,
dep_name))) / 1000.0
color_raw = IMG(color_image)
depth_raw = IMG(depth_im.astype(np.float32))
rgbd_image = create_rgbd_image_from_color_and_depth(
color_raw, depth_raw, depth_scale=1.0, convert_rgb_to_intensity=False)
pinhole_camera_intrinsic = read_pinhole_camera_intrinsic("camera_redwood.json")
new_pc = create_point_cloud_from_rgbd_image(rgbd_image,
pinhole_camera_intrinsic)
# you can get the next_pc in the same way
#(2) direct change from .xyz
from open3d import PointCloud
new_pc = PointCloud()
new_pc.points = Vector3dVector(verts)
new_pc.normals = Vector3dVector(norms)
new_pc.colors = Vector3dVector(colors / 255.)
# you can get the next_pc in the same way
