AR openCV ArUco - How to apply pose estimation to panda camera?

Hi, welcome to the community!

It looks like OpenCV is giving a Y-up matrix, whereas Panda expects a Z-up matrix (unless you set panda to Y-up mode). Rather than applying manual operations to the rotation, I suggest multiplying your matrix with the Z-up to Y-up conversion matrix that is provided by Panda.

Try this instead to set the rotation:

panda.transNode.setMat(Mat3.convert_mat(CS_yup_right, CS_zup_right) * mat3)

You could simplify the code to not use a dummy node this way:

mat = Mat4.convert_mat(CS_yup_right, CS_zup_right) \
    * Mat4.translate_mat(-xTrans, -yTrans, -zTrans) \
    * Mat4(mat3)

xform = TransformState.make_mat(mat)
panda.poseData['pandaCamPose']['trans'] = list(xform.pos)
panda.poseData['pandaCamPose']['rot'] = list(xform.hpr)
1 Like