def random_point_inside_fov(camera_info, maxdist, mindist=0, Tcamera=np.eye(4)): """ Generates a random XYZ point inside the camera field of view @type camera_info: sensor_msgs.CameraInfo @param camera_info: Message with the meta information for a camera @type maxdist: float @param maxdist: distance from the camera ref frame in the z direction @type mindist: float @param mindist: distance from the camera ref frame in the z direction @type Tcamera: np.array @param Tcamera: homogeneous transformation for the camera ref frame @rtype: array @return: The random XYZ point """ cam_model = PinholeCameraModel() cam_model.fromCameraInfo(camera_info) z = np.random.uniform(mindist, maxdist) delta_x = cam_model.getDeltaX(cam_model.width / 2, z) delta_y = cam_model.getDeltaY(cam_model.height / 2, z) point = np.array([0, 0, z, 1]) point[:2] = np.array([delta_x, delta_y ]) * (2 * np.random.random_sample(2) - 1.) return np.dot(Tcamera, point)[:3]
def random_point_inside_fov(camera_info, maxdist, Tcamera=np.eye(4)): """ Generates a random XYZ point inside the camera field of view @type camera_info: sensor_msgs.CameraInfo @param camera_info: Message with the meta information for a camera @type maxdist: float @param maxdist: distance from the camera ref frame in the z direction @type Tcamera: np.array @param Tcamera: homogeneous transformation for the camera ref frame @rtype: array @return: The random XYZ point """ cam_model = PinholeCameraModel() cam_model.fromCameraInfo(camera_info) z = maxdist*np.random.random() delta_x = cam_model.getDeltaX(cam_model.width/2, z) delta_y = cam_model.getDeltaY(cam_model.height/2, z) point = np.array([0, 0, z, 1]) point[:2] = np.array([delta_x, delta_y]) * (2*np.random.random_sample(2) - 1.) return np.dot(Tcamera, point)[:3]
def camera_fov_corners(camera_info, zdist, Tcamera=np.eye(4)): """ Generates the 5 corners of the camera field of view @type camera_info: sensor_msgs.CameraInfo @param camera_info: Message with the meta information for a camera @type zdist: float @param zdist: distance from the camera ref frame in the z direction @type Tcamera: np.array @param Tcamera: homogeneous transformation for the camera ref frame @rtype: list @return: The 5 corners of the camera field of view """ cam_model = PinholeCameraModel() cam_model.fromCameraInfo(camera_info) delta_x = cam_model.getDeltaX(cam_model.width / 2, zdist) delta_y = cam_model.getDeltaY(cam_model.height / 2, zdist) corners = [Tcamera[:3, 3]] for k in itertools.product([-1, 1], [-1, 1]): point = np.array([0, 0, zdist, 1]) point[:2] = np.array([delta_x, delta_y]) * np.array(k) corners.append(np.dot(Tcamera, point)[:3]) return np.array(corners)
def camera_fov_corners(camera_info, zdist, Tcamera=np.eye(4)): """ Generates the 5 corners of the camera field of view @type camera_info: sensor_msgs.CameraInfo @param camera_info: Message with the meta information for a camera @type zdist: float @param zdist: distance from the camera ref frame in the z direction @type Tcamera: np.array @param Tcamera: homogeneous transformation for the camera ref frame @rtype: list @return: The 5 corners of the camera field of view """ cam_model = PinholeCameraModel() cam_model.fromCameraInfo(camera_info) delta_x = cam_model.getDeltaX(cam_model.width/2, zdist) delta_y = cam_model.getDeltaY(cam_model.height/2, zdist) corners = [Tcamera[:3,3]] for k in itertools.product([-1,1],[-1,1]): point = np.array([0, 0, zdist, 1]) point[:2] = np.array([delta_x, delta_y]) * np.array(k) corners.append( np.dot(Tcamera, point)[:3] ) return np.array(corners)
class CameraFOV(object): """ Base class for the field-of-view of a Pin hole camera. """ def __init__(self, camera_info, maxdist, transform=None): """ Parameters ---------- camera_info: sensor_msgs/CameraInfo Meta information of the camera. For more details check the `sensor_msgs/CameraInfo <http://docs.ros.org/api/sensor_msgs/html/msg/CameraInfo.html>`_ documentation. maxdist: float Maximum distance the FOV covers in the Z direction of the camera frame. transform: array_like Homogenous transformation of the camera reference frame. If `None` then the identity matrix is used. """ if transform is None: transform = np.eye(4) self.maxdist = maxdist self.transform = np.array(transform) self.cam_model = PinholeCameraModel() self.cam_model.fromCameraInfo(camera_info) # Compute FOV corners delta_x = self.cam_model.getDeltaX(self.cam_model.width/2, self.maxdist) delta_y = self.cam_model.getDeltaY(self.cam_model.height/2, self.maxdist) self.corners = np.zeros((5,3)) self.corners[0,:] = transform[:3,3] idx = 1 for k in itertools.product([-1,1],[-1,1]): point = np.array([0, 0, self.maxdist, 1]) point[:2] = np.array([delta_x, delta_y]) * np.array(k) self.corners[idx,:] = np.dot(transform, point)[:3] idx += 1 def get_corners(self): """ Get the five corners of the camera field of view Returns ------- corners: array_like A 5x3 array with the five corners of the camera field of view """ corners = np.array(self.corners) return corners def get_trimesh(self): """ Get the convex hull that representes the field of view as a trimesh Returns ------- vertices: array_like The trimesh vertices faces: array_like The trimesh faces See Also -------- raveutils.mesh.trimesh_from_point_cloud """ corners = self.get_corners() vertices, faces = ru.mesh.trimesh_from_point_cloud(corners) return vertices, faces def contains(self, points): """ Check if all the XYZ points are inside the camera field of view Parameters ---------- points: array_like List of XYZ points Returns ------- all_inside: bool True if all the points are inside the FOV. False otherwise. """ hull = ConvexHull(self.corners) triangulation = Delaunay(self.corners[hull.vertices]) all_inside = np.alltrue(triangulation.find_simplex(points)>=0) return all_inside def random_point_inside(self): """ Generate a random XYZ point inside the camera field of view Returns ------- random_point: array_like The random XYZ point inside the camera field of view """ z = self.maxdist*np.random.random() delta_x = self.cam_model.getDeltaX(self.cam_model.width/2, z) delta_y = self.cam_model.getDeltaY(self.cam_model.height/2, z) point = np.array([0, 0, z, 1]) point[:2] = np.array([delta_x,delta_y]) * (2*np.random.random_sample(2)-1.) random_point = np.dot(self.transform, point)[:3] return random_point