Skip to content

Commit

Permalink
format python examples
Browse files Browse the repository at this point in the history
  • Loading branch information
neka-nat committed Jul 1, 2022
1 parent 7a3d250 commit aa74a50
Show file tree
Hide file tree
Showing 58 changed files with 1,840 additions and 1,695 deletions.
29 changes: 13 additions & 16 deletions examples/python/advanced/colored_pointcloud_registration.py
Original file line number Diff line number Diff line change
Expand Up @@ -17,19 +17,17 @@ def draw_registration_result_original_color(source, target, transformation):

# draw initial alignment
current_transformation = np.identity(4)
draw_registration_result_original_color(source, target,
current_transformation)
draw_registration_result_original_color(source, target, current_transformation)

# point to plane ICP
current_transformation = np.identity(4)
print("2. Point-to-plane ICP registration is applied on original point")
print(" clouds to refine the alignment. Distance threshold 0.02.")
result_icp = cph.registration.registration_icp(
source, target, 0.02, current_transformation,
cph.registration.TransformationEstimationPointToPlane())
source, target, 0.02, current_transformation, cph.registration.TransformationEstimationPointToPlane()
)
print(result_icp)
draw_registration_result_original_color(source, target,
result_icp.transformation)
draw_registration_result_original_color(source, target, result_icp.transformation)

# colored pointcloud registration
# This is implementation of following paper
Expand All @@ -49,18 +47,17 @@ def draw_registration_result_original_color(source, target, transformation):
target_down = target.voxel_down_sample(radius)

print("3-2. Estimate normal.")
source_down.estimate_normals(
cph.geometry.KDTreeSearchParamRadius(radius=radius * 2, max_nn=30))
target_down.estimate_normals(
cph.geometry.KDTreeSearchParamRadius(radius=radius * 2, max_nn=30))
source_down.estimate_normals(cph.geometry.KDTreeSearchParamRadius(radius=radius * 2, max_nn=30))
target_down.estimate_normals(cph.geometry.KDTreeSearchParamRadius(radius=radius * 2, max_nn=30))

print("3-3. Applying colored point cloud registration")
result_icp = cph.registration.registration_colored_icp(
source_down, target_down, radius, current_transformation,
cph.registration.ICPConvergenceCriteria(relative_fitness=1e-6,
relative_rmse=1e-6,
max_iteration=iter))
source_down,
target_down,
radius,
current_transformation,
cph.registration.ICPConvergenceCriteria(relative_fitness=1e-6, relative_rmse=1e-6, max_iteration=iter),
)
current_transformation = result_icp.transformation
print(result_icp)
draw_registration_result_original_color(source, target,
result_icp.transformation)
draw_registration_result_original_color(source, target, result_icp.transformation)
25 changes: 11 additions & 14 deletions examples/python/advanced/fast_global_registration.py
Original file line number Diff line number Diff line change
Expand Up @@ -6,31 +6,28 @@
import time


def execute_fast_global_registration(source_down, target_down, source_fpfh,
target_fpfh, voxel_size):
def execute_fast_global_registration(source_down, target_down, source_fpfh, target_fpfh, voxel_size):
distance_threshold = voxel_size * 0.5
print(":: Apply fast global registration with distance threshold %.3f" \
% distance_threshold)
print(":: Apply fast global registration with distance threshold %.3f" % distance_threshold)
result = cph.registration.registration_fast_based_on_feature_matching(
source_down, target_down, source_fpfh, target_fpfh,
cph.registration.FastGlobalRegistrationOption(
maximum_correspondence_distance=distance_threshold))
source_down,
target_down,
source_fpfh,
target_fpfh,
cph.registration.FastGlobalRegistrationOption(maximum_correspondence_distance=distance_threshold),
)
return result


if __name__ == "__main__":
cph.utility.set_verbosity_level(cph.utility.Debug)

voxel_size = 0.05 # means 5cm for the dataset
source, target, source_down, target_down, source_fpfh, target_fpfh = \
prepare_dataset(voxel_size)
source, target, source_down, target_down, source_fpfh, target_fpfh = prepare_dataset(voxel_size)

