Use external odometry in enu for px4

Hello,
As I am trying to apply vo from rtabmap, I do have problems converting the ros enu odometry message from rtabmap to feed into px4 vio. The general setup is to use gz (harmonic) as the simulator, and the x500_depth model with 5002 configuration in a warehouse (previously tugbot in a warehouse in which I removed tugbot), initially towards east. The drone is at some position else than (0,0,0) (the original tugbot location), and I use this position as the ros map and drone’s odom frame. rtabmap works well with an extra camera I added in the gz x500_depth model in rgbd mode. If I run the rtabmap as well as other simulation setups, the drone is flying normally and rtabmap showing somehow correct trajectory with drifts. But, once I bring up my node subscribing the odometry from rtabmap and feed it to fmu/in/visual_odometry with the navigation interface from ros2_cpp_lib, the vo would only work without rotation. If I takeoff and move around without rotation (always towards east), the vo works well, and the drone is much more stable than that without vo. But as long as I horizontally rotate the drone to point some direction else than east, the drone would start circling


And the larger the angle I yawed, the larger the radius of the circle is:

So I doubt if I had my vo node wrong. My conversion code:

    auto onRtabmapOdom = [this](const nav_msgs::msg::Odometry &odomMsg) {
      // PoseWithCovariance
      Eigen::Affine3d poseEnu;
      tf2::fromMsg(odomMsg.pose.pose, poseEnu);

      Eigen::Quaterniond qNed{frame_transforms::ros_to_px4_orientation(
          Eigen::Quaterniond(poseEnu.linear()))};
      qNed.normalize();

      Eigen::Vector3d poseNed{frame_transforms::enu_to_ned_local_frame(
          Eigen::Vector3d(poseEnu.translation()))};

      const frame_transforms::Covariance6d &poseCovEnu =
          odomMsg.pose.covariance;
      frame_transforms::Covariance6d poseCovNed =
          frame_transforms::transform_static_frame(
              poseCovEnu, frame_transforms::StaticTF::ENU_TO_NED);

      // TwistWithCovariance
      Eigen::Matrix<double, 6, 1> twistEnu;
      tf2::fromMsg(odomMsg.twist.twist, twistEnu);
      Eigen::Vector3d linearVelocityEnu{twistEnu.head(3)};
      Eigen::Vector3d linearVelocityNed{
          frame_transforms::enu_to_ned_local_frame(linearVelocityEnu)};

      // struct LocalPositionMeasurement does not contain angular velocity so
      // ignore the angualr velocity
      Eigen::Quaterniond qAngularVelocityEnu{
          frame_transforms::utils::quaternion::quaternion_from_euler(
              twistEnu.tail(3))};
      Eigen::Quaterniond qAngularVelocityNed{
          frame_transforms::ros_to_px4_orientation(qAngularVelocityEnu)};

      const frame_transforms::Covariance6d &twistCovEnu =
          odomMsg.twist.covariance;
      frame_transforms::Covariance6d twistCovNed =
          frame_transforms::transform_static_frame(
              twistCovEnu, frame_transforms::StaticTF::ENU_TO_NED);

      // Updating localPose

      px4_ros2::LocalPositionMeasurement localPose;
      localPose.timestamp_sample = odomMsg.header.stamp;

      // Pose part
      localPose.position_xy = poseNed.head(2).cast<float>();
      localPose.position_xy_variance =
          Eigen::Vector2f{poseCovNed[0], poseCovNed[1 + 1 * 6]};
      localPose.position_z = poseNed(2);
      localPose.position_z_variance = poseCovNed[2 + 2 * 6];

      localPose.velocity_xy = linearVelocityNed.head(2).cast<float>();
      localPose.velocity_xy_variance =
          Eigen::Vector2f{twistCovNed[0], twistCovNed[1 + 1 * 6]};
      localPose.velocity_z = linearVelocityNed(2);
      localPose.velocity_z_variance = twistCovNed[2 + 2 * 6];

      localPose.attitude_quaternion = qNed.cast<float>();
      localPose.attitude_variance =
          Eigen::Vector3f{static_cast<float>(poseCovNed[3 + 3 * 6]),
                          static_cast<float>(poseCovNed[4 + 4 * 6]),
                          static_cast<float>(poseCovNed[5 + 5 * 6])};

      this->_interface->updateLocalPosition(localPose);
    };

The pose frame and velocity frame of the vehicleodometry are both set to localned.

So I would doubt that there is something wrong in the conversion here. And if not I would appreciate any suggestion on how to further debug this problem. Thanks