def remove_invalid_pcd(pcd):
"""
remove invalid in valid points that have all-zero coordinates
pcd: open3d pcd objective
"""
pcd_np = np.asarray(pcd.points) # <Nx3>
non_zero_coord = np.abs(pcd_np) > 1e-6 # <Nx3>
valid_ind = np.sum(non_zero_coord, axis=-1) > 0 #<N>
valid_ind = list(np.nonzero(valid_ind)[0])
valid_pcd = open3d.select_down_sample(pcd, valid_ind)
return valid_pcd
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
print(elapsed_time)
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(
"====================================================")
rospy.sleep(1.0)
def main():
# 加载数据
cls_id = '9'
n_data = 500
# testDataList = './datasets/BOP/BOP_Dataset/LM-O/train_pbr/trainListSplit{}_{}.txt'.format(n_data, cls_id)
testDataList = './datasets/BOP/BOP_Dataset/LM-O/BOP_test19-20/validList_{}.txt'.format(cls_id)
testDataset = BOPDataset(testDataList, cls_id)
testDataLoader = DataLoader(dataset=testDataset, batch_size=1, shuffle=False,
num_workers=6)
# 定义模型并加载参数
checkPointPath = './datasets/BOP/BOP_Dataset/LM-O/checkPoint/{}_pvn3d_best_{}.pth.tar'.format(cls_id, n_data)
model = PVN3D(
num_classes=2, pcld_input_channels=6, pcld_use_xyz=True,
num_points=20000
).cuda()
model = convert_model(model) # 搞清楚什么作用
model.cuda()
checkpoint_status = load_checkpoint(
model, None, filename=checkPointPath
)
model = nn.DataParallel(model)
# 推理
for i, data in tqdm.tqdm(
enumerate(testDataLoader), leave=False, desc="val"
):
if data:
# scene
scenePointcloud = data[2][0].numpy()
scenePointcloudO3D = o3d.geometry.PointCloud()
scenePointcloudO3D.points = o3d.Vector3dVector(scenePointcloud[:, 0:3])
color = scenePointcloud[:, 3:6]/255
scenePointcloudO3D.colors = o3d.Vector3dVector(color[:, ::-1])
# label
labels = data[5][0].numpy()
# pre
predPoints, predPointsMask = predSeg(model, data, epoch=i, obj_id=cls_id)
# mask
TP_mask = labels & predPointsMask # true positive
FN_mask = labels - TP_mask # false negative
FP_mask = predPointsMask - TP_mask # false positive
# idx
TP_idx = TP_mask.nonzero()[0]
FN_idx = FN_mask.nonzero()[0]
FP_idx = FP_mask.nonzero()[0]
# points
TP_points = o3d.select_down_sample(scenePointcloudO3D, TP_idx)
FN_points = o3d.select_down_sample(scenePointcloudO3D, FN_idx)
FP_points = o3d.select_down_sample(scenePointcloudO3D, FP_idx)
# set color
TP_points.paint_uniform_color([255, 0, 0]) # red
FN_points.paint_uniform_color([0, 255, 0]) # green
FP_points.paint_uniform_color([0, 0, 255]) #
# remove pred and target from scene
predPointsIdx = predPointsMask.nonzero()[0]
target_unpred_and_pred_points_idx = np.append(predPointsIdx, FN_idx)
scenePointcloudO3D = o3d.select_down_sample(scenePointcloudO3D, target_unpred_and_pred_points_idx, True)
print(len(TP_idx))
o3d.visualization.draw_geometries([scenePointcloudO3D, TP_points, FN_points, FP_points])
else:
print("{}th data is None!".format(i))
def pcd_down_sample(self, src, voxel_size, n_sample_points, original_idx):
# # numpy -> PointCloud
# cloud = o3d.geometry.PointCloud()
# src = src.astype(np.float32)
# cloud.points = o3d.Vector3dVector(src)
original_idx = np.array([original_idx, original_idx,
original_idx]).transpose()
color_idx = original_idx.tolist()
src.colors = o3d.Vector3dVector(color_idx)
max_bound = src.get_max_bound()
min_bound = src.get_min_bound()
# 截取物体范围内的点
max_distance = 1.5 # m
min_distance = 0.3 # m
src_tree = o3d.KDTreeFlann(src)
_, max_idx, _ = src_tree.search_radius_vector_3d([0, 0, 0],
max_distance)
_, min_idx, _ = src_tree.search_radius_vector_3d([0, 0, 0],
min_distance)
src = o3d.select_down_sample(src, max_idx)
if len(min_idx) > 0:
src_tree = o3d.KDTreeFlann(src)
_, min_idx, _ = src_tree.search_radius_vector_3d([0, 0, 0],
min_distance)
src = o3d.select_down_sample(src, min_idx, True)
_, point_idx = o3d.voxel_down_sample_and_trace(src,
voxel_size=voxel_size,
min_bound=min_bound,
max_bound=max_bound)
max_idx_in_row = np.max(point_idx, axis=1)
pcd_down = o3d.select_down_sample(src, max_idx_in_row)
# pcd_down_tree = o3d.KDTreeFlann(pcd_down)
# _, idx, _ = pcd_down_tree.search_radius_vector_3d([0,0,0], 1500)
# pcd_down = o3d.select_down_sample(pcd_down, idx)
# 如果采样后的点数大于指定值, 随机减少一定数量的
n_points = len(max_idx_in_row)
np.random.seed(666)
if n_points > n_sample_points:
n_minus = n_points - n_sample_points
n_pcd_dow = n_points
minus_idx = np.random.choice(n_pcd_dow, n_minus, replace=False)
pcd_down = o3d.select_down_sample(pcd_down, minus_idx, True)
return pcd_down
elif n_points < n_sample_points:
n_add = n_sample_points - n_points
n_cuted_points = len(src.points)
n_unsample_points = n_cuted_points - max_idx_in_row.shape[0]
if n_add < n_unsample_points:
# select unsampled points
unsample_points = o3d.select_down_sample(
src, max_idx_in_row, True)
add_idx = np.random.choice(n_unsample_points,
n_add,
replace=False)
add_points = o3d.select_down_sample(unsample_points, add_idx)
pcd_down += add_points
return pcd_down
else:
return None
else:
return pcd_down
def main():
cls = "12"
show = True
test_all = False
test_range = 10
pre_processing = False
# dataListPath = '/media/yumi/Datas/6D_Dataset/BOP_Dataste/LM-O/BOP_test19-20/validList_8.txt'
dataListPath = './BOP_Dataset/LM-O/train_pbr/trainListSplit500_{}.txt'.format(
cls)
config = Config(dataListPath, dataListPath)
ds = BOPDataset(dataListPath, cls)
ds_loader = torch.utils.data.DataLoader(ds,
batch_size=8,
shuffle=False,
num_workers=6)
time_start = time.clock()
# 没有预处理数据集的情况下,评估数据加载程序
if test_all:
for ibs, batch in enumerate(ds_loader):
data = [item.numpy() for item in batch]
rgb, pcd_down, cld_rgb_nrm, choose, cls_ids, labels = data
if test_range:
for i in range(test_range):
data = ds.__getitem__(i)
if data:
data = [item.numpy() for item in data]
rgb, pcd_down, cld_rgb_nrm, choose, cls_ids, labels = data
# n = pcd_down.size()
# print(n)
if show:
# 显示rgb
rgb1 = rgb.transpose(1, 2, 0) / 255
rgb1 = rgb1[:, :, ::-1]
plt.figure("rgb")
plt.imshow(rgb1)
plt.title("rgb")
plt.show()
# 显示法线
nrm_map = ds.bs_utils.get_normal_map(
cld_rgb_nrm[:, 6:], choose[0])
plt.figure("nrm_map")
plt.imshow(nrm_map)
plt.title("nrm_map")
plt.show()
# 显示rgb_point_map
rgb_point_map = ds.bs_utils.get_rgb_pts_map(
cld_rgb_nrm[:, 3:6], choose[0])
plt.figure("rgb_point_map")
plt.imshow(rgb_point_map)
plt.title("rgb_point_map")
plt.show()
# 显示点云 和 颜色 和 label
pcd = o3d.geometry.PointCloud()
pcd.points = o3d.Vector3dVector(cld_rgb_nrm[:, 0:3])
color = cld_rgb_nrm[:, 3:6] / 255
# color[:, [0, 2]] = color[:, [2, 0]]
pcd.colors = o3d.Vector3dVector(color[:, ::-1])
pcd.normals = o3d.Vector3dVector(cld_rgb_nrm[:, 6:])
print(pcd)
pcd_mask = labels.nonzero()
label_idx = pcd_mask[0].tolist()
target_pcd = o3d.select_down_sample(pcd, label_idx)
target_pcd.paint_uniform_color([0, 255, 0])
pcd = o3d.select_down_sample(pcd, label_idx, True)
o3d.visualization.draw_geometries([pcd, target_pcd])
print(i)
else:
print("{} data is None".format(i))
if pre_processing:
for ibs, batch in enumerate(ds_loader):
data = [item.numpy() for item in batch]
rgb, pcd_down, cld_rgb_nrm, choose, cls_ids, labels = data
# 保存cld_rgb_nrm
cld_rgb_nrm = o3d.geometry.PointCloud()
cld_rgb_nrm.points = o3d.Vector3dVector(cld_rgb_nrm[:, 0:3])
color = cld_rgb_nrm[:, 3:6]
cld_rgb_nrm.colors = o3d.Vector3dVector(color)
cld_rgb_nrm.normals = o3d.Vector3dVector(cld_rgb_nrm[:, 6:])
time_end = time.clock()
print("time:{}".format(time_end - time_start))