From e00f1ed8615ecca8c8a2f55f6c1302fd53bce973 Mon Sep 17 00:00:00 2001 From: ymd-stella Date: Sun, 24 Sep 2023 15:55:33 +0900 Subject: [PATCH] Fixed handling of transformations in odom2d mode --- src/stella_vslam_ros.cc | 17 +++++++++-------- 1 file changed, 9 insertions(+), 8 deletions(-) diff --git a/src/stella_vslam_ros.cc b/src/stella_vslam_ros.cc index 276173a..6ffecbb 100644 --- a/src/stella_vslam_ros.cc +++ b/src/stella_vslam_ros.cc @@ -14,13 +14,12 @@ namespace { Eigen::Affine3d project_to_xy_plane(const Eigen::Affine3d& affine) { Eigen::Matrix4d mat = affine.matrix(); - mat(2, 0) = mat(2, 1) = 0.0; - mat(0, 2) = mat(1, 2) = 0.0; - mat(2, 2) = 1.0; mat(2, 3) = 0.0; Eigen::Translation trans(mat.col(3).head<3>()); - Eigen::Matrix3d rot(mat.block<3, 3>(0, 0)); - return trans * rot; + double rx = mat(0, 0); + double ry = mat(1, 0); + double yaw = std::atan2(ry, rx); + return trans * Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ()); } } // namespace @@ -73,13 +72,15 @@ void system::publish_pose(const Eigen::Matrix4d& cam_pose_wc, const rclcpp::Time tf2::durationFromSec(0.0)); Eigen::Affine3d camera_to_odom_affine = tf2::transformToEigen(camera_to_odom.transform); - Eigen::Affine3d map_to_odom3d_affine = map_to_camera_affine * camera_to_odom_affine; geometry_msgs::msg::TransformStamped map_to_odom_msg; if (odom2d_) { - map_to_odom_msg = tf2::eigenToTransform(project_to_xy_plane(map_to_odom3d_affine)); + Eigen::Affine3d map_to_camera_affine_2d = project_to_xy_plane(map_to_camera_affine * rot_ros_to_cv_map_frame_.inverse()) * rot_ros_to_cv_map_frame_; + Eigen::Affine3d camera_to_odom_affine_2d = (project_to_xy_plane(camera_to_odom_affine.inverse() * rot_ros_to_cv_map_frame_.inverse()) * rot_ros_to_cv_map_frame_).inverse(); + Eigen::Affine3d map_to_odom_affine_2d = map_to_camera_affine_2d * camera_to_odom_affine_2d; + map_to_odom_msg = tf2::eigenToTransform(map_to_odom_affine_2d); } else { - map_to_odom_msg = tf2::eigenToTransform(map_to_odom3d_affine); + map_to_odom_msg = tf2::eigenToTransform(map_to_camera_affine * camera_to_odom_affine); } tf2::TimePoint transform_timestamp = tf2_ros::fromMsg(stamp) + tf2::durationFromSec(transform_tolerance_); map_to_odom_msg.header.stamp = tf2_ros::toMsg(transform_timestamp);