Fix Eigen Quaternion transform

gleichdick · gleichdick · commit 7fb4715ce235 · 2021-02-02T16:17:17.000Z
diff --git a/tf2_eigen/include/tf2_eigen/tf2_eigen.h b/tf2_eigen/include/tf2_eigen/tf2_eigen.h
@@ -285,7 +285,7 @@ void doTransform(const Eigen::Quaterniond& t_in,
                  const geometry_msgs::msg::TransformStamped& transform) {
   Eigen::Quaterniond t;
   fromMsg(transform.transform.rotation, t);
-  t_out = t.inverse() * t_in * t;
+  t_out = t * t_in;
 }
 
 /** \brief Convert a stamped Eigen Quaterniond type to a QuaternionStamped message.

Original file line number	Diff line number	Diff line change
`@@ -285,7 +285,7 @@ void doTransform(const Eigen::Quaterniond& t_in,`
`285`	`285`	`const geometry_msgs::msg::TransformStamped& transform) {`
`286`	`286`	`Eigen::Quaterniond t;`
`287`	`287`	`fromMsg(transform.transform.rotation, t);`
`288`		`- t_out = t.inverse() * t_in * t;`
	`288`	`+ t_out = t * t_in;`
`289`	`289`	`}`
`290`	`290`
`291`	`291`	`/** \brief Convert a stamped Eigen Quaterniond type to a QuaternionStamped message.`