start = time.time()
result_fast = execute_fast_global_registration(source_down, target_down,
source_fpfh, target_fpfh,
voxel_size)
result_fast = execute_fast_global_registration(source_down, target_down, source_fpfh, target_fpfh, voxel_size)
print("Fast global registration took %.3f sec.\n" % (time.time() - start))
print(result_fast)
print(result_fast.transformation)
draw_registration_result(source_down, target_down,
result_fast.transformation)
draw_registration_result(source_down, target_down, result_fast.transformation)
10 changes: 4 additions & 6 deletions examples/python/advanced/global_registration.py
Original file line number Diff line number Diff line change
Expand Up @@ -18,23 +18,21 @@ def preprocess_point_cloud(pcd, voxel_size):

radius_normal = voxel_size * 2
print(":: Estimate normal with search radius %.3f." % radius_normal)
pcd_down.estimate_normals(
cph.geometry.KDTreeSearchParamRadius(radius=radius_normal, max_nn=30))
pcd_down.estimate_normals(cph.geometry.KDTreeSearchParamRadius(radius=radius_normal, max_nn=30))

radius_feature = voxel_size * 5
print(":: Compute FPFH feature with search radius %.3f." % radius_feature)
pcd_feature = cph.registration.compute_shot_feature(
pcd_down, radius_feature,
cph.geometry.KDTreeSearchParamRadius(radius=radius_feature, max_nn=100))
pcd_down, radius_feature, cph.geometry.KDTreeSearchParamRadius(radius=radius_feature, max_nn=100)
)
return pcd_down, pcd_feature


def prepare_dataset(voxel_size):
print(":: Load two point clouds and disturb initial pose.")
source = cph.io.read_point_cloud("../../testdata/icp/cloud_bin_0.pcd")
target = cph.io.read_point_cloud("../../testdata/icp/cloud_bin_1.pcd")
trans_init = np.asarray([[0.0, 0.0, 1.0, 0.0], [1.0, 0.0, 0.0, 0.0],
[0.0, 1.0, 0.0, 0.0], [0.0, 0.0, 0.0, 1.0]])
trans_init = np.asarray([[0.0, 0.0, 1.0, 0.0], [1.0, 0.0, 0.0, 0.0], [0.0, 1.0, 0.0, 0.0], [0.0, 0.0, 0.0, 1.0]])
source.transform(trans_init)
draw_registration_result(source, target, np.identity(4))

Expand Down
5 changes: 3 additions & 2 deletions examples/python/advanced/iss_keypoint_detector.py
Original file line number Diff line number Diff line change
Expand Up @@ -2,8 +2,9 @@
import sys
import time
import cupoch as cph

dir_path = os.path.dirname(os.path.realpath(__file__))
sys.path.append(os.path.join(dir_path, '../misc'))
sys.path.append(os.path.join(dir_path, "../misc"))
import meshes

# Compute ISS Keypoints on Armadillo
Expand All @@ -20,4 +21,4 @@
mesh.compute_vertex_normals()
mesh.paint_uniform_color([0.5, 0.5, 0.5])
keypoints.paint_uniform_color([1.0, 0.75, 0.0])
cph.visualization.draw_geometries([keypoints, mesh])
cph.visualization.draw_geometries([keypoints, mesh])
12 changes: 6 additions & 6 deletions examples/python/advanced/kinfu.py
Original file line number Diff line number Diff line change
Expand Up @@ -3,20 +3,20 @@

if __name__ == "__main__":
camera_intrinsics = cph.camera.PinholeCameraIntrinsic(
cph.camera.PinholeCameraIntrinsicParameters.PrimeSenseDefault)
cph.camera.PinholeCameraIntrinsicParameters.PrimeSenseDefault
)
kop = cph.kinfu.KinfuOption()
kop.distance_threshold = 5.0
kinfu = cph.kinfu.KinfuPipeline(camera_intrinsics, kop)

