def broadcast(self):
f = open("/home/davinci2/catkin_ws/src/davinci_vision/launch/BC_registration/camera_frame.p", "rb")
self.camera_transform = pickle.load(f)
f.close()
#SOMETIMES NEED TO INVERT X & Y AXES; just change from 1 to -1 and vice versa
offset = tfx.transform([SKETCH_OFFSET, 0, CAP_OFFSET], [[1, 0, 0], [0, 1, 0], [0, 0, 1]])
self.camera_transform = tfx.transform(self.camera_transform).as_transform() * offset
transform = tfx.inverse_tf(self.camera_transform)
pt = transform.translation
rot = transform.rotation
msg = TransformStamped()
msg.header.stamp = rospy.Time.now()
msg.transform.rotation.x = rot.x
msg.transform.rotation.y = rot.y
msg.transform.rotation.z = rot.z
msg.transform.rotation.w = rot.w
msg.child_frame_id = "left_BC"
msg.transform.translation.x = pt.x
msg.transform.translation.y = pt.y
msg.transform.translation.z = pt.z
msg.header.frame_id = "registration_brick_right"
msg.header.stamp = rospy.Time.now()
print('boo')
while not rospy.is_shutdown():
msg.header.stamp = rospy.Time.now()
self.pub.publish([msg])
rospy.sleep(0.1)
def broadcast(self):
f = open(
"/home/davinci2/catkin_ws/src/davinci_vision/launch/BC_registration/camera_frame.p",
"rb")
self.camera_transform = pickle.load(f)
f.close()
#SOMETIMES NEED TO INVERT X & Y AXES; just change from 1 to -1 and vice versa
offset = tfx.transform([SKETCH_OFFSET, 0, CAP_OFFSET],
[[1, 0, 0], [0, 1, 0], [0, 0, 1]])
self.camera_transform = tfx.transform(
self.camera_transform).as_transform() * offset
transform = tfx.inverse_tf(self.camera_transform)
pt = transform.translation
rot = transform.rotation
msg = TransformStamped()
msg.header.stamp = rospy.Time.now()
msg.transform.rotation.x = rot.x
msg.transform.rotation.y = rot.y
msg.transform.rotation.z = rot.z
msg.transform.rotation.w = rot.w
msg.child_frame_id = "left_BC"
msg.transform.translation.x = pt.x
msg.transform.translation.y = pt.y
msg.transform.translation.z = pt.z
msg.header.frame_id = "registration_brick_right"
msg.header.stamp = rospy.Time.now()
print('boo')
while not rospy.is_shutdown():
msg.header.stamp = rospy.Time.now()
self.pub.publish([msg])
rospy.sleep(0.1)
def calculate(self, dimensions):
x = []
y = []
z = []
for pose in self.pose_data:
x.append(pose.pose.position.x)
y.append(pose.pose.position.y)
z.append(pose.pose.position.z)
# self.pose_data = [[pose.pose.position.x, pose.pose.position.y, pose.pose.position.z] for pose in self.pose_data]
point, normal = self.plane_fit([x, y, z])
print(point)
print(normal)
d = -point.dot(normal)
xyz = np.array([x, y, z])
p0 = list(normal)
p0.append(d)
def f_min(X, p):
plane_xyz = p[0:3]
distance = (plane_xyz*X.T).sum(axis=1) + p[3]
return distance / np.linalg.norm(plane_xyz)
def residuals(params, signal, X):
return f_min(X, params)
sol = leastsq(residuals, p0, args=(None, xyz))[0]
print "Old solution: ", p0
print "Solution: ", sol
print "Old Error: ", (f_min(xyz, p0)**2).sum()
print "New Error: ", (f_min(xyz, sol)**2).sum()
self.plot(point, normal, min(x) - 0.1, max(x) + 0.1, min(y) - 0.1, max(y) + 0.1)
# oh fit_3d_line_pca already gives a unit vector
cols = dimensions[0]
rows = dimensions[1]
row_vectors = []
col_vectors = []
for row in range(rows):
row_vectors.append(self.fit_3d_line_pca([i + (row*cols) for i in reversed(range(cols))], self.pose_data)[0])
for col in range(cols):
col_vectors.append(self.fit_3d_line_pca([col + cols * i for i in range(rows)], self.pose_data)[0])
row_vector = self.fit_3d_line_pca(range(cols), self.pose_data)[0]
col_vector = self.fit_3d_line_pca([cols * i for i in range(rows)], self.pose_data)[0]
mean_row_vector = [0, 0, 0]
for vector in row_vectors:
mean_row_vector[0] += vector[0]
mean_row_vector[1] += vector[1]
mean_row_vector[2] += vector[2]
mean_row_vector[0] /= len(row_vectors)
mean_row_vector[1] /= len(row_vectors)
mean_row_vector[2] /= len(row_vectors)
mean_row_vector = np.array(mean_row_vector)
print("meanrow " + str(mean_row_vector))
mean_row_vector /= np.linalg.norm(mean_row_vector)
mean_col_vector = [0, 0, 0]
for vector in col_vectors:
mean_col_vector[0] += vector[0]
mean_col_vector[1] += vector[1]
mean_col_vector[2] += vector[2]
mean_col_vector[0] /= len(col_vectors)
mean_col_vector[1] /= len(col_vectors)
mean_col_vector[2] /= len(col_vectors)
print("meancol " + str(mean_col_vector))
mean_col_vector /= np.linalg.norm(mean_col_vector)
cross = np.cross(row_vector, col_vector)
meancross = np.cross(mean_row_vector, mean_col_vector)
meancross /= np.linalg.norm(meancross)
meancolcross = np.cross(meancross, mean_row_vector)
meancolcross /= np.linalg.norm(meancolcross)
print("row " + str(row_vector))
print("meanrow " + str(mean_row_vector))
print("col " + str(col_vector))
print("meancol " + str(mean_col_vector))
print("meancolcross " + str(meancolcross))
print("cross " + str(cross))
print("meancross " + str(meancross))
print("compare to norm " + str(normal))
x = [mean_row_vector[0], meancolcross[0], meancross[0]]
y = [mean_row_vector[1], meancolcross[1], meancross[1]]
z = [mean_row_vector[2], meancolcross[2], meancross[2]]
print(x)
print(y)
print(z)
x = np.array(x)
y = np.array(y)
z = np.array(z)
data = np.concatenate((x[:, np.newaxis], y[:, np.newaxis], z[:, np.newaxis]), axis=1)
#okay so dot product between meanrow & meancol is 0.0159.
plt3d = plt.figure().gca(projection='3d')
plt3d.scatter3D(*data.T, c=['red', 'green', 'blue'])
# for pose in self.pose_data:
# plt3d.plot(pose.pose.position.x, pose.pose.position.y, pose.pose.position.z)
# plt.axis([0, 1000, 0, 12000])
plt.show()
xa = mean_row_vector
# xa = -xa
ya = meancolcross
# ya = -ya
za = meancross
# print("normalized norm " + str(normal/np.linalg.norm(normal)))
# I'll just take the average of the rows and average of the columns...
transform = tfx.pose(point, [[xa[0], ya[0], za[0]],[xa[1], ya[1], za[1]],[xa[2], ya[2], za[2]]])
# transform.translation.z += CAP_OFFSET
f = open("/home/davinci2/catkin_ws/src/davinci_vision/launch/BC_registration/robot_transform_" + self.arm +\
".p", "wb")
pickle.dump(transform, f)
f.close()
camera_transform = tfx.pose(self.camera_transform)
# print("CAM TRANS")
# print(camera_transform)
# print("NEW CAM TRANS")
print(camera_transform)
final_transform = tfx.inverse_tf(camera_transform).as_transform() * transform
# why doesn't this work......
# maybe this should be the inverse lol
# final_transform = tfx.inverse_tf(final_transform)
print(final_transform)
pt = final_transform.translation
rot = final_transform.rotation
print("x: " + str(pt.x))
print("y: " + str(pt.y))
print("z: " + str(pt.z))
print("x: " + str(rot.x))
print("y: " + str(rot.y))
print("z: " + str(rot.z))
print("w: " + str(rot.w))
final_transform_str = str(pt.x) + " " + str(pt.y) + " " + str(pt.z) + " " + str(rot.x) + \
" " + str(rot.y) + " " + str(rot.z) + " " + str(rot.w)
f = open("/home/davinci2/catkin_ws/src/davinci_vision/launch/BC_registration/transform_" + self.arm +\
".txt", "w")
f.write(final_transform_str)
f.close()
print("wrote transform to transform_" + self.arm + ".txt.")
def calculate(self, dimensions):
x = []
y = []
z = []
for pose in self.pose_data:
x.append(pose.pose.position.x)
y.append(pose.pose.position.y)
z.append(pose.pose.position.z)
# self.pose_data = [[pose.pose.position.x, pose.pose.position.y, pose.pose.position.z] for pose in self.pose_data]
point, normal = self.plane_fit([x, y, z])
print(point)
print(normal)
d = -point.dot(normal)
xyz = np.array([x, y, z])
p0 = list(normal)
p0.append(d)
def f_min(X, p):
plane_xyz = p[0:3]
distance = (plane_xyz * X.T).sum(axis=1) + p[3]
return distance / np.linalg.norm(plane_xyz)
def residuals(params, signal, X):
return f_min(X, params)
sol = leastsq(residuals, p0, args=(None, xyz))[0]
print "Old solution: ", p0
print "Solution: ", sol
print "Old Error: ", (f_min(xyz, p0)**2).sum()
print "New Error: ", (f_min(xyz, sol)**2).sum()
self.plot(point, normal,
min(x) - 0.1,
max(x) + 0.1,
min(y) - 0.1,
max(y) + 0.1)
# oh fit_3d_line_pca already gives a unit vector
cols = dimensions[0]
rows = dimensions[1]
row_vectors = []
col_vectors = []
for row in range(rows):
row_vectors.append(
self.fit_3d_line_pca(
[i + (row * cols) for i in reversed(range(cols))],
self.pose_data)[0])
for col in range(cols):
col_vectors.append(
self.fit_3d_line_pca([col + cols * i for i in range(rows)],
self.pose_data)[0])
row_vector = self.fit_3d_line_pca(range(cols), self.pose_data)[0]
col_vector = self.fit_3d_line_pca([cols * i for i in range(rows)],
self.pose_data)[0]
mean_row_vector = [0, 0, 0]
for vector in row_vectors:
mean_row_vector[0] += vector[0]
mean_row_vector[1] += vector[1]
mean_row_vector[2] += vector[2]
mean_row_vector[0] /= len(row_vectors)
mean_row_vector[1] /= len(row_vectors)
mean_row_vector[2] /= len(row_vectors)
mean_row_vector = np.array(mean_row_vector)
print("meanrow " + str(mean_row_vector))
mean_row_vector /= np.linalg.norm(mean_row_vector)
mean_col_vector = [0, 0, 0]
for vector in col_vectors:
mean_col_vector[0] += vector[0]
mean_col_vector[1] += vector[1]
mean_col_vector[2] += vector[2]
mean_col_vector[0] /= len(col_vectors)
mean_col_vector[1] /= len(col_vectors)
mean_col_vector[2] /= len(col_vectors)
print("meancol " + str(mean_col_vector))
mean_col_vector /= np.linalg.norm(mean_col_vector)
cross = np.cross(row_vector, col_vector)
meancross = np.cross(mean_row_vector, mean_col_vector)
meancross /= np.linalg.norm(meancross)
meancolcross = np.cross(meancross, mean_row_vector)
meancolcross /= np.linalg.norm(meancolcross)
print("row " + str(row_vector))
print("meanrow " + str(mean_row_vector))
print("col " + str(col_vector))
print("meancol " + str(mean_col_vector))
print("meancolcross " + str(meancolcross))
print("cross " + str(cross))
print("meancross " + str(meancross))
print("compare to norm " + str(normal))
x = [mean_row_vector[0], meancolcross[0], meancross[0]]
y = [mean_row_vector[1], meancolcross[1], meancross[1]]
z = [mean_row_vector[2], meancolcross[2], meancross[2]]
print(x)
print(y)
print(z)
x = np.array(x)
y = np.array(y)
z = np.array(z)
data = np.concatenate(
(x[:, np.newaxis], y[:, np.newaxis], z[:, np.newaxis]), axis=1)
#okay so dot product between meanrow & meancol is 0.0159.
plt3d = plt.figure().gca(projection='3d')
plt3d.scatter3D(*data.T, c=['red', 'green', 'blue'])
# for pose in self.pose_data:
# plt3d.plot(pose.pose.position.x, pose.pose.position.y, pose.pose.position.z)
# plt.axis([0, 1000, 0, 12000])
plt.show()
xa = mean_row_vector
# xa = -xa
ya = meancolcross
# ya = -ya
za = meancross
# print("normalized norm " + str(normal/np.linalg.norm(normal)))
# I'll just take the average of the rows and average of the columns...
transform = tfx.pose(point,
[[xa[0], ya[0], za[0]], [xa[1], ya[1], za[1]],
[xa[2], ya[2], za[2]]])
# transform.translation.z += CAP_OFFSET
f = open("/home/davinci2/catkin_ws/src/davinci_vision/launch/BC_registration/robot_transform_" + self.arm +\
".p", "wb")
pickle.dump(transform, f)
f.close()
camera_transform = tfx.pose(self.camera_transform)
# print("CAM TRANS")
# print(camera_transform)
# print("NEW CAM TRANS")
print(camera_transform)
final_transform = tfx.inverse_tf(
camera_transform).as_transform() * transform
# why doesn't this work......
# maybe this should be the inverse lol
# final_transform = tfx.inverse_tf(final_transform)
print(final_transform)
pt = final_transform.translation
rot = final_transform.rotation
print("x: " + str(pt.x))
print("y: " + str(pt.y))
print("z: " + str(pt.z))
print("x: " + str(rot.x))
print("y: " + str(rot.y))
print("z: " + str(rot.z))
print("w: " + str(rot.w))
final_transform_str = str(pt.x) + " " + str(pt.y) + " " + str(pt.z) + " " + str(rot.x) + \
" " + str(rot.y) + " " + str(rot.z) + " " + str(rot.w)
f = open("/home/davinci2/catkin_ws/src/davinci_vision/launch/BC_registration/transform_" + self.arm +\
".txt", "w")
f.write(final_transform_str)
f.close()
print("wrote transform to transform_" + self.arm + ".txt.")
