From 756199b2ef3e6412467084f75e25fdf4ecd6e226 Mon Sep 17 00:00:00 2001 From: Moju Zhao Date: Fri, 16 Feb 2024 22:16:29 +0900 Subject: [PATCH] [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 76f0a7d60..78feaf2ca 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)) {