markers = []
for i in range(5):
print("Integrate {:d}-th image into the volume.".format(i))
color = cph.io.read_image(
"../../testdata/rgbd/color/{:05d}.jpg".format(i))
depth = cph.io.read_image(
"../../testdata/rgbd/depth/{:05d}.png".format(i))
color = cph.io.read_image("../../testdata/rgbd/color/{:05d}.jpg".format(i))
depth = cph.io.read_image("../../testdata/rgbd/depth/{:05d}.png".format(i))
rgbd = cph.geometry.RGBDImage.create_from_color_and_depth(
color, depth, depth_trunc=4.0, convert_rgb_to_intensity=False)
color, depth, depth_trunc=4.0, convert_rgb_to_intensity=False
)
res = kinfu.process_frame(rgbd)
if res:
markers.append(cph.geometry.LineSet.create_camera_marker(camera_intrinsics, np.linalg.inv(kinfu.cur_pose)))
Expand Down
5 changes: 2 additions & 3 deletions examples/python/advanced/pointcloud_outlier_removal.py
Original file line number Diff line number Diff line change
Expand Up @@ -26,10 +26,9 @@ def display_inlier_outlier(cloud, ind):
cph.visualization.draw_geometries([uni_down_pcd])

print("Statistical oulier removal")
cl, ind = voxel_down_pcd.remove_statistical_outlier(nb_neighbors=20,
std_ratio=2.0)
cl, ind = voxel_down_pcd.remove_statistical_outlier(nb_neighbors=20, std_ratio=2.0)
display_inlier_outlier(voxel_down_pcd, ind)

print("Radius oulier removal")
cl, ind = voxel_down_pcd.remove_radius_outlier(nb_points=16, radius=0.05)
display_inlier_outlier(voxel_down_pcd, ind)
display_inlier_outlier(voxel_down_pcd, ind)
22 changes: 10 additions & 12 deletions examples/python/advanced/rgbd_integration.py
Original file line number Diff line number Diff line change
Expand Up @@ -5,24 +5,22 @@
if __name__ == "__main__":
camera_poses = read_trajectory("../../testdata/rgbd/odometry.log")
volume = cph.integration.ScalableTSDFVolume(
voxel_length=8.0 / 512.0,
sdf_trunc=0.04,
color_type=cph.integration.TSDFVolumeColorType.RGB8)
voxel_length=8.0 / 512.0, sdf_trunc=0.04, color_type=cph.integration.TSDFVolumeColorType.RGB8
)

for i in range(len(camera_poses)):
print("Integrate {:d}-th image into the volume.".format(i))
color = cph.io.read_image(
"../../testdata/rgbd/color/{:05d}.jpg".format(i))
depth = cph.io.read_image(
"../../testdata/rgbd/depth/{:05d}.png".format(i))
color = cph.io.read_image("../../testdata/rgbd/color/{:05d}.jpg".format(i))
depth = cph.io.read_image("../../testdata/rgbd/depth/{:05d}.png".format(i))
rgbd = cph.geometry.RGBDImage.create_from_color_and_depth(
color, depth, depth_trunc=4.0, convert_rgb_to_intensity=False)
color, depth, depth_trunc=4.0, convert_rgb_to_intensity=False
)
volume.integrate(
rgbd,
cph.camera.PinholeCameraIntrinsic(
cph.camera.PinholeCameraIntrinsicParameters.PrimeSenseDefault),
np.linalg.inv(camera_poses[i].pose))
cph.camera.PinholeCameraIntrinsic(cph.camera.PinholeCameraIntrinsicParameters.PrimeSenseDefault),
np.linalg.inv(camera_poses[i].pose),
)

