diff --git a/include/gnss/sensor/sensor_gnss.h b/include/gnss/sensor/sensor_gnss.h index ffed1ce29082e4421816528899e905b78206c493..25165629893402f569e45312af8c135bab0532ef 100644 --- a/include/gnss/sensor/sensor_gnss.h +++ b/include/gnss/sensor/sensor_gnss.h @@ -124,6 +124,7 @@ class SensorGnss : public SensorBase const Eigen::VectorXd& _pose_MAP_frame2, const Eigen::Vector3d& _t_ECEF_antena2); void initializeEnuMapYaw(const Eigen::VectorXd& _pose_MAP_frame1, const Eigen::VectorXd& _pose_MAP_frame2, const Eigen::Vector3d& _v_ECEF_antena1_antena2); + Eigen::Vector3d computeFrameAntennaPosEcef(const FrameBasePtr&) const; }; inline bool SensorGnss::isEnuDefined() const diff --git a/src/sensor/sensor_gnss.cpp b/src/sensor/sensor_gnss.cpp index 471384faccb0c9734dca708ef7c9946675c391f3..be27def15b5e3c6e2aae30437d674b47b926b0f4 100644 --- a/src/sensor/sensor_gnss.cpp +++ b/src/sensor/sensor_gnss.cpp @@ -218,6 +218,14 @@ void SensorGnss::setEnuMapYawState(const double& yaw_ENU_MAP) R_ENU_MAP_initialized_ = true; } +Eigen::Vector3d SensorGnss::computeFrameAntennaPosEcef(const FrameBasePtr& frm) const +{ + assert(isEnuDefined()); + assert(frm and frm->getP() and frm->getO()); + + return R_ENU_ECEF_.transpose() * (-t_ENU_ECEF_ + gettEnuMap() + getREnuMap() * (frm->getP()->getState() + Eigen::Quaterniond(Eigen::Vector4d(frm->getO()->getState())) * this->getP()->getState())); +} + } // namespace wolf diff --git a/test/gtest_factor_gnss_pseudo_range.cpp b/test/gtest_factor_gnss_pseudo_range.cpp index 77b2743894214f556bfbee30fcd948d6dc102e20..108d322f0f348dc18fdc0ffa8efd2cd3886629ec 100644 --- a/test/gtest_factor_gnss_pseudo_range.cpp +++ b/test/gtest_factor_gnss_pseudo_range.cpp @@ -129,13 +129,15 @@ void setUpProblem() ASSERT_FLOAT_EQ(gnss_sensor->getEnuMapPitch()->getState()(0), rpy_enu_map(1)); ASSERT_FLOAT_EQ(gnss_sensor->getEnuMapYaw()->getState()(0), rpy_enu_map(2)); ASSERT_MATRIX_APPROX(gnss_sensor->getEnuMapTranslation()->getState(), t_enu_map, 1e-3); - // Antena + // extrinsics ASSERT_MATRIX_APPROX(gnss_sensor->getP()->getState(), t_base_antena, 1e-3); // Frame ASSERT_MATRIX_APPROX(frm->getP()->getState(), t_map_base, 1e-3); ASSERT_MATRIX_APPROX(frm->getO()->getState(), q_map_base.coeffs(), 1e-3); - // clock drift + // clock drift ASSERT_MATRIX_APPROX(clock_drift, cap->getStateBlock('T')->getState(), 1e-3); + // composition + ASSERT_MATRIX_APPROX(gnss_sensor->computeFrameAntennaPosEcef(frm), t_ecef_antena, 1e-3); } void fixAllStates()