UbiquityRobotics / fiducials

Simultaneous localization and mapping using fiducial markers.
BSD 3-Clause "New" or "Revised" License
263 stars 137 forks source link

How to calculate camera pose from 2 distinct fiducial transforms from same aruco marker #230

Open Rezenders opened 3 years ago

Rezenders commented 3 years ago

Hello,

I am trying to calculate my camera position (CP1) using a aruco marker, for this I put my camera at a known position and I use aruco_detect to get its FiducialTransform (FT1), then I move my camera to an unknown position and once again I use aruco_detect to get the FiducialTransformation (FT2) to the same aruco marker. Now how can I calculate the new camera position (CP2)?

Can anyone link me some references to this? Or help me out any other way? I have been stuck on this for quite some time already

Thanks in advance

Rezenders commented 3 years ago

From http://wiki.ros.org/fiducials?distro=melodic I see that when the camera position is fixed and there are two aruco markes and one of them is at a known position the position of the second one can be calculated with:

T_map_fid2 = T_map_fid1 T_cam_fid2 T_fid1_cam

So, in the case i described in the issue description can I just replace T_map_fid1 for the camera position?

Something like:

CP2 = CP1 FT1 FT2

ulassbin commented 3 years ago

If your aruco is fixed, and you know the initial position of the aruco you can uniquely detect your camera's position at each measurement as: Since; T_map_aruco = T_map_to_camera T_camera_to_aruco T_map_to_camera = T_map_aruco T_camera_to_aruco^-1;

T_camera_to_aruco is basically fiducial transform, and you can invert it for T_camera_to_aruco. T_map_aruco is the known fixed position of aruco on the map.