solvePnP uses camera focal length, not projection focal length

[jpapon]

I noticed that the apriltag detector uses the camera intrinsic focal lengths and principal point, rather than the projection matrix in solvePnP : https://github.com/RIVeR-Lab/apriltags_ros/blob/indigo-devel/apriltags_ros/src/apriltag_detector.cpp#L83-L84 https://github.com/RIVeR-Lab/apriltags_ros/blob/indigo-devel/apriltags/src/TagDetection.cc#L95-L98

Is this correct? We're using rectified images here (thus the zero'd distortion coefficients), so it seems the projection matrix values make more sense. Often this isn't much of a problem, but for wide FOV cameras and/or april tags on the edge of the image, there is some difference.

In my own experiments, I found using projection matrix values gave substantially more accurate pose values - especially in distance to the markers.

The change to fix this would be simple - just switch from using:

  double fx = cam_info->K[0];
  double fy = cam_info->K[4];
  double px = cam_info->K[2];
  double py = cam_info->K[5];

to

double fx = cam_info->P[0];
  double fy = cam_info->P[5];
  double px = cam_info->P[2];
  double py = cam_info->P[6];

[brentyi]

+1 This seems right, though it might be a little tricky to merge/release because fixing it has the potential of breaking existing systems.

[velinddimitrov]

We could make it a parameter to switch between the existing implementation and projection matrix values implementation. Thoughts?

[brentyi]

Sure. It's not exactly ideal but seems like the best option available. The parameter can be removed (or its default value flipped) if a release for kinetic ever happens.

[velinddimitrov]

I will work on a release next.