def move_to_right(self):
T, dist, _ = icp.icp(self.Real_P, self.Fake_P, max_iterations=20)
#assert dist < 1e-2
self.Loc[0] += T[0][2]
self.Loc[1] += T[1][2]
#print(T[0][0], T[1][0], self.Loc)
self.Argument = Calc_Arg([T[0][0], T[1][0]
]) / self.delt_arg % self.Max_Argument + 0.5
print(
"The Precise Location of the Agent is (%.0f, %.0f) with Argument %d"
% (self.Loc[0], self.Loc[1], self.Argument))
self.Loc[0] -= T[0][2]
self.Loc[1] -= T[1][2]
start = []
end = []
flag = False
for i in range(dist.shape[0]):
if dist[i] > 40:
#if i == self.Max_Argument - 1: break
self.hindrance[i] = self.Real_Radar[i]
if flag == False:
print(i)
start.append(i)
flag = True
if dist[i] <= 40 and flag:
end.append(i)
flag = False
#i+=1
start = np.array(start)
end = np.array(end)
for i in range(start.shape[0]):
print("Possible human tracks on with Argument Range %d to %d" %
(start[i], end[i]))
#second_time_optimization with hindrance available
for i in range(start.shape[0]):
st = self.Real_Radar[start[i] - 1]
ed = self.Real_Radar[end[i]]
inv = end[i] - start[i] + 1
for j in range(start[i], end[i]):
self.Real_Radar[j] = st + (ed - st) * (j - start[i] + 1) / inv
print(self.Real_Radar[j - 1])
self.arg2ind()
T, dist, _ = icp.icp(self.Real_P, self.Fake_P, max_iterations=40)
#assert dist < 1e-2
self.Loc[0] += T[0][2]
self.Loc[1] += T[1][2]
#print(T[0][0], T[1][0], self.Loc)
self.Argument = Calc_Arg([T[0][0], T[1][0]
]) / self.delt_arg % self.Max_Argument + 0.5
print(
"The Precise Location of the Agent is (%.0f, %.0f) with Argument %d"
% (self.Loc[0], self.Loc[1], self.Argument))
def get_linear_transform_matrix(ideal_pos, real_pos):
"""
To get the linear transformation matrix mapping the Bragg peaks in the data
set to their theoretical positions, using the Iterative Closest Point method.
"""
src, dst = np.array(real_pos), np.array(ideal_pos)
return icp(src, dst)[0]
def getRotMat(self):
if self.tablepnt.shape[0] == 0:
self.tablepnt = self.getTablePcd()
T, distances = icp.icp(self.temppnt, self.tablepnt, max_iterations=20)
return T, distances
def move_to_right(self):
T, dist, _ = icp.icp(self.Real_P, self.Fake_P, max_iterations= 40)
#assert dist < 1e-2
self.Loc[0] += T[0][2]
self.Loc[1] += T[1][2]
#print(T[0][0], T[1][0], self.Loc)
self.Argument = Calc_Arg([T[0][0], T[1][0]]) / self.delt_arg % self.Max_Argument + 0.5
print("The Precise Location of the Agent is (%.0f, %.0f) with Argument %d" % (self.Loc[0],
self.Loc[1],
self.Argument))
start = []
end = []
flag = False
for i in range(dist.shape[0]):
while dist[i] > 40:
if i == self.Max_Argument - 1: break
self.hindrance[i] = self.Real_Radar[i]
if flag == False:
flag = True
start.append(i)
if dist[i + 1] <= 40:
end.append(i + 1)
flag = False
break
i+=1
print("Possible human tracks on with Argument Range %d to %d" % (start[0], end[0]))
def fit_from_candidates(node_array, pdb_array, max_iterations, req_tolerance,
num_candidates):
"""
This program uses an ICP (iterative closest point) algorithm to get a
transformation matrix that will align the FFEA structure to a PDB. However,
this algorithm tends to get stuck in local minima. This function will
randomly rotate the FFEA structure and run the algorithm, and output
the rotation and the following translation for the result with the lowest
RMSD.
"""
candidates = {}
for candidate in range(num_candidates):
print_progress_bar("Finding optimal alignment", num_candidates,
candidate)
temp_node_array = copy.copy(node_array)
XYZ = [
random.random() * 2 * np.pi,
random.random() * 2 * np.pi,
random.random() * 2 * np.pi
]
rot_euler(temp_node_array, XYZ)
T, distances = icp.icp(temp_node_array,
pdb_array,
max_iterations=max_iterations,
tolerance=req_tolerance)
candidates[np.average(distances)] = [T, XYZ]
return min(candidates.items())
def getRotMat(self):
if self.tablepnt.shape[0] == 0:
self.tablepnt = self.getTablePcd()
T, distances = icp.icp(self.temppnt, self.tablepnt, max_iterations=20)
return T, distances
def test(dist_thres, percent):
# Reading some data
scanList = datasets.read_u2is(56)
scanOriginal = scanList[55]
scanTruePose = np.array(
[0.3620, 0.0143,
0.0483]) # Manual estimation for scan 55 of u2is dataset
# Initialise error log
nb_test = 10
poseError = np.zeros((3, nb_test))
time_start = time.process_time()
for a in range(nb_test):
idref = np.random.randint(50)
refscan = scanList[idref]
# Generate random displacement and applies it to the second scan
randT = np.random.rand(2, 1) - 0.5
randR = 0.6 * np.random.rand(1, 1) - 0.3
R = np.array([[math.cos(randR), -math.sin(randR)],
[math.sin(randR), math.cos(randR)]])
scan = datasets.transform_scan(scanOriginal, R, randT)
# # Displays initial positions
plt.cla()
# for stopping simulation with the esc key.
plt.gcf().canvas.mpl_connect(
'key_release_event',
lambda event: [exit(0) if event.key == 'escape' else None])
plt.plot(refscan["x"], refscan["y"], "ob", label='Ref Scan')
plt.plot(scan["x"], scan["y"], ".r", label='Scan before ICP')
plt.axis("equal")
# perform ICP
R, t, error = icp.icp(refscan, scan, 200, 1e-7, dist_thres, percent)
# Apply motion to scan
scan = datasets.transform_scan(scan, R, t)
poseError[:, a] = np.transpose(scan["pose"] - scanTruePose)
# # Display
plt.axis("equal")
plt.plot(scan["x"], scan["y"], ".g", label='Scan after ICP')
plt.legend()
plt.pause(0.1)
time_elapsed = time.process_time() - time_start
tErrors = np.sqrt(np.square(poseError[0, :]) + np.square(poseError[1, :]))
oErrors = np.sqrt(np.square(poseError[2, :]))
print("Mean (var) translation error : {:e} ({:e})".format(
np.mean(tErrors), np.var(tErrors)))
print("Mean (var) rotation error : {:e} ({:e})".format(
np.mean(oErrors), np.var(oErrors)))
print("Mean computation time : {:f}".format(time_elapsed / nb_test))
print("Press Q in figure to finish...")
plt.show()
return np.mean(tErrors), np.var(tErrors), np.mean(oErrors), np.var(
oErrors), time_elapsed / nb_test
def scale_transformation(car_points_3d,keypoints):
#car_points_3d_center = [[0],[0],[0],[1]]
#for
# print(np.mean(car_points_3d))
# get the overlapping key points
points_mesh = []
points_ours = []
for ind,points in enumerate(car_points_3d):
if len(points) < 4 or ind == 8 or ind == 9:
#print(keypoints[ind])
#print(points)
ind = ind
else:
points_ours.append(points[0:3].reshape(1,3))
points_mesh.append(keypoints[ind].astype(np.float))
#print(points_ours)
#print(points_mesh)
# find the scale between the mesh and the triangulated points
scale_sum = 0
coun = 0
scale =[0] * 100
for indices,points_3d in enumerate(points_ours):
for indices_2,points_3d_2 in enumerate(points_ours):
if indices_2 > indices and np.abs(np.mean(points_3d - points_3d_2))/np.abs(np.mean(points_mesh[indices] - points_mesh[indices_2])) < 5:
scale[coun] = np.abs(np.mean(points_3d - points_3d_2))/np.abs(np.mean(points_mesh[indices] - points_mesh[indices_2]))
coun += 1
# ransac compute transformation
scale_all = scale[0:coun]
ransac_max = 10000
scale_best = 1
RT_transform_best = np.zeros([4,4])
for i,scale_check in enumerate(scale_all):
#scale_check = np.mean(scale_all)
points_ours_icp = np.array([[0,0,0]] * len(points_ours)).astype(np.float)
points_mesh_icp = np.array([[0,0,0]] * len(points_ours)).astype(np.float)
for indices,points_3d in enumerate(points_ours):
points_ours_icp[indices] = points_ours[indices]
points_mesh_icp[indices] = points_mesh[indices]*scale_check
try:
RT_transform,distance = icp(points_mesh_icp,points_ours_icp, None, 200, 0.0001)
icp_error = 0
for k,asa in enumerate(points_mesh_icp):
icp_error += np.mean((np.dot(RT_transform,np.transpose(np.append(points_mesh_icp[k],1)))[0:3] - points_ours_icp[k])**2)
if ransac_max > icp_error:#np.mean(distance) :
#print(points_mesh_icp)
#print(points_ours_icp)
#print(icp_error)
ransac_max = icp_error
RT_transform_best = RT_transform
scale_best = scale_check
except:
print('icp failed')#scale = print(points_3d)
# print(np.dot(RT_transform_best,np.transpose(np.append(points_mesh_icp[k],1)))[0:3])
# print(points_ours_icp[k])
return scale_best,RT_transform_best
def test_plot_icp_cluster2cluster(laser_a, laser_b):
# source: a.
mst_a = EMST(laser_a)
clusters_a = Cluster(mst_a, k=1.0, min_size=10)
# destination: b.
mst_b = EMST(laser_b)
clusters_b = Cluster(mst_b, k=1.0, min_size=10)
# icp
dst_indices, T = icp(laser_a.cartesian,
laser_b.cartesian,
init_pose=None,
max_iterations=20,
tolerance=0.001)
matched_cluster_indices, _ = match_clusters(dst_indices, laser_a, laser_b,
clusters_a, clusters_b)
# plot
plt.figure()
# colors to iterate over
colors_a = ['r', 'y'] * 20
colors_b = ['b', 'g'] * 20
# plot source clusters and arrows to destination
count = 1
for cluster, color, cluster_b_index in zip(clusters_a.clusters, colors_a,
matched_cluster_indices):
plt.scatter(laser_a.cartesian[cluster, [0]],
laser_a.cartesian[cluster, [1]],
s=10.0,
c=color,
linewidths=0)
if cluster_b_index >= 0:
# plot arrow points to the corresponding cluster
x1 = np.mean(laser_a.cartesian[cluster, [0]])
y1 = np.mean(laser_a.cartesian[cluster, [1]])
cluster_b = clusters_b.clusters[cluster_b_index]
x2 = np.mean(laser_b.cartesian[cluster_b, [0]])
y2 = np.mean(laser_b.cartesian[cluster_b, [1]])
dx = x2 - x1
dy = y2 - y1
plt.arrow(x1, y1, dx, dy, width=0.001, head_width=0.05)
plt.text((x1 + x2) / 2.0, (y1 + y2) / 2.0, str(count), fontsize=18)
count += 1
# plot destination clusters
for cluster, color in zip(clusters_b.clusters, colors_b):
plt.scatter(laser_b.cartesian[cluster, [0]],
laser_b.cartesian[cluster, [1]],
s=10.0,
c=color,
linewidths=0)
plt.show()
def compare_maps(path1, path2):
points1 = get_coordinates(path1)
points2 = get_coordinates(path2)
n_samples = int(min(len(points1), len(points2)) * 0.7) # use 70% of the smallest map for sampling
sp1, sp2 = sample_arrays(np.asarray(points1), np.asarray(points2), n_samples)
T, distances, iters = icp.icp(sp1, sp2)
score = np.sqrt(np.mean(distances ** 2))
print("Map score: " + str(score) + " (took " + str(iters) + "iterations)")
def main():
# Each point is a row vector
data = read_pc_from_file('images/depthImage1ForHW.txt')
model = read_pc_from_file('images/depthImage2ForHW.txt')
# Toy PCs for debugging
# x = np.linspace(0, 2*np.pi, 100)
# y = np.zeros(100)
# z1 = np.sin(x)
# z2 = np.sin(x) + 0.2
# data = np.vstack((x,y,z1)).transpose()
# model = np.vstack((x,y,z2)).transpose()
# Plot the two orginal point clouds
fig = plt.figure()
ax = fig.gca(projection='3d')
ax.scatter(data[:, 0],
data[:, 1],
data[:, 2],
c='b',
marker='.',
edgecolor='none',
depthshade=False)
ax.scatter(model[:, 0],
model[:, 1],
model[:, 2],
c='r',
marker='.',
edgecolor='none',
depthshade=False)
ax.view_init(elev=16, azim=-52)
# plt.savefig('./images/before.png')
plt.show()
aligned = icp(data, model, 20)
fig = plt.figure()
ax = fig.gca(projection='3d')
ax.scatter(aligned[:, 0],
aligned[:, 1],
aligned[:, 2],
c='b',
marker='.',
edgecolor='none',
depthshade=False)
ax.scatter(model[:, 0],
model[:, 1],
model[:, 2],
c='r',
marker='.',
edgecolor='none',
depthshade=False)
ax.view_init(elev=16, azim=-52)
# plt.savefig('./images/after.png')
plt.show()
def test_icp():
# Generate a random dataset
#A = np.random.rand(N, dim)
A = np.array([])
# Generate a random dataset
plydata = PlyData.read('bun045.ply')
# Generate a random dataset
for j in range(0, plydata.elements[0].count):
A = np.append(A, plydata['vertex'][j][0])
A = np.reshape(A, (plydata.elements[0].count, 1))
total_time = 0
B = np.copy(A)
# Translate
t = np.random.rand(dim) * translation
B += t
# Rotate
R = rotation_matrix(np.random.rand(dim), np.random.rand() * rotation)
B = np.dot(R, B.T).T
# Add noise
#B += np.random.randn(N, dim) * noise_sigma
# Shuffle to disrupt correspondence
np.random.shuffle(B)
# Run ICP
start = time.time()
T, distances, iterations = icp.icp(B, A, None, num_tests, tolerance)
total_time += time.time() - start
# Make C a homogeneous representation of B
C = np.ones((A.size, 2))
C[:, 0:1] = np.copy(B)
# Transform C
C = np.dot(T, C.T).T
assert np.mean(distances) < 6 * noise_sigma # mean error should be small
assert np.allclose(T[0:1, 0:1].T, R,
atol=6 * noise_sigma) # T and R should be inverses
assert np.allclose(-T[0:1, 1], t,
atol=6 * noise_sigma) # T and t should be inverses
#print('icp time: {:.3}'.format(total_time/num_tests))
print('T_ICP\n', T)
print('R_ICP\n', R)
return
def refine(pose, cloud_observed, cloud_model, p_distance=0.1, iterations=1):
obj_T = pose.copy()
cloud_estimated = np.dot(cloud_model, obj_T[:3, :3].T) + obj_T[:3, 3].T
if cloud_estimated.shape[0] == 0 or cloud_observed.shape[0] == 0:
return obj_T
T = icp.icp(cloud_observed, cloud_estimated, iterations, p_distance)
obj_T = T @ obj_T
return obj_T
def test_icp():
# Generate a random dataset
A = np.random.rand(N, dim)
total_time = 0
for i in range(num_tests):
B = np.copy(A)
# Translate
t = np.random.rand(dim) * translation
B += t
# Rotate
#R = rotation_matrix(np.random.rand(dim), np.random.rand() * rotation)
#B = np.dot(R, B.T).T
# Add noise
B += np.random.randn(N, dim) * noise_sigma
# Shuffle to disrupt correspondence
np.random.shuffle(B)
# Run ICP
start = time.time()
T, distances, iterations, r, t2 = icp.icp(B,
A,
max_iterations=20,
tolerance=0.000001)
total_time += time.time() - start
# Make C a homogeneous representation of B
C = np.ones((N, dim + 1))
C[:, 0:dim] = np.copy(B)
# Transform C
C = np.dot(T, C.T).T
print('icp time: {:.3}'.format(total_time / num_tests))
print(r)
print('\n')
print(t2)
fig = plt.figure()
ax = fig.add_subplot(111)
ax.scatter([x[0] for x in A], [y[1] for y in A], c='red')
ax.scatter([x[0] for x in B], [y[1] for y in B], c='blue')
ax.scatter([x[0] for x in C], [y[1] for y in C], c='green')
plt.show()
return
def test_icp(A, shuffle = 1):
# Generate a random dataset
for i in range(num_tests):
print('test', i)
B = np.copy(A)
# Translate
t = np.random.rand(dim)*translation
B += t
# Rotate
R = rotation_matrix(np.random.rand(dim), np.random.rand() * rotation)
B = np.dot(R, B.T).T
# Add noise
B += np.random.randn(N, dim) * noise_sigma
# Shuffle to disrupt correspondence
if (shuffle == 1):
np.random.shuffle(B)
#B[0:100] = 0
#Bcenter = rmsd.centroid(B)
#B[0:100] = Bcenter
print("Normal squared_distance", squared_distance(A, B))
plot3d(A, B)
# Run ICP
A -= rmsd.centroid(A)
B -= rmsd.centroid(B)
T, distances, iterations = icp.icp(B, A, tolerance=0.000001)
print(T.shape)
# Make C a homogeneous representation of B
C = np.ones((N, 4))
C[:,0:3] = np.copy(B)
# Transform C
C = np.dot(T, C.T).T
C = C[:, 0:3]
print("Rotated squared_distance", squared_distance(A, C))
plot3d(A, C)
#assert np.mean(distances) < 6*noise_sigma # mean error should be small
#assert np.allclose(T[0:3,0:3].T, R, atol=6*noise_sigma) # T and R should be inverses
#assert np.allclose(-T[0:3,3], t, atol=6*noise_sigma) # T and t should be inverses
return
def test_icp():
# Generate a random dataset
A = np.random.rand(N, dim)
total_time = 0
for i in range(num_tests):
B = np.copy(A)
# Translate
t = np.random.rand(dim) * translation
B += t
# Rotate
R = rotation_matrix(np.random.rand(dim), np.random.rand() * rotation)
B = np.dot(R, B.T).T
# Add noise
B += np.random.randn(N, dim) * noise_sigma
# Shuffle to disrupt correspondence
np.random.shuffle(B)
# Run ICP
start = time.time()
T, distances, iterations = icp.icp(B,
A,
tolerance=0.000001,
use_colors=False)
total_time += time.time() - start
# Make C a homogeneous representation of B
C = np.ones((N, 4))
C[:, 0:3] = np.copy(B)
# Transform C
C = np.dot(T, C.T).T
assert np.mean(
distances) < 6 * noise_sigma # mean error should be small
assert np.allclose(T[0:3, 0:3].T, R,
atol=6 * noise_sigma) # T and R should be inverses
assert np.allclose(-T[0:3, 3], t,
atol=6 * noise_sigma) # T and t should be inverses
print('icp time: {:.3}'.format(total_time / num_tests))
print(T)
return
def on_laser_data(data):
global compass, last_laser_data, current_position
points = []
for i, distance in enumerate(data.ranges):
angle = data.angle_min + i * data.angle_increment
dy = sin(angle) * distance
dx = cos(angle) * distance
points.append((dx, dy))
B = np.array(points)
if last_laser_data is not None:
T, _, _ = icp(B,
last_laser_data,
max_iterations=100,
tolerance=0.000001)
current_position = np.matmul(current_position, T)
if MIMIC_COMPASS:
stamp = data.header.stamp
if len(compass) == 0:
time.sleep(
0.001
) #XXX: wait for position to arrive, is there a yield in python?
_, theta = min(compass, key=lambda x: abs(cmp_stamp(x[0], stamp)))
compass = [x for x in compass if cmp_stamp(x[0], stamp) > 0]
current_position[0][0] = cos(theta)
current_position[0][1] = -sin(theta)
current_position[1][0] = sin(theta)
current_position[1][1] = cos(theta)
else:
theta = atan2(current_position[1][0], current_position[0][0])
p = PoseStamped()
p.header.frame_id = "pose"
p.header.stamp = data.header.stamp
p.pose.position.x = current_position[0][2]
p.pose.position.y = current_position[1][2]
p.pose.position.z = 0
p.pose.orientation.w = cos(theta / 2.0)
p.pose.orientation.x = 0
p.pose.orientation.y = 0
p.pose.orientation.z = sin(theta / 2.0)
path.append(p)
pub.publish(p)
last_laser_data = B
def icp_show(self):
b=np.array(self.icp_medal)
a=np.array(self.icp_data)
M2 = icp.icp(a, b, [0.1, 0.33, np.pi / 2.2], 30)
# Plot the result
src = np.array([a.T]).astype(np.float32)
res = cv2.transform(src, M2)
plt.figure()
# plt.plot(b[0], b[1], 'g')
# plt.plot(res[0].T[0], res[0].T[1], 'r.')
# plt.plot(a[0], a[1], 'b')
plt.scatter(b[0],b[1])
plt.scatter(res[0].T[0], res[0].T[1])
plt.scatter(a[0], a[1])
plt.show()
def test_plot_icp_point2point(a, b):
''' Deprecated
Need to change icp code (last few lines) in order to work
'''
#
A = a.cartesian
B = b.cartesian
# ICP
dst_indices, T, distances, iterations = icp(A,
B,
init_pose=None,
max_iterations=20,
tolerance=0.001)
A_matched = A[src_indices, :]
B_matched = B[dst_indices, :]
# transform A according to T
m = A.shape[1]
A_homo = np.ones((m + 1, A.shape[0]))
A_homo[:m, :] = np.copy(A.T)
A_homo_after = np.dot(T, A_homo)
A_after = A_homo_after[:m, :].T
# print results
print('Homogeneous Transformation Matrix: ' + str(T))
print('Number of iterations: %f' % iterations)
# plot setup
plt.figure()
plt.title('Result of Iterative Closest Point')
# plot scan
plt.scatter(A[:, 0], A[:, 1], c='r', linewidths=0, s=300)
plt.scatter(B[:, 0], B[:, 1], c='g', linewidths=0, s=300)
plt.scatter(A_after[:, 0], A_after[:, 1], c='b', linewidths=0, s=300)
plt.scatter(A_matched[:, 0], A_matched[:, 1], c='w', linewidths=0, s=100)
plt.scatter(B_matched[:, 0], B_matched[:, 1], c='w', linewidths=0, s=100)
# plot lines between matching points
for a, b in zip(A_matched, B_matched):
x = a[0]
y = a[1]
dx = b[0] - a[0]
dy = b[1] - a[1]
plt.arrow(x, y, dx, dy, width=0.0001, head_width=0.005)
plt.show()
def single_line_fit(skyline_r, skyline_q):
dim = 2 # number of dimensions of the points
noise_sigma = 10 # standard deviation error to be added
translation = 100 # max translation of the test set
rotation = .1 # max rotation (radians) of the test set
# image_base = np.zeros((4800, 640, 3), dtype=np.uint8)
new_points = np.array(list(zip(range(len(skyline_q)), skyline_q)),
dtype=np.float32)
t = np.random.rand(dim) * translation
R = rotation_matrix()
new_points += t
new_points = np.dot(R, new_points.T).T
# linePoints(image_base, new_points, size=2, color=(0, 0, 255))
skyline = skyline_r
length = len(skyline)
ref_points = np.array(list(zip(range(length), skyline)), dtype=np.float32)
# linePoints(image_base, ref_points, size=2, color=(0, 255, 0))
# cv2.imshow('image', image_base)
# cv2.waitKey(0)
fit_points = np.copy(new_points)
for i in range(10):
# image = np.copy(image_base)
r, t, err, indices = icp.icp(fit_points,
ref_points,
max_iterations=1,
tolerance=0.8,
index=True)
# for p1, p2 in zip(fit_points, ref_points[indices]):
# cv2.line(image, (int(p1[0]), int(p1[1])), (int(p2[0]), int(p2[1])), (0, 0, 255), 1)
# cv2.circle(image, (int(p1[0]), int(p1[1])), 1, (255, 0, 255), 1)
# linePoints(image, fit_points, size=2, color=(255, 0, 255))
fit_points = np.dot(r, fit_points.T).T + t
# cv2.putText(image, 'err = {}'.format(err), (20, 30), 2, cv2.FONT_HERSHEY_PLAIN, (255, 0, 0))
# cv2.imshow('image', image)
# cv2.waitKey(max(int(err) * 5, 10))
if err < 0.8:
break
# print 'r = {}\nt = {}\nmean_err = {}'.format(r, t, err)
# print 'Done.'
# cv2.imshow('image', image)
# cv2.waitKey(0)
return err
def test_icp():
# Generate a random dataset
A = np.random.rand(N, dim)
print ("(#pts, dimen, num_test) = ({}, {}, {})".format(N, dim, num_tests))
print ("Noise added in tests: ")
print ("(sigma, translation, rotation) = ({}, {}, {})".format(noise_sigma, translation, rotation))
total_time = 0
for i in range(num_tests):
B = np.copy(A)
# Translate
t = np.random.rand(dim)*translation
B += t
# Rotate
R = rotation_matrix(np.random.rand(dim), np.random.rand() * rotation)
B = np.dot(R, B.T).T
# Add noise
B += np.random.randn(N, dim) * noise_sigma
# Shuffle to disrupt correspondence
np.random.shuffle(B)
# Run ICP
start = time.time()
T, distances, iterations = icp.icp(B, A, tolerance=0.000001)
total_time += time.time() - start
# Make C a homogeneous representation of B
C = np.ones((N, 4))
C[:,0:3] = np.copy(B)
# Transform C
C = np.dot(T, C.T).T
if i % 10 == 0:
print ("result round {} mean error: {}".format(i, np.mean(distances)))
assert np.mean(distances) < 6*noise_sigma # mean error should be small
assert np.allclose(T[0:3,0:3].T, R, atol=6*noise_sigma) # T and R should be inverses
assert np.allclose(-T[0:3,3], t, atol=6*noise_sigma) # T and t should be inverses
print('icp time: {:.3}'.format(total_time/num_tests))
return
def label(templates: np.array, samples: np.array) -> np.array:
"""Use ICP to classify all samples."""
labels = None
for sample in samples:
results = [icp(sample, template) for template in templates]
distances = [np.sum(distance) for _, _, distance in results]
i = int(np.argmin(distances))
T, s, _ = results[i] # T (4x4 homogenous transformation)
label = np.hstack(
(i, distances[i], np.ravel(T), s, distances)).reshape((1, -1))
print(' * [Info] Final distance:', distances[i], 'Label:',
template_index_to_name[i])
labels = label if labels is None else np.vstack((labels, label))
if labels is None:
raise UserWarning('No samples found.')
labels[:, 1] = 1 - (labels[:, 1] / np.max(labels[:, 1]))
return labels
def test_match(map_name, image_name, tolerance):
im_model_raw = cv2.imread(map_name, cv2.IMREAD_GRAYSCALE)
_, im_model = cv2.threshold(
im_model_raw, 254, 255, cv2.THRESH_BINARY)
im_raw = cv2.imread(image_name, cv2.IMREAD_GRAYSCALE)
_, im = cv2.threshold(im_raw, 1, 255, cv2.THRESH_BINARY)
points_model = read_image(im_model)
points = read_image(im)
np.random.shuffle(points_model)
extension = points.shape[0] - points_model.shape[0]
points_model_upsampled = np.append(
points_model, points_model[0:extension, :], axis=0)
T, distances, iterations = icp.icp(
points, points_model_upsampled, tolerance=tolerance)
# Make C a homogeneous representation of B
points_transformed = np.ones((points.shape[0], 4))
points_transformed[:, 0:3] = points
# Transform C
points_transformed = np.dot(T, points_transformed.T).T
print('Transformation Matrix:', T)
print('Point distances:', distances)
print('Pixel squared error:', np.average(distances ** 2))
print('Number of iterations:', iterations)
plt.plot(points_model[:, 0], points_model[:, 1], 'ro',
points_transformed[:, 0], points_transformed[:, 1], 'bo')
plt.axis('equal')
plt.show()
def main_icp(A, B):
# takes point arrays A & B
# Run ICP
# start = time.time()
T, distances, iterations = icp.icp(B, A, tolerance=0.11)
# total_time = time.time() - start
# Make C a homogeneous representation of B
C = np.ones((len(B), 4))
C[:, 0:3] = np.copy(B)
print T
sf = 6.125 # timescale factor
S = np.array([[1.0, sf, sf, sf], [sf, 1.0, sf, sf], [sf, sf, 1.0, sf],
[0.0, 0.0, 0.0, 1.0]])
# print S
# print S*T
# print T*S
# Transform C
C = np.dot(T, C.T).T
# C = np.dot((T*S), C.T).T
# Test results
noise_sigma = 0.1
# assert np.mean(distances) < 6*noise_sigma # mean error should be small
print "mean distances:"
print np.mean(distances)
# assert np.allclose(C[:,[0,1,2]], B, atol=6*noise_sigma)
print "number of iterations:"
print iterations
return list(C[:, [0, 1, 2]])
def test_icp():
A = np.random.rand(N, dim)
total_time = 0
for i in range(num_tests):
B = np.copy(A)
#Applying Random transformation
# Translate
t = np.random.rand(dim)*translation
B += t
# Rotate
R = rotation_matrix(np.random.rand(dim), np.random.rand()*rotation)
B = np.dot(R, B.T).T
# Add noise
B += np.random.randn(N, dim) * noise_sigma
# Shuffle to disrupt correspondence
np.random.shuffle(B)
# Run ICP
start = time.time()
T,_,_ = icp.icp(B, A, tolerance=0.000001)
total_time += time.time() - start
# Make C a homogeneous representation of B
C = np.ones((N, 4))
C[:,0:3] = B
# Transform C
C = np.dot(T, C.T).T
print('icp fit time: {:.3}'.format(total_time/num_tests))
def callback(data):
global cluster_pointcloud, dock_pose, T, find_dock
angle_min = data.angle_min
angle_max = data.angle_max
angle_increment = data.angle_increment
# rospy.loginfo("scan type, min angle: %f, max_angle: %f, increment: %f" % (angle_min, angle_max, angle_increment))
# rospy.loginfo("get scan data: %d points" % (len(data.ranges)))
last_range = 0
last_point = Point32()
cluster_index = 0
clouds = []
potential_clouds = []
for i in xrange(len(data.ranges)):
if (angle_min + i * angle_increment) >= min_detect_angle and (
angle_min + i * angle_increment) <= max_detect_angle:
point = Point32()
point.x = math.cos(angle_min +
i * angle_increment) * data.ranges[i]
point.y = math.sin(angle_min +
i * angle_increment) * data.ranges[i]
point.z = 0
distance = math.sqrt(
math.pow(point.x - last_point.x, 2) +
math.pow(point.y - last_point.y, 2))
if (distance > cluster_threshold):
if (len(clouds) > potential_clouds_min_points):
potential_clouds.append(clouds)
clouds = []
clouds.append(point)
last_point = point
last_range = data.ranges[i]
# add last cloud into potential_clouds
# filter cloud with points count
if (len(clouds) > potential_clouds_min_points):
potential_clouds.append(clouds)
if (len(potential_clouds) <= 0):
find_dock = False
# rospy.logwarn("can not find dock")
return
best_p_i = -1
best_distance = icp_distance_threshold
best_T = None
for p_i in xrange(len(potential_clouds)):
ideal_dock_cloud = GenerateIdealDock(1000)
ideal_dock_np = Point32ToNumpy(ideal_dock_cloud)
potential_cloud_np = Point32ToNumpy(potential_clouds[p_i])
T, distance, i = icp.icp(ideal_dock_np, potential_cloud_np)
mean_distance = np.mean(distance)
if mean_distance < icp_distance_threshold:
if (mean_distance < best_distance):
best_p_i = p_i
best_distance = mean_distance
best_T = T
if best_p_i >= 0:
qt = tf.transformations.quaternion_from_matrix(best_T)
find_dock = True
dock_pose = PoseStamped()
dock_pose.header.frame_id = "laser_link"
dock_pose.header.stamp = rospy.Time.now()
dock_pose.pose.position.x = best_T[0][3]
dock_pose.pose.position.y = best_T[1][3]
dock_pose.pose.position.z = best_T[2][3]
dock_pose.pose.orientation.x = qt[0]
dock_pose.pose.orientation.y = qt[1]
dock_pose.pose.orientation.z = qt[2]
dock_pose.pose.orientation.w = qt[3]
else:
find_dock = False
# rospy.loginfo("icp distance %f" % np.mean(distance))
# rospy.loginfo(T)
cluster_pointcloud = PointCloud()
cluster_pointcloud.header = data.header
cluster_pointcloud.header.stamp = rospy.Time.now()
color_r = ChannelFloat32('rgb', [])
color_map = [0x00ff00, 0xffff00, 0xff00ff, 0x00ffff]
for i in xrange(len(potential_clouds)):
for j in xrange(len(potential_clouds[i])):
cluster_pointcloud.points.append(potential_clouds[i][j])
color_r.values.append(color_map[i % 4])
cluster_pointcloud.channels.append(color_r)
pc_ref = np.zeros((len(ref_pts),3))
pc_cur = np.zeros((len(cur_pts),3))
for id in range(len(ref_pts)):
# z component
pc_ref[id, 2] = ref_depth[int(ref_pts[id, 1]), int(ref_pts[id, 0])]*depth_scale
pc_cur[id, 2] = cur_depth[int(cur_pts[id, 1]), int(cur_pts[id, 0])]*depth_scale
# x component
pc_ref[id, 0] = (ref_pts[id, 0]-cx)*(pc_ref[id, 2]/fx)
pc_cur[id, 0] = (cur_pts[id, 0]-cx)*(pc_cur[id, 2]/fx)
# y component
pc_ref[id, 1] = (ref_pts[id, 1]-cy)*(pc_ref[id, 2]/fy)
pc_cur[id, 1] = (cur_pts[id, 1]-cy)*(pc_cur[id, 2]/fy)
# ICP
T, distances, iterations = icp.icp(pc_cur, pc_ref, tolerance=0.000001)
ROT = T[0:3, 0:3]
TR = np.array([[T[0,3]],[T[1,3]],[T[2,3]]])
# trajectory calculation
Rot_pose = np.dot(ROT, Rot_pose)
Tr_pose = Tr_pose + np.dot(Rot_pose, TR)
# update
ref_frame = cur_frame
ref_depth = cur_depth
kp_ref = kp_cur
d_ref = d_cur
if cv2.waitKey(1) == 27:
break
tplRot = cv2.warpPerspective(tpl, M, (w,h))
tplKeyRot = cv2.warpPerspective(tplKey, M, (w,h))
tplRot2 = cv2.cvtColor(tplRot, cv2.COLOR_GRAY2BGR)
dst = cv2.addWeighted(tplRot2, 0.4, frame,0.6, 0)
score = np.sum(np.logical_and(blank, tplRot))
cv2.putText(dst, str(score), (50, 50), cv2.FONT_HERSHEY_SIMPLEX, 1, 255, 2)
pos = (0,0,0)
if matched is not None:
#dilate = cv2.dilate(matched,kernel,iterations = 2)
#mask = 255*np.logical_and(blank, dilate).astype(np.uint8)
#cv2.imshow('detected', mask)
#import pdb; pdb.set_trace()
src = np.transpose(np.nonzero(matched)).T
tgt = np.transpose(np.nonzero(blank)).T
tr, pos = icp(src, tgt, init_pose=pos, no_iterations=30)
M = np.dot(tr, M)
cv2.imshow('detected', cv2.addWeighted(tplRot, 0.6, blank, 0.4, 0))
#cv2.imshow('detected', tplKeyRot)
k = cv2.waitKey(50) & 0xFF
if k == 27: break
#matched = tplKeyRot
matched = 255*np.logical_and(blank, tplRot).astype(np.uint8)
cap.release()
#out.release()
cv2.destroyAllWindows()
cor = np.concatenate((cor, ST), axis=0)
cO = camOrds
cT = camT
testT = np.reshape([], (0, 3))
testO = np.reshape([], (0, 3, 3))
Sprev = S
preDes = des
triag = triangulate(ST)
else:
bf = cv2.BFMatcher(cv2.NORM_HAMMING, crossCheck=True)
match = bf.match(preDes[:, -(window - shift) + 2, :], des[:, 0, :])
idx = np.reshape(map(lambda x: [x.queryIdx, x.trainIdx], match),
(len(match), 2))
preIdx = idx[:, 0]
curIdx = idx[:, 1]
K, t = icp.icp(S[:, curIdx], Sprev[:, preIdx])
S = np.dot(K, S) + t
camT = (np.dot(K, camT.T) + t).T
camOrds = np.reshape(
map(lambda x: normalize(np.dot(K, x.T).T), camOrds), camOrds.shape)
ST = S.T
cO = np.concatenate((cO, camOrds), axis=0)
cT = np.concatenate((cT, camT), axis=0)
cor = np.concatenate((cor, ST), axis=0)
Sprev = S
preDes = des
tr = triangulate(ST)
triag = np.concatenate((triag, tr), axis=0)
q += 1
k += shift
xx, yy, zz = getS(ST)
my_r_unrefined = quaternion_matrix(my_r)[:3, :3]
my_t_unrefined = my_t + 0
emb = last_emb
ICP = False
if ICP:
#Refinement
new_cloud = torch.bmm((points - T), R).contiguous()
# dummy = points.squeeze().detach().cpu().numpy()
init_cloud = new_cloud.squeeze().detach().cpu().numpy().copy()
original_cloud = model_points[0].cpu().detach().numpy().copy()
# my_r = quaternion_matrix(my_r)[:3, :3]
# original_cloud_shifted = np.dot(original_cloud, my_r.T) + my_t
# from IPython import embed; embed()
delta_T, distances, iterations = icp.icp(original_cloud,
init_cloud,
max_iterations=20000,
tolerance=0.000001)
t_itr.append(iterations)
# pcd_src = o3d.geometry.PointCloud()
# pcd_target = o3d.geometry.PointCloud()
# pcd_src.points = o3d.utility.Vector3dVector(init_cloud)
# pcd_target.points = o3d.utility.Vector3dVector(original_cloud)
# t_itr.append(0)
# reg_p2p = o3d.pipelines.registration.registration_icp(pcd_target, pcd_src, 0.2, np.eye(4),o3d.pipelines.registration.TransformationEstimationPointToPoint(), o3d.pipelines.registration.ICPConvergenceCriteria(max_iteration = 20000, relative_rmse = 1.0e-10, relative_fitness=1.000000e-10))
# delta_T = reg_p2p.transformation
my_mat_final = np.dot(my_mat, delta_T)
my_r_final = copy.deepcopy(my_mat_final)
my_r_final[0:3, 3] = 0
my_r_final = quaternion_from_matrix(my_r_final, True)
my_t_final = np.array(
depth = depth[::down_factor, ::down_factor]
vertex_map = transforms.unproject(depth, intrinsic)
color_map = np.asarray(o3d.io.read_image('{}/{}.png'.format(rgb_path, i))).astype(float) / 255.0
color_map = color_map[::down_factor, ::down_factor]
normal_map = np.load('{}/{}.npy'.format(normal_path, i))
normal_map = normal_map[::down_factor, ::down_factor]
if i > 1:
print('Frame-to-model icp')
T_world_to_cam = np.linalg.inv(T_cam_to_world)
T_world_to_cam = icp(m.points[::down_factor],
m.normals[::down_factor],
vertex_map,
normal_map,
intrinsic,
T_world_to_cam,
debug_association=False)
T_cam_to_world = np.linalg.inv(T_world_to_cam)
print('Point-based fusion')
m.fuse(vertex_map, normal_map, color_map, intrinsic, T_cam_to_world, time=i)
# A shift is required as gt starts from 1
T_gt.append(gt_poses[i - 1])
T_est.append(T_cam_to_world)
global_pcd = o3d_utility.make_point_cloud(m.points, colors=m.colors, normals=m.normals)
o3d.visualization.draw_geometries([global_pcd.transform(o3d_utility.flip_transform)])
# Visualize the trajectories
def realtime_test():
''' Internal test.
'''
# laser sub
laser = LaserSub()
# init loop
r = rospy.Rate(1.0)
while not rospy.is_shutdown(
): # wait until first scan is received so that laser is initialized
if laser.first_recieved_done: # laser specs initialized by ROS sensor_msgs/LaserScan message
break
else:
rospy.loginfo(
'[coarse_level_association] Waiting for laser to init.')
r.sleep()
laser_prev = None
clusters_prev = None
clusters_id = None
# init plot
fig, ax = plt.subplots()
ax.set_aspect('equal', adjustable='box')
ax.set_title('Clusters Tracking', fontsize=18)
ax.axis([-5.0, 5.0, -5.0, 5.0])
ax.plot([0], [0], marker='>', markersize=20,
color="red") # plot the origin
# loop
r = rospy.Rate(30)
while not rospy.is_shutdown():
# --- 1. laser input (copy self.laser at this moment)
laser_now = copy.deepcopy(laser)
# --- 2. EMST (create input graph for segmentation)
mst = EMST(laser_now)
# --- 3. EGBIS (apply image segmentation technique to clustering)
clusters_now = Cluster(mst, k=1.0, min_size=10)
if clusters_id is None: # first set of clusters generated
clusters_id = [i for i in range(len(clusters_now.clusters))]
# --- 4. ICP
if laser_prev is not None:
dst_indices, T = icp(laser_now.cartesian,
laser_prev.cartesian,
init_pose=None,
max_iterations=20,
tolerance=0.001)
matched_cluster_indices, _ = match_clusters(
dst_indices, laser_now, laser_prev, clusters_now,
clusters_prev)
# --- 5. Visualize
laser_a = laser_now
laser_b = laser_prev
clusters_a = clusters_now
clusters_b = clusters_prev
clusters_id_new = []
# colors to iterate over
colors = ['b', 'g', 'r', 'c', 'm', 'y', 'k'] * 20
# plot source clusters with the same id as the matched destination clusters
ax_list = []
for cluster, color, cluster_b_index in zip(
clusters_a.clusters, colors, matched_cluster_indices):
a = ax.scatter(laser_a.cartesian[cluster, [0]],
laser_a.cartesian[cluster, [1]],
s=20.0,
c=color,
linewidths=0)
ax_list.append(a)
if cluster_b_index >= 0:
# show id
x1 = np.mean(laser_a.cartesian[cluster, [0]])
y1 = np.mean(laser_a.cartesian[cluster, [1]])
cluster_b = clusters_b.clusters[cluster_b_index]
x2 = np.mean(laser_b.cartesian[cluster_b, [0]])
y2 = np.mean(laser_b.cartesian[cluster_b, [1]])
_id = clusters_id[cluster_b_index]
clusters_id_new.append(_id)
a = ax.text((x1 + x2) / 2.0, (y1 + y2) / 2.0,
str(_id),
fontsize=22)
ax_list.append(a)
else: # no cluster_b match me.
_id = max(clusters_id) + 1
clusters_id_new.append(_id)
clusters_id = clusters_id_new
# plot
plt.pause(1e-12) # pause for real time display
for a in ax_list: # clear data on the plot
a.remove()
# end of loop
laser_prev = laser_now
clusters_prev = clusters_now
r.sleep()
# plot
plt.show()
