Ros2 odometry example

[klintan]

Added an example which would publish the odometry message information from openvslam. Seen some issues created looking for this. I needed it as well, so I thought it would be good to have an example for others as well.

This is a very rudimentary implementation, and there might even be some issues, but maybe someone will improve upon it if need be and perhaps add some more features etc.

This branch is checked out from ros2 latest commit, so this will hopefully pass all tests etc as soon as the main ros2 PR is merged.

edit: of course all commits are here as well, but I still think the other one should be a separate merge, and if needed I'll just merge master into this one after that PR merge.

[mirellameelo]

@klintan tks so much for sharing this example! I'm using exactly this part in run_localization.cc

auto publi(auto cam_pose_, auto odometry_pub_, auto node){
    Eigen::Matrix3d rotation_matrix = cam_pose_.block(0, 0, 3, 3);
    Eigen::Vector3d translation_vector = cam_pose_.block(0, 3, 3, 1);

    tf2::Matrix3x3 tf_rotation_matrix(rotation_matrix(0, 0), rotation_matrix(0, 1), rotation_matrix(0, 2),
                                      rotation_matrix(1, 0), rotation_matrix(1, 1), rotation_matrix(1, 2),
                                      rotation_matrix(2, 0), rotation_matrix(2, 1), rotation_matrix(2, 2));

    tf2::Vector3 tf_translation_vector(translation_vector(0), translation_vector(1), translation_vector(2));

    tf2::Transform transform_tf(tf_rotation_matrix, tf_translation_vector);
    nav_msgs::msg::Odometry odom_msg_;
    odom_msg_.header.stamp = node->now();
    odom_msg_.header.frame_id = "map";
    odom_msg_.child_frame_id = "base_link_frame";
    odom_msg_.pose.pose.orientation.x = transform_tf.getRotation().getX();
    odom_msg_.pose.pose.orientation.y = transform_tf.getRotation().getY();
    odom_msg_.pose.pose.orientation.z = transform_tf.getRotation().getZ();
    odom_msg_.pose.pose.orientation.w = transform_tf.getRotation().getW();
    odom_msg_.pose.pose.position.x = transform_tf.getOrigin().getX();
    odom_msg_.pose.pose.position.y = transform_tf.getOrigin().getY();
    odom_msg_.pose.pose.position.z = transform_tf.getOrigin().getZ();
    odometry_pub_->publish(odom_msg_);
}

Then, I'm receiving the odometry data into Rviz2, which shows a trajectory very unexpected. I tried a couple of things, also, I debug the data, but nothing... Do you (or anyone else reading this) have any idea what is going on? Tks in advance!

[mirellameelo]

I found some answers. An additional operation is necessary to transform the Openvslam coordinates to ROS coordinates.

It was necessary to add, right after declaring tf_translation_vector:

    tf2::Matrix3x3 tf_open_to_ros (0, 0, 1,
                                 -1, 0, 0,
                                 0,-1, 0);

    //Transform from orb coordinate system to ros coordinate system on camera coordinates
    tf_rotation_matrix = tf_open_to_ros*tf_rotation_matrix;
    tf_translation_vector = tf_open_to_ros*tf_translation_vector;

    tf_rotation_matrix = tf_rotation_matrix.transpose();
    tf_translation_vector = -(tf_rotation_matrix*tf_translation_vector);

Result:

[klintan]

I found some answers. An additional operation is necessary to transform the Openvslam coordinates to ROS coordinates.

It was necessary to add, right after declaring tf_translation_vector:

    tf2::Matrix3x3 tf_open_to_ros (0, 0, 1,
                                 -1, 0, 0,
                                 0,-1, 0);

    //Transform from orb coordinate system to ros coordinate system on camera coordinates
    tf_rotation_matrix = tf_open_to_ros*tf_rotation_matrix;
    tf_translation_vector = tf_open_to_ros*tf_translation_vector;

    tf_rotation_matrix = tf_rotation_matrix.transpose();
    tf_translation_vector = -(tf_rotation_matrix*tf_translation_vector);

Result:

Awesome, great work! I was about to look into it this weekend, but now I don't have to :) If you want to feel free to add a PR to my fork. I'm going to try to finish up the PR that is in so that we can get it into the openvslam repo.

I converted the old kitti2bag to be used in ROS2 https://github.com/klintan/kitti2bag2 so maybe we can add a ros2 kitti bag as test data as well.

[mirellameelo]

@klintan well, I hope you had a weekend more fun than figuring out bugs. hehe

So, in the odometry branch, I created more two executable: "run_slam_odometry" (to publish data into /odometry topic while running in SLAM mode); and "run_localization_odometry" (to publish odometry data into /odometry topic while running in localization mode), both with the viewer. I tested with Rviz2. It seems ok! Regarding "run_odometry", it ran and finished the process at the same time, I don't know what's going on... Sorry. =/

It is a good idea to PR those executables? Let me know any suggestions.

[anbello]

@mirellameelo and @klintan do you think could be useful if I do a PR with my ROS1 version of your work (see #347) ?