add 3x3 rotation matrix to quaternion function

[chobitsfan]

In https://github.com/AprilRobotics/apriltag/issues/90#issuecomment-656802651, @mkrogius suggests using doubles_mat_to_quat to get a quaternion from the matrix. However, rotation matrix in apriltag_pose_t is a 3x3 matrix and doubles_mat_to_quat expects a 4x4 matrix. It would be easier to use if we have a doubles_mat33_to_quat.

[christian-rauch]

Thank you for the patch. Where is this new function used? I cannot find references to doubles_mat_to_quat. Can you provide more details on why this new function is required?

[chobitsfan]

Hi @christian-rauch Thank you for reviewing

I cannot find references to doubles_mat_to_quat

Yes. doubles_mat_to_quat is defined but never used. But in https://github.com/AprilRobotics/apriltag/issues/90, @mkrogius suggests it can be used (with doubles_quat_to_rpy) to obtain euler angles from apriltag_pose_t. I found it is quite useful. However, rotation matrix in apriltag_pose_t is a 3x3 matrix and doubles_mat_to_quat expects a 4x4 matrix.