Add apriltag_tag_pose.c in the project

[KangKaiChen]

"I'm trying to add apriltag_tag_pose.c and apriltag_tag_pose.h to calculate the pose of a tag, but I've encountered some issues. I would like to ask if the author could please try to take a look."