Converting received ARKit pose to ROS pose
I'm receiving ARKit's pose as is from iPhone and publishing in a topic. In ROS I need to convert it to the ROS reference frame. ARKit is right-handed x-right, y-up and z-backward so my conversion is two rotations: -90 around X and -90 around Z. However it does not give desired result in rviz. Would appreciate pointing me in the right direction (practical/theoretical) to fix this.
ARKit -> ROS conversion:
tf::Pose convertCameraPoseToRos(const tf::Pose& cameraPose)
{
// arkit to ros conversion
// 1. rotate -90 around X
// 2. rotate -90 around Z
#define PI 3.1415926535897
tf::Transform rotate1 = tf::Transform(tf::Quaternion(tf::Vector3(1,0,0), -PI/2));
tf::Transform rotate2 = tf::Transform(tf::Quaternion(tf::Vector3(0,0,1), -PI/2));
tf::Transform arCameraRotation = tf::Transform(cameraPose.getRotation());
tf::Transform arCameraTranslation = tf::Transform(tf::Quaternion(1,0,0,0), cameraPose.getOrigin());
// tried this one - didn't work
//return arCameraTranslation * rotate1 * rotate2 * arCameraRotation;
return cameraPose * rotate1 * rotate2; // this one is current, but doesn't work either
}
ARKit pose obtaining on iOS in swift:
// inside delegate "func session(_ session: ARSession, didUpdate frame: ARFrame)"
self.arKitT = frame.camera.transform
arKitPose.position_.x_ = arKitT!.columns.3.x
arKitPose.position_.y_ = arKitT!.columns.3.y
arKitPose.position_.z_ = arKitT!.columns.3.z
let poseQ = simd_quatf(self.arKitT!)
arKitPose.rotation_.x_ = poseQ.vector.x
arKitPose.rotation_.y_ = poseQ.vector.y
arKitPose.rotation_.z_ = poseQ.vector.z
arKitPose.rotation_.w_ = poseQ.vector.w
// arKitPose is being published as PoseStamped