def filter_outliers(self, neighborhood=20, std_ratio=2.0):
pcd_open3d = open3d.PointCloud()
pcd_open3d.points = open3d.Vector3dVector(copy.deepcopy(self.points))
inlier_pcd_open3d, ind = open3d.statistical_outlier_removal(
pcd_open3d, nb_neighbors=neighborhood, std_ratio=std_ratio)
self.points = self.points[ind]
if self.has_normals() == True:
self.normals = self.normals[ind]
return
def load_pcds(path, downsample=True, interval=1):
"""
load pointcloud by path and down samle (if True) based on voxel_size
"""
global voxel_size, camera_intrinsics, plane_equation
pcds = []
for Filename in trange(
len(glob.glob1(path + "JPEGImages", "*.jpg")) / interval):
img_file = path + 'JPEGImages/%s.jpg' % (Filename * interval)
cad = cv2.imread(img_file)
cad = cv2.cvtColor(cad, cv2.COLOR_BGR2RGB)
depth_file = path + 'depth/%s.png' % (Filename * interval)
reader = png.Reader(depth_file)
pngdata = reader.read()
depth = np.array(map(np.uint16, pngdata[2]))
mask = depth.copy()
depth = convert_depth_frame_to_pointcloud(depth, camera_intrinsics)
aruco_center = get_aruco_center(cad, depth)
# remove plane and anything underneath the plane from the pointcloud
sol = findplane(cad, depth)
distance = point_to_plane(depth, sol)
sol = fitplane(sol, depth[(distance > -0.01) & (distance < 0.01)])
# record the plane equation on the first frame for point projections
if Filename == 0:
plane_equation = sol
distance = point_to_plane(depth, sol)
mask[distance < 0.002] = 0
# use statistical outlier remover to remove isolated noise from the scene
distance2center = np.linalg.norm(depth - aruco_center, axis=2)
mask[distance2center > MAX_RADIUS] = 0
source = open3d.PointCloud()
source.points = open3d.Vector3dVector(depth[mask > 0])
source.colors = open3d.Vector3dVector(cad[mask > 0])
cl, ind = open3d.statistical_outlier_removal(source,
nb_neighbors=500,
std_ratio=0.5)
if downsample == True:
pcd_down = open3d.voxel_down_sample(cl, voxel_size=voxel_size)
open3d.estimate_normals(
pcd_down, KDTreeSearchParamHybrid(radius=0.002 * 2, max_nn=30))
pcds.append(pcd_down)
else:
pcds.append(cl)
return pcds
def filter_cloud(xyza, nb_points=16, radius=1.0):
''' Filter cloud. Remove points with few neighbors.'''
cloud = form_cloud(xyza=xyza)
# Statistical oulier removal (no need to call this)
if 0:
cloud, inliers_ind = open3d.statistical_outlier_removal(
cloud, nb_neighbors=20, std_ratio=2.0)
# Radius oulier removal
if 1:
cloud, inliers_ind = open3d.radius_outlier_removal(cloud,
nb_points=nb_points,
radius=radius)
xyza = get_xyza(cloud)
return xyza
# examples/Python/Advanced/outlier_removal.py
import open3d as o3d
import time
if __name__ == "__main__":
k = 12
m = 0.01
opt = "_k" + str(k) + "_m" + str(m)
prefix = "pcd/" + opt + "/" + "open3D/"
filename = "./pcd/Byg72.pcd"
start_time = time.time()
print("open3d " + filename + " :")
pcd = o3d.io.read_point_cloud(filename)
filename = filename[6:-4]
cl, ind = o3d.statistical_outlier_removal(pcd, nb_neighbors=k, std_ratio=m)
pcd = o3d.select_down_sample(pcd, ind)
o3d.io.write_point_cloud(prefix + filename + opt + ".pcd", pcd)
elapsed_time = time.time() - start_time
print(elapsed_time)
print("------")
filename = "./pcd/Plan3D_1st.pcd"
start_time = time.time()
print("open3d " + filename + " :")
pcd = o3d.io.read_point_cloud(filename)
filename = filename[6:-4]
cl, ind = o3d.statistical_outlier_removal(pcd, nb_neighbors=k, std_ratio=m)
pcd = o3d.select_down_sample(pcd, ind)
o3d.io.write_point_cloud(prefix + filename + opt + ".pcd", pcd)
elapsed_time = time.time() - start_time
pc_rot = pc_rot.astype(np.float).T.copy()
pcl_local = o3.PointCloud()
pcl_local.points = o3.Vector3dVector(pc_rot[:, :3])
pcl_local.colors = o3.Vector3dVector(
np.vstack((intensity, intensity, intensity)).T)
downpcd = o3.voxel_down_sample(pcl_local, voxel_size=args.voxel_size)
pcl.points.extend(downpcd.points)
pcl.colors.extend(downpcd.colors)
print("Finihsed making the map")
downpcd_full = o3.voxel_down_sample(pcl, voxel_size=args.voxel_size)
downpcd, ind = o3.statistical_outlier_removal(downpcd_full,
nb_neighbors=40,
std_ratio=0.3)
#o3.draw_geometries(downpcd)
o3.write_point_cloud(
f'./map-{sequence}_{args.voxel_size}_{first_frame}-{last_frame}.pcd',
downpcd)
else:
downpcd = o3.read_point_cloud(map_file)
exit()
voxelized = torch.tensor(downpcd.points, dtype=torch.float)
voxelized = torch.cat(
(voxelized, torch.ones([voxelized.shape[0], 1], dtype=torch.float)), 1)
voxelized = voxelized.t()
voxelized = voxelized.to(args.device)
vox_intensity = torch.tensor(downpcd.colors, dtype=torch.float)[:, 0:1].t()
if cnt < num_goalposes and cloud_subscriber.hasNewCloud():
cnt += 1
rospy.loginfo("=========================================")
rospy.loginfo(
"Node 3: Received the {}th segmented cloud.".format(cnt))
# Register Point Cloud
new_cloud = cloud_subscriber.popCloud()
if getCloudSize(new_cloud) == 0:
print " The received cloud is empty. Not processing it."
continue
# Filter
cl, ind = open3d.statistical_outlier_removal(
new_cloud, # Statistical oulier removal
nb_neighbors=20,
std_ratio=2.0)
new_cloud = open3d.select_down_sample(new_cloud, ind)
# Regi
res_cloud = cloud_register.addCloud(new_cloud)
print "Size of the registered cloud: ", getCloudSize(res_cloud)
# Update and save to file
viewer.updateCloud(res_cloud)
if cnt == num_goalposes:
rospy.loginfo(
"=========== Cloud Registration Completes ===========")
rospy.loginfo(
"====================================================")
H_base_in_camera = np.array([[0.0101817, -0.985716, -0.16811, 393.806],
[-0.999944, -0.00953149, -0.00467393, 274.026],
[0.00300482, 0.168148, -0.985757, 1340.7],
[0, 0, 0, 1]])
camera_coordinate_frame.transform(H_base_in_camera)
base_coordinate_frame = o3d.create_mesh_coordinate_frame(size=1000,
origin=[0, 0, 0])
o3d.visualization.draw_geometries([pcd])
ply_path = "/home/yumi/Share/haonan/000011_pc.ply"
o3d.write_point_cloud(ply_path, pcd, write_ascii=True)
show_obj_pc = True
if show_obj_pc:
obj_mask_path = "/home/yumi/Share/haonan/src/mask/000011/000011_000000.png"
obj_mask = cv2.imread(obj_mask_path, cv2.IMREAD_ANYDEPTH)
obj_mask = (np.asarray(obj_mask > 0)).astype("uint8")
# mask_idx = obj_mask > 0
# mask_idx = mask_idx.nonzero()
# np_depth[~mask_idx] = 0
np_depth = np_depth * obj_mask
obj_depth_raw = o3d.geometry.Image(np_depth)
obj_pc = o3d.create_point_cloud_from_depth_image(depth=obj_depth_raw,
intrinsic=intrinsic,
extrinsic=extrinsic,
depth_scale=depth_scale,
depth_trunc=depth_trunc,
stride=int(1))
obj_pc, _ = o3d.statistical_outlier_removal(obj_pc, 10, 5)
o3d.visualization.draw_geometries([obj_pc])
obj_ply_path = "/home/yumi/Share/haonan/observed_pc.ply"
o3d.write_point_cloud(obj_ply_path, pcd, write_ascii=True)