From 84c16a729635dc2202ae2621a1ce8eda7bc98a61 Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Thu, 21 Mar 2019 20:54:54 +0300 Subject: [PATCH] aruco_pose: fix snapping alrogithm --- aruco_pose/src/utils.h | 34 +++++++++++++++++++++++++++------- 1 file changed, 27 insertions(+), 7 deletions(-) diff --git a/aruco_pose/src/utils.h b/aruco_pose/src/utils.h index 2822cca3..f18aa760 100644 --- a/aruco_pose/src/utils.h +++ b/aruco_pose/src/utils.h @@ -1,5 +1,6 @@ #pragma once +#include #include #include #include @@ -90,14 +91,33 @@ inline void fillTranslation(geometry_msgs::Vector3& translation, const cv::Vec3d translation.z = tvec[2]; } -inline void snapOrientation(geometry_msgs::Quaternion& to, const geometry_msgs::Quaternion& from) +inline bool isFlipped(tf::Quaternion& q) { - tf::Quaternion q; - q.setRPY(0, 0, -tf::getYaw(to) + tf::getYaw(from)); - tf::Quaternion pq; - tf::quaternionMsgToTF(from, pq); - pq = pq * q; - tf::quaternionTFToMsg(pq, to); + double yaw, pitch, roll; + tf::Matrix3x3(q).getEulerYPR(yaw, pitch, roll); + return (abs(pitch) > M_PI / 2) || (abs(roll) > M_PI / 2); +} + +/* Set roll and pitch from "from" to "to", keeping yaw */ +inline void snapOrientation(geometry_msgs::Quaternion& to, const geometry_msgs::Quaternion& from, bool auto_flip = false) +{ + tf::Quaternion _from, _to; + tf::quaternionMsgToTF(from, _from); + tf::quaternionMsgToTF(to, _to); + + if (auto_flip) { + if (!isFlipped(_from)) { + static const tf::Quaternion flip = tf::createQuaternionFromRPY(M_PI, 0, 0); + _from *= flip; // flip "from" + } + } + + auto diff = tf::Matrix3x3(_to).transposeTimes(tf::Matrix3x3(_from)); + double _, yaw; + diff.getRPY(_, _, yaw); + auto q = tf::createQuaternionFromRPY(0, 0, -yaw); + _from = _from * q; // set yaw from "to" to "from" + tf::quaternionTFToMsg(_from, to); // set "from" to "to" } inline void transformToPose(const geometry_msgs::Transform& transform, geometry_msgs::Pose& pose)