Secure your code as it's written. Use Snyk Code to scan source code in minutes - no build needed - and fix issues immediately.
# Main
if __name__ == "__main__":
# Params setting
node_name = "read_cloud_and_pub_by_open3d"
topic_name_rgbd_cloud = rospy.get_param( "/topic_name_rgbd_cloud",
"camera/depth_registered/points")
file_folder=rospy.get_param("file_folder")
file_name=rospy.get_param("file_name")
filename = file_folder+file_name
# Set node
rospy.init_node(node_name, anonymous=True)
# Read file
open3d_cloud = open3d.read_point_cloud(filename)
rospy.loginfo("Loading cloud file from: \n " + filename)
print(open3d_cloud)
ros_cloud = convertCloudFromOpen3dToRos(open3d_cloud)
# Publish
pub = rospy.Publisher(topic_name_rgbd_cloud, PointCloud2, queue_size=10)
cnt = 0
while not rospy.is_shutdown():
rospy.sleep(1)
pub.publish(ros_cloud)
rospy.loginfo("Publish {:d}th cloud...\n".format(cnt))
cnt += 1
rospy.loginfo("this node stops: "+node_name)
print("txt: {}".format(txt_file))
print("pts: {}".format(pts_file))
with open(txt_file, "r") as txt_f, open(pts_file, "w") as pts_f:
for line in txt_f:
# x, y, z, i, r, g, b
tokens = line.split()
tokens[3] = str(int(float(tokens[3])))
line = " ".join(tokens)
pts_f.write(line + "\n")
prepend_line(pts_file, str(wc(txt_file)))
# .pts -> .pcd
print("[pts->pcd]")
print("pts: {}".format(pts_file))
print("pcd: {}".format(pcd_file))
point_cloud = open3d.read_point_cloud(pts_file)
open3d.write_point_cloud(pcd_file, point_cloud)
os.remove(pts_file)
def down_sample(
dense_pcd_path, dense_label_path, sparse_pcd_path, sparse_label_path, voxel_size
):
# Skip if done
if os.path.isfile(sparse_pcd_path) and (
not os.path.isfile(dense_label_path) or os.path.isfile(sparse_label_path)
):
print("Skipped:", file_prefix)
return
else:
print("Processing:", file_prefix)
# Downsample points
pcd = open3d.read_point_cloud(dense_pcd_path)
min_bound = pcd.get_min_bound() - voxel_size * 0.5
max_bound = pcd.get_max_bound() + voxel_size * 0.5
sparse_pcd, cubics_ids = open3d.voxel_down_sample_and_trace(
pcd, voxel_size, min_bound, max_bound, False
)
print("Number of points before:", np.asarray(pcd.points).shape[0])
print("Number of points after:", np.asarray(sparse_pcd.points).shape[0])
print("Point cloud written to:", sparse_pcd_path)
open3d.write_point_cloud(sparse_pcd_path, sparse_pcd)
# Downsample labels
try:
dense_labels = np.array(load_labels(dense_label_path))
except:
return
else:
threshold_list = [args.th]
class_list = util.get_class_list()
for cat in class_list:
os.mkdir(os.path.join(out_path, cat))
model_list = util.get_class_models(cat)
f_f = open(os.path.join(out_path, cat, "fscore.txt"), "w")
f_p = open(os.path.join(out_path, cat, "precision.txt"), "w")
f_r = open(os.path.join(out_path, cat, "recall.txt"), "w")
for i in tqdm.tqdm(range(len(model_list))):
model = model_list[i]
for v in range(util.VIEW_COUNT):
gt = open3d.read_point_cloud(os.path.join(util.POINTS_PATH, cat, model, str(v) + ".ply"))
pr = open3d.read_point_cloud(os.path.join(args.pr_path, cat, model, str(v) + ".ply"))
f_f.write(model + "_" + str(v))
f_p.write(model + "_" + str(v))
f_r.write(model + "_" + str(v))
for th in threshold_list:
f, p, r = util.calculate_fscore(gt, pr, th=th)
f_f.write(" " + str(f))
f_p.write(" " + str(p))
f_r.write(" " + str(r))
f_f.write("\n")
f_p.write("\n")
f_r.write("\n")
def show_point_cloud(self):
pcd = read_point_cloud(self.pc_file)
draw_geometries([pcd])
def read_pcd(filename):
pcd_load = open3d.read_point_cloud(filename)
xyz_load = np.asarray(pcd_load.points)
return xyz_load
sparse_points_path = os.path.join(sparse_dir, file_prefix + ".pcd")
sparse_labels_path = os.path.join(sparse_dir, file_prefix + ".labels")
dense_points_path = os.path.join(gt_dir, file_prefix + ".pcd")
dense_gt_labels_path = os.path.join(gt_dir, file_prefix + ".labels")
# Sparse points
sparse_pcd = open3d.read_point_cloud(sparse_points_path)
sparse_points = np.asarray(sparse_pcd.points)
print("sparse_pcd loaded")
# Sparse labels
sparse_labels = load_labels(sparse_labels_path)
print("sparse_labels loaded")
# Dense points
dense_pcd = open3d.read_point_cloud(dense_points_path)
dense_points = np.asarray(dense_pcd.points)
print("dense_pcd loaded")
# Dense Ground-truth labels
dense_gt_labels = load_labels(os.path.join(gt_dir, file_prefix + ".labels"))
print("dense_gt_labels loaded")
# Build voxel to label container map
map_voxel_to_label_counter = dict()
for sparse_point, sparse_label in zip(sparse_points, sparse_labels):
voxel = get_voxel(sparse_point)
if voxel not in map_voxel_to_label_counter:
map_voxel_to_label_counter[voxel] = LabelCounter()
map_voxel_to_label_counter[voxel].increment(sparse_label)
print(
"{} sparse points, {} registered voxels".format(
import open3d
import argparse
import util
open3d.set_verbosity_level(open3d.utility.VerbosityLevel.Error)
parser = argparse.ArgumentParser(description='Precision/recall visualization')
parser.add_argument('--gt', type=str, required=True)
parser.add_argument('--pr', type=str, required=True)
parser.add_argument('--th', type=float)
args = parser.parse_args()
gt = open3d.read_point_cloud(args.gt)
pr = open3d.read_point_cloud(args.pr)
if args.th is not None:
f, p, r = util.calculate_fscore(gt, pr, args.th)
print("F-score: %f, Precision: %f, Recall: %f" % (f, p, r))
util.visualize_distance(gt, pr, max_distance=args.th)
def prepare_source_and_target_rigid_3d(source_filename,
noise_amp=0.001,
n_random=500,
orientation=np.deg2rad([0.0, 0.0, 30.0]),
translation=np.zeros(3),
normals=False):
source = o3.read_point_cloud(source_filename)
source = o3.voxel_down_sample(source, voxel_size=0.005)
print(source)
target = copy.deepcopy(source)
tp = np.asarray(target.points)
np.random.shuffle(tp)
rg = 1.5 * (tp.max(axis=0) - tp.min(axis=0))
rands = (np.random.rand(n_random, 3) - 0.5) * rg + tp.mean(axis=0)
target.points = o3.Vector3dVector(np.r_[tp + noise_amp * np.random.randn(*tp.shape), rands])
ans = trans.euler_matrix(*orientation)
ans[:3, 3] = translation
target.transform(ans)
if normals:
estimate_normals(source, o3.geometry.KDTreeSearchParamHybrid(radius=0.3, max_nn=50))
estimate_normals(target, o3.geometry.KDTreeSearchParamHybrid(radius=0.3, max_nn=50))
return source, target