How do I detect if an object is in front of me?
I have a Point Cloud, a vector map of fixed objects along the way, a geometry_msgs/PoseStamped
stream of my robot's position, and a camera fixed relative to the PoseStamped
position.
Now, I want to calculate if there is, say, a doorway in the robot's field of view, so I can pass its coordinates in the camera image to a detection routine that will check, say, if it is open, where the door handle is, or whatever.
However, I can't quite get my head around how to combine the tf::stampedTransform
and the geometry_msgs::PoseStamped
to detect if a point in 3D space falls within the camera's vision. Note that the camera doesn't publish a sensor_msgs/CameraInfo
, but I do have focal point and principal point data.