Beispiel #1
0
    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)
Beispiel #2
0
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
Beispiel #3
0
    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)
Beispiel #4
0
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