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
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()