-
Notifications
You must be signed in to change notification settings - Fork 278
New issue
Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.
By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.
Already on GitHub? Sign in to your account
conversions for Eigen::Isometry3d #335
Conversation
@@ -72,7 +72,7 @@ geometry_msgs::TransformStamped eigenToTransform(const Eigen::Affine3d& T) | |||
t.transform.translation.y = T.translation().y(); | |||
t.transform.translation.z = T.translation().z(); | |||
|
|||
Eigen::Quaterniond q(T.rotation()); |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Affine3d::rotation() performs a costly SVD.
msg.orientation.y = Eigen::Quaterniond(in.rotation()).y(); | ||
msg.orientation.z = Eigen::Quaterniond(in.rotation()).z(); | ||
msg.orientation.w = Eigen::Quaterniond(in.rotation()).w(); | ||
Eigen::Quaterniond q(in.linear()); |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
compute the quaternion only once!
@@ -314,6 +314,12 @@ geometry_msgs::Pose toMsg(const Eigen::Affine3d& in) { | |||
msg.orientation.y = q.y(); | |||
msg.orientation.z = q.z(); | |||
msg.orientation.w = q.w(); | |||
if (msg.orientation.w < 0) { |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Constrain quaternions to the half-space w >= 0 as in tf1
.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Thanks for adding this support.
@tfoote Do you plan a new release of geometry2 any time soon? |
Yes, I've just cleared the backlog of PRs and plan to release after the next indigo/kinetic sync |
This change needs to be ported to the ros2 version of geometry2 for moveit's conversion: https://github.com/ros2/geometry2 |
Ideally the whole geometry2 would be synced to the ros2 version |
As of C++11, the size() method of a std::list is guaranteed to have constant cost, so we don't need to track the message count ourselves. Signed-off-by: Chris Lalancette <[email protected]>
In the vein of ros/geometry#113, I added converters for
Eigen::Isometry3d
also totf2_eigen
.Eigen::Affine3d
cannot exploit the SE(3) structure of transforms and thus needs toHence,
Affine3d
should be actually avoided.