def main():
X = np.loadtxt('data/fish_target.txt')
Y = np.loadtxt('data/fish_source.txt')
fig = plt.figure()
fig.add_axes([0, 0, 1, 1])
callback = partial(visualize, ax=fig.axes[0])
reg = RigidRegistration(**{'X': X, 'Y': Y})
reg.register(callback)
plt.show()
def main():
X = np.loadtxt('data/bunny_target.txt')
# synthetic data, equaivalent to X + 1
Y = np.loadtxt('data/bunny_source.txt')
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
callback = partial(visualize, ax=ax)
reg = RigidRegistration(**{'X': X, 'Y': Y})
reg.register(callback)
plt.show()
def test_2D():
theta = np.pi / 6.0
R = np.array([[np.cos(theta), -np.sin(theta)],
[np.sin(theta), np.cos(theta)]])
t = np.array([0.5, 1.0])
Y = np.loadtxt('data/fish_target.txt')
X = np.dot(Y, R) + np.tile(t, (np.shape(Y)[0], 1))
reg = RigidRegistration(**{'X': X, 'Y': Y})
TY, (s_reg, R_reg, t_reg) = reg.register()
assert_almost_equal(1.0, s_reg)
assert_array_almost_equal(R, R_reg)
assert_array_almost_equal(t, t_reg)
assert_array_almost_equal(X, TY)
def main():
fish_target = np.loadtxt('data/fish_target.txt')
X = np.zeros((fish_target.shape[0], fish_target.shape[1] + 1))
X[:, :-1] = fish_target
fish_source = np.loadtxt('data/fish_source.txt')
Y = np.zeros((fish_source.shape[0], fish_source.shape[1] + 1))
Y[:, :-1] = fish_source
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
callback = partial(visualize, ax=ax)
reg = RigidRegistration(**{'X': X, 'Y': Y})
reg.register(callback)
plt.show()
def _match_points_with_point_cloud_registration(dot_centers_cv : np.ndarray, local_dots_to_project : np.ndarray, approximate_pose_result : PoseResult, camera_matrix : np.ndarray, distortion_coefficients : np.ndarray, matching_style = 'rigid_registration'):
"""Matches projected (from approximate pose result) with found dot center points, using Coherent Point Drift Algorithm registration between the 2D sets of points.
Returns lists of projected and found indices for matching points."""
#How to match (in 2D, in particular)? Affine transformations? etc? Match by (relative) dot-to-dot distances?
#See, for example, https://en.wikipedia.org/wiki/Point_set_registration, http://greatpanic.com/pntmatch.html, or Affine Consistency Check of Features in KLT Tracker http://cecas.clemson.edu/~stb/klt/
#Implementations can be found in https://pypi.org/project/pycpd/, ...
#Here we'll try to use RigidRegistration or AffineRegistration as an alternative to matching points directly by distance function
#While this type of approach can perform fairly well with only a rough approximate pose, we find that a multi-stage matching and pose estimation approach is more consistent.
local_dots_projected, local_dots_projected_jacobian = cv2.projectPoints(local_dots_to_project, approximate_pose_result.pose_cv.rotation_rodrigues, approximate_pose_result.pose_cv.translation, camera_matrix, distortion_coefficients)
local_dots_projected = np.squeeze(local_dots_projected)
registration_dictionary = {'X': dot_centers_cv, 'Y': local_dots_projected}
if matching_style == 'affine_registration':
registration = AffineRegistration(**registration_dictionary)
else:
registration = RigidRegistration(**registration_dictionary)
registration.register()
registration.transform_point_cloud()
dot_centers_transformed = registration.TY
match_cutoff_score = 0.9
#Registration.P has shape M, N where M is num projected, N num found (in dot_centers_cv)
projected_indices_assigned, found_indices_assigned = linear_sum_assignment(1 - registration.P)#Returns (row_indices, column_indices)
registration_match_score_mask = registration.P[projected_indices_assigned, found_indices_assigned] > match_cutoff_score
projected_indices, found_indices = projected_indices_assigned[registration_match_score_mask], found_indices_assigned[registration_match_score_mask]
additional_data = { 'dot_centers_transformed' : dot_centers_transformed, 'local_dots_projected' : local_dots_projected}
return projected_indices, found_indices, additional_data
def main():
target_file = '' # put target file name here
target = np.loadtxt(target_file)
X = np.zeros((target.shape[0], target.shape[1] + 1))
X[:, :-1] = target
source_file = '' # put source file name here
source = np.loadtxt(source_file)
Y = np.zeros((source.shape[0], source.shape[1] + 1))
Y[:, :-1] = source
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
callback = partial(visualize, ax=ax)
reg = RigidRegistration(**{'X': X, 'Y': Y})
reg.register(callback)
def test_3D():
theta = np.pi / 6.0
R = np.array([[np.cos(theta), -np.sin(theta), 0],
[np.sin(theta), np.cos(theta), 0], [0, 0, 1]])
t = np.array([0.5, 1.0, -2.0])
fish_target = np.loadtxt('data/fish_target.txt')
Y = np.zeros((fish_target.shape[0], fish_target.shape[1] + 1))
Y[:, :-1] = fish_target
X = np.dot(Y, R) + np.tile(t, (np.shape(Y)[0], 1))
reg = RigidRegistration(**{'X': X, 'Y': Y})
TY, (s_reg, R_reg, t_reg) = reg.register()
assert_almost_equal(1.0, s_reg)
assert_array_almost_equal(R, R_reg)
assert_array_almost_equal(t, t_reg)
assert_array_almost_equal(X, TY)
def main():
# fish_target = np.loadtxt('data/fish_target.txt')
ft='C:/00_work/05_src/data/frm_t/20201015155835/pcd_extracted.ply'
fs='C:/00_work/05_src/data/frm_t/20201015155844/pcd_extracted.ply'
pcdt = open3d.io.read_point_cloud(ft)
pcds = open3d.io.read_point_cloud(fs)
voxel_size=0.01
keypoints_src = pcds.voxel_down_sample(voxel_size)
keypoints_tgt = pcdt.voxel_down_sample(voxel_size)
fish_source = np.array(keypoints_src.points)
fish_target = np.array(keypoints_tgt.points)
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
callback = partial(visualize, ax=ax)
# callback = partial(save_dt)
# reg = DeformableRegistration(**{'X': fish_target, 'Y': fish_source})
reg = RigidRegistration(**{'X': fish_target, 'Y': fish_source})
# reg = AffineRegistration(**{'X': fish_target, 'Y': fish_source})
reg.register(callback)
plt.show()
def overlap_animation(reference, student, error):
# Point set registration of reference and student
transformed_student = []
for idx in range(len(reference)):
rt = RigidRegistration(X=reference[idx], Y=student[idx])
rt.register()
rt.transform_point_cloud()
transformed_student.append(np.expand_dims(rt.TY, axis=0))
student = np.concatenate(transformed_student, axis=0)
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
error_text = ax.text2D(1, 1, 'Error: 0', transform=ax.transAxes)
# There are 17 joints, therefore 16 limbs
ref_limbs = [ax.plot3D([], [], [], c='b') for _ in range(16)]
stu_limbs = [ax.plot3D([], [], [], c='r') for _ in range(16)]
limb_map = [
[0, 1], [1, 2], [2, 3], # Right leg
[0, 4], [4, 5], [5, 6], # Left leg
[0, 7], [7, 8], # Spine
[8, 14], [14, 15], [15, 16], # Right arm
[8, 11], [11, 12], [12, 13], # Left arm
[8, 9], [9, 10] # Neck
]
def update_animation(idx):
ref_frame = reference[idx]
stu_frame = student[idx]
for i in range(len(limb_map)):
ref_limbs[i][0].set_data(ref_frame[limb_map[i], :2].T)
ref_limbs[i][0].set_3d_properties(ref_frame[limb_map[i], 2])
stu_limbs[i][0].set_data(stu_frame[limb_map[i], :2].T)
stu_limbs[i][0].set_3d_properties(stu_frame[limb_map[i], 2])
if idx < len(error):
error_text.set_text('Error: {}'.format(int(error[idx])))
iterations = len(reference)
ani = animation.FuncAnimation(fig, update_animation, iterations,
interval=50, blit=False, repeat=True)
Writer = animation.writers['ffmpeg']
writer = Writer(fps=15, metadata=dict(artist='Me'), bitrate=1800)
return ani, writer
def initial_rigid_registration(segmentation_list, save_path, n_target=500, n_source=800, keep_id=4):
print "rigid registration"
# prepare the source point cloud
seg_path, identifier = segmentation_list[0]
identifier = identifier.replace(" ", "")
target_cloud = np.load(save_path + identifier + "_" + str(keep_id) + ".npy")
indices = np.random.randint(0, target_cloud.shape[0]-1, size=n_target)
target_cloud = target_cloud - np.mean(target_cloud, axis=0)
target_cloud = target_cloud[indices]
# data structures for storing transformations, correspondences
ss = []
Rs = []
ts = []
Ps = []
corresponded_sources = []
disparities = []
transformed_sources = []
# iterate over the target point clouds
for seg_path, identifier in segmentation_list[1:]:
identifier = identifier.replace(" ", "")
print '-- ' + identifier
source_cloud = np.load(save_path + identifier + "_" + str(keep_id) + ".npy")
indices_source = np.random.randint(0, source_cloud.shape[0]-1, size=n_source)
source_cloud = source_cloud - np.mean(source_cloud, axis=0)
if "LT" in identifier:
source_cloud[:,0] = -1.*source_cloud[:,0]
source_cloud = source_cloud[indices_source]
reg = RigidRegistration(**{'X': target_cloud, 'Y': source_cloud})
reg.register()
s, R, t = reg.get_registration_parameters()
P = reg.P
ss.append(s)
Rs.append(R)
ts.append(t)
Ps.append(P)
corresponded = source_cloud[np.argmax(P, axis=0), :]
corresponded_sources.append(corresponded)
transformed = s*np.dot(source_cloud, R) + t
transformed = transformed[np.argmax(P, axis=0), :]
transformed_sources.append(transformed)
disparity = compute_disparity(corresponded, transformed)
disparities.append(disparity)
transformed_sources.append(target_cloud)
all_initial = np.array(transformed_sources)
mean_shape = np.mean(all_initial, axis=0)
return ss, Rs, ts, Ps, corresponded_sources, disparities, transformed_sources, mean_shape
def rigid_registration(self, pcl_data):
"""READ BOTH POINTCLOUDS"""
self.ros_cloud = self.full_pcl
pcl_header = pcl_data.header
pointcloud_data = pcl_data
xyz_array = ros_numpy.point_cloud2.pointcloud2_to_xyz_array(
pointcloud_data)
pcd = o3d.geometry.PointCloud()
pcd.points = o3d.utility.Vector3dVector(xyz_array)
dir_path = os.path.dirname(os.path.realpath(__file__))
#print("CURRENT PATH",dir_path)
pcd_2 = o3d.io.read_point_cloud("container_big_upper.ply")
pcd_2_copy = o3d.io.read_point_cloud("container_big_upper.ply")
pcd_box = o3d.io.read_point_cloud("container_big_downsampled.ply")
upper_1, upper_2, upper_3, upper_4 = [0.23, -0.87, 0.24], [
-0.13, -0.93, 0.23
], [-0.13, -0.95, -0.53], [0.23, -0.91, -0.52]
down_1, down_2, down_3, down_4 = [0.35, -1.3,
0.27], [-0.1, -1.4, 0.26
], [-0.093, -1.4, -0.54
], [0.34, -1.4, -0.53]
octapoint_array = [
upper_1, upper_2, upper_3, upper_4, down_1, down_2, down_3, down_4
]
octapoint_hull = o3d.geometry.PointCloud()
octapoint_hull.points = o3d.utility.Vector3dVector(octapoint_array)
"""PLANE SEGMENTATION"""
plane_model, inliers = pcd.segment_plane(distance_threshold=0.01,
ransac_n=5,
num_iterations=250)
plane_model_2, inliers_2 = pcd_2.segment_plane(distance_threshold=0.01,
ransac_n=5,
num_iterations=250)
plane_model_2_copy, inliers_2_copy = pcd_2_copy.segment_plane(
distance_threshold=0.01, ransac_n=5, num_iterations=250)
"""SAVING INLIERS"""
inlier_cloud = pcd.select_down_sample(inliers)
inlier_cloud_2 = pcd_2.select_down_sample(inliers_2)
inlier_cloud_2_copy = pcd_2_copy.select_down_sample(inliers_2_copy)
"""DOWNSAMPLING POINTCLOUDS"""
downpcd = inlier_cloud
downpcd_2 = inlier_cloud_2.voxel_down_sample(voxel_size=0.1)
downpcd_2_copy = inlier_cloud_2_copy.voxel_down_sample(voxel_size=0.1)
downpcd_box = pcd_box #.voxel_down_sample(voxel_size=0.1)
"""PREPARATION OF DATA AND POINTSET REGISTRATION"""
downpcd_as_array = np.asarray(downpcd.points)
downpcd_2_as_array = np.asarray(downpcd_2.points)
downpcd_2_copy_as_array = np.asarray(downpcd_2_copy.points)
downpcd_box_as_array = np.asarray(downpcd_box.points)
pcd_box_as_array = np.asarray(pcd_box.points)
octapoint_hull_as_array = np.asarray(octapoint_hull.points)
X = downpcd_as_array
Y = downpcd_2_as_array
reg = RigidRegistration(**{'X': X, 'Y': Y})
reg.register()
#plt.close(fig)
rotation = reg.R
translation = reg.t
scale = reg.s
copied_Y = np.dot(scale, downpcd_2_copy_as_array) + translation
copied_Y = np.dot(copied_Y, rotation)
copied_box = np.dot(scale, downpcd_box_as_array) + translation
copied_box = np.dot(copied_box, rotation)
troti_box = np.dot(scale, pcd_box_as_array) + translation
troti_box = np.dot(troti_box, rotation)
octapoint = np.dot(scale, octapoint_hull_as_array) + translation
octapoint = np.dot(octapoint, rotation)
wonder_container = o3d.geometry.PointCloud()
wonder_container.points = o3d.utility.Vector3dVector(copied_Y)
wonder_container_2 = o3d.geometry.PointCloud()
wonder_container_2.points = o3d.utility.Vector3dVector(reg.TY)
woner_box = o3d.geometry.PointCloud()
woner_box.points = o3d.utility.Vector3dVector(copied_box)
troted_box = o3d.geometry.PointCloud()
troted_box.points = o3d.utility.Vector3dVector(troti_box)
octabox = o3d.geometry.PointCloud()
octabox.points = o3d.utility.Vector3dVector(octapoint)
original_translation_2 = wonder_container.get_center(
) - woner_box.get_center()
wonder_container.translate(wonder_container_2.get_center(),
relative=False)
woner_box.translate(wonder_container_2.get_center(), relative=False)
octabox.translate(wonder_container_2.get_center(), relative=False)
woner_box.translate(original_translation_2, relative=True)
woner_box.rotate([[-1, 0, 0], [0, -1, 0], [0, 0, -1]], center=True)
octabox.translate(original_translation_2, relative=True)
octabox.rotate([[-1, 0, 0], [0, -1, 0], [0, 0, -1]], center=True)
#woner_box.translate([0.05,0.0,0.0], relative=True)
"""PUBLISH RESULT IN ROS"""
numpy_open3d_box = np.asarray(woner_box.points)
self.array_to_pcd = pc2.create_cloud_xyz32(pcl_header,
numpy_open3d_box)
self.publisher_container.publish(self.array_to_pcd)
numpy_open3d = np.asarray(octabox.points)
self.scaled_polygon_pcl = pc2.create_cloud_xyz32(
pcl_header, numpy_open3d)
self.publisher.publish(self.scaled_polygon_pcl)
horizontalalignment='center',
verticalalignment='center',
transform=ax.transAxes,
fontsize='x-large')
ax.legend(loc='upper left', fontsize='x-large')
plt.draw()
plt.pause(0.001)
BUFFER_SAVE_PATH = "/Users/jamesashford/Data Store/ThompsonData/lidar_scans"
with open(f'{BUFFER_SAVE_PATH}/angle1.json') as set1:
xs, ys = json.load(set1)
X = np.array([[x, y] for (x, y) in zip(xs, ys)])
with open(f'{BUFFER_SAVE_PATH}/angle2.json') as set2:
xs, ys = json.load(set2)
Y = np.array([[x, y] for (x, y) in zip(xs, ys)])
fig = plt.figure()
fig.add_axes([0, 0, 1, 1])
callback = partial(visualize, ax=fig.axes[0])
reg = RigidRegistration(**{'X': X, 'Y': Y})
reg.register(callback)
params = reg.get_registration_parameters()
print(params)
# IT WORKS!
#TODO; Use these parameters to transform the target set
#TODO; Merge the sets by nearest neighbours
#TODO; Store the params in a way that meaningfully describes the movement
def compareMesh(src_mesh, tgt_mesh, tolerance):
src_points = src_mesh.points(copy=True)
tgt_points = tgt_mesh.points(copy=True)
#Sample points over surface of both meshes
src_samples = generateSamples(src_mesh, 2000)
tgt_samples = generateSamples(tgt_mesh, 2000)
hull_pt1, hull_pt2 = furthest_pts(src_samples)
cnt_pnt = (hull_pt1+hull_pt2)/2
src_points = src_points - cnt_pnt
src_samples = src_samples - cnt_pnt
pt_a, pt_b = furthest_pts(tgt_samples)
orig_dist = pt_dist(pt_a, pt_b)
new_dist = pt_dist(hull_pt1, hull_pt2)
const_mult = orig_dist/new_dist
src_points = src_points*const_mult
src_samples = src_samples*const_mult
vp = Plotter(interactive=0, axes=7, size='fullscreen', bg='bb')
vp.legendBC = (0.22, 0.22, 0.22)
vp.legendPos = 1
txt = Text2D("Loading Models...", pos = 8, c='gray', s=1.31)
vp += txt
tgt_pts = Points(tgt_samples, r=6, c='deepskyblue', alpha= 1).legend("Target")
vp += tgt_pts
vp.show()
txt.SetText(7, "Initiating Alignment")
src_pts = Points(src_samples, r=6, c='yellow', alpha = 1).legend("Source")
vp += src_pts
vp.show()
#Roughly align both meshes (global registration)
spacing = np.mean(distance.pdist(src_samples))
src_samples, src_points = perform_global_registration(src_samples,tgt_samples,src_points, spacing)
txt.SetText(7, "Refining Alignment")
vp.show()
#Refine mesh alignment (local registration)
cpd_ittr = 60
callback = partial(visualize, vp=vp, pts = src_pts, text = txt, max_ittr = cpd_ittr)
reg = RigidRegistration(max_iterations = cpd_ittr, **{ 'X': tgt_samples, 'Y': src_samples })
src_samples, [s,R,t] = reg.register(callback)
src_points = s*np.dot(src_points, R) + t
src_pts.points(src_samples)
vp.renderer.RemoveAllViewProps()
txt.SetText(7, "Alignment Complete")
vp.show()
for i in range(360):
if i == 60:
txt.SetText(7, "")
vp.camera.Azimuth(.75)
vp.show()
src_mesh.points(src_points)
tgt_samples = generateSamples(tgt_mesh, 6000)
txt.SetText(7,"Performing Added Surface Analysis...")
vp.show()
ratio = 2000/min(len(tgt_points), len(src_points))
spacing = 3*spacing*ratio
#Generate distances for heat map
dists = distance.cdist(src_points,tgt_samples).min(axis=1)
txt.SetText(7,"Press Q to Continue")
vp.show(interactive=1)
show_mesh(src_mesh, dists, vp, spacing/2, tolerance=tolerance)
txt2 = Text2D("Displaying Input Object 1 \n \nUse slider to isolate \ndefective/added surfaces")
vp += txt2
vp.addGlobalAxes(axtype=8, c='white')
vp.show(axes=8, interactive=1)
txt.SetText(7,"Performing Missing Surface Analysis...")
vp += txt
vp.show(interactive=0)
src_mesh.points(src_points)
src_samples = generateSamples(src_mesh, 6000)
#Generate distances for heat map
dists = distance.cdist(tgt_points,src_samples).min(axis=1)
show_mesh(tgt_mesh, dists, vp, spacing/2, tolerance=tolerance)
txt2.SetText(2,"Displaying Input Object 2 \n \nUse slider to isolate \ndefective/missing surfaces")
vp += txt2
vp.addGlobalAxes(axtype=8, c='white')
vp.show(axes=8, interactive=1)