Verify if world point is in camera's field of view

edit

I was able to check if the objects are in the field of view, using the approach that I mentioned above. I project the object point to the camera_depth_optical_frame. Using the arctan of (x/z) you get the angle the horizontal angle and so on. I have also another verification, if the object is not in the room where the robot is located is not in field of view also.

def verify_field_of_view(self,object_x_pos,object_y_pos,object_z_pos): 
        # object should be in the field of view of the camera and should be in the room that we are in
        bl_pose = PoseStamped() 
        bl_pose.header.frame_id = "base_link" #or odom?!?
        bl_pose.pose.orientation.w = 1.0
        bl_pose.pose.position.x = 0.0
        bl_pose.pose.position.y = 0.0

    transform = self.tf_buffer.lookup_transform("map", "base_link",  rospy.Time(0), rospy.Duration(1.0))
    bot_in_world = tf2_geometry_msgs.do_transform_pose(bl_pose, transform) #bot_in_world_position
    current_room = self.room_retriever(xpos = bot_in_world.pose.position.x, ypos = bot_in_world.pose.position.y) #room where robot is located
    if current_room != self.room_retriever(xpos = object_x_pos, ypos = object_y_pos): #room where object belongs
        print 'object doesnt belong to this room'
        return False

    object_on_world = PoseStamped()
    object_on_world.header.frame_id = 'map'
    object_on_world.pose.orientation.w = 1.0
    object_on_world.pose.position.x = object_x_pos
    object_on_world.pose.position.y = object_y_pos
    object_on_world.pose.position.z = object_z_pos

    transform = self.tf_buffer.lookup_transform('camera_depth_optical_frame', 'map', rospy.Time(0), rospy.Duration(1.0))
    object_on_camera = tf2_geometry_msgs.do_transform_pose(object_on_world, transform) #point on camera_depth_optical_frame

    if object_on_camera.pose.position.z < self.min_depth or object_on_camera.pose.position.z > self.max_depth:
        return False #objects behind the camera, with a distance minor/higher than the accepted are not considered in the field of fiew 

    yangle = np.arctan(object_on_camera.pose.position.y/object_on_camera.pose.position.z) #rads
    xangle = np.arctan(object_on_camera.pose.position.x/object_on_camera.pose.position.z) #rads

    fovx = 57 #horizontal field of view in deg
    fovy = 43 #vertical field of view in deg 
    if xangle < abs(fovx/2*math.pi/180) and yangle < abs(fovy/2*math.pi/180): #57/43
        return True
    else:
        return False
 

Another thing, is really easy to get the field of view of your kinect, but I need to know the intrinsic parameters of the camera, where is that info? Wich parameters does openni uses by default, I never calibrated the kinect. Any suggestion to get the intrinsics? We can get the field of view from Intrinsics