Secure your code as it's written. Use Snyk Code to scan source code in minutes - no build needed - and fix issues immediately.
true_idxs = true_xyz[:, [1, 0]]
fig, axes = plt.subplots(1, 2)
axes[0].imshow(color_f)
axes[0].scatter(hole_idxs[:, 1], hole_idxs[:, 0])
axes[1].imshow(color_f)
axes[1].scatter(true_idxs[:, 1], true_idxs[:, 0])
for ax in axes:
ax.axis('off')
plt.show()
obj_xyz_pred = transform_xyz(obj_xyz, estimated_pose)
obj_xyz_true = transform_xyz(obj_xyz, true_pose)
pcs = []
pts = obj_xyz_pred[:, :3].copy().astype(np.float64)
pc = o3d.PointCloud()
pc.points = o3d.Vector3dVector(pts)
pcs.append(pc)
pts = obj_xyz_true[:, :3].copy().astype(np.float64)
pc = o3d.PointCloud()
pc.points = o3d.Vector3dVector(pts)
pcs.append(pc)
o3d.draw_geometries(pcs)
# compute metric
add_err = compute_ADD(true_pose, estimated_pose, obj_xyz)
reproj_err = reprojection_error(true_pose, estimated_pose, obj_xyz, config.VIEW_BOUNDS, config.HEIGHTMAP_RES)
gt_final_pose = init_pose.copy()
est_final_pose = estimated_pose @ final_pose
rot_err = rotational_error(gt_final_pose[:3, :3], est_final_pose[:3, :3])
trans_err = translational_error(gt_final_pose[:3, 3], est_final_pose[:3, 3])
if data_dir.split("/")[-1] == "fruits" and folder_idx in config.FRUIT_IDXS:
def Cloud2Image( self, cloud_in ):
img = np.zeros( [self.height, self.width], dtype=np.uint8 )
img_zero = np.zeros( [self.height, self.width], dtype=np.uint8 )
cloud_np1 = np.asarray(cloud_in.points)
sorted_indices = np.argsort(cloud_np1[:,2])[::-1]
cloud_np = cloud_np1[sorted_indices]
cloud_np_xy = cloud_np[:,0:2] / cloud_np[:,[2]]
# cloud_np ... (x/z, y/z, z)
cloud_np = np.hstack((cloud_np_xy,cloud_np[:,[2]]))
cloud_color1 = np.asarray(cloud_in.colors)
cloud_mapped = o3.PointCloud()
cloud_mapped.points = o3.Vector3dVector(cloud_np)
cloud_mapped.transform(self.camera_intrinsic4x4)
""" If cloud_in has the field of color, color is mapped into the image. """
if len(cloud_color1) == len(cloud_np):
cloud_color = cloud_color1[sorted_indices]
img = cv2.merge((img,img,img))
for i, pix in enumerate(cloud_mapped.points):
if pix[0]
def assign_colors(pc: open3d.geometry.PointCloud, dist: np.array, cmap_name: str='hot') -> open3d.geometry.PointCloud:
'''Assigns per-point colors to a point cloud given a distance array.'''
cmap = plt.cm.get_cmap(cmap_name)
points = np.asarray(pc.points)
colors = np.zeros(points.shape)
ind = 0
for pt in points:
colors[ind, :] = cmap(1-dist[ind])[0:3]
ind += 1
out_pc = open3d.PointCloud()
out_pc.points = open3d.Vector3dVector(points)
out_pc.colors = open3d.Vector3dVector(colors)
return out_pc
n = len(depth_paths)
print('found %d clean depth images...' % n)
intrinsic = np.array([[525.0,0,319.5],[0,525.0,239.5],[0,0,1]])
np.random.seed(816)
indices = np.random.permutation(n)
print(indices[:100])
#indices = sorted(indices)
make_dirs(PATH_MAT.format(args.shapeid, 0))
import open3d
pcd_combined = open3d.PointCloud()
for i, idx in enumerate(indices):
import ipdb; ipdb.set_trace()
print('%d / %d' % (i, len(indices)))
mesh = Mesh.read(depth_paths[idx], mode='depth', intrinsic = intrinsic)
pcd = open3d.PointCloud()
pcd.points = open3d.Vector3dVector(mesh.vertex.T)
pcd.transform(inverse(T[idx]))
#pcd = open3d.voxel_down_sample(pcd, voxel_size=0.02)
pcd_combined += pcd
pcd_combined = open3d.voxel_down_sample(pcd_combined, voxel_size=0.02)
sio.savemat(PATH_MAT.format(args.shapeid, i), mdict={
'vertex': mesh.vertex,
'validIdx_rowmajor': mesh.validIdx,
'pose': T[idx],
'depth_path': depth_paths[idx],
'pose_path': pose_paths[idx]})
if i <= 50 and i >= 40:
pcd_combined_down = open3d.voxel_down_sample(pcd_combined, voxel_size=0.02)
open3d.draw_geometries([pcd_combined_down])
pcd_combined_down = open3d.voxel_down_sample(pcd_combined, voxel_size=0.02)
x,y,z = rgbdTools.getPosition(RealSense,depth,m2,n2)
Pt2.append([x,y,z])
for mm in range(0,480):
for nn in range(0,640):
x,y,z = rgbdTools.getPosition(RealSense,depth,mm,nn)
if y < (TablePlane.a * x + TablePlane.c * z + TablePlane.d)/(-TablePlane.b) - 0.01 and z > 0.15 and z < 0.35:
Pt4.append([x,y,z])
Color2.append(rgbdTools.getColor(rgb,mm,nn))
Point2.points = o3d.Vector3dVector(np.array(Pt2))
Point3.points = o3d.Vector3dVector(np.array(Pt3))
Point4.points = o3d.Vector3dVector(np.array(Pt4))
Point4.colors = o3d.Vector3dVector(np.array(Color2))
Point2_list.append(Point2)
Point3_list.append(Point3)
Point4_list.append(Point4)
global_pcd = o3d.geometry.PointCloud()
for cloud_i in range(cloud_number-1):
print('regitrating NO.',cloud_i,'view .')
source = Point3_list[cloud_i]
target = Point3_list[cloud_i+1]
sFeatureList = registration.extractFeatures(source,3)
tFeatureList = registration.extractFeatures(target,3)
def open3d_icp(src, trgt, init_rotation=np.eye(3, 3)):
source = open3d.PointCloud()
source.points = open3d.Vector3dVector(src)
target = open3d.PointCloud()
target.points = open3d.Vector3dVector(trgt)
init_rotation_4x4 = np.eye(4, 4)
init_rotation_4x4[0:3, 0:3] = init_rotation
threshold = 0.2
reg_p2p = open3d.registration_icp(source, target, threshold, init_rotation_4x4,
open3d.TransformationEstimationPointToPoint())
return reg_p2p
# Set open3d_cloud
if "rgb" in field_names:
IDX_RGB_IN_FIELD=3 # x, y, z, rgb
# Get xyz
xyz = [(x,y,z) for x,y,z,rgb in cloud_data ] # (why cannot put this line below rgb?)
# Get rgb
# Check whether int or float
if type(cloud_data[0][IDX_RGB_IN_FIELD])==float: # if float (from pcl::toROSMsg)
rgb = [convert_rgbFloat_to_tuple(rgb) for x,y,z,rgb in cloud_data ]
else:
rgb = [convert_rgbUint32_to_tuple(rgb) for x,y,z,rgb in cloud_data ]
# combine
open3d_cloud.points = open3d.Vector3dVector(np.array(xyz))
open3d_cloud.colors = open3d.Vector3dVector(np.array(rgb)/255.0)
else:
xyz = [(x,y,z) for x,y,z in cloud_data ] # get xyz
open3d_cloud.points = open3d.Vector3dVector(np.array(xyz))
# return
return open3d_cloud
def assign_colors(pc: open3d.geometry.PointCloud, dist: np.array, cmap_name: str='hot') -> open3d.geometry.PointCloud:
'''Assigns per-point colors to a point cloud given a distance array.'''
cmap = plt.cm.get_cmap(cmap_name)
points = np.asarray(pc.points)
colors = np.zeros(points.shape)
ind = 0
for pt in points:
colors[ind, :] = cmap(1-dist[ind])[0:3]
ind += 1
out_pc = open3d.PointCloud()
out_pc.points = open3d.Vector3dVector(points)
out_pc.colors = open3d.Vector3dVector(colors)
return out_pc
def interpolate_dense_labels(sparse_points, sparse_labels, dense_points, k=3):
sparse_pcd = open3d.PointCloud()
sparse_pcd.points = open3d.Vector3dVector(sparse_points)
sparse_pcd_tree = open3d.KDTreeFlann(sparse_pcd)
dense_labels = []
for dense_point in dense_points:
result_k, sparse_indexes, _ = sparse_pcd_tree.search_knn_vector_3d(
dense_point, k
)
knn_sparse_labels = sparse_labels[sparse_indexes]
dense_label = np.bincount(knn_sparse_labels).argmax()
dense_labels.append(dense_label)
return dense_labels
def draw(vertex):
import open3d
pcd = open3d.PointCloud()
pcd.points = open3d.Vector3dVector(vertex.T)
open3d.draw_geometries([pcd])