Skip to content
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

Merged
merged 4 commits into from
Oct 19, 2018
Merged

Conversation

rhaschke
Copy link
Contributor

In the vein of ros/geometry#113, I added converters for Eigen::Isometry3d also to tf2_eigen. Eigen::Affine3d cannot exploit the SE(3) structure of transforms and thus needs to

  • perform an SVD to compute rotation()
  • perform real matrix inversion for inverse()

Hence, Affine3d should be actually avoided.

@@ -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());
Copy link
Contributor Author

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());
Copy link
Contributor Author

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) {
Copy link
Contributor Author

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.

Copy link
Member

@tfoote tfoote left a 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 tfoote merged commit f46e072 into ros:melodic-devel Oct 19, 2018
@rhaschke rhaschke deleted the eigen-isometry branch October 24, 2018 07:16
@rhaschke
Copy link
Contributor Author

@tfoote Do you plan a new release of geometry2 any time soon?
This is a blocker for moveit/moveit#1096.
Thanks.

@tfoote
Copy link
Member

tfoote commented Oct 24, 2018

Yes, I've just cleared the backlog of PRs and plan to release after the next indigo/kinetic sync

@davetcoleman
Copy link

This change needs to be ported to the ros2 version of geometry2 for moveit's conversion: https://github.com/ros2/geometry2

@davetcoleman
Copy link

Ideally the whole geometry2 would be synced to the ros2 version

seanyen pushed a commit to ms-iot/geometry2 that referenced this pull request Mar 22, 2021
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]>
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

Successfully merging this pull request may close these issues.

3 participants