diff --git a/src/pose_estimation.cpp b/src/pose_estimation.cpp index e07cfbf..35a9e11 100644 --- a/src/pose_estimation.cpp +++ b/src/pose_estimation.cpp @@ -14,6 +14,14 @@ homography(apriltag_detection_t* const detection, const std::array& i apriltag_pose_t pose; estimate_pose_for_tag_homography(&info, &pose); + // rotate frame such that z points in the opposite direction towards the camera + for(int i = 0; i < 3; i++) { + // swap x and y axes + std::swap(MATD_EL(pose.R, 0, i), MATD_EL(pose.R, 1, i)); + // invert z axis + MATD_EL(pose.R, 2, i) *= -1; + } + return tf2::toMsg(const_cast(pose)); }