def run(self):
input_fileset = self.input().get()
point_cloud_file = input_fileset.get_file("pointcloud")
point_cloud = db_read_point_cloud(point_cloud_file)
points, triangles = cgal.poisson_mesh(np.asarray(point_cloud.points),
np.asarray(point_cloud.normals))
mesh = TriangleMesh()
mesh.vertices = Vector3dVector(points)
mesh.triangles = Vector3iVector(triangles)
output_fileset = self.output().get()
triangle_mesh_file = output_fileset.get_file("mesh", create=True)
db_write_triangle_mesh(triangle_mesh_file, mesh)
return (0, 0, 1)
if x < 15:
return (0, 1, 0)
else:
return (1, 0, 0)
first_px = get_first_plan(pc, "BreR")
ply = get_first_surface(first_px)
ply.compute_vertex_normals()
origin = TriangleMesh.create_coordinate_frame(size=100, origin=[0, 0, 0])
# open3d.draw_geometries([ply, origin])
pcd = PointCloud()
pcd2 = PointCloud()
pcd.points = ply.vertices
xyz = np.asarray(pcd.points) + 10
pcd2.points = Vector3dVector(xyz)
# open3d.draw_geometries([pcd, pcd2])
start = time.time()
diff = pcd2.compute_point_cloud_distance(pcd)
end = time.time()
print(f"The difference calculation was computed in {end - start} seconds.")
print(f"The min and max differences are {min(diff)} and {max(diff)}")
ply.vertex_colors = Vector3dVector(np.array(list(map(mag_to_color, diff))))
draw_geometries([ply, pcd2, origin], width=1080, height=640)
def run_pairwise_registration(i, j, visualize=False, icp_refine=False):
model_depth = cv2.imread(DEPTH_FILES[i], cv2.IMREAD_ANYCOLOR | cv2.IMREAD_ANYDEPTH)
scene_depth = cv2.imread(DEPTH_FILES[j], cv2.IMREAD_ANYCOLOR | cv2.IMREAD_ANYDEPTH)
model = convert_depth_to_pcloud(model_depth)
scene = convert_depth_to_pcloud(scene_depth)
pcloud_model = PointCloud()
pcloud_model.points = Vector3dVector(model)
pcloud_scene = PointCloud()
pcloud_scene.points = Vector3dVector(scene)
if visualize:
pcloud_model.paint_uniform_color([1, 0, 0]) # red
pcloud_scene.paint_uniform_color([0, 1, 0]) # green
draw_geometries([pcloud_model, pcloud_scene])
down_model = voxel_down_sample(pcloud_model, voxel_size=0.065)
down_scene = voxel_down_sample(pcloud_scene, voxel_size=0.065)
model_pts = np.array(down_model.points)
scene_pts = np.array(down_scene.points)
res = run_rigid_pairwise(BINARY_FULLPATH, model_pts, scene_pts, CONFIG_FILE)
# http://www.boris-belousov.net/2016/12/01/quat-dist/
print("Transformation estimated by gmmreg:")
print(res[1])
gt = np.dot(np.linalg.inv(GT_POS[j]), GT_POS[i])
print("Transformation from ground truth:")
print(gt)
theta_before = np.arccos((np.trace(gt[:3,:3]) - 1) * 0.5)
print("pose difference (in degrees) before alignment:", theta_before * 180 / np.pi)
R = np.dot(gt[:3,:3].T, res[1][:3,:3])
theta_after = np.arccos((np.trace(R) - 1) * 0.5)
print("pose difference (in degrees) after alignment:", theta_after * 180 / np.pi)
transformed = np.loadtxt(os.path.join(TMP_PATH, 'transformed_model.txt'))
pcloud_transformed = PointCloud()
pcloud_transformed.points = Vector3dVector(transformed)
pcloud_transformed.paint_uniform_color([0, 0, 1]) # blue
if visualize:
draw_geometries([pcloud_transformed, down_model, down_scene])
matrix = np.loadtxt(os.path.join(TMP_PATH, 'final_rigid_matrix.txt'))
transformed = np.dot(model, matrix[:3,:3].T) + matrix[:3, 3].T
pcloud_transformed.points = Vector3dVector(transformed)
pcloud_transformed.paint_uniform_color([0, 0, 1]) # blue
draw_geometries([pcloud_transformed, pcloud_scene])
if icp_refine:
print("Apply point-to-point ICP")
threshold = 0.02
trans_init = matrix
t1 = time.time()
reg_p2p = registration_icp(
down_model, down_scene, threshold, trans_init,
TransformationEstimationPointToPoint())
t2 = time.time()
print("ICP Run time : %s seconds" % (t2 - t1))
print(reg_p2p)
print("Transformation by ICP is:")
print(reg_p2p.transformation)
print("")
R = np.dot(gt[:3,:3].T, reg_p2p.transformation[:3,:3])
theta = np.arccos((np.trace(R) - 1) * 0.5)
print("pose difference (in degrees) after icp-refinement:", theta * 180 / np.pi)
if visualize:
draw_registration_result(pcloud_model, pcloud_scene, reg_p2p.transformation)
return res, theta_before * 180 / np.pi, theta_after * 180 / np.pi
def run(self):
fileset_masks = self.input()['masks'].get()
fileset_colmap = self.input()['colmap'].get()
scan = self.input()['colmap'].scan
pcd = db_read_point_cloud(fileset_colmap.get_file('sparse'))
poses = json.loads(fileset_colmap.get_file('images').read_text())
try:
camera = scan.get_metadata()['computed']['camera_model']
except:
camera = scan.get_metadata()['scanner']['camera_model']
if camera is None:
raise Exception("Could not find camera model for space carving")
width = camera['width']
height = camera['height']
intrinsics = camera['params'][0:4]
points = np.asarray(pcd.points)
x_min, y_min, z_min = points.min(axis=0)
x_max, y_max, z_max = points.max(axis=0)
center = [(x_max + x_min) / 2, (y_max + y_min) / 2,
(z_max + z_min) / 2]
widths = [x_max - x_min, y_max - y_min, z_max - z_min]
nx = int((x_max - x_min) / self.voxel_size) + 1
ny = int((y_max - y_min) / self.voxel_size) + 1
nz = int((z_max - z_min) / self.voxel_size) + 1
origin = np.array([x_min, y_min, z_min])
sc = romiscan.cl.Backprojection([nx, ny, nz], [x_min, y_min, z_min],
self.voxel_size)
for fi in fileset_masks.get_files():
mask = fi.read_image()
key = None
for k in poses.keys():
if os.path.splitext(poses[k]['name'])[0] == fi.id:
key = k
break
if key is not None:
rot = sum(poses[key]['rotmat'], [])
tvec = poses[key]['tvec']
sc.process_view(intrinsics, rot, tvec, mask)
labels = sc.values()
idx = np.argwhere(labels == 2)
pts = index2point(idx, origin, self.voxel_size)
output = PointCloud()
output.points = Vector3dVector(pts)
output_fileset = self.output().get()
output_file = output_fileset.get_file('voxels', create=True)
db_write_point_cloud(output_file, output)
def _obj_to_open3d(obj_file, roi_file, tfm_file):
# Scan file for ps, fs for array pre-allocation
found_ps = False
found_fs = False
ps = 0
fs = 0
with open(obj_file) as obj:
while not (found_ps and found_fs):
elements = obj.readline().split()
if len(elements) > 0:
if elements[0] == "ps":
found_ps = True
ps = int(elements[1])
if elements[0] == "fs":
found_fs = True
fs = int(elements[1])
# Preallocate arrays
vertices = np.zeros((ps, 3)).astype(np.float32)
normals = np.zeros((ps, 3)).astype(np.float32)
faces = np.zeros((fs, 3)).astype(np.int32)
# indices for filling the arrays
ind_v = 0
ind_n = 0
ind_f = 0
# fill the arrays
with open(obj_file) as obj:
for line in obj.readlines():
elements = line.split()
if len(elements) > 0:
if elements[0] == "v":
n = np.array(elements[1:], np.float64)
vertices[ind_v, :] = n
ind_v += 1
if elements[0] == "vn":
n = np.array(elements[1:], np.float64)
normals[ind_n, :] = n
ind_n += 1
if elements[0] == "f":
f = [int(e.split("//")[0]) for e in elements[1:]]
n = np.array(f, np.float)
# obj face indices start at 1, ply at 0
# Using (n-1) converts between conventions
faces[ind_f, :] = n - 1
ind_f += 1
# Verify they are full
assert (
ps == ind_v
), f"The final vertex array index {ind_v-1} does not equal ps {ps}"
assert (
ps == ind_n
), f"The final normal array index {ind_v-1} does not equal ps {ps}"
assert (
fs == ind_f
), f"The final face array index {ind_f-1} does not equal fs {fs}"
# Pass matricies to Open3D.TriangleMesh() and visualize
ply = TriangleMesh()
ply.vertices = Vector3dVector(vertices)
ply.vertex_normals = Vector3dVector(normals)
ply.triangles = Vector3iVector(faces)
# Open the transformation matrix
tfm = np.loadtxt(tfm_file, dtype=float, delimiter="\t")
# Apply transformation to the mesh
ply.transform(tfm)
# Prepar normals for visualization
ply.compute_vertex_normals()
return ply