global pose estimation from modular sensors
Factor graph is used to estimate global UAV pose by fusing multiple modular odometers such as VIO and GPS+IMU.
We have two odometries, one from VIO and one from GPS+IMU (flight controller pose estimation). Given the external calibration T_vio_uav, a transform from VIO coordinates to UAV coordinates, we need to transfer the UAV's pose measured from VIO (in VIO coordinates) to UAV's coordinates,
P_uav_uav = T_vio_uav * P_vio_vio * T_uav_vio
We don't consider covariance transform because the VIO's covariance from realsense T265 is fake, so we impose predefined covariance instead.