def __init__(self, cx, cy, cz, ax, ay, az, radius, arr):
super(CylinderPointCloudTransformer, self).__init__()
assert arr.ndim == 2
assert arr.shape[1] == 3
#we need to create a rotation matrix to apply to the 3d points to align them on z axis
axis = (ax, ay, az)
rotation_axis = np.cross(axis, (0, 0, 1))
rotation_angle = simple_geom.angle_between_vectors((0, 0, 1), axis)
rotation_quaternion = tf.transformations.quaternion_about_axis(
rotation_angle, axis)
print "FIXING SCALE " * 5
self._s = 1.0 / radius
#rotate points
Rh = tf.transformations.rotation_matrix(rotation_angle, rotation_axis)
self._R = Rh[0:3, 0:3]
new = np.dot(self._s * self._R, arr.T).T
#move this to the origin
cx, cy, _ = np.mean(new, axis=0)
_, _, zmin = np.min(new, axis=0)
_, _, zmax = np.max(new, axis=0)
self._t = np.array([[cx, cy, zmin]])
self.cloud = new - self._t
height = zmax - zmin
self._cyl = simple_geom.Cylinder(base=dict(x=0, y=0, z=0),
axis=dict(x=0, y=0, z=height),
radius=radius)
def create_cylinder(cx,cy,cz,ax,ay,az,radius, frame_id='/', obj_id=0, length=1, color=(0,1,0,0.3)):
#The cylinder's axis if unrotated is originally pointing along the z axis of the referential
#so that will be the up direction (0, 0 1).
#
#The cross product of the desired cylinder axis (normalized) and the up vector will give us
#the axis of rotation, perpendicular to the plane defined by the cylinder axis and the up vector.
#
#The dot product of the cylinder axis and the up vector will provide the angle of rotation.
axis = (ax, ay, az)
rotation_axis = np.cross((0, 0, 1), axis)
rotation_angle = simple_geom.angle_between_vectors(axis, (0, 0, 1))
cyl = Marker()
cyl.id = obj_id
cyl.header.frame_id = frame_id
cyl.type = Marker.CYLINDER
cyl.action = Marker.ADD
cyl.pose.position = Point(x=cx,y=cy,z=cz)
cyl.pose.orientation = Quaternion(*tf.transformations.quaternion_about_axis(rotation_angle, axis))
cyl.scale.x = 2*radius
cyl.scale.y = 2*radius
cyl.scale.z = length
cyl.color = ColorRGBA(*color)
return cyl
def __init__(self, cx,cy,cz,ax,ay,az,radius,arr):
super(CylinderPointCloudTransformer,self).__init__()
assert arr.ndim==2
assert arr.shape[1]==3
#we need to create a rotation matrix to apply to the 3d points to align them on z axis
axis = (ax, ay, az)
rotation_axis = np.cross(axis, (0, 0, 1))
rotation_angle = simple_geom.angle_between_vectors((0, 0, 1), axis)
rotation_quaternion = tf.transformations.quaternion_about_axis(rotation_angle, axis)
print "FIXING SCALE " * 5
self._s = 1.0/radius
#rotate points
Rh = tf.transformations.rotation_matrix(rotation_angle, rotation_axis)
self._R = Rh[0:3,0:3]
new = np.dot(self._s * self._R,arr.T).T
#move this to the origin
cx,cy,_ = np.mean(new,axis=0)
_,_,zmin = np.min(new,axis=0)
_,_,zmax = np.max(new,axis=0)
self._t = np.array([[cx,cy,zmin]])
self.cloud = new - self._t
height = zmax - zmin
self._cyl = simple_geom.Cylinder(
base=dict(x=0,y=0,z=0),
axis=dict(x=0,y=0,z=height),
radius=radius)
def create_cylinder(cx,
cy,
cz,
ax,
ay,
az,
radius,
frame_id='/',
obj_id=0,
length=1,
color=(0, 1, 0, 0.3)):
#The cylinder's axis if unrotated is originally pointing along the z axis of the referential
#so that will be the up direction (0, 0 1).
#
#The cross product of the desired cylinder axis (normalized) and the up vector will give us
#the axis of rotation, perpendicular to the plane defined by the cylinder axis and the up vector.
#
#The dot product of the cylinder axis and the up vector will provide the angle of rotation.
axis = (ax, ay, az)
rotation_axis = np.cross((0, 0, 1), axis)
rotation_angle = simple_geom.angle_between_vectors(axis, (0, 0, 1))
cyl = Marker()
cyl.id = obj_id
cyl.header.frame_id = frame_id
cyl.type = Marker.CYLINDER
cyl.action = Marker.ADD
cyl.pose.position = Point(x=cx, y=cy, z=cz)
cyl.pose.orientation = Quaternion(
*tf.transformations.quaternion_about_axis(rotation_angle, axis))
cyl.scale.x = 2 * radius
cyl.scale.y = 2 * radius
cyl.scale.z = length
cyl.color = ColorRGBA(*color)
return cyl