print("Extract a point cloud from the volume and visualize it.")
pcd = volume.extract_point_cloud()
cph.visualization.draw_geometries([pcd])
cph.visualization.draw_geometries([pcd])
16 changes: 9 additions & 7 deletions examples/python/advanced/rgbd_integration_uniform.py
Original file line number Diff line number Diff line change
Expand Up @@ -5,7 +5,8 @@
if __name__ == "__main__":
camera_poses = read_trajectory("../../testdata/rgbd/odometry.log")
camera_intrinsics = cph.camera.PinholeCameraIntrinsic(
cph.camera.PinholeCameraIntrinsicParameters.PrimeSenseDefault)
cph.camera.PinholeCameraIntrinsicParameters.PrimeSenseDefault
)
volume = cph.integration.UniformTSDFVolume(
length=8.0,
resolution=512,
Expand All @@ -16,18 +17,19 @@
markers = []
for i in range(len(camera_poses)):
print("Integrate {:d}-th image into the volume.".format(i))
color = cph.io.read_image(
"../../testdata/rgbd/color/{:05d}.jpg".format(i))
depth = cph.io.read_image(
"../../testdata/rgbd/depth/{:05d}.png".format(i))
color = cph.io.read_image("../../testdata/rgbd/color/{:05d}.jpg".format(i))
depth = cph.io.read_image("../../testdata/rgbd/depth/{:05d}.png".format(i))
rgbd = cph.geometry.RGBDImage.create_from_color_and_depth(
color, depth, depth_trunc=4.0, convert_rgb_to_intensity=False)
color, depth, depth_trunc=4.0, convert_rgb_to_intensity=False
)
volume.integrate(
rgbd,
camera_intrinsics,
np.linalg.inv(camera_poses[i].pose),
)
markers.append(cph.geometry.LineSet.create_camera_marker(camera_intrinsics, np.linalg.inv(camera_poses[i].pose)))
markers.append(
cph.geometry.LineSet.create_camera_marker(camera_intrinsics, np.linalg.inv(camera_poses[i].pose))
)

print("Extract triangle mesh")
mesh = volume.extract_triangle_mesh()
Expand Down
2 changes: 1 addition & 1 deletion examples/python/advanced/stereo.py
Original file line number Diff line number Diff line change
Expand Up @@ -12,4 +12,4 @@
sgm = cph.imageproc.SemiGlobalMatching(params)
disp = sgm.process_frame(limg, rimg)
disp.linear_transform(255.0 / 127.0)
cph.visualization.draw_geometries([disp])
cph.visualization.draw_geometries([disp])
17 changes: 7 additions & 10 deletions examples/python/advanced/trajectory_io.py
Original file line number Diff line number Diff line change
Expand Up @@ -2,36 +2,33 @@


class CameraPose:

def __init__(self, meta, mat):
self.metadata = meta
self.pose = mat

def __str__(self):
return 'Metadata : ' + ' '.join(map(str, self.metadata)) + '\n' + \
"Pose : " + "\n" + np.array_str(self.pose)
return "Metadata : " + " ".join(map(str, self.metadata)) + "\n" + "Pose : " + "\n" + np.array_str(self.pose)


def read_trajectory(filename):
traj = []
with open(filename, 'r') as f:
with open(filename, "r") as f:
metastr = f.readline()
while metastr:
metadata = list(map(int, metastr.split()))
mat = np.zeros(shape=(4, 4))
for i in range(4):
matstr = f.readline()
mat[i, :] = np.fromstring(matstr, dtype=float, sep=' \t')
mat[i, :] = np.fromstring(matstr, dtype=float, sep=" \t")
traj.append(CameraPose(metadata, mat))
metastr = f.readline()
return traj


def write_trajectory(traj, filename):
with open(filename, 'w') as f:
with open(filename, "w") as f:
for x in traj:
p = x.pose.tolist()
f.write(' '.join(map(str, x.metadata)) + '\n')
f.write('\n'.join(
' '.join(map('{0:.12f}'.format, p[i])) for i in range(4)))
f.write('\n')
f.write(" ".join(map(str, x.metadata)) + "\n")
f.write("\n".join(" ".join(map("{0:.12f}".format, p[i])) for i in range(4)))
f.write("\n")
Loading

0 comments on commit aa74a50

Please sign in to comment.