def show_points_in_box(points, bboxes, bbox_corners, surfaces, point_masks, is_yx_zb):
if is_yx_zb:
bboxes = Bbox3D.convert_from_yx_zb_boxes(bboxes)
pn_in_box = np.sum(point_masks, 0)
bn = bboxes.shape[0]
points_in_boxes = []
for i in range(bn):
points_in_boxes.append( points[point_masks[:,i]] )
pcd0 = points2pcd_open3d(points[:,:3])
pcd0_aug = points2pcd_open3d(points[:,:3]-np.array([[0,0,4]]))
corner_pcds = [points2pcd_open3d(corners) for corners in bbox_corners]
surface_pcds = [points2pcd_open3d(surface) for surface in surfaces]
points_inb_pcds = [points2pcd_open3d(points[:,:3]) for points in points_in_boxes]
bboxes_ls0 = [Bbox3D.draw_bbox_open3d(box, 'Z') for box in bboxes]
frame_mesh = open3d.create_mesh_coordinate_frame(size = 2.0, origin = bboxes[0,0:3])
points_inb_pcds_valid = [pcd for i,pcd in enumerate(points_inb_pcds) if pn_in_box[i]>0]
open3d.draw_geometries( bboxes_ls0 + points_inb_pcds_valid + [pcd0_aug])
show_corners = False
show_surfaces = False
show_points_inb = False
for i in range(bn):
frame_mesh = open3d.create_mesh_coordinate_frame(size = 2.0, origin = bboxes[i,0:3])
if show_corners:
open3d.draw_geometries(bboxes_ls0[i:i+1]+corner_pcds[i:i+1]+[frame_mesh])
if show_surfaces:
open3d.draw_geometries(bboxes_ls0[i:i+1]+surface_pcds[i:i+1]+[frame_mesh])
if show_points_inb:
open3d.draw_geometries(bboxes_ls0[i:i+1]+points_inb_pcds[i:i+1]+[frame_mesh])
pass
def main(filepath):
print("{} is loaded ... ".format(filepath))
text_data = open(filepath, "r")
lines = text_data.readlines()
scale = 0.1
origin_coordinate = o3d.create_mesh_coordinate_frame(size=0.15,
origin=[0, 0, 0])
coordinates_list = [origin_coordinate]
for i, line in enumerate(lines):
if i == 0:
# 0th line is Youtube-URL
continue
values = [float(v) for j, v in enumerate(line.split(' ')) if j > 6]
if False: # this is not correct transformation in RealEstate10K
inv_transform_matrix = np.array(
[[values[0], values[1], values[2], values[3]],
[values[4], values[5], values[6], values[7]],
[values[8], values[9], values[10], values[11]],
[0.0, 0.0, 0.0, 1.0]])
else:
R = np.array([[values[0], values[1], values[2]],
[values[4], values[5], values[6]],
[values[8], values[9], values[10]]])
t = np.array([values[3], values[7], values[11]])
R_inv = np.linalg.inv(R)
T = -R_inv @ t
if i == 1:
transform_matrix0 = np.array(
[[R[0, 0], R[0, 1], R[0, 2], t[0]],
[R[1, 0], R[1, 1], R[1, 2], t[1]],
[R[2, 0], R[2, 1], R[2, 2], t[2]], [0.0, 0.0, 0.0, 1.0]])
inv_transform_matrix = np.array(
[[R_inv[0, 0], R_inv[0, 1], R_inv[0, 2], T[0]],
[R_inv[1, 0], R_inv[1, 1], R_inv[1, 2], T[1]],
[R_inv[2, 0], R_inv[2, 1], R_inv[2, 2], T[2]],
[0.0, 0.0, 0.0, 1.0]])
inv_transform_matrix = transform_matrix0 @ inv_transform_matrix
current_coordinate = o3d.create_mesh_coordinate_frame(size=scale,
origin=[0, 0, 0])
current_coordinate.transform(inv_transform_matrix)
coordinates_list.append(current_coordinate)
o3d.draw_geometries(coordinates_list)
return 1
def main():
parser = argparse.ArgumentParser(description="Argument Parser")
parser.add_argument("--floor_height",
type=float,
default=0.03,
help="the z coordinate of the floor")
args = parser.parse_args()
np.set_printoptions(precision=4, suppress=True)
bot = Robot("locobot")
bot.camera.set_pan_tilt(0, 0.7, wait=True)
bot.arm.go_home()
bot.arm.set_joint_positions([1.96, 0.52, -0.51, 1.67, 0.01], plan=False)
vis = open3d.Visualizer()
vis.create_window("3D Map")
pcd = open3d.PointCloud()
coord = open3d.create_mesh_coordinate_frame(1, [0, 0, 0])
vis.add_geometry(pcd)
vis.add_geometry(coord)
while True:
pts, colors = bot.camera.get_current_pcd(in_cam=False)
pts, colors = filter_points(pts, colors, z_lowest=args.floor_height)
pcd.clear()
# note that open3d.Vector3dVector is slow
pcd.points = open3d.Vector3dVector(pts)
pcd.colors = open3d.Vector3dVector(colors / 255.0)
vis.update_geometry()
vis.poll_events()
vis.update_renderer()
time.sleep(0.1)
def main():
bot = Robot("kinect2")
vis = open3d.Visualizer()
vis.create_window("3D Map")
pcd = open3d.PointCloud()
coord = open3d.create_mesh_coordinate_frame(1, [0, 0, 0])
vis.add_geometry(pcd)
vis.add_geometry(coord)
# We update the viewer every a few seconds
update_time = 4
while True:
start_time = time.time()
pts, colors = bot.camera.get_current_pcd()
pcd.clear()
# note that open3d.Vector3dVector is slow
pcd.points = open3d.Vector3dVector(pts)
pcd.colors = open3d.Vector3dVector(colors / 255.0)
vis.update_geometry()
vis.poll_events()
vis.update_renderer()
s_t = time.time()
while time.time() - s_t < update_time:
vis.poll_events()
vis.update_renderer()
time.sleep(0.1)
end_time = time.time()
process_time = end_time - start_time
print("Updating every {0:.2f}s".format(process_time))
def draw_registration_result(source, target, transformation):
# display results having two point clouds and transformation for source
source_temp = copy.deepcopy(source)
target_temp = copy.deepcopy(target)
source_temp.paint_uniform_color([1, 0.706, 0])
target_temp.paint_uniform_color([0, 0.651, 0.929])
source_temp.transform(transformation)
mesh_frame = o3d.create_mesh_coordinate_frame(size = 200, origin = [0, 0, 0])
o3d.draw_geometries([source_temp, target_temp, mesh_frame])
def show_mesh(vertices, triangle, color=[0, 0, 0], only_genmesh=False):
mesh = open3d.TriangleMesh()
mesh.vertices = open3d.Vector3dVector(vertices)
mesh.triangles = open3d.Vector3iVector(triangle)
mesh.paint_uniform_color(color)
centroid = np.mean(vertices, 0)
mesh_frame = open3d.create_mesh_coordinate_frame(size=1.6, origin=centroid)
if not only_genmesh:
open3d.draw_geometries([mesh, mesh_frame])
return mesh
def main():
unit = 'mm'
# get mesh list
mesh_folder_dir = "/home/yumi/Datas/mesh/01/o3d_vis"
mesh_list = os.listdir(mesh_folder_dir)
# print(mesh_list)
# suffix = '.stl'
for mesh_file in mesh_list:
# load mesh and visualize
mesh_path = os.path.join(mesh_folder_dir, mesh_file)
mesh_path_string = str(mesh_path)
if ".stl" in mesh_path_string:
mesh = trimesh.load_mesh(mesh_path)
mesh.export(mesh_path.replace('.stl', '.ply'))
mesh_o3d = o3d.read_triangle_mesh(mesh_path.replace('.stl', '.ply'))
else:
mesh_o3d = o3d.read_triangle_mesh(mesh_path)
print("mesh name:{}".format(mesh_file))
# unit conversion (m -> mm)
max_bound = mesh_o3d.get_max_bound()
if ((unit == 'mm') & (max_bound[0] < 1)):
R = np.identity(3)
T = np.zeros(3)
Z = [1000.0, 1000.0, 1000.0]
H = t3d.affines.compose(T, R, Z)
mesh_o3d.transform(H)
if ((unit == 'cm') & (max_bound[0] < 100)):
R = np.identity(3)
T = np.zeros(3)
Z = [100.0, 100.0, 100.0]
H = t3d.affines.compose(T, R, Z)
mesh_o3d.transform(H)
# draw object's bounding box
new_mesh_bound = mesh_o3d.get_max_bound()
color = [1, 0, 0]
object_bbox = bounding_box(new_mesh_bound, color)
# visualization, x, y, z axis will be rendered as red, green, and blue
base_coordinate = o3d.create_mesh_coordinate_frame(size=50)
# reference bbox
reference_bound = [50, 50, 50]
reference_color = [0, 1, 0]
reference_bbox = bounding_box(reference_bound, reference_color)
o3d.visualization.draw_geometries([reference_bbox, mesh_o3d, base_coordinate, object_bbox])
def main():
pcds = []
for i in range(file_id_start, file_id_stop + 1, 1):
pcd_file = './data/pcd_%d.pcd' % (i)
print("Reading %s..." % (pcd_file))
pcd = o3d.read_point_cloud(pcd_file)
pcds.append(pcd)
pcds = crop_clouds_by_depth(pcds, max_point_depth)
pcds = remove_clouds_outliers(
pcds, 30, voxel_size,
1) # removing outliers before downsample give good result.
pcds = downsample_clouds(pcds, voxel_size)
# pcds = remove_clouds_outliers(pcds, 5, 0.03, 1)
estimate_clouds_normals(pcds, voxel_size * 5, 30)
poses = np.loadtxt('./data/poses.txt')[:, 1:]
transforms = translations_quaternions_to_transforms(poses)
pcds = transform_clouds_by_pose(pcds, transforms)
mesh_frame = open3d.create_mesh_coordinate_frame(
size=0.5, origin=[0, 0, 0]) #original camera frame
print("Showing initial cloud, pre-registration")
o3d.draw_geometries(pcds + [mesh_frame])
# source = 5
# target = 6
# # delta_transform = transforms[target] @ np.linalg.inv(transforms[source])
# transform01, information01 = pairwise_registration(
# pcds[source], pcds[target], np.eye(4), icp_dist_coarse, icp_dist_fine)
# pcds[source].transform(transform01)
#
# coord = o3d.create_mesh_coordinate_frame(0.2, poses[0, :3])
# o3d.draw_geometries([*pcds[source:target+1], coord])
# # o3d.draw_geometries([pcds[source], pcds[target], coord])
# pcds = pcds[:6]
pose_graph = build_pose_graph(pcds, transforms, icp_dist_coarse,
icp_dist_fine)
option = o3d.GlobalOptimizationOption(
max_correspondence_distance=icp_dist_fine,
edge_prune_threshold=0.25,
reference_node=0)
o3d.global_optimization(pose_graph,
o3d.GlobalOptimizationLevenbergMarquardt(),
o3d.GlobalOptimizationConvergenceCriteria(),
option)
for i in range(len(pcds)):
pcds[i].transform(pose_graph.nodes[i].pose)
o3d.draw_geometries(pcds)
def eval_graph(sess, ops, test_handle):
is_training = False
while True:
try:
feed_dict = {
ops['is_training_pl']: is_training,
ops['handle']: test_handle
}
# trans_pred and rot_pred are estimation results
trans_pred, rot_pred, xyz, rgb, obj_batch = sess.run(
[
ops['trans_pred'], ops['rot_pred'], ops['xyz'], ops['rgb'],
ops['obj_batch']
],
feed_dict=feed_dict)
print "translation prediction ", trans_pred
print "rotation prediction ", rot_pred
# Visualize pose alignment
if b_visual:
batch_sample_idx = 0
current_rot = rot_pred[batch_sample_idx]
current_ag = np.linalg.norm(current_rot, ord=2)
current_ax = current_rot / current_ag
rotmat = transforms3d.axangles.axangle2mat(
current_ax, current_ag)
xyz_remove_rot = np.dot(xyz[batch_sample_idx, :, :], rotmat)
xyz_remove_trans = xyz_remove_rot - np.dot(
rotmat.T, trans_pred[batch_sample_idx, :])
segment_ptCloud = open3d.PointCloud()
segment_ptCloud.points = open3d.Vector3dVector(
xyz_remove_trans)
segment_ptCloud.colors = open3d.Vector3dVector(
rgb[batch_sample_idx, :, :])
model_pCloud = open3d.PointCloud()
model_pCloud.points = open3d.Vector3dVector(
obj_batch[batch_sample_idx, 0:512, 0:3])
# model_pCloud.colors = open3d.Vector3dVector(obj_batch[batch_sample_idx, 0:512, 3:6])
model_pCloud.paint_uniform_color([0.1, 0.9, 0.1])
model_frame = open3d.create_mesh_coordinate_frame(
size=0.1, origin=[0, 0, 0])
open3d.draw_geometries(
[model_pCloud, segment_ptCloud, model_frame])
except tf.errors.OutOfRangeError:
print('End of data!')
break
def main():
home_dir = expanduser("~")
parser = argparse.ArgumentParser()
parser.add_argument(
"--img_dir",
help="path to the directory that saves"
" depth and rgb images from ORB_SLAMA2",
default="%s/.ros/Imgs" % home_dir,
type=str,
)
parser.add_argument("--depth_max",
default=1.5,
help="maximum value for the depth",
type=float)
parser.add_argument("--depth_min",
default=0.2,
help="minimum value for the depth",
type=float)
parser.add_argument(
"--cfg_filename",
default="realsense_d435.yaml",
help="which config file to use",
type=str,
)
parser.add_argument(
"--subsample_pixs",
default=1,
help="sample every n pixels in row/column"
"when the point cloud is reconstructed",
type=int,
)
args = parser.parse_args()
rospy.init_node("reconstruct_world", anonymous=True)
rgb_dir = os.path.join(args.img_dir, "RGBImgs")
depth_dir = os.path.join(args.img_dir, "DepthImgs")
pcd_processor = PointCloudProcessor(
rgb_dir=rgb_dir,
depth_dir=depth_dir,
cfg_filename=args.cfg_filename,
subsample_pixs=args.subsample_pixs,
depth_threshold=(args.depth_min, args.depth_max),
)
time.sleep(2)
points, colors = pcd_processor.get_current_pcd()
if USE_OPEN3D:
pcd = open3d.PointCloud()
pcd.points = open3d.Vector3dVector(points)
pcd.colors = open3d.Vector3dVector(colors / 255.0)
coord = open3d.create_mesh_coordinate_frame(1, [0, 0, 0])
open3d.draw_geometries([pcd, coord])
def visualize_grasp(self, transform):
# Create visualizer
vis = o3d.visualization.Visualizer()
vis.create_window()
# Plot the coordinate frame
vis.add_geometry(o3d.create_mesh_coordinate_frame(size=0.05))
# Plot the object cloud
vis.add_geometry(self.pcd)
self.plot_gripper_cloud(vis, self.gripper_pcd, transform)
vis.run()
vis.destroy_window()
def visualize_hand(self, joints3d, grasp_points, mid_point, wrist_point):
# Create visualizer
vis = o3d.visualization.Visualizer()
vis.create_window()
# Plot the coordinate frame
vis.add_geometry(o3d.create_mesh_coordinate_frame(size=0.05))
# Plot the object cloud
vis.add_geometry(self.pcd)
# Plot the finger joints
for i in range(len(all_indices)):
for j in range(len(all_indices[i])):
mm = o3d.create_mesh_sphere(radius=joint_sizes[j])
mm.compute_vertex_normals()
mm.paint_uniform_color(finger_colors[i])
trans3d = joints3d[all_indices[i][j]]
tt = np.eye(4)
tt[0:3, 3] = trans3d
mm.transform(tt)
vis.add_geometry(mm)
# Plot lines between joints
lines = [[0, 13], [0, 1], [0, 4], [0, 10], [0, 7],
[13, 14], [14, 15], [15, 16],
[1, 2], [2, 3], [3, 17],
[4, 5], [5, 6], [6, 18],
[10, 11], [11, 12], [12, 19],
[7, 8], [8, 9], [9, 20]]
line_colors = [finger_colors[1], finger_colors[2], finger_colors[3], finger_colors[4], finger_colors[5],
finger_colors[1], finger_colors[1], finger_colors[1],
finger_colors[2], finger_colors[2], finger_colors[2],
finger_colors[3], finger_colors[3], finger_colors[3],
finger_colors[4], finger_colors[4], finger_colors[4],
finger_colors[5], finger_colors[5], finger_colors[5]]
line_set = o3d.geometry.LineSet()
line_set.points = o3d.utility.Vector3dVector(joints3d)
line_set.lines = o3d.utility.Vector2iVector(lines)
line_set.colors = o3d.utility.Vector3dVector(line_colors)
vis.add_geometry(line_set)
# Plot the grasp points
self.plot_gripper_end_points(vis, grasp_points)
vis.run()
vis.destroy_window()
def render_suncg_raw_house_walls(house_fn):
from suncg import split_room_parts, Suncg
with open(house_fn) as f:
house = json.loads(f.read())
scaleToMeters = house['scaleToMeters']
assert scaleToMeters == 1
bboxes = defaultdict(list)
bboxes['house'].append(Bbox3D.bbox_from_minmax(house['bbox']))
for level in house['levels']:
if 'bbox' not in level:
continue
bboxes['level'].append(Bbox3D.bbox_from_minmax(level['bbox']))
nodes = level['nodes']
for node in nodes:
node_type = node['type']
if node_type == 'Object':
modelId = node['modelId']
category = Suncg.modelId_2_class[modelId]
bboxes[category].append(Bbox3D.bbox_from_minmax(node['bbox']))
elif node_type == 'Room':
if 'bbox' in node:
bboxes['room'].append(Bbox3D.bbox_from_minmax(
node['bbox']))
room_bboxes = split_room_parts(house_fn, node['modelId'])
for c in room_bboxes:
bboxes[c] += room_bboxes[c]
else:
if 'bbox' in node:
bboxes[node_type].append(
Bbox3D.bbox_from_minmax(node['bbox']))
centroid = (np.array(house['bbox']['min']) +
np.array(house['bbox']['max'])) / 2.0
mesh_frame = open3d.create_mesh_coordinate_frame(size=0.6, origin=centroid)
for obj in bboxes:
bboxes[obj] = np.concatenate([b.reshape([1, 7]) for b in bboxes[obj]],
0)
bboxes[obj] = cam2world_box(bboxes[obj])
walls = bboxes['wall']
print('\nThe raw SUNCG walls\n')
#Bbox3D.draw_bboxes(walls, up_axis='Z', is_yx_zb=False)
Bbox3D.draw_bboxes_mesh(walls, up_axis='Z', is_yx_zb=False)
def __init__(self):
# Kinect runtime object
self.joint_count = PyKinectV2.JointType_Count # 25
self.kinect = PyKinectRuntime.PyKinectRuntime(
PyKinectV2.FrameSourceTypes_Body
| PyKinectV2.FrameSourceTypes_Color
| PyKinectV2.FrameSourceTypes_Depth)
self.depth_width, self.depth_height = self.kinect.depth_frame_desc.Width, self.kinect.depth_frame_desc.Height # Default: 512, 424
self.color_width, self.color_height = self.kinect.color_frame_desc.Width, self.kinect.color_frame_desc.Height # Default: 1920, 1080
# User defined variables
self.depth_scale = 0.001 # Default kinect depth scale where 1 unit = 0.001 m = 1 mm
# depth_scale = 1.0 # Default kinect depth scale where 1 unit = 0.001 m = 1 mm
self.clipping_distance_in_meters = 1.5 # Set the maximum distance to display the point cloud data
self.clipping_distance = self.clipping_distance_in_meters / self.depth_scale # Convert dist in mm to unit
# Hardcode the camera intrinsic parameters for backprojection
# width=depth_width; height=depth_height; ppx=258.981; ppy=208.796; fx=367.033; fy=367.033 # Hardcode the camera intrinsic parameters for backprojection
ppx = 260.166
ppy = 205.197
fx = 367.535
fy = 367.535
# Open3D visualisation
self.intrinsic = open3d.PinholeCameraIntrinsic(self.depth_width,
self.depth_height, fx,
fy, ppx, ppy)
# To convert [x,y,z] -> [x.-y,-z]
self.flip_transform = [[1, 0, 0, 0], [0, -1, 0, 0], [0, 0, -1, 0],
[0, 0, 0, 1]]
# 定义图像点云
self.image_pcd = open3d.PointCloud()
# self.joint_pcd = open3d.PointCloud()
# 定义原点
self.origin_point = open3d.geometry.create_mesh_coordinate_frame(
size=0.5, origin=[0, 0, 0])
# 定义关节点坐标点云
self.axis_pcd = []
for i in range(self.joint_count): # 25 axes for 25 joints
# XYZ axis length of 0.1 m
pre_axis = open3d.create_mesh_coordinate_frame(size=0.1,
origin=[0, 0, 0])
self.axis_pcd.append(pre_axis)
# 定义关节点点云连接线：24关节点连接线
self.bone_line_pcd = utils.create_line_set_bones(
np.zeros((24, 3), dtype=np.float32))
def visualize(self, source_pcd, target_pcd, transform):
# Get aligned cloud
source_pcd_temp = copy.deepcopy(source_pcd)
source_pcd_temp.transform(transform)
# Load the gripper
gripper_pcd_target = o3d.io.read_point_cloud(
self.args.gripper_cloud_path)
gripper_pcd_source = copy.deepcopy(gripper_pcd_target)
gripper_transform_file = join(self.args.ho3d_path, 'train',
self.args.target,
'meta/grasp_bl_0000.pkl')
gripper_transform = load_pickle_data(gripper_transform_file)
gripper_transform = gripper_transform.reshape(4, 4)
gripper_pcd_target.transform(gripper_transform)
tt = np.matmul(np.linalg.inv(transform), gripper_transform)
gripper_pcd_source.transform(tt)
gripper_pcd_source.paint_uniform_color([0.9, 0.1, 0.1])
# Create visualizer
vis = o3d.visualization.Visualizer()
vis.create_window()
# Plot the coordinate frame
vis.add_geometry(o3d.create_mesh_coordinate_frame(size=0.05))
# Plot the clouds
source_pcd.paint_uniform_color([0.8, 0.3, 0.3])
vis.add_geometry(source_pcd)
gripper_pcd_source.paint_uniform_color([0.9, 0.1, 0.1])
vis.add_geometry(gripper_pcd_source)
target_pcd.paint_uniform_color([0.4, 0.4, 0.4])
vis.add_geometry(target_pcd)
gripper_pcd_target.paint_uniform_color([0.6, 0.6, 0.6])
vis.add_geometry(gripper_pcd_target)
source_pcd_temp.paint_uniform_color([0.3, 0.3, 0.8])
vis.add_geometry(source_pcd_temp)
vis.run()
vis.destroy_window()
def visualize(object_pcd, hand_pcd, scene_pcd):
vis = o3d.visualization.Visualizer()
vis.create_window()
# Coordinate frame
vis.add_geometry(o3d.create_mesh_coordinate_frame(size=0.05))
# Object cloud
vis.add_geometry(object_pcd)
# Hand cloud
vis.add_geometry(hand_pcd)
# Scene cloud
vis.add_geometry(scene_pcd)
# Run and end
vis.run()
vis.destroy_window()
def visualize_3D(self):
# Create visualizer
vis = o3d.visualization.Visualizer()
vis.create_window()
# Coordinate frame
vis.add_geometry(o3d.create_mesh_coordinate_frame(size=0.05))
# Object cloud
vis.add_geometry(self.pcd)
# Ground truth hand joints
self.visualize_hand(vis, self.joints_gt, shape=JointShapes.SPHERE)
# Estimated hand joints
self.visualize_hand(vis, self.joints_estimated, shape=JointShapes.BOX)
# Close visualizer
vis.run()
vis.destroy_window()
def render_depth_pointcloud(im,
depth,
pose_data,
points,
intrinsic_matrix,
factor_depth=1):
rgb = im.copy()[:, :, ::-1]
if rgb.dtype == np.uint8:
rgb = rgb.astype(np.float32) / 255
X = backproject_camera(depth, intrinsic_matrix, factor_depth)
cloud_rgb = rgb # .astype(np.float32)[:,:,::-1] / 255
cloud_rgb = cloud_rgb.reshape((cloud_rgb.shape[0] * cloud_rgb.shape[1], 3))
scene_cloud = create_cloud(X.T, colors=cloud_rgb)
coord_frame = open3d.create_mesh_coordinate_frame(size=0.6,
origin=[0, 0, 0])
if len(pose_data) == 0:
open3d.draw_geometries([scene_cloud, coord_frame])
return
all_objects_cloud = open3d.PointCloud()
for pd in pose_data:
object_cls = pd["cls"]
object_pose = pd["pose"]
# object_cloud_file = osp.join(object_model_dir,object_name,"points.xyz")
object_pose_matrix4f = get_4x4_transform(object_pose)
if object_pose_matrix4f is None:
continue
# object_pose_matrix4f[2,3]
object_pts3d = points[object_cls] # read_xyz_file(object_cloud_file)
object_cloud = create_cloud(object_pts3d)
object_cloud.transform(object_pose_matrix4f)
all_objects_cloud += object_cloud
# print("Showing %s"%(object_name))
open3d.draw_geometries([scene_cloud, all_objects_cloud, coord_frame])
def visualize(self, voxels, counts):
# Create visualizer
vis = o3d.visualization.Visualizer()
vis.create_window()
# Add the coordinate frame
vis.add_geometry(o3d.create_mesh_coordinate_frame(size=0.05))
# Add the voxels as a point cloud
cloud = o3d.geometry.PointCloud()
cloud.points = o3d.utility.Vector3dVector(voxels)
cloud.paint_uniform_color([1, 0, 0])
colors = np.asarray(cloud.colors)
for i in range(voxels.shape[0]):
if counts[i] == 0:
colors[i] = [0.7, 0.7, 0.7]
cloud.colors = o3d.utility.Vector3dVector(colors)
vis.add_geometry(cloud)
# Add the voxels
box_dim = 0.1 * self.res
for i in range(voxels.shape[0]):
if counts[i] > 0:
mm = o3d.create_mesh_box(width=box_dim,
height=box_dim,
depth=box_dim)
mm.compute_vertex_normals()
mm.paint_uniform_color([1, 0, 0])
tt = np.eye(4)
tt[0:3, 3] = voxels[i] - 0.5 * box_dim
mm.transform(tt)
vis.add_geometry(mm)
# Run the visualizer
vis.run()
vis.destroy_window()
mask = np.zeros((h, w, 3), dtype=np.uint8)
for d in object_data:
color = get_random_color()
projc = d['projected_cuboid'].astype(np.int32)
# cv2.fillConvexPoly(mask, projc[::2].copy(), color)
projc = [tuple(pt) for pt in projc]
for pt in projc:
cv2.circle(im_copy, pt, 3, color, -1)
draw_cuboid_lines(im_copy, projc, color)
cv2.imshow("proj_cuboid", im_copy)
# cv2.imshow("proj_cuboid_masks", mask)
coord_frame = open3d.create_mesh_coordinate_frame(size=0.6, origin=[0, 0, 0])
if __name__ == '__main__':
import glob
import json
CLASSES = ('__background__', '002_master_chef_can', '003_cracker_box', '004_sugar_box', '005_tomato_soup_can', '006_mustard_bottle', \
'007_tuna_fish_can', '008_pudding_box', '009_gelatin_box', '010_potted_meat_can', '011_banana', '019_pitcher_base', \
'021_bleach_cleanser', '024_bowl', '025_mug', '035_power_drill', '036_wood_block', '037_scissors', '040_large_marker', \
'051_large_clamp', '052_extra_large_clamp', '061_foam_brick')
CLASSES_INDEX = dict((c, ix) for ix, c in enumerate(CLASSES))
ROOT_DIR = "/home/vincent/hd/datasets/FAT"
MODEL_DIR = ROOT_DIR + "/models"
SUPERCATEGORY = "FAT"
#!/usr/bin/env python3 # -*- coding: utf-8 -*- """ Created on Wed Dec 5 16:17:50 2018 @author: karol """ import open3d as o3d from os.path import join import numpy as np path = r'/media/karol/SSD/Data/omsws/SEQ_1.B 011/2/results' pcd = o3d.read_point_cloud(join(path, 'flatten.pcd')) print(np.asarray(pcd.points).shape) print(np.asarray(pcd.colors).shape) # comment this to see the point cloud in rainbow #pcd.paint_uniform_color([1, 0.706, 0]) mesh_frame = o3d.create_mesh_coordinate_frame(size=600, origin=[0, 0, 0]) # find normals so it's displayed nicer #o3d.estimate_normals(pcd, search_param = o3d.KDTreeSearchParamHybrid( # radius = 5, max_nn = 30)) o3d.draw_geometries([pcd, mesh_frame])
if __name__ == '__main__':
import time
import open3d
np.set_printoptions(suppress=True, precision=4)
# Local coordinate system for the camera:
# 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)
def callback(self,*args):
print("YEET")
norm=0
rgb = IRos.rosImg2RGB(args[0])
depth_reg = IRos.rosImg2Depth(args[1])
#copy image
hello = rgb.astype(np.uint8).copy()
#cv2.imshow("wow",hello)
#cv2.waitKey(0)
#cv2.destroyAllWindows()
#finds markers
det_corners, ids, rejected = aruco.FindMarkers(rgb, self.K,self.D)
#draw maerkers
if ids is not None:
hello = cv2.aruco.drawDetectedMarkers(hello,det_corners,np.asarray(ids))
#cv2.imshow("Detected Markers",hello)
#cv2.waitKey(0)
#cv2.destroyAllWindows()
if ids is None:
return
ids = ids.squeeze()
#makes a single id into a list with only it self
if (helperfuncs.is_empty(ids.shape)):
ids=[int(ids)]
#place where all geometries will be stores
sphs = []
'''
#3D WAY (Scaled Procrustes)
if ids is not None and len(ids)>0:
#filter ids and cornerds
validids=[]
validcordners= []
#fetches only ids that are on the cangalho
for i in range(0,len(ids)):
if ids[i] in self.arucoData['ids']:
#print("Valid marker id: "+str(ids[i]))
validids.append(ids[i])
validcordners.append(det_corners[i])
Rr,tt = aruco.GetCangalhoFromMarkersProcrustes(validids,validcordners,self.K,self.arucoData,self.arucoModel,depth_reg)
DATAprocrustes= (Rr,tt)
if(Rr is not None):
H = mmnip.Rt2Homo(Rr.T,tt)
refe = open3d.create_mesh_coordinate_frame(0.16, origin = [0, 0, 0])
refe.transform(H)
sphs.append(refe)
sphere2 = open3d.create_mesh_sphere(0.02)
sphere2.transform(H)
sphere2.paint_uniform_color([1,0,1])
sphs.append(sphere2)
#PnP way
if ids is not None and len(ids)>0:
#only fetches corners and ids, for the markers ids that exist in cangalho (2-13)
validids=[]
validcordners=[]
for i in range(0,len(ids)):
if ids[i] in self.arucoData['ids']:
validids.append(ids[i])
validcordners.append(det_corners[i])
#calculates pose
Rr,tt = aruco.GetCangalhoFromMarkersPnP(validids,validcordners,self.K,self.D,self.arucoData,self.arucoModel,None)#(Rr.T,tt)
#converts in homogeneous
H = mmnip.Rt2Homo(Rr,tt.T)
#prints results, in green
sphere1 = open3d.create_mesh_sphere(0.02)
sphere1.transform(H)
sphere1.paint_uniform_color([0,1,0])
sphs.append(sphere1)
refe = open3d.create_mesh_coordinate_frame(0.16, origin = [0, 0, 0])
refe.transform(H) #Transform it according tom p
sphs.append(refe)
DATApnp= (Rr,tt)
print(DATAprocrustes[1])
print(DATApnp[1].squeeze())
if(DATAprocrustes[1] is not None):
norm = np.linalg.norm(DATAprocrustes[1]-DATApnp[1].squeeze())
print(norm)
if(norm>self.prevnorm):
self.dir=self.dir*-1
self.prevnorm=norm
self.iterations=self.iterations+1
self.K[0,0]=self.K[0,0]+self.dir
self.K[1,1]=self.K[1,1]+self.dir
print(self.K)
pointsu = np.empty((3,0))
corneee = np.squeeze(det_corners)
#find detected corners in 3D and paint them
for cor in det_corners:
for i in range(0,4):
point = mmnip.singlePixe2xyz(depth_reg,cor[0,i,:],self.K)
point = np.expand_dims(point,axis=1)
H = np.eye(4)
H[0:3,3]=point.T
#paints corners in 3D space
sphere = open3d.create_mesh_sphere(0.006)
sphere.transform(H)
sphere.paint_uniform_color([1,0,1])
sphs.append(sphere)
pointsu=np.hstack((pointsu,point))
'''
#Marker-by-Marker WAY
rvecs, tvecs, _ = cv2.aruco.estimatePoseSingleMarkers(det_corners,self.arucoData['size'],K,D)
tvecs=np.squeeze(tvecs)
#turn 0D into 1D
if len(tvecs.shape)==1:
tvecs = np.expand_dims(tvecs,axis=0)
for i in range(0,tvecs.shape[0]):
sphere = open3d.create_mesh_sphere(0.016)
#converts in 3x3 rotation matrix
Rr,_ = cv2.Rodrigues(rvecs[i])
H = mmnip.Rt2Homo(Rr,tvecs[i,:])
#prints marker position estimates
refe = open3d.create_mesh_coordinate_frame(0.1, origin = [0, 0, 0])
refe.transform(H)
sphere.transform(H)
sphere.paint_uniform_color([0,0,1])
sphs.append(sphere)
sphs.append(refe)
#converts image 2 depth
points = mmnip.depthimg2xyz2(depth_reg,self.K)
points = points.reshape((480*640, 3))
#print(colors.shape)
rgb1 = rgb.reshape((480*640, 3))#colors
pc = pointclouder.Points2Cloud(points,rgb1)
pc2 = pointclouder.Points2Cloud(pointsu.T)
pc2.paint_uniform_color([1,0,1])
#DRAW
if(norm<0.005):
open3d.draw_geometries([pc]+sphs)
def display_pcd(pcd_path,
weishu=None,
gt=None,
detection=None,
assigned_anchor=None,
predict_anchor=None,
gt_dis=True,
dt_dis=True,
ass_dis=True,
predict_dis=True):
if pcd_path[-4:] == ".pcd":
pcd = open3d.io.read_point_cloud(pcd_path)
else:
if weishu is not None:
points = np.fromfile(pcd_path, dtype=np.float32)
points = points.reshape(-1, weishu)
points = points[:, :3]
pcd = open3d.geometry.PointCloud()
pcd.points = open3d.utility.Vector3dVector(points)
else:
points = np.fromfile(pcd_path, dtype=np.float32)
points = points.reshape(-1, 4)
points = points[:, :3]
pcd = open3d.geometry.PointCloud()
pcd.points = open3d.utility.Vector3dVector(points)
mesh_frame = open3d.create_mesh_coordinate_frame(size=3, origin=[0, 0, 0])
vis = open3d.Visualizer()
vis.create_window()
opt = vis.get_render_option()
opt.background_color = np.asarray([0, 0, 0])
vis.add_geometry(pcd)
vis.add_geometry(mesh_frame)
if gt is not None and gt_dis:
locs_predict = gt[:, :3]
dims_predict = gt[:, 3:6]
rots_predict = -gt[:, 6]
gt_vis = create_boundingbox(locs_predict, dims_predict, rots_predict,
[1, 0, 0])
for i in range(len(gt_vis)):
vis.add_geometry(gt_vis[i])
if detection is not None and dt_dis:
locs_predict = detection[:, :3]
dims_predict = detection[:, 3:6]
rots_predict = -detection[:, 6]
detection_vis = create_boundingbox(locs_predict, dims_predict,
rots_predict, [0, 1, 0])
for i in range(len(detection_vis)):
vis.add_geometry(detection_vis[i])
if assigned_anchor is not None and ass_dis:
locs_predict = assigned_anchor[:, :3]
dims_predict = assigned_anchor[:, 3:6]
rots_predict = -assigned_anchor[:, 6]
assigned_anchor_vis = create_boundingbox(locs_predict, dims_predict,
rots_predict, [0, 0, 1])
for i in range(len(assigned_anchor_vis)):
vis.add_geometry(assigned_anchor_vis[i])
if predict_anchor is not None and predict_dis:
locs_predict = predict_anchor[:, :3]
dims_predict = predict_anchor[:, 3:6]
rots_predict = -predict_anchor[:, 6]
predict_anchor_vis = create_boundingbox(locs_predict, dims_predict,
rots_predict, [0, 1, 1])
for i in range(len(predict_anchor_vis)):
vis.add_geometry(predict_anchor_vis[i])
vis.run()
vis.destroy_window()
# width=depth_width; height=depth_height; ppx=258.981; ppy=208.796; fx=367.033; fy=367.033 # Hardcode the camera intrinsic parameters for backprojection
############################
### Open3D visualisation ###
############################
intrinsic = open3d.PinholeCameraIntrinsic(width, height, fx, fy, ppx, ppy)
flip_transform = [[1, 0, 0, 0], [0, -1, 0, 0], [0, 0, -1, 0],
[0, 0, 0, 1]] # To convert [x,y,z] -> [x.-y,-z]
# Define the objects to be drawn
image_pcd = open3d.PointCloud()
bone_line_pcd = utils.create_line_set_bones(np.zeros(
(24, 3), dtype=np.float32)) # 24 bones connecting 25 joints
axis_pcd = []
for i in range(PyKinectV2.JointType_Count): # 25 axes for 25 joints
axis_pcd.append(
open3d.create_mesh_coordinate_frame(
size=0.1, origin=[0, 0, 0])) # XYZ axis length of 0.1 m
# Create Open3D Visualizer
vis = open3d.Visualizer()
vis.create_window(width=width, height=height)
vis.get_render_option().point_size = 3
vis.add_geometry(bone_line_pcd)
for i in range(PyKinectV2.JointType_Count): # 25 axes for 25 joints
vis.add_geometry(axis_pcd[i])
first_loop = True
while True:
##############################
### Get images from camera ###
##############################
if kinect.has_new_body_frame() and kinect.has_new_color_frame(
) and kinect.has_new_depth_frame():
def callback(self, *args):
rgb = IRos.rosImg2RGB(args[0])
depth_reg = IRos.rosImg2Depth(args[1])
#copy image
hello = rgb.astype(np.uint8).copy()
cv2.imshow("wow", hello)
cv2.waitKey(0)
cv2.destroyAllWindows()
#get matrix intrinsics
K = self.intrinsics['K'][self.camName]
D = self.intrinsics['D'][self.camName]
#finds markers
det_corners, ids, rejected = aruco.FindMarkers(rgb, K, D)
#draw maerkers
if ids is not None:
hello = cv2.aruco.drawDetectedMarkers(hello, det_corners,
np.asarray(ids))
cv2.imshow("Detected Markers", hello)
cv2.waitKey(0)
cv2.destroyAllWindows()
if ids is None:
return
ids = ids.squeeze()
#makes a single id into a list with only it self
if (helperfuncs.is_empty(ids.shape)):
ids = [int(ids)]
#place where all geometries will be stores
sphs = []
#3D WAY (Scaled Procrustes)
if ids is not None and len(ids) > 0:
#filter ids and cornerds
validids = []
validcordners = []
#fetches only ids that are on the cangalho
for i in range(0, len(ids)):
if ids[i] in self.arucoData['ids']:
#print("Valid marker id: "+str(ids[i]))
validids.append(ids[i])
validcordners.append(det_corners[i])
Rr, tt = aruco.GetCangalhoFromMarkersProcrustes(
validids, validcordners, K, self.arucoData, self.arucoModel,
depth_reg)
if (Rr is not None):
H = mmnip.Rt2Homo(Rr.T, tt)
refe = open3d.create_mesh_coordinate_frame(0.16,
origin=[0, 0, 0])
refe.transform(H)
sphs.append(refe)
sphere2 = open3d.create_mesh_sphere(0.02)
sphere2.transform(H)
sphere2.paint_uniform_color([1, 0, 1])
sphs.append(sphere2)
#PnP way
if ids is not None and len(ids) > 0:
#only fetches corners and ids, for the markers ids that exist in cangalho (2-13)
validids = []
validcordners = []
for i in range(0, len(ids)):
if ids[i] in self.arucoData['ids']:
validids.append(ids[i])
validcordners.append(det_corners[i])
#calculates pose
Rr, tt = aruco.GetCangalhoFromMarkersPnP(validids, validcordners,
K, D, self.arucoData,
self.arucoModel,
None) #(Rr.T,tt)
#converts in homogeneous
H = mmnip.Rt2Homo(Rr, tt.T)
#prints results, in green
sphere1 = open3d.create_mesh_sphere(0.02)
sphere1.transform(H)
sphere1.paint_uniform_color([0, 1, 0])
sphs.append(sphere1)
refe = open3d.create_mesh_coordinate_frame(0.16, origin=[0, 0, 0])
refe.transform(H) #Transform it according tom p
sphs.append(refe)
pointsu = np.empty((3, 0))
#print(hello.shape)
#print("detected cornerds")
#print(det_corners)
corneee = np.squeeze(det_corners)
#print(corneee)
#corn2paint = corneee[2,:]
#offset=5
#for ii in range(int(corn2paint[0])-offset,int(corn2paint[0])+offset):
# for jj in range(int(corn2paint[1])-offset,int(corn2paint[1])+offset):
# hello[jj,ii,:]= [255,0,255]
#cv2.imshow("wow",hello)
#cv2.waitKey(0)
#cv2.destroyAllWindows()
#map1,map2 = cv2.initUndistortRectifyMap(K,D,np.eye(3),K,(640,480),cv2.CV_32FC1)
#print(wowl)
#img2 = cv2.remap(hello, map1, map2,cv2.INTER_NEAREST)
#cv2.imshow("woweeeeeeeeeeeeeeeeeeeeeeeeeeeeeeee",img2)
#cv2.waitKey(0)
#cv2.destroyAllWindows()
#find detected corners in 3D and paint them
for cor in det_corners:
for i in range(0, 4):
point = mmnip.singlePixe2xyz(depth_reg, cor[0, i, :], K)
point = np.expand_dims(point, axis=1)
H = np.eye(4)
H[0:3, 3] = point.T
#paints corners in 3D space
sphere = open3d.create_mesh_sphere(0.006)
sphere.transform(H)
sphere.paint_uniform_color([1, 0, 1])
sphs.append(sphere)
pointsu = np.hstack((pointsu, point))
#Marker-by-Marker WAY
rvecs, tvecs, _ = cv2.aruco.estimatePoseSingleMarkers(
det_corners, self.arucoData['size'], K, D)
tvecs = np.squeeze(tvecs)
#turn 0D into 1D
if len(tvecs.shape) == 1:
tvecs = np.expand_dims(tvecs, axis=0)
for i in range(0, tvecs.shape[0]):
sphere = open3d.create_mesh_sphere(0.016)
#converts in 3x3 rotation matrix
Rr, _ = cv2.Rodrigues(rvecs[i])
H = mmnip.Rt2Homo(Rr, tvecs[i, :])
#prints marker position estimates
refe = open3d.create_mesh_coordinate_frame(0.1, origin=[0, 0, 0])
refe.transform(H)
sphere.transform(H)
sphere.paint_uniform_color([0, 0, 1])
sphs.append(sphere)
sphs.append(refe)
#converts image 2 depth
points = mmnip.depthimg2xyz2(depth_reg, K)
points = points.reshape((480 * 640, 3))
#print(colors.shape)
rgb1 = rgb.reshape((480 * 640, 3)) #colors
pc = pointclouder.Points2Cloud(points, rgb1)
pc2 = pointclouder.Points2Cloud(pointsu.T)
pc2.paint_uniform_color([1, 0, 1])
open3d.draw_geometries([pc] + sphs)
def visualize_hand_and_grasp(self, joints3d, transform, grasp_points, mid_point, wrist_point, scene_pcd=None,
hand_mesh=None):
# Create visualizer
vis = o3d.visualization.Visualizer()
vis.create_window()
# Plot the coordinate frame
vis.add_geometry(o3d.create_mesh_coordinate_frame(size=0.05))
# Plot the object cloud
vis.add_geometry(self.pcd)
# Plot the finger joints
for i in range(len(all_indices)):
for j in range(len(all_indices[i])):
mm = o3d.create_mesh_sphere(radius=joint_sizes[j])
mm.compute_vertex_normals()
mm.paint_uniform_color(finger_colors[i])
trans3d = joints3d[all_indices[i][j]]
tt = np.eye(4)
tt[0:3, 3] = trans3d
mm.transform(tt)
vis.add_geometry(mm)
# Plot lines between joints
lines = [[0, 13], [0, 1], [0, 4], [0, 10], [0, 7],
[13, 14], [14, 15], [15, 16],
[1, 2], [2, 3], [3, 17],
[4, 5], [5, 6], [6, 18],
[10, 11], [11, 12], [12, 19],
[7, 8], [8, 9], [9, 20]]
line_colors = [finger_colors[1], finger_colors[2], finger_colors[3], finger_colors[4], finger_colors[5],
finger_colors[1], finger_colors[1], finger_colors[1],
finger_colors[2], finger_colors[2], finger_colors[2],
finger_colors[3], finger_colors[3], finger_colors[3],
finger_colors[4], finger_colors[4], finger_colors[4],
finger_colors[5], finger_colors[5], finger_colors[5]]
line_set = o3d.geometry.LineSet()
line_set.points = o3d.utility.Vector3dVector(joints3d)
line_set.lines = o3d.utility.Vector2iVector(lines)
line_set.colors = o3d.utility.Vector3dVector(line_colors)
vis.add_geometry(line_set)
# Plot the gripper cloud
self.plot_gripper_cloud(vis, self.gripper_pcd, transform)
# Plot the grasp points
self.plot_gripper_end_points(vis, grasp_points)
# Plot scene
if scene_pcd is not None:
vis.add_geometry(scene_pcd)
# Visualize hand
'''
if hand_mesh is not None:
mesh = o3d.geometry.TriangleMesh()
if hasattr(hand_mesh, 'r'):
mesh.vertices = o3d.utility.Vector3dVector(np.copy(hand_mesh.r) * 0.001)
numVert = hand_mesh.r.shape[0]
elif hasattr(hand_mesh, 'v'):
mesh.vertices = o3d.utility.Vector3dVector(np.copy(hand_mesh.v) * 0.001)
numVert = hand_mesh.v.shape[0]
else:
raise Exception('Unknown Mesh format')
mesh.triangles = o3d.utility.Vector3iVector(np.copy(hand_mesh.f))
mesh.vertex_colors = o3d.utility.Vector3dVector(
np.tile(np.array([[0.9, 0.4, 0.4]]), [numVert, 1]))
o3d.visualization.draw_geometries([mesh])
'''
vis.run()
vis.destroy_window()
T = [20, 30, 40]
R = [[0, -1, 0], [1, 0, 0], [0, 0, 1]] # rotation matrix
Z = [1.0, 1.0, 1.0] # zooms
A = t3d.affines.compose(T, R, Z)
print(A)
# rotation matrix to euler
rx, ry, rz = t3d.euler.mat2euler(R, axes='sxyz')
print(rx, ry, rz)
# euler to rotation matrix
R1 = t3d.euler.euler2mat(rx, ry, rz, axes='sxyz')
print(R1.astype(float))
# visualization, x, y, z axis will be rendered as red, green, and blue
base_coordinate = o3d.create_mesh_coordinate_frame(size=1000)
coordinate1 = o3d.create_mesh_coordinate_frame(size=500)
coordinate2 = o3d.create_mesh_coordinate_frame(size=300)
# r_xyz = np.array([180.272, 9.67795, 270.592]) # camera in base pose
# r_xyz = r_xyz/180*math.pi
R_1 = np.array([[0.010182, -0.999944, 0.003005],
[-0.985716, -0.009532, 0.168148],
[-0.168110, -0.004674, -0.985757]])
T_1 = np.array([393.100000, -280.894000, 1338.030000])
H_1 = t3d.affines.compose(T_1, R_1, Z) # camera in base pose
rx_1, ry_1, rz_1 = t3d.euler.mat2euler(R_1, axes='sxyz')
r_xyz_1 = np.array([rx_1, ry_1, rz_1])/math.pi*180
print("camera in base matrix:{}".format(H_1))
print("rx_1, ry_1, rz_1:{}".format(r_xyz_1))
coordinate1.transform(H_1)
from pyrobot import Robot
import os
import open3d
# Please change this to match your habitat_sim repo's path
path_to_habitat_scene = os.path.dirname(os.path.realpath(__file__))
relative_path = "scenes/skokloster-castle.glb"
common_config = dict(
scene_path=os.path.join(path_to_habitat_scene, relative_path))
bot = Robot("habitat", common_config=common_config)
# fetch the point
pts, colors = bot.camera.get_current_pcd(in_cam=False)
# convert points to open3d point cloud object
pcd = open3d.PointCloud()
pcd.points = open3d.Vector3dVector(pts)
pcd.colors = open3d.Vector3dVector(colors / 255.0)
# for visualizing the origin
coord = open3d.create_mesh_coordinate_frame(1, [0, 0, 0])
# visualize point cloud
open3d.visualization.draw_geometries([pcd, coord])
def gen_bbox(house_fn):
always_gen_bbox = Debug and 0
parsed_dir = get_pcl_path(house_fn)
summary = read_summary(parsed_dir)
box_intact = 'level_num' in summary and 'wall_num' in summary
if box_intact and (not always_gen_bbox):
print(f'skip gen_bbox, summary intact: {parsed_dir}')
return
with open(house_fn) as f:
house = json.loads(f.read())
scaleToMeters = house['scaleToMeters']
assert scaleToMeters == 1
bboxes = defaultdict(list)
bboxes['house'].append(Bbox3D.bbox_from_minmax(house['bbox']))
for level in house['levels']:
if 'bbox' not in level:
continue
bboxes['level'].append(Bbox3D.bbox_from_minmax(level['bbox']))
nodes = level['nodes']
for node in nodes:
node_type = node['type']
if node_type == 'Object':
modelId = node['modelId']
category = Suncg.modelId_2_class[modelId]
bboxes[category].append(Bbox3D.bbox_from_minmax(node['bbox']))
elif node_type == 'Room':
if 'bbox' in node:
bboxes['room'].append(Bbox3D.bbox_from_minmax(
node['bbox']))
room_bboxes = split_room_parts(house_fn, node['modelId'])
for c in room_bboxes:
bboxes[c] += room_bboxes[c]
else:
if 'bbox' in node:
bboxes[node_type].append(
Bbox3D.bbox_from_minmax(node['bbox']))
centroid = (np.array(house['bbox']['min']) +
np.array(house['bbox']['max'])) / 2.0
mesh_frame = open3d.create_mesh_coordinate_frame(size=0.6, origin=centroid)
for obj in bboxes:
if len(bboxes[obj]) > 0:
bboxes[obj] = np.concatenate(
[b.reshape([1, 7]) for b in bboxes[obj]], 0)
else:
bboxes[obj] = np.array(bboxes[obj]).reshape([-1, 7])
bboxes[obj] = cam2world_box(bboxes[obj])
for obj in DSET_METAS0.class_2_label:
if obj == 'background':
continue
if obj not in bboxes:
bboxes[obj] = np.array(bboxes[obj]).reshape([-1, 7])
level_num = len(house['levels'])
if level_num == 1:
bboxes['wall'] = preprocess_walls(bboxes['wall'])
bboxes['window'] = preprocess_windows(bboxes['window'], bboxes['wall'])
bboxes['door'] = preprocess_doors(bboxes['door'], bboxes['wall'])
bboxes['ceiling_raw'] = bboxes['ceiling'].copy()
bboxes['floor_raw'] = bboxes['floor'].copy()
bboxes['ceiling'] = preprocess_cfr(bboxes['ceiling'], bboxes['wall'],
'ceiling')
bboxes['floor'] = preprocess_cfr(bboxes['floor'], bboxes['wall'],
'floor')
# save bbox in ply and txt
object_bbox_dir = os.path.join(parsed_dir, 'object_bbox')
if not os.path.exists(object_bbox_dir):
os.makedirs(object_bbox_dir)
bboxes_num = {}
for category in bboxes.keys():
bboxes_num[category] = len(bboxes[category])
boxes_fn = os.path.join(object_bbox_dir, category + '.txt')
boxes = np.array(bboxes[category])
np.savetxt(boxes_fn, boxes)
#######################
print(f'parsed_dir: {parsed_dir}')
write_summary(parsed_dir, 'level_num', level_num, 'a')
for obj in ['room', 'wall', 'window', 'door', 'floor', 'ceiling']:
write_summary(parsed_dir, f'{obj}_num', bboxes_num[obj], 'a')
#######################
save_ply = False
if save_ply:
for category in bboxes.keys():
for i, bbox in enumerate(bboxes[category]):
box_dir = os.path.join(object_bbox_dir, '{}'.format(category))
if not os.path.exists(box_dir):
os.makedirs(box_dir)
box_fn = os.path.join(box_dir, '%d.ply' % (i))
bbox_cam = world2cam_box(bbox.reshape([1, 7]))[0]
Bbox3D.draw_bbox_open3d(bbox_cam, 'Y', plyfn=box_fn)
