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