def test_create_axis_angle(self): axis = tft.random_vector(3) angle = random.random() * 2 * pi q = tft.quaternion_about_axis(angle, axis) q2 = tb_angles(axis,angle).quaternion self.assertTrue(np.allclose(q, q2) or np.allclose(-q, q2),msg='%s and %s not close!' % (list(q),list(q2))) q2 = tb_angles(angle,axis).quaternion self.assertTrue(np.allclose(q, q2) or np.allclose(-q, q2),msg='%s and %s not close!' % (list(q),list(q2))) q2 = tb_angles((axis,angle)).quaternion self.assertTrue(np.allclose(q, q2) or np.allclose(-q, q2),msg='%s and %s not close!' % (list(q),list(q2))) q2 = tb_angles((angle,axis)).quaternion self.assertTrue(np.allclose(q, q2) or np.allclose(-q, q2),msg='%s and %s not close!' % (list(q),list(q2))) for _ in xrange(1000): axis = tft.random_vector(3) angle = random.random() * 2 * pi q = tft.quaternion_about_axis(angle, axis) q2 = tb_angles(axis,angle).quaternion self.assertTrue(np.allclose(q, q2) or np.allclose(-q, q2),msg='%s and %s not close! for axis %s and angle %f' % (list(q),list(q2),tuple(axis),angle))
def find_mesh_holes(vert,
faces,
radius,
scale=1.,
fitplane_eps=1e-8,
fitplane_attempts=10):
vertices = np.array(vert) * scale
# Circles have lots of vertices. Use clustering to locate them
eps = radius + 1e-3
db = sklearn.cluster.DBSCAN(eps=eps, min_samples=10).fit(vertices)
unique_labels = set(db.labels_)
circles_info = []
for k in unique_labels:
if k == -1: # Unknown cluster
continue
points = vertices[db.labels_ == k]
for _ in range(fitplane_attempts):
seed = np.zeros(4)
seed[:3] = tr.unit_vector(tr.random_vector(3))
res = criros.spalg.fit_plane_optimize(points, seed=seed)
equation = res[0]
fit_error = res[2]
if fit_error < fitplane_eps:
break
data = dict()
data['center'] = np.mean(points, axis=0)
data['plane'] = criros.spalg.Plane(equation=res[0])
circles_info.append(data)
if fit_error > fitplane_eps:
# Report circles that weren't fitted properly
print 'Circle planefit error above threshold: {0}'.format(
fit_error)
# One hole is composed by two circles, pair them
holes = []
num_circles = len(circles_info)
found = set()
for i in range(num_circles):
if i in found:
continue # Skip already paired circles
for j in range(1, num_circles):
if (j in found) or (i == j):
continue # Skip already paired circles
plane_i = circles_info[i]['plane']
plane_j = circles_info[j]['plane']
ni = plane_i.normal
nj = plane_j.normal
parallel = np.isclose(abs(np.dot(ni, nj)), 1.)
center_i = circles_info[i]['center']
center_j = circles_info[j]['center']
pi = center_i
pj = plane_i.project(center_j)
if parallel and np.allclose(pi, pj):
found.add(i)
found.add(j)
position = center_j
diff = center_i - center_j
direction = tr.unit_vector(diff)
depth = np.linalg.norm(diff)
holes.append(Hole(position, direction, depth))
return holes
def fit_plane_optimize(points, seed=None):
"""
Fits a plane to a point cloud using C{scipy.optimize.leastsq}
@type points: list
@param points: list of points
@rtype: np.array
@return: normalized normal vector of the plane
"""
if seed is None:
seed = np.zeros(4)
seed[:3] = tr.unit_vector(tr.random_vector(3))
# Optimization functions
def f_min(X,p):
normal = p[0:3]
d = p[3]
result = ((normal*X.T).sum(axis=1) + d) / np.linalg.norm(normal)
return result
def residuals(params, signal, X):
return f_min(X, params)
# Optimize
XYZ = np.array(points).T
p0 = np.array(seed)
sol = scipy.optimize.leastsq(residuals, p0, args=(None, XYZ))[0]
nn = np.linalg.norm(sol[:3])
sol /= nn
seed_error = (f_min(XYZ, p0)**2).sum()
fit_error = (f_min(XYZ, sol)**2).sum()
return sol, seed_error, fit_error
def test_to_axis_angle(self): for _ in xrange(1000): axis = tft.random_vector(3) axis = axis / np.linalg.norm(axis) for angle in list(np.linspace(-pi, pi, 10)) + [0]: q = tft.quaternion_about_axis(angle, axis) axis2,angle2 = tb_angles(q).axis_angle q2 = tft.quaternion_about_axis(angle2, axis2) self.assertTrue(np.allclose(q, q2) or np.allclose(-q, q2),msg="axis %s and angle %f don't match %s, %f" % (tuple(axis),angle,tuple(axis2),angle2))
def test_to_axis_angle(self):
for _ in xrange(1000):
axis = tft.random_vector(3)
axis = axis / np.linalg.norm(axis)
for angle in list(np.linspace(-pi, pi, 10)) + [0]:
q = tft.quaternion_about_axis(angle, axis)
axis2, angle2 = tb_angles(q).axis_angle
q2 = tft.quaternion_about_axis(angle2, axis2)
self.assertTrue(np.allclose(q, q2) or np.allclose(-q, q2),
msg="axis %s and angle %f don't match %s, %f" %
(tuple(axis), angle, tuple(axis2), angle2))
def test_create_axis_angle(self):
axis = tft.random_vector(3)
angle = random.random() * 2 * pi
q = tft.quaternion_about_axis(angle, axis)
q2 = tb_angles(axis, angle).quaternion
self.assertTrue(np.allclose(q, q2) or np.allclose(-q, q2),
msg='%s and %s not close!' % (list(q), list(q2)))
q2 = tb_angles(angle, axis).quaternion
self.assertTrue(np.allclose(q, q2) or np.allclose(-q, q2),
msg='%s and %s not close!' % (list(q), list(q2)))
q2 = tb_angles((axis, angle)).quaternion
self.assertTrue(np.allclose(q, q2) or np.allclose(-q, q2),
msg='%s and %s not close!' % (list(q), list(q2)))
q2 = tb_angles((angle, axis)).quaternion
self.assertTrue(np.allclose(q, q2) or np.allclose(-q, q2),
msg='%s and %s not close!' % (list(q), list(q2)))
for _ in xrange(1000):
axis = tft.random_vector(3)
angle = random.random() * 2 * pi
q = tft.quaternion_about_axis(angle, axis)
q2 = tb_angles(axis, angle).quaternion
self.assertTrue(
np.allclose(q, q2) or np.allclose(-q, q2),
msg='%s and %s not close! for axis %s and angle %f' %
(list(q), list(q2), tuple(axis), angle))
def random_landmark(self):
landmark = LandmarkEntry()
landmark.translation_weight = self.options.translation_weight
landmark.rotation_weight = self.options.rotation_weight
landmark.id = self.landmark_id_sampler.sample_id()
if landmark.id in self._sampled_ids:
if not self.options.allow_duplicate_ids:
rospy.logwarn("Ignoring duplicate ID: {}".format(landmark.id))
return None
else:
rospy.logwarn("Duplicate ID: {}".format(landmark.id))
self._sampled_ids.append(landmark.id)
vector = transformations.random_vector(3) * self.options.max_distance
landmark.tracking_from_landmark_transform.position.x = vector[0]
landmark.tracking_from_landmark_transform.position.y = vector[1]
landmark.tracking_from_landmark_transform.position.z = vector[2]
quaternion = transformations.random_quaternion()
landmark.tracking_from_landmark_transform.orientation.x = quaternion[0]
landmark.tracking_from_landmark_transform.orientation.y = quaternion[1]
landmark.tracking_from_landmark_transform.orientation.z = quaternion[2]
landmark.tracking_from_landmark_transform.orientation.w = quaternion[3]
return landmark
def random_landmark(self):
landmark = LandmarkEntry()
landmark.translation_weight = self.options.translation_weight
landmark.rotation_weight = self.options.rotation_weight
landmark.id = self.landmark_id_sampler.sample_id()
if landmark.id in self._sampled_ids:
if not self.options.allow_duplicate_ids:
rospy.logwarn("Ignoring duplicate ID: {}".format(landmark.id))
return None
else:
rospy.logwarn("Duplicate ID: {}".format(landmark.id))
self._sampled_ids.append(landmark.id)
vector = transformations.random_vector(3) * self.options.max_distance
landmark.tracking_from_landmark_transform.position.x = vector[0]
landmark.tracking_from_landmark_transform.position.y = vector[1]
landmark.tracking_from_landmark_transform.position.z = vector[2]
quaternion = transformations.random_quaternion()
landmark.tracking_from_landmark_transform.orientation.x = quaternion[0]
landmark.tracking_from_landmark_transform.orientation.y = quaternion[1]
landmark.tracking_from_landmark_transform.orientation.z = quaternion[2]
landmark.tracking_from_landmark_transform.orientation.w = quaternion[3]
return landmark
p = [0, 0, 0]
m1 = tf.quaternion_matrix(q)
m2 = homogeneous_matrix(p + q)
successes += int(np.allclose(m1, m2))
print(' -> successes: {}/{}'.format(successes, num_test))
print("--------------------\n")
tot_successess += successes
tot_test += num_test
# homogeneous_matrix_inverse
if test_enabled[8]:
print("test homogeneous_matrix_inverse")
successes = 0
for i in range(num_test):
m = tf.random_rotation_matrix()
m[:3, 3:4] = tf.random_vector(3).reshape(3, 1)
m1 = tf.inverse_matrix(m)
m2 = homogeneous_matrix_inverse(m)
successes += int(np.allclose(m1, m2))
print(' -> successes: {}/{}'.format(successes, num_test))
print("--------------------\n")
tot_successess += successes
tot_test += num_test
# transform_from_matrix
if test_enabled[9]:
print("test transform_from_matrix")
successes = 0
for i in range(num_test):
p0 = tf.random_vector(3).tolist()
q0 = tf.random_quaternion().tolist()
