From 77b0b146f6b287c72e6d3614f91a8c022a406dea Mon Sep 17 00:00:00 2001 From: Moju Zhao Date: Fri, 16 Feb 2024 22:16:29 +0900 Subject: [PATCH 1/2] [Estimation][VIO] refactor the calculation of the transfrom from the world frame to the origin frame for VO/VIO --- aerial_robot_estimation/src/sensor/vo.cpp | 35 ++++++++++++----------- 1 file changed, 18 insertions(+), 17 deletions(-) diff --git a/aerial_robot_estimation/src/sensor/vo.cpp b/aerial_robot_estimation/src/sensor/vo.cpp index cfb1b7d40..62ac8166c 100644 --- a/aerial_robot_estimation/src/sensor/vo.cpp +++ b/aerial_robot_estimation/src/sensor/vo.cpp @@ -238,33 +238,34 @@ namespace sensor_plugin } } - /* step1: set the init offset from world to the baselink of UAV from egomotion estimation (e.g. yaw) */ - /** ^{w}H_{b} **/ - world_offset_tf_.setRotation(tf::createQuaternionFromYaw(estimator_->getState(State::YAW_BASE, aerial_robot_estimation::EGOMOTION_ESTIMATE)[0])); + /** step1: ^{w}H_{b'}, b': level frame of b **/ + tf::Transform w_bdash_f(tf::createQuaternionFromYaw(estimator_->getState(State::YAW_BASE, aerial_robot_estimation::EGOMOTION_ESTIMATE)[0])); - tf::Vector3 world_offset_pos = estimator_->getPos(Frame::BASELINK, aerial_robot_estimation::EGOMOTION_ESTIMATE); + tf::Vector3 baselink_pos = estimator_->getPos(Frame::BASELINK, aerial_robot_estimation::EGOMOTION_ESTIMATE); if(estimator_->getStateStatus(State::X_BASE, aerial_robot_estimation::EGOMOTION_ESTIMATE)) - world_offset_tf_.getOrigin().setX(world_offset_pos.x()); + w_bdash_f.getOrigin().setX(baselink_pos.x()); if(estimator_->getStateStatus(State::Y_BASE, aerial_robot_estimation::EGOMOTION_ESTIMATE)) - world_offset_tf_.getOrigin().setY(world_offset_pos.y()); + w_bdash_f.getOrigin().setY(baselink_pos.y()); if(estimator_->getStateStatus(State::Z_BASE, aerial_robot_estimation::EGOMOTION_ESTIMATE)) - world_offset_tf_.getOrigin().setZ(world_offset_pos.z()); + w_bdash_f.getOrigin().setZ(baselink_pos.z()); - /* set the init offset from world to the baselink of UAV if we know the ground truth */ + /* set the offset if we know the ground truth */ if(estimator_->getStateStatus(State::YAW_BASE, aerial_robot_estimation::GROUND_TRUTH)) { - world_offset_tf_.setOrigin(estimator_->getPos(Frame::BASELINK, aerial_robot_estimation::GROUND_TRUTH)); - world_offset_tf_.setRotation(tf::createQuaternionFromYaw(estimator_->getState(State::YAW_BASE, aerial_robot_estimation::GROUND_TRUTH)[0])); - - double y, p, r; world_offset_tf_.getBasis().getRPY(r, p, y); + w_bdash_f.setOrigin(estimator_->getPos(Frame::BASELINK, aerial_robot_estimation::GROUND_TRUTH)); + w_bdash_f.setRotation(tf::createQuaternionFromYaw(estimator_->getState(State::YAW_BASE, aerial_robot_estimation::GROUND_TRUTH)[0])); } - /* step2: also consider the offset tf from baselink to sensor */ - /** ^{w}H_{b} * ^{b}H_{vo} * ^{vo}H_{w_vo} = ^{w}H_{w_vo} **/ - world_offset_tf_ *= (sensor_tf_ * raw_sensor_tf.inverse()); + /** step2: ^{vo}H_{b'} **/ + tf::Transform vo_bdash_f = raw_sensor_tf * sensor_tf_.inverse(); // ^{vo}H_{b} + double r,p,y; + vo_bdash_f.getBasis().getRPY(r,p,y); + vo_bdash_f.setRotation(tf::createQuaternionFromYaw(y)); // ^{vo}H_{b'} + + /** step3: ^{w}H_{vo} = ^{w}H_{b'} * ^{b'}H_{vo} **/ + world_offset_tf_ = w_bdash_f * vo_bdash_f.inverse(); - //double y, p, r; raw_sensor_tf.getBasis().getRPY(r, p, y); - tf::Vector3 init_pos = (world_offset_tf_ * raw_sensor_tf * sensor_tf_.inverse()).getOrigin(); + tf::Vector3 init_pos = w_bdash_f.getOrigin(); for(auto& fuser : estimator_->getFuser(aerial_robot_estimation::EGOMOTION_ESTIMATE)) { From a9c5418726de1152d6ed5cea3146af43b36c4c99 Mon Sep 17 00:00:00 2001 From: Moju Zhao Date: Fri, 16 Feb 2024 22:57:41 +0900 Subject: [PATCH 2/2] [Estimation][VIO] add static tf from the world frame to the vo origin frame --- .../include/aerial_robot_estimation/sensor/vo.h | 5 +++++ aerial_robot_estimation/src/sensor/vo.cpp | 9 +++++++++ 2 files changed, 14 insertions(+) diff --git a/aerial_robot_estimation/include/aerial_robot_estimation/sensor/vo.h b/aerial_robot_estimation/include/aerial_robot_estimation/sensor/vo.h index b1281487a..a26e6efcf 100644 --- a/aerial_robot_estimation/include/aerial_robot_estimation/sensor/vo.h +++ b/aerial_robot_estimation/include/aerial_robot_estimation/sensor/vo.h @@ -37,11 +37,14 @@ #include #include +#include #include #include #include #include #include +#include + namespace sensor_plugin { @@ -109,6 +112,8 @@ namespace sensor_plugin double reference_timestamp_; aerial_robot_msgs::States vo_state_; + tf2_ros::StaticTransformBroadcaster static_broadcaster_; // publish the transfrom between the work and vo frame + void rosParamInit(); void servoControl(const ros::TimerEvent & e); void estimateProcess(); diff --git a/aerial_robot_estimation/src/sensor/vo.cpp b/aerial_robot_estimation/src/sensor/vo.cpp index 62ac8166c..1d9e324f6 100644 --- a/aerial_robot_estimation/src/sensor/vo.cpp +++ b/aerial_robot_estimation/src/sensor/vo.cpp @@ -265,8 +265,17 @@ namespace sensor_plugin /** step3: ^{w}H_{vo} = ^{w}H_{b'} * ^{b'}H_{vo} **/ world_offset_tf_ = w_bdash_f * vo_bdash_f.inverse(); + /* publish the offset tf if necessary */ + geometry_msgs::TransformStamped static_transformStamped; + static_transformStamped.header.stamp = vo_msg->header.stamp; + static_transformStamped.header.frame_id = "world"; + static_transformStamped.child_frame_id = vo_msg->header.frame_id; + tf::transformTFToMsg(world_offset_tf_, static_transformStamped.transform); + static_broadcaster_.sendTransform(static_transformStamped); + tf::Vector3 init_pos = w_bdash_f.getOrigin(); + for(auto& fuser : estimator_->getFuser(aerial_robot_estimation::EGOMOTION_ESTIMATE)) { string plugin_name = fuser.first;