Open rdiverdi opened 7 years ago
I was having a similar problem and realized it was because the camera_info topic wasnt populated. This package uses the calibration matrices populated in camera_info to calculate the tag poses. These lines in apriltag_detector.cpp:
if (projected_optics_) {
// use projected focal length and principal point
// these are the correct values
fx = cam_info->P[0];
fy = cam_info->P[5];
px = cam_info->P[2];
py = cam_info->P[6];
} else {
// use camera intrinsic focal length and principal point
// for backwards compatability
fx = cam_info->K[0];
fy = cam_info->K[4];
px = cam_info->K[2];
py = cam_info->K[5];
}
So I would double check and make sure that the K matrix in your camera_info topic is populated. If- like me - you're using projected optics you can set this flag in a launch file like so:
<node pkg="apriltags_ros" type="apriltag_detector_node" name="apriltag_detector">
<rosparam>
tag_descriptions: [
{id: 0, size: 0.215, frame_id: myAprilTag0},
{id: 1, size: 0.215, frame_id: myAprilTag1},
sensor_frame_id: apriltag_frame
</rosparam>
<param name="projected_optics" type="bool" value="true" />
</node>
I am using an Odroid XU-4 with ubuntu 14.04 and ROS indigo. I had apriltags_ros working on my laptop (also running ubuntu 14.04 and ROS indigo), and I was getting tag detections with correct pose outputs and tf transforms, as expected, however, when I ran the same code on an Odroid, the code no longer returned correct pose estimates, instead it would just output a pose of all zeros (except w was 1). There were no errors thrown, the tag_detections image was being published and had the tag outlined, and the pose message was only published when the tag was in view. Also, it correctly identified the tag's id number, so a large portion of the code is still running, but the 3D matrix transformations are just outputting zeros. Does anyone have an idea on why this could be happening? am I missing a dependency? Is there any reason part of the code wouldn't work on an Odroid when it is fine on a laptop?