def inverse_transform_list(point, pose):
    '''
    Inverse transforms a list using pose as the transform.

    Takes a point defined in the frame defined in which pose is defined and converts it into the frame defined
    by pose (i.e. the frame in which pose is the origin).  This is useful, for example, if we know the position
    of the object in the world is (x, y, z) and the pose of a table is table_pose.  We can find the height of
    the object above the table by transforming it into the pose of the table using 
    transform_point(point, table_pose) and looking at the z coordinate of the result.
    
    **Args:**

        **point ((double, double, double)):** Point to transform as (x, y, z)

        **pose (geometry_msgs.msg.Pose):** The transform

    **Returns:**
        (x', y', z')
    '''
    translation = point_to_list(pose.position)
    rotMatrix = rotation_matrix(pose.orientation)

    newpoint = []
    for row in range(0, 3):
        newpoint.append(rotMatrix[0][row] * (point[0] - translation[0]) +
                        rotMatrix[1][row] * (point[1] - translation[1]) +
                        rotMatrix[2][row] * (point[2] - translation[2]))
    return newpoint
def transform_list(point, pose):
    '''
    Transforms a list using pose as the transform

    Takes a point defined in the frame defined by pose (i.e. the frame in which pose is the origin) and returns
    it in the frame in which pose is defined.  Calling this with the point (0, 0, 0) will return pose.  This
    is useful, for example, in finding the corner of a box arbitrarily positioned in space.  In the box's frame
    the corner is (xdim, ydim, zdim).  In the world, the corner is at transform_point(corner, box_pose).
    
    **Args:**

        **point ((double, double, double)):** Point to transform as (x, y, z)

        **pose (geometry_msgs.msg.Pose):** The transform
    
    Returns:
        (x', y', z')
    '''
    translation = point_to_list(pose.position)
    rotMatrix = rotation_matrix(pose.orientation)

    newpoint = []
    for row in range(0, 3):
        newpoint.append(rotMatrix[row][0] * point[0] +
                        rotMatrix[row][1] * point[1] +
                        rotMatrix[row][2] * point[2] + translation[row])
    return newpoint
def inverse_transform_list(point, pose):
    '''
    Inverse transforms a list using pose as the transform.

    Takes a point defined in the frame defined in which pose is defined and converts it into the frame defined
    by pose (i.e. the frame in which pose is the origin).  This is useful, for example, if we know the position
    of the object in the world is (x, y, z) and the pose of a table is table_pose.  We can find the height of
    the object above the table by transforming it into the pose of the table using 
    transform_point(point, table_pose) and looking at the z coordinate of the result.
    
    **Args:**

        **point ((double, double, double)):** Point to transform as (x, y, z)

        **pose (geometry_msgs.msg.Pose):** The transform

    **Returns:**
        (x', y', z')
    '''
    translation = point_to_list(pose.position)
    rotMatrix = rotation_matrix(pose.orientation)

    newpoint = []
    for row in range(0,3):
        newpoint.append(rotMatrix[0][row]*(point[0]-translation[0]) +
                        rotMatrix[1][row]*(point[1]-translation[1]) +
                        rotMatrix[2][row]*(point[2]-translation[2]))
    return newpoint
def transform_list(point, pose):
    '''
    Transforms a list using pose as the transform

    Takes a point defined in the frame defined by pose (i.e. the frame in which pose is the origin) and returns
    it in the frame in which pose is defined.  Calling this with the point (0, 0, 0) will return pose.  This
    is useful, for example, in finding the corner of a box arbitrarily positioned in space.  In the box's frame
    the corner is (xdim, ydim, zdim).  In the world, the corner is at transform_point(corner, box_pose).
    
    **Args:**

        **point ((double, double, double)):** Point to transform as (x, y, z)

        **pose (geometry_msgs.msg.Pose):** The transform
    
    Returns:
        (x', y', z')
    '''
    translation = point_to_list(pose.position)
    rotMatrix = rotation_matrix(pose.orientation)

    newpoint = []
    for row in range(0,3):
        newpoint.append(rotMatrix[row][0]*point[0] + 
                        rotMatrix[row][1]*point[1] +
                        rotMatrix[row][2]*point[2] + 
                        translation[row])
    return newpoint
def transform_point(point, pose):
    '''
    Transforms a point using pose as the transformation.

    Takes a point defined in the frame defined by pose (i.e. the frame in which pose is the origin) and returns
    it in the frame in which pose is defined.  Calling this with the point (0, 0, 0) will return pose.  This
    is useful, for example, in finding the corner of a box arbitrarily positioned in space.  In the box's frame
    the corner is (xdim, ydim, zdim).  In the world, the corner is at transform_point(corner, box_pose).
    
    **Args:**
    
        **point (geometry_msgs.msg.Point):** Point to transform

        **pose (geometry_msgs.msg.Pose):** The transform
    
    **Returns:**
        A geometry_msgs.msg.Point
    '''
    return list_to_point(transform_list(point_to_list(point), pose))
def transform_point(point, pose):
    '''
    Transforms a point using pose as the transformation.

    Takes a point defined in the frame defined by pose (i.e. the frame in which pose is the origin) and returns
    it in the frame in which pose is defined.  Calling this with the point (0, 0, 0) will return pose.  This
    is useful, for example, in finding the corner of a box arbitrarily positioned in space.  In the box's frame
    the corner is (xdim, ydim, zdim).  In the world, the corner is at transform_point(corner, box_pose).
    
    **Args:**
    
        **point (geometry_msgs.msg.Point):** Point to transform

        **pose (geometry_msgs.msg.Pose):** The transform
    
    **Returns:**
        A geometry_msgs.msg.Point
    '''
    return list_to_point(transform_list(point_to_list(point), pose))
def inverse_transform_point(point, pose):
    '''
    Inverse transforms a point using pose as the transform

    Takes a point defined in the frame defined in which pose is defined and converts it into the frame defined
    by pose (i.e. the frame in which pose is the origin).  This is useful, for example, if we know the position
    of the object in the world is (x, y, z) and the pose of a table is table_pose.  We can find the height of
    the object above the table by transforming it into the pose of the table using 
    transform_point(point, table_pose) and looking at the z coordinate of the result.
    
    **Args:**

        **point (geometry_msgs.msg.Point):** Point to transform

        **pose (geometry_msgs.msg.Pose):** The transform
    
    **Returns:**
        A geometry_msgs.msg.Point
    '''
    return list_to_point(inverse_transform_list(point_to_list(point), pose))
def inverse_transform_point(point, pose):
    '''
    Inverse transforms a point using pose as the transform

    Takes a point defined in the frame defined in which pose is defined and converts it into the frame defined
    by pose (i.e. the frame in which pose is the origin).  This is useful, for example, if we know the position
    of the object in the world is (x, y, z) and the pose of a table is table_pose.  We can find the height of
    the object above the table by transforming it into the pose of the table using 
    transform_point(point, table_pose) and looking at the z coordinate of the result.
    
    **Args:**

        **point (geometry_msgs.msg.Point):** Point to transform

        **pose (geometry_msgs.msg.Pose):** The transform
    
    **Returns:**
        A geometry_msgs.msg.Point
    '''
    return list_to_point(inverse_transform_list(point_to_list(point), pose))