diff --git a/.gitlab-ci.yml b/.gitlab-ci.yml
index f6e2ba23ed1e793e8072ff66bfa71531f5637010..e0272a1524b72a3895e0750863dc86dcb8c9cb2f 100644
--- a/.gitlab-ci.yml
+++ b/.gitlab-ci.yml
@@ -73,8 +73,7 @@ stages:
- if [ -d wolf ]; then
- echo "directory wolf exists"
- cd wolf
- - git checkout devel
- - git pull
+ - git fetch --all
- git checkout $WOLF_CORE_BRANCH
- git pull
- else
@@ -92,10 +91,10 @@ stages:
- if [ -d gnss_utils ]; then
- echo "directory gnss_utils exists"
- cd gnss_utils
- - git checkout main
- - git pull
+ - git fetch --all
- git checkout ${GNSSUTILS_BRANCH}
- git pull
+ - git submodule update
- else
- git clone -b ${GNSSUTILS_BRANCH} ssh://git@gitlab.iri.upc.edu:2202/mobile_robotics/gauss_project/gnss_utils.git
- cd gnss_utils
diff --git a/CMakeLists.txt b/CMakeLists.txt
index f3030d47ebc43bc959b318fea80ab97d570f965d..b0e32133f73896f6e2917ec60561308ce939e62b 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -105,6 +105,7 @@ SET(HDRS_CAPTURE
SET(HDRS_FACTOR
include/${PROJECT_NAME}/factor/factor_gnss_fix_2d.h
include/${PROJECT_NAME}/factor/factor_gnss_fix_3d.h
+ include/${PROJECT_NAME}/factor/factor_gnss_fix_3d_with_clock.h
include/${PROJECT_NAME}/factor/factor_gnss_pseudo_range.h
include/${PROJECT_NAME}/factor/factor_gnss_tdcp.h
include/${PROJECT_NAME}/factor/factor_gnss_tdcp_2d.h
diff --git a/include/gnss/capture/capture_gnss_fix.h b/include/gnss/capture/capture_gnss_fix.h
index c894d3e7f887ee2deaed7241f9a3fc819f0e8ace..700b43d5d53b9f2c1350d0c243f1da54a09e06a2 100644
--- a/include/gnss/capture/capture_gnss_fix.h
+++ b/include/gnss/capture/capture_gnss_fix.h
@@ -37,42 +37,95 @@ WOLF_PTR_TYPEDEFS(CaptureGnssFix);
class CaptureGnssFix : public CaptureBase
{
protected:
- Eigen::Vector3d position_ECEF_; ///< position in ECEF coordinates.
- Eigen::Matrix3d covariance_ECEF_; ///< Noise of the fix in ECEF coordinates.
+ Eigen::Vector4d fix_ECEF_; ///< position in ECEF coordinates and clock_bias.
+ Eigen::Matrix4d covariance_ECEF_; ///< Noise of the fix in ECEF coordinates.
TimeStamp ts_GPST_; ///< Time stamp in GPS time
public:
- CaptureGnssFix(const TimeStamp& _ts, SensorBasePtr _sensor_ptr, const Eigen::Vector3d& _position, const Eigen::Matrix3d& _covariance, bool _ecef_coordinates = true);
+ CaptureGnssFix(const TimeStamp&,
+ SensorBasePtr,
+ const Eigen::Vector4d&,
+ const Eigen::Matrix4d&,
+ bool _ecef_coordinates = true);
~CaptureGnssFix() override;
- const Eigen::Vector3d& getPositionEcef() const;
- const Eigen::Matrix3d& getCovarianceEcef() const;
- void getPositionAndCovarianceEcef(Eigen::Vector3d& position, Eigen::Matrix3d& cov) const;
- const TimeStamp& getGpsTimeStamp() const;
- void setGpsTimeStamp(const TimeStamp& _ts_GPST);
+ Eigen::Vector4d getFixEcef() const;
+ Eigen::Matrix4d getFixCovarianceEcef() const;
+ double getClockBias() const;
+ double getClockBiasVariance() const;
+ Eigen::Vector3d getPositionEcef() const;
+ Eigen::Matrix3d getPositionCovarianceEcef() const;
+ void getPositionAndCovarianceEcef(Eigen::Vector3d&, Eigen::Matrix3d&) const;
+ TimeStamp getGpsTimeStamp() const;
+
+ void setFixEcef(const Eigen::Vector4d&);
+ void setFixCovarianceEcef(const Eigen::Matrix4d&);
+ void setClockBias(const double&);
+ void setPositionEcef(const Eigen::Vector3d&);
+ void setGpsTimeStamp(const TimeStamp&);
};
-inline const Eigen::Vector3d& CaptureGnssFix::getPositionEcef() const
+inline Eigen::Vector4d CaptureGnssFix::getFixEcef() const
{
- return position_ECEF_;
+ return fix_ECEF_;
}
-inline const Eigen::Matrix3d& CaptureGnssFix::getCovarianceEcef() const
+inline Eigen::Matrix4d CaptureGnssFix::getFixCovarianceEcef() const
{
return covariance_ECEF_;
}
+inline double CaptureGnssFix::getClockBias() const
+{
+ return fix_ECEF_(3);
+}
+
+inline double CaptureGnssFix::getClockBiasVariance() const
+{
+ return covariance_ECEF_(3,3);
+}
+
+inline Eigen::Vector3d CaptureGnssFix::getPositionEcef() const
+{
+ return fix_ECEF_.head<3>();
+}
+
+inline Eigen::Matrix3d CaptureGnssFix::getPositionCovarianceEcef() const
+{
+ return covariance_ECEF_.topLeftCorner<3,3>();
+}
+
inline void CaptureGnssFix::getPositionAndCovarianceEcef(Eigen::Vector3d& position, Eigen::Matrix3d& cov) const
{
- position = position_ECEF_;
- cov = covariance_ECEF_;
+ position = fix_ECEF_.head<3>();
+ cov = covariance_ECEF_.topLeftCorner<3,3>();
}
-inline const wolf::TimeStamp& CaptureGnssFix::getGpsTimeStamp() const
+inline wolf::TimeStamp CaptureGnssFix::getGpsTimeStamp() const
{
return ts_GPST_;
}
+inline void CaptureGnssFix::setFixEcef(const Eigen::Vector4d& _fix_ECEF)
+{
+ fix_ECEF_ = _fix_ECEF;
+}
+
+inline void CaptureGnssFix::setFixCovarianceEcef(const Eigen::Matrix4d& _covariance_ECEF)
+{
+ covariance_ECEF_ = _covariance_ECEF;
+}
+
+inline void CaptureGnssFix::setClockBias(const double& _clock_bias)
+{
+ fix_ECEF_(3) = _clock_bias;
+}
+
+inline void CaptureGnssFix::setPositionEcef(const Eigen::Vector3d& _fix_position)
+{
+ fix_ECEF_.head<3>() = _fix_position;
+}
+
inline void CaptureGnssFix::setGpsTimeStamp(const TimeStamp& _ts_GPST)
{
ts_GPST_ = _ts_GPST;
diff --git a/include/gnss/factor/factor_gnss_fix_2d.h b/include/gnss/factor/factor_gnss_fix_2d.h
index 26a8735be406bfdadb7c62f52dd0c3507adf21ed..0ee135f16c8e76e430f0464251da42731c9e53ce 100644
--- a/include/gnss/factor/factor_gnss_fix_2d.h
+++ b/include/gnss/factor/factor_gnss_fix_2d.h
@@ -89,35 +89,34 @@ inline bool FactorGnssFix2d::operator ()(const T* const _x,
{
Eigen::Map<const Eigen::Matrix<T,2,1> > t_map_base(_x);
Eigen::Matrix<T,2,2> R_map_base = Eigen::Rotation2D<T>(_o[0]).matrix();
- Eigen::Map<const Eigen::Matrix<T,3,1> > t_base_antena(_x_antena);
+ Eigen::Map<const Eigen::Matrix<T,2,1> > t_base_antena(_x_antena);
Eigen::Map<Eigen::Matrix<T,3,1> > residuals_ECEF(_residuals);
- Eigen::Matrix<T,3,3> R_ENU_ECEF = sensor_gnss_ptr_->getREnuEcef().cast<T>();
+ Eigen::Quaternion<T> q_ENU_ECEF = sensor_gnss_ptr_->getQEnuEcef().cast<T>();
Eigen::Matrix<T,3,1> t_ENU_ECEF = sensor_gnss_ptr_->gettEnuEcef().cast<T>();
Eigen::Matrix<T,2,3> R_map_ENU = sensor_gnss_ptr_->computeREnuMap(_roll_ENU_map[0], _pitch_ENU_map[0], _yaw_ENU_map[0]).transpose().topRows(2);
Eigen::Map<const Eigen::Matrix<T,3,1> > t_ENU_map(_t_ENU_map);
- Eigen::Matrix<T,3,1> fix_ECEF = getMeasurement().cast<T>();
//std::cout << "computeREnuMap():\n" << sensor_gnss_ptr_->computeREnuMap((const double) _roll[0], (const double) _pitch[0], (const double) _yaw[0]).transpose().topRows(2) << std::endl;
- //std::cout << "getREnuEcef():\n" << sensor_gnss_ptr_->gettEnuEcef() << std::endl;
+ //std::cout << "getQEnuEcef():\n" << sensor_gnss_ptr_->getQEnuEcef() << std::endl;
//std::cout << "gettEnuEcef(): " << sensor_gnss_ptr_->gettEnuEcef().transpose() << std::endl;
// Transform ECEF 3d Fix Feature to 2d Fix in Map coordinates (removing z)
//std::cout << "R_map_ENU:\n\t" << R_map_ENU(0,0) << "\t" << R_map_ENU(0,1) << "\t" << R_map_ENU(0,2) << "\n\t" << R_map_ENU(1,0) << "\t" << R_map_ENU(1,1) << "\t" << R_map_ENU(1,2) << std::endl;
- //std::cout << "R_ENU_ECEF:\n\t" << R_ENU_ECEF(0,0) << "\t" << R_ENU_ECEF(0,1) << "\t" << R_ENU_ECEF(0,2) << "\n\t" << R_ENU_ECEF(1,0) << "\t" << R_ENU_ECEF(1,1) << "\t" << R_ENU_ECEF(1,2) << "\n\t" << R_ENU_ECEF(2,0) << "\t" << R_ENU_ECEF(2,1) << "\t" << R_ENU_ECEF(2,2) << std::endl;
+ //std::cout << "q_ENU_ECEF:\t" << q_ENU_ECEF.x() << "\t" << q_ENU_ECEF.y() << "\t" << q_ENU_ECEF.z() << "\t" << q_ENU_ECEF.w() << std::endl;
//std::cout << "t_ENU_ECEF:\n\t" << t_ENU_ECEF(0) << "\n\t" << t_ENU_ECEF(1) << "\n\t" << t_ENU_ECEF(2) << std::endl;
- Eigen::Matrix<T,2,1> fix_map = R_map_ENU * (R_ENU_ECEF * fix_ECEF + t_ENU_ECEF - t_ENU_map);
+ Eigen::Matrix<T,2,1> fix_map = R_map_ENU * (q_ENU_ECEF * getMeasurement().head<3>().cast<T>() + t_ENU_ECEF - t_ENU_map);
//std::cout << "fix 3d:\n\t" << getMeasurement()(0) << "\n\t" << getMeasurement()(1) << "\n\t" << getMeasurement()(2) << std::endl;
//std::cout << "fix_map:\n\t" << fix_map[0] << "\n\t" << fix_map[1] << std::endl;
// Antena position in Map coordinates
- Eigen::Matrix<T,2,1> antena_map = R_map_base * t_base_antena.head(2) + t_map_base;
+ Eigen::Matrix<T,2,1> antena_map = R_map_base * t_base_antena + t_map_base;
//std::cout << "antena_map:\n\t" << antena_map[0] << "\n\t" << antena_map[1] << std::endl;
// Compute residual rotating information matrix to 2d Map coordinates
// Covariance & information are rotated by R*S*R' so for the squred root upper (or right) R*L*U*R'->U*R'
- // In this case R = R_map_ENU * R_ENU_ECEF, then R' = R_ENU_ECEF' * R_map_ENU'
- residuals_ECEF = (getMeasurementSquareRootInformationUpper().cast<T>() * R_ENU_ECEF.transpose() * R_map_ENU.transpose()) * (antena_map - fix_map);
+ // In this case R = R_map_ENU * q_ENU_ECEF, then R' = q_ENU_ECEF' * R_map_ENU'
+ residuals_ECEF = (getMeasurementSquareRootInformationUpper().topLeftCorner<3,3>() * q_ENU_ECEF.conjugate() * R_map_ENU.transpose()) * (antena_map - fix_map);
//std::cout << "residuals_ECEF:\n\t" << _residuals[0] << "\n\t" << _residuals[1] << "\n\t" << _residuals[2]<< std::endl;
return true;
diff --git a/include/gnss/factor/factor_gnss_fix_3d.h b/include/gnss/factor/factor_gnss_fix_3d.h
index 650a1055f52b804bccf1f46352b71b3819a56721..5ebb4dcb3d8e4a4a25a132e9f188d1b358f45e2e 100644
--- a/include/gnss/factor/factor_gnss_fix_3d.h
+++ b/include/gnss/factor/factor_gnss_fix_3d.h
@@ -89,25 +89,18 @@ inline bool FactorGnssFix3d::operator ()(const T* const _x,
Eigen::Map<const Eigen::Matrix<T,3,1> > t_map_base(_x);
Eigen::Map<const Eigen::Quaternion<T> > q_map_base(_o);
Eigen::Map<const Eigen::Matrix<T,3,1> > t_base_antena(_x_antena);
- Eigen::Map<Eigen::Matrix<T,3,1> > residuals_ECEF(_residuals);
+ Eigen::Map<Eigen::Matrix<T,3,1> > residuals(_residuals);
- Eigen::Matrix<T,3,3> R_ENU_ECEF = sensor_gnss_ptr_->getREnuEcef().cast<T>();
+ Eigen::Quaternion<T> q_ECEF_ENU = sensor_gnss_ptr_->getQEnuEcef().cast<T>().conjugate();
Eigen::Matrix<T,3,1> t_ENU_ECEF = sensor_gnss_ptr_->gettEnuEcef().cast<T>();
- Eigen::Matrix<T,3,3> R_map_ENU = sensor_gnss_ptr_->computeREnuMap(_roll_ENU_map[0], _pitch_ENU_map[0], _yaw_ENU_map[0]).transpose();
+ Eigen::Quaternion<T> q_ENU_map = sensor_gnss_ptr_->computeQEnuMap(_roll_ENU_map[0], _pitch_ENU_map[0], _yaw_ENU_map[0]);
Eigen::Map<const Eigen::Matrix<T,3,1> > t_ENU_map(_t_ENU_map);
- Eigen::Matrix<T,3,1> fix_ECEF = getMeasurement().cast<T>();
- // Transform ECEF 3d Fix Feature to Map coordinates
- Eigen::Matrix<T,3,1> fix_map = R_map_ENU * (R_ENU_ECEF * fix_ECEF + t_ENU_ECEF - t_ENU_map);
-
- // Antena position in Map coordinates
- Eigen::Matrix<T,3,1> antena_map = q_map_base * t_base_antena + t_map_base;
-
- // Compute residual rotating information matrix in Map coordinates
- // Covariance & information are rotated by R*S*R' so for the squred root upper (or right) R*L*U*R'->U*R'
- // In this case R = R_map_ENU * R_ENU_ECEF, then R' = R_ENU_ECEF' * R_map_ENU'
- residuals_ECEF = (getMeasurementSquareRootInformationUpper().cast<T>() * R_ENU_ECEF.transpose() * R_map_ENU.transpose()) * (antena_map - fix_map);
+ // Expected antena position in ECEF coordinates
+ Eigen::Matrix<T,3,1> fix_expected = q_ECEF_ENU * (q_ENU_map * (q_map_base * t_base_antena + t_map_base) + t_ENU_map - t_ENU_ECEF);
+ // Compute residual
+ residuals = getMeasurementSquareRootInformationUpper().topLeftCorner<3,3>() * (fix_expected - getMeasurement().head<3>());
return true;
}
diff --git a/include/gnss/factor/factor_gnss_fix_3d_with_clock.h b/include/gnss/factor/factor_gnss_fix_3d_with_clock.h
new file mode 100644
index 0000000000000000000000000000000000000000..ec94aeb77d66babbdfd70b5f0640107a1dd74488
--- /dev/null
+++ b/include/gnss/factor/factor_gnss_fix_3d_with_clock.h
@@ -0,0 +1,116 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+
+#pragma once
+
+//Wolf includes
+#include "core/common/wolf.h"
+#include "core/factor/factor_autodiff.h"
+#include "core/frame/frame_base.h"
+
+namespace wolf {
+
+WOLF_PTR_TYPEDEFS(FactorGnssFix3dWithClock);
+
+class FactorGnssFix3dWithClock : public FactorAutodiff<FactorGnssFix3dWithClock, 4, 3, 4, 3, 3, 1, 1, 1, 1>
+{
+ protected:
+ SensorGnssPtr sensor_gnss_ptr_;
+
+ public:
+
+ FactorGnssFix3dWithClock(FeatureBasePtr& _ftr_ptr,
+ const SensorGnssPtr& _sensor_gnss_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status = FAC_ACTIVE) :
+ FactorAutodiff("FactorGnssFix3dWithClock",
+ TOP_ABS,
+ _ftr_ptr,
+ nullptr,
+ nullptr,
+ nullptr,
+ nullptr,
+ _processor_ptr,
+ _apply_loss_function,
+ _status,
+ _ftr_ptr->getFrame()->getP(),
+ _ftr_ptr->getFrame()->getO(),
+ _sensor_gnss_ptr->getP(),
+ _sensor_gnss_ptr->getEnuMapTranslation(),
+ _sensor_gnss_ptr->getEnuMapRoll(),
+ _sensor_gnss_ptr->getEnuMapPitch(),
+ _sensor_gnss_ptr->getEnuMapYaw(),
+ _ftr_ptr->getCapture()->getStateBlock('T')),
+ sensor_gnss_ptr_(_sensor_gnss_ptr)
+ {
+ WOLF_WARN_COND(!sensor_gnss_ptr_->isEnuDefined() && _status == FAC_ACTIVE, "Creating an ACTIVE GNSS Fix 3d factor without initializing ENU");
+ }
+
+ ~FactorGnssFix3dWithClock() override = default;
+
+ template<typename T>
+ bool operator ()(const T* const _x,
+ const T* const _o,
+ const T* const _x_antena,
+ const T* const _t_ENU_map,
+ const T* const _roll_ENU_map,
+ const T* const _pitch_ENU_map,
+ const T* const _yaw_ENU_map,
+ const T* const _clock_bias,
+ T* _residuals) const;
+
+};
+
+template<typename T>
+inline bool FactorGnssFix3dWithClock::operator ()(const T* const _x,
+ const T* const _o,
+ const T* const _x_antena,
+ const T* const _t_ENU_map,
+ const T* const _roll_ENU_map,
+ const T* const _pitch_ENU_map,
+ const T* const _yaw_ENU_map,
+ const T* const _clock_bias,
+ T* _residuals) const
+{
+ Eigen::Map<const Eigen::Matrix<T,3,1> > t_map_base(_x);
+ Eigen::Map<const Eigen::Quaternion<T> > q_map_base(_o);
+ Eigen::Map<const Eigen::Matrix<T,3,1> > t_base_antena(_x_antena);
+ Eigen::Map<Eigen::Matrix<T,4,1> > residuals(_residuals);
+
+ Eigen::Quaternion<T> q_ECEF_ENU = sensor_gnss_ptr_->getQEnuEcef().cast<T>().conjugate();
+ Eigen::Matrix<T,3,1> t_ENU_ECEF = sensor_gnss_ptr_->gettEnuEcef().cast<T>();
+ Eigen::Quaternion<T> q_ENU_map = sensor_gnss_ptr_->computeQEnuMap(_roll_ENU_map[0], _pitch_ENU_map[0], _yaw_ENU_map[0]);
+ Eigen::Map<const Eigen::Matrix<T,3,1> > t_ENU_map(_t_ENU_map);
+
+ // Expected antena position in ECEF coordinates
+ Eigen::Matrix<T,4,1> fix_expected;
+ fix_expected << q_ECEF_ENU * (q_ENU_map * (q_map_base * t_base_antena + t_map_base) + t_ENU_map - t_ENU_ECEF),
+ _clock_bias[0];
+
+ // Compute residual
+ residuals = getMeasurementSquareRootInformationUpper() * (fix_expected - getMeasurement());
+
+ return true;
+}
+
+} // namespace wolf
\ No newline at end of file
diff --git a/include/gnss/factor/factor_gnss_pseudo_range.h b/include/gnss/factor/factor_gnss_pseudo_range.h
index ac1825f5aed30fbd12f95b7367ed301109cdeffe..a2c1c69773199f19d634cd0b430dff0d92394095 100644
--- a/include/gnss/factor/factor_gnss_pseudo_range.h
+++ b/include/gnss/factor/factor_gnss_pseudo_range.h
@@ -38,9 +38,10 @@ class FactorGnssPseudoRange : public FactorAutodiff<FactorGnssPseudoRange, 1, 3,
{
protected:
SensorGnssPtr sensor_gnss_ptr_;
- Eigen::Vector3d satellite_ENU_, satellite_ECEF_;
+ mutable Eigen::Vector3d satellite_ENU_;
mutable double sagnac_correction_;
- mutable bool sagnac_set_;
+ mutable bool apply_sagnac_correction_;
+ mutable bool sats_ENU_computed_;
bool not_GPS_;
public:
@@ -48,6 +49,7 @@ class FactorGnssPseudoRange : public FactorAutodiff<FactorGnssPseudoRange, 1, 3,
FactorGnssPseudoRange(FeatureGnssSatellitePtr& _ftr_ptr,
const SensorGnssPtr& _sensor_gnss_ptr,
const ProcessorBasePtr& _processor_ptr,
+ bool _apply_sagnac_correction,
bool _apply_loss_function,
FactorStatus _status = FAC_ACTIVE) :
FactorAutodiff<FactorGnssPseudoRange, 1, 3, 4, 1, 1, 3, 3, 1, 1, 1>("FactorGnssPseudoRange",
@@ -74,16 +76,27 @@ class FactorGnssPseudoRange : public FactorAutodiff<FactorGnssPseudoRange, 1, 3,
_sensor_gnss_ptr->getEnuMapPitch(),
_sensor_gnss_ptr->getEnuMapYaw()),
sensor_gnss_ptr_(_sensor_gnss_ptr),
- satellite_ENU_(sensor_gnss_ptr_->getREnuEcef() * _ftr_ptr->getSatellite().pos + sensor_gnss_ptr_->gettEnuEcef()),
- satellite_ECEF_(_ftr_ptr->getSatellite().pos),
+ //satellite_ENU_(sensor_gnss_ptr_->getQEnuEcef() * _ftr_ptr->getSatellite().pos + sensor_gnss_ptr_->gettEnuEcef()),
+ satellite_ENU_(_ftr_ptr->getSatellite().pos),
sagnac_correction_(0),
- sagnac_set_(false),
+ apply_sagnac_correction_(_apply_sagnac_correction),
+ sats_ENU_computed_(false),
not_GPS_(_ftr_ptr->getSatellite().sys != SYS_GPS)
{
+ computeSatsEnu();
//WOLF_INFO("FactorPseudoRange: sat ", _ftr_ptr->getSatellite().sat,"\n\tpos = ", _ftr_ptr->getSatellite().pos.transpose(), "\n\tprange = ", _ftr_ptr->getMeasurement());
WOLF_WARN_COND(!sensor_gnss_ptr_->isEnuDefined() && _status == FAC_ACTIVE, "Creating FactorGnssPseudoRange without initializing ENU");
}
+ void computeSatsEnu() const
+ {
+ if (not sensor_gnss_ptr_->isEnuDefined())
+ return;
+
+ satellite_ENU_ = sensor_gnss_ptr_->getQEnuEcef() * satellite_ENU_ + sensor_gnss_ptr_->gettEnuEcef();
+ sats_ENU_computed_ = true;
+ }
+
~FactorGnssPseudoRange() override = default;
template<typename T>
@@ -115,41 +128,37 @@ inline bool FactorGnssPseudoRange::operator ()(const T* const _x,
const T* const _yaw_ENU_map,
T* _residual) const
{
+ if (not sats_ENU_computed_)
+ computeSatsEnu();
+
+ if (not sats_ENU_computed_)
+ {
+ WOLF_WARN("Evaluating a FactorGnssPseudoRange without initializing ENU! returning zero residual.");
+ _residual[0] = T(0);
+ return true;
+ }
+
Eigen::Map<const Eigen::Matrix<T,3,1> > t_map_base(_x);
Eigen::Map<const Eigen::Quaternion<T> > q_map_base(_o);
Eigen::Map<const Eigen::Matrix<T,3,1> > t_base_antena(_x_antena);
Eigen::Map<const Eigen::Matrix<T,3,1> > t_ENU_map(_t_ENU_map);
- /* NOT EFFICIENT IMPLE;ENTATION
-
- Eigen::Matrix<T,3,3> R_ENU_map = sensor_gnss_ptr_->computeREnuMap(_roll_ENU_map[0], _pitch_ENU_map[0], _yaw_ENU_map[0]);
-
- // Antenna position in ENU coordinates
- Eigen::Matrix<T,3,1> antena_ENU = t_ENU_map + R_ENU_map * (t_map_base + q_map_base * t_base_antena);
- */
-
/* SAGNAC EFFECT CORRECTION
* It should be recomputed with the new antenna position in ECEF coordinates,
* but it is multiplied by a factor of 1e-14 (earth rotation / CLIGHT).
* We use instead a precomputed sagnac effect taking the first value of antenna position
*/
- if (!sagnac_set_)
+ if (apply_sagnac_correction_)
{
- Eigen::Matrix<T,3,1> antena_ENU = t_ENU_map + sensor_gnss_ptr_->computeREnuMap(_roll_ENU_map[0], _pitch_ENU_map[0], _yaw_ENU_map[0]) * (t_map_base + q_map_base * t_base_antena);
+ Eigen::Matrix<T,3,1> antena_ENU = t_ENU_map + sensor_gnss_ptr_->computeQEnuMap(_roll_ENU_map[0], _pitch_ENU_map[0], _yaw_ENU_map[0]) * (t_map_base + q_map_base * t_base_antena);
initSagnac(antena_ENU);
}
// Expected pseudo-range
- T exp = (t_ENU_map + sensor_gnss_ptr_->computeREnuMap(_roll_ENU_map[0], _pitch_ENU_map[0], _yaw_ENU_map[0]) * (t_map_base + q_map_base * t_base_antena) - satellite_ENU_.cast<T>()).norm() + // norm
+ T exp = (t_ENU_map + sensor_gnss_ptr_->computeQEnuMap(_roll_ENU_map[0], _pitch_ENU_map[0], _yaw_ENU_map[0]) * (t_map_base + q_map_base * t_base_antena) - satellite_ENU_.cast<T>()).norm() + // norm
sagnac_correction_ + // sagnac correction (precomputed)
_clock_bias[0] + // receiver clock bias (w.r.t. GPS clock)
(not_GPS_ ? _clock_bias_constellation[0] : T(0)); // interconstellation clock bias
- /*
- T exp = (antena_ENU-satellite_ENU_.cast<T>()).norm() + // norm
- sagnac_correction_ + // sagnac correction (precomputed)
- _clock_bias[0] + // receiver clock bias (w.r.t. GPS clock)
- (not_GPS_ ? _clock_bias_constellation[0] : T(0)); // interconstellation clock bias
- */
//std::cout << "sat " << std::static_pointer_cast<FeatureGnssSatellite>(getFeature())->getSatellite().sat << std::endl;
//std::cout << "\tsys " << std::static_pointer_cast<FeatureGnssSatellite>(getFeature())->getSatellite().sys << std::endl;
@@ -180,9 +189,10 @@ void FactorGnssPseudoRange::initSagnac(const Eigen::Matrix<T,3,1>& antena_ENU) c
template<>
void FactorGnssPseudoRange::initSagnac<double>(const Eigen::Matrix<double,3,1>& antena_ENU) const
{
- Eigen::Vector3d antena_ECEF = sensor_gnss_ptr_->getREnuEcef().transpose() * (antena_ENU - sensor_gnss_ptr_->gettEnuEcef());
- sagnac_correction_ = GnssUtils::computeSagnacCorrection(antena_ECEF, satellite_ECEF_);
- sagnac_set_ = true;
+ Eigen::Vector3d antena_ECEF = sensor_gnss_ptr_->getQEnuEcef().conjugate() * (antena_ENU - sensor_gnss_ptr_->gettEnuEcef());
+ Eigen::Vector3d sat_ECEF = sensor_gnss_ptr_->getQEnuEcef().conjugate() * (satellite_ENU_ - sensor_gnss_ptr_->gettEnuEcef());
+ sagnac_correction_ = GnssUtils::computeSagnacCorrection(antena_ECEF, sat_ECEF);
+ apply_sagnac_correction_ = false;
}
} // namespace wolf
diff --git a/include/gnss/factor/factor_gnss_tdcp.h b/include/gnss/factor/factor_gnss_tdcp.h
index fb19510fb738d8d4e94d86ff849d14196efc3125..c312c57f9a950fc258e9c7dbc677723109d6e75b 100644
--- a/include/gnss/factor/factor_gnss_tdcp.h
+++ b/include/gnss/factor/factor_gnss_tdcp.h
@@ -39,9 +39,10 @@ class FactorGnssTdcp : public FactorAutodiff<FactorGnssTdcp, 1, 3, 4, 1, 3, 4, 1
protected:
SensorGnssPtr sensor_gnss_ptr_;
mutable double d_pseudo_range_;
- mutable bool sagnac_set_;
+ mutable bool apply_sagnac_correction_;
+ mutable bool sats_ENU_computed_;
double std_dev_;
- Eigen::Vector3d satellite_ENU_k_, satellite_ENU_r_;
+ mutable Eigen::Vector3d satellite_ENU_k_, satellite_ENU_r_;
public:
@@ -50,6 +51,7 @@ class FactorGnssTdcp : public FactorAutodiff<FactorGnssTdcp, 1, 3, 4, 1, 3, 4, 1
FeatureGnssSatellitePtr& _ftr_k,
const SensorGnssPtr& _sensor_gnss_ptr,
const ProcessorBasePtr& _processor_ptr,
+ bool _apply_sagnac_correction,
bool _apply_loss_function,
FactorStatus _status = FAC_ACTIVE) :
FactorAutodiff<FactorGnssTdcp, 1, 3, 4, 1, 3, 4, 1, 3, 3, 1, 1, 1>("FactorGnssTdcp",
@@ -74,20 +76,31 @@ class FactorGnssTdcp : public FactorAutodiff<FactorGnssTdcp, 1, 3, 4, 1, 3, 4, 1
_sensor_gnss_ptr->getEnuMapPitch(),
_sensor_gnss_ptr->getEnuMapYaw()),
sensor_gnss_ptr_(_sensor_gnss_ptr),
- sagnac_set_(false),
+ apply_sagnac_correction_(_apply_sagnac_correction),
+ sats_ENU_computed_(false),
std_dev_(_std_dev),
- satellite_ENU_k_(sensor_gnss_ptr_->getREnuEcef() * _ftr_k->getSatellite().pos + sensor_gnss_ptr_->gettEnuEcef()),
- satellite_ENU_r_(sensor_gnss_ptr_->getREnuEcef() * _ftr_r->getSatellite().pos + sensor_gnss_ptr_->gettEnuEcef())
+ satellite_ENU_k_(_ftr_k->getSatellite().pos),
+ satellite_ENU_r_(_ftr_r->getSatellite().pos)
{
assert(_ftr_r != _ftr_k);
assert(_ftr_r->getCapture()->getStateBlock('T') != nullptr);
assert(_ftr_k->getCapture()->getStateBlock('T') != nullptr);
- WOLF_WARN_COND(!sensor_gnss_ptr_->isEnuDefined() && _status == FAC_ACTIVE, "Creating an FactorGnssTdcp without initializing ENU");
+ computeSatsEnu();
d_pseudo_range_ = _ftr_k->getRange().L_corrected - _ftr_r->getRange().L_corrected;
}
+ void computeSatsEnu() const
+ {
+ if (not sensor_gnss_ptr_->isEnuDefined())
+ return;
+
+ satellite_ENU_k_ = sensor_gnss_ptr_->getQEnuEcef() * satellite_ENU_k_ + sensor_gnss_ptr_->gettEnuEcef();
+ satellite_ENU_r_ = sensor_gnss_ptr_->getQEnuEcef() * satellite_ENU_r_ + sensor_gnss_ptr_->gettEnuEcef();
+ sats_ENU_computed_ = true;
+ }
+
~FactorGnssTdcp() override = default;
template<typename T>
@@ -106,7 +119,6 @@ class FactorGnssTdcp : public FactorAutodiff<FactorGnssTdcp, 1, 3, 4, 1, 3, 4, 1
template<typename T>
void initSagnac(const Eigen::Matrix<T,3,1>& antena_r_ENU, const Eigen::Matrix<T,3,1>& antena_k_ENU) const;
-
};
template<typename T>
@@ -123,6 +135,16 @@ inline bool FactorGnssTdcp::operator ()(const T* const _x_r,
const T* const _yaw_ENU_map,
T* _residual) const
{
+ if (not sats_ENU_computed_)
+ computeSatsEnu();
+
+ if (not sats_ENU_computed_)
+ {
+ WOLF_WARN("Evaluating a FactorGnssTdcp without initializing ENU! returning zero residual.");
+ _residual[0] = T(0);
+ return true;
+ }
+
Eigen::Map<const Eigen::Matrix<T,3,1> > t_map_base_r(_x_r);
Eigen::Map<const Eigen::Quaternion<T> > q_map_base_r(_o_r);
Eigen::Map<const Eigen::Matrix<T,3,1> > t_map_base_k(_x_k);
@@ -130,7 +152,7 @@ inline bool FactorGnssTdcp::operator ()(const T* const _x_r,
Eigen::Map<const Eigen::Matrix<T,3,1> > t_base_antena(_x_antena);
Eigen::Map<const Eigen::Matrix<T,3,1> > t_ENU_map(_t_ENU_map);
- Eigen::Matrix<T,3,3> R_ENU_map = sensor_gnss_ptr_->computeREnuMap(_roll_ENU_map[0], _pitch_ENU_map[0], _yaw_ENU_map[0]);
+ Eigen::Quaternion<T> q_ENU_map = sensor_gnss_ptr_->computeQEnuMap(_roll_ENU_map[0], _pitch_ENU_map[0], _yaw_ENU_map[0]);
/* INEFFICIENT IMPLEMENTATION
// Antenna position in ECEF coordinates
@@ -170,17 +192,17 @@ inline bool FactorGnssTdcp::operator ()(const T* const _x_r,
* but it is multiplied by a factor of 1e-14 (earth rotation / CLIGHT).
* We use instead a precomputed sagnac effect taking the first values of antenna position
*/
- if (not sagnac_set_)
+ if (apply_sagnac_correction_)
{
- Eigen::Matrix<T,3,1> antena_r_ENU = t_ENU_map + sensor_gnss_ptr_->computeREnuMap(_roll_ENU_map[0], _pitch_ENU_map[0], _yaw_ENU_map[0]) * (t_map_base_r + q_map_base_r * t_base_antena);
- Eigen::Matrix<T,3,1> antena_k_ENU = t_ENU_map + sensor_gnss_ptr_->computeREnuMap(_roll_ENU_map[0], _pitch_ENU_map[0], _yaw_ENU_map[0]) * (t_map_base_k + q_map_base_k * t_base_antena);
+ Eigen::Matrix<T,3,1> antena_r_ENU = t_ENU_map + sensor_gnss_ptr_->computeQEnuMap(_roll_ENU_map[0], _pitch_ENU_map[0], _yaw_ENU_map[0]) * (t_map_base_r + q_map_base_r * t_base_antena);
+ Eigen::Matrix<T,3,1> antena_k_ENU = t_ENU_map + sensor_gnss_ptr_->computeQEnuMap(_roll_ENU_map[0], _pitch_ENU_map[0], _yaw_ENU_map[0]) * (t_map_base_k + q_map_base_k * t_base_antena);
initSagnac(antena_r_ENU, antena_k_ENU);
}
// Residual
- _residual[0] = ((t_ENU_map + R_ENU_map * (t_map_base_k + q_map_base_k * t_base_antena)-satellite_ENU_k_.cast<T>()).norm()
+ _residual[0] = ((t_ENU_map + q_ENU_map * (t_map_base_k + q_map_base_k * t_base_antena)-satellite_ENU_k_.cast<T>()).norm()
+ _clock_bias_k[0]
- - (t_ENU_map + R_ENU_map * (t_map_base_r + q_map_base_r * t_base_antena)-satellite_ENU_r_.cast<T>()).norm()
+ - (t_ENU_map + q_ENU_map * (t_map_base_r + q_map_base_r * t_base_antena)-satellite_ENU_r_.cast<T>()).norm()
- _clock_bias_r[0]
- d_pseudo_range_) / std_dev_;
@@ -195,17 +217,17 @@ void FactorGnssTdcp::initSagnac(const Eigen::Matrix<T,3,1>& antena_r_ENU, const
template<>
void FactorGnssTdcp::initSagnac<double>(const Eigen::Matrix<double,3,1>& antena_r_ENU, const Eigen::Matrix<double,3,1>& antena_k_ENU) const
{
- Eigen::Vector3d antena_r_ECEF = sensor_gnss_ptr_->getREnuEcef().transpose() * (antena_r_ENU - sensor_gnss_ptr_->gettEnuEcef());
- Eigen::Vector3d sat_r_ECEF = sensor_gnss_ptr_->getREnuEcef().transpose() * (satellite_ENU_r_ - sensor_gnss_ptr_->gettEnuEcef());
+ Eigen::Vector3d antena_r_ECEF = sensor_gnss_ptr_->getQEnuEcef().conjugate() * (antena_r_ENU - sensor_gnss_ptr_->gettEnuEcef());
+ Eigen::Vector3d sat_r_ECEF = sensor_gnss_ptr_->getQEnuEcef().conjugate() * (satellite_ENU_r_ - sensor_gnss_ptr_->gettEnuEcef());
double sagnac_corr_r = GnssUtils::computeSagnacCorrection(antena_r_ECEF, sat_r_ECEF);
- Eigen::Vector3d antena_k_ECEF = sensor_gnss_ptr_->getREnuEcef().transpose() * (antena_k_ENU - sensor_gnss_ptr_->gettEnuEcef());
- Eigen::Vector3d sat_k_ECEF = sensor_gnss_ptr_->getREnuEcef().transpose() * (satellite_ENU_k_ - sensor_gnss_ptr_->gettEnuEcef());
+ Eigen::Vector3d antena_k_ECEF = sensor_gnss_ptr_->getQEnuEcef().conjugate() * (antena_k_ENU - sensor_gnss_ptr_->gettEnuEcef());
+ Eigen::Vector3d sat_k_ECEF = sensor_gnss_ptr_->getQEnuEcef().conjugate() * (satellite_ENU_k_ - sensor_gnss_ptr_->gettEnuEcef());
double sagnac_corr_k = GnssUtils::computeSagnacCorrection(antena_k_ECEF, sat_k_ECEF);
d_pseudo_range_ += -sagnac_corr_k + sagnac_corr_r;
- sagnac_set_ = true;
+ apply_sagnac_correction_ = false;
}
} // namespace wolf
diff --git a/include/gnss/factor/factor_gnss_tdcp_2d.h b/include/gnss/factor/factor_gnss_tdcp_2d.h
index 049106a4d22b36ccb9818cb64d936cd5bb369e7c..33645c59fc50ca625884837883ca122f3c984e5e 100644
--- a/include/gnss/factor/factor_gnss_tdcp_2d.h
+++ b/include/gnss/factor/factor_gnss_tdcp_2d.h
@@ -41,24 +41,24 @@ class FactorGnssTdcp2d : public FactorAutodiff<FactorGnssTdcp2d, 3, 2, 1, 2, 1,
public:
FactorGnssTdcp2d(const FeatureBasePtr& _ftr_ptr, const FrameBasePtr& _frame_other_ptr, const SensorGnssPtr& _sensor_gnss_ptr, const ProcessorBasePtr& _processor_ptr, bool _apply_loss_function = false, FactorStatus _status = FAC_ACTIVE) :
- FactorAutodiff<FactorGnssTdcp2d, 3, 2, 1, 2, 1, 3, 1, 1, 1>("FactorGnssTdcp2d",
- TOP_GEOM,
- _ftr_ptr,
- _frame_other_ptr,
- nullptr,
- nullptr,
- nullptr,
- _processor_ptr,
- _apply_loss_function,
- _status,
- _frame_other_ptr->getP(),
- _frame_other_ptr->getO(),
- _ftr_ptr->getFrame()->getP(),
- _ftr_ptr->getFrame()->getO(),
- _sensor_gnss_ptr->getP(),
- _sensor_gnss_ptr->getEnuMapRoll(),
- _sensor_gnss_ptr->getEnuMapPitch(),
- _sensor_gnss_ptr->getEnuMapYaw()),
+ FactorAutodiff("FactorGnssTdcp2d",
+ TOP_GEOM,
+ _ftr_ptr,
+ _frame_other_ptr,
+ nullptr,
+ nullptr,
+ nullptr,
+ _processor_ptr,
+ _apply_loss_function,
+ _status,
+ _frame_other_ptr->getP(),
+ _frame_other_ptr->getO(),
+ _ftr_ptr->getFrame()->getP(),
+ _ftr_ptr->getFrame()->getO(),
+ _sensor_gnss_ptr->getP(),
+ _sensor_gnss_ptr->getEnuMapRoll(),
+ _sensor_gnss_ptr->getEnuMapPitch(),
+ _sensor_gnss_ptr->getEnuMapYaw()),
sensor_gnss_ptr_(_sensor_gnss_ptr)
{
WOLF_WARN_COND(!sensor_gnss_ptr_->isEnuDefined(), "Creating a GNSS SingleDiff 2D factor without initializing ENU");
@@ -81,35 +81,35 @@ class FactorGnssTdcp2d : public FactorAutodiff<FactorGnssTdcp2d, 3, 2, 1, 2, 1,
template<typename T>
inline bool FactorGnssTdcp2d::operator ()(const T* const _x1,
- const T* const _o1,
- const T* const _x2,
- const T* const _o2,
- const T* const _x_antena,
- const T* const _roll_ENU_MAP,
- const T* const _pitch_ENU_MAP,
- const T* const _yaw_ENU_MAP,
- T* _residuals) const
+ const T* const _o1,
+ const T* const _x2,
+ const T* const _o2,
+ const T* const _x_antena,
+ const T* const _roll_ENU_MAP,
+ const T* const _pitch_ENU_MAP,
+ const T* const _yaw_ENU_MAP,
+ T* _residuals) const
{
Eigen::Map<const Eigen::Matrix<T,2,1> > t_MAP_BASE1(_x1);
- Eigen::Matrix<T,2,2> R_MAP_BASE1 = Eigen::Rotation2D<T>(_o1[0]).matrix();
+ Eigen::Rotation2D<T> R_MAP_BASE1(_o1[0]);
Eigen::Map<const Eigen::Matrix<T,2,1> > t_MAP_BASE2(_x2);
- Eigen::Matrix<T,2,2> R_MAP_BASE2 = Eigen::Rotation2D<T>(_o2[0]).matrix();
- Eigen::Map<const Eigen::Matrix<T,3,1> > t_BASE_ANTENA(_x_antena);
+ Eigen::Rotation2D<T> R_MAP_BASE2(_o2[0]);
+ Eigen::Map<const Eigen::Matrix<T,2,1> > t_BASE_ANTENA(_x_antena);
Eigen::Map<Eigen::Matrix<T,3,1> > residuals_ECEF(_residuals);
- Eigen::Matrix<T,3,3> R_ENU_ECEF = sensor_gnss_ptr_->getREnuEcef().cast<T>();
+ Eigen::Quaternion<T> q_ENU_ECEF = sensor_gnss_ptr_->getQEnuEcef().cast<T>();
Eigen::Matrix<T,2,3> R_MAP_ENU = sensor_gnss_ptr_->computeREnuMap(_roll_ENU_MAP[0], _pitch_ENU_MAP[0], _yaw_ENU_MAP[0]).transpose().topRows(2);
// Transform ECEF 3D SingleDiff Feature to 2D SingleDiff in Map coordinates (removing z)
- Eigen::Matrix<T,2,1> measured_diff_2D_MAP = R_MAP_ENU * (R_ENU_ECEF * getMeasurement().cast<T>());
+ Eigen::Matrix<T,2,1> measured_diff_2D_MAP = R_MAP_ENU * (q_ENU_ECEF * getMeasurement().cast<T>());
// Substraction of expected antena positions in Map coordinates
- Eigen::Matrix<T,2,1> expected_diff_2D_MAP = (R_MAP_BASE2 * t_BASE_ANTENA.head(2) + t_MAP_BASE2) - (R_MAP_BASE1 * t_BASE_ANTENA.head(2) + t_MAP_BASE1);
+ Eigen::Matrix<T,2,1> expected_diff_2D_MAP = (R_MAP_BASE2 * t_BASE_ANTENA + t_MAP_BASE2) - (R_MAP_BASE1 * t_BASE_ANTENA + t_MAP_BASE1);
// Compute residual rotating information matrix to 2D Map coordinates
// Covariance & information are rotated by R*S*R' so for the squred root upper (or right) R*L*U*R'->U*R'
- // In this case R = R_2DMAP_ENU * R_ENU_ECEF, then R' = R_ENU_ECEF' * R_2DMAP_ENU'
- residuals_ECEF = (getMeasurementSquareRootInformationUpper().cast<T>() * R_ENU_ECEF.transpose() * R_MAP_ENU.transpose()) * (expected_diff_2D_MAP - measured_diff_2D_MAP);
+ // In this case R = R_2DMAP_ENU * q_ENU_ECEF, then R' = q_ENU_ECEF' * R_2DMAP_ENU'
+ residuals_ECEF = (getMeasurementSquareRootInformationUpper() * q_ENU_ECEF.conjugate() * R_MAP_ENU.transpose()) * (expected_diff_2D_MAP - measured_diff_2D_MAP);
//std::cout << "frame1: " << _x1[0] << " " << _x1[1] << " " << _o1[0] << std::endl;
//std::cout << "frame2: " << _x2[0] << " " << _x2[1] << " " << _o2[0] << std::endl;
diff --git a/include/gnss/factor/factor_gnss_tdcp_3d.h b/include/gnss/factor/factor_gnss_tdcp_3d.h
index 8a7f2ccd88a0e90ca22e74ec6156a324c602cb93..572a4ad738a2fc169dd87e8dfcf27deda62885b7 100644
--- a/include/gnss/factor/factor_gnss_tdcp_3d.h
+++ b/include/gnss/factor/factor_gnss_tdcp_3d.h
@@ -103,14 +103,14 @@ inline bool FactorGnssTdcp3d::operator ()(const T* const _x1,
Eigen::Map<const Eigen::Matrix<T,3,1>> t_BASE_ANTENA(_x_antena);
Eigen::Map<Eigen::Matrix<T,3,1> > residuals_ECEF(_residuals);
- Eigen::Matrix<T,3,3> R_ECEF_ENU = sensor_gnss_ptr_->getREnuEcef().transpose().cast<T>();
- Eigen::Matrix<T,3,3> R_ENU_MAP = sensor_gnss_ptr_->computeREnuMap(_roll_ENU_MAP[0], _pitch_ENU_MAP[0], _yaw_ENU_MAP[0]);
+ Eigen::Quaternion<T> q_ECEF_ENU = sensor_gnss_ptr_->getQEnuEcef().conjugate().cast<T>();
+ Eigen::Quaternion<T> q_ENU_MAP = sensor_gnss_ptr_->computeQEnuMap(_roll_ENU_MAP[0], _pitch_ENU_MAP[0], _yaw_ENU_MAP[0]);
// Expected displacement
- Eigen::Matrix<T,3,1> expected_ECEF = R_ECEF_ENU * R_ENU_MAP * ((q_MAP_BASE2 * t_BASE_ANTENA + t_MAP_BASE2) - (q_MAP_BASE1 * t_BASE_ANTENA + t_MAP_BASE1));
+ Eigen::Matrix<T,3,1> expected_ECEF = q_ECEF_ENU * q_ENU_MAP * ((q_MAP_BASE2 * t_BASE_ANTENA + t_MAP_BASE2) - (q_MAP_BASE1 * t_BASE_ANTENA + t_MAP_BASE1));
// Compute residual
- residuals_ECEF = getMeasurementSquareRootInformationUpper().cast<T>() * (expected_ECEF - getMeasurement().cast<T>());
+ residuals_ECEF = getMeasurementSquareRootInformationUpper() * (expected_ECEF - getMeasurement());
//std::cout << "frame1: " << _x1[0] << " " << _x1[1] << " " << _x1[2] << " " << _o1[0] << " " << _o1[1] << " " << _o1[2] << " " << _o1[3] << std::endl;
//std::cout << "frame2: " << _x2[0] << " " << _x2[1] << " " << _x2[2] << " " << _o2[0] << " " << _o2[1] << " " << _o2[2] << " " << _o2[3] << std::endl;
diff --git a/include/gnss/feature/feature_gnss_fix.h b/include/gnss/feature/feature_gnss_fix.h
index 60f8debf65e8e60e3d58703611cf7e3f42b5bc84..37d2acf42869fbfc67eed01fe316d9fe2cc96db3 100644
--- a/include/gnss/feature/feature_gnss_fix.h
+++ b/include/gnss/feature/feature_gnss_fix.h
@@ -49,12 +49,14 @@ class FeatureGnssFix : public FeatureBase
~FeatureGnssFix() override{};
private:
- GnssUtils::ComputePosOutput gnss_fix_output_;
+ GnssUtils::ComputePosOutput gnss_fix_output_;
};
inline FeatureGnssFix::FeatureGnssFix(const GnssUtils::ComputePosOutput& _gnss_fix_output) :
- FeatureBase("FeatureGnssFix", _gnss_fix_output.pos, _gnss_fix_output.pos_covar),
+ FeatureBase("FeatureGnssFix",
+ (Eigen::Vector4d() << _gnss_fix_output.pos, _gnss_fix_output.rcv_bias(0)).finished(),
+ _gnss_fix_output.sol_cov),
gnss_fix_output_(_gnss_fix_output)
{
//
diff --git a/include/gnss/processor/processor_tracker_gnss.h b/include/gnss/processor/processor_tracker_gnss.h
index 319ce9deb7fe70fe5d770578ae15ee81276e3c3d..fafecfac9f9ac5239ac33664ff5bad02541d07e7 100644
--- a/include/gnss/processor/processor_tracker_gnss.h
+++ b/include/gnss/processor/processor_tracker_gnss.h
@@ -40,13 +40,10 @@ struct ParamsProcessorTrackerGnss : public ParamsProcessorTrackerFeature
GnssUtils::Options gnss_opt;
GnssUtils::Options fix_opt{GnssUtils::default_options};
GnssUtils::TdcpOptions tdcp_params;
- double max_time_span;
bool remove_outliers, remove_outliers_tdcp, remove_outliers_with_fix;
- double outlier_residual_th;
- bool init_frames, pseudo_ranges, fix;
- double enu_map_fix_dist;
+ double max_time_span, outlier_residual_th, enu_map_fix_dist;
+ bool init_frames, pseudo_ranges, fix, tdcp_enabled, apply_sagnac_correction;
int min_sbas_sats;
- bool tdcp_enabled;
std::string tdcp_structure;
ParamsProcessorTrackerGnss() = default;
@@ -62,6 +59,7 @@ struct ParamsProcessorTrackerGnss : public ParamsProcessorTrackerFeature
fix = _server.getParam<bool> (prefix + _unique_name + "/fix");
pseudo_ranges = _server.getParam<bool> (prefix + _unique_name + "/pseudo_ranges");
min_sbas_sats = _server.getParam<int> (prefix + _unique_name + "/gnss/min_sbas_sats");
+ apply_sagnac_correction = _server.getParam<bool> (prefix + _unique_name + "/gnss/apply_sagnac_correction");
// GNSS OPTIONS (see rtklib.h)
gnss_opt.sateph = _server.getParam<int> (prefix + _unique_name + "/gnss/sateph"); // satellite ephemeris option: EPHOPT_BRDC(0):broadcast ephemeris, EPHOPT_PREC(1): precise ephemeris, EPHOPT_SBAS(2): broadcast + SBAS, EPHOPT_SSRAPC(3): broadcast + SSR_APC, EPHOPT_SSRCOM(4): broadcast + SSR_COM, EPHOPT_LEX(5): QZSS LEX ephemeris, EPHOPT_SBAS2(6):broadcast + SBAS(sats with SBAS corr and sats with BRDC eph), EPHOPT_SBAS3(7):broadcast + SBAS(EPHOPT_SBAS if possible, otherwise EPHOPT_SBAS2), EPHOPT_SBAS4(8):broadcast + SBAS(EPHOPT_SBAS if possible, otherwise EPHOPT_BRDC)
@@ -85,17 +83,17 @@ struct ParamsProcessorTrackerGnss : public ParamsProcessorTrackerFeature
tdcp_enabled = _server.getParam<bool>(prefix + _unique_name + "/gnss/tdcp/enabled");
if (tdcp_enabled)
{
- tdcp_structure = _server.getParam<std::string>(prefix + _unique_name + "/gnss/tdcp/structure");
- remove_outliers_tdcp = _server.getParam<bool> (prefix + _unique_name + "/gnss/tdcp/remove_outliers");
- tdcp_params.batch = _server.getParam<bool> (prefix + _unique_name + "/gnss/tdcp/batch");
- gnss_opt.carrier_opt.corr_iono = _server.getParam<bool> (prefix + _unique_name + "/gnss/tdcp/corr_iono");
- gnss_opt.carrier_opt.corr_tropo = _server.getParam<bool> (prefix + _unique_name + "/gnss/tdcp/corr_tropo");
- gnss_opt.carrier_opt.corr_clock = _server.getParam<bool> (prefix + _unique_name + "/gnss/tdcp/corr_clock");
- tdcp_params.loss_function = _server.getParam<bool> (prefix + _unique_name + "/gnss/tdcp/loss_function");
- tdcp_params.time_window = _server.getParam<double>(prefix + _unique_name + "/gnss/tdcp/time_window");
- tdcp_params.sigma_atm = _server.getParam<double>(prefix + _unique_name + "/gnss/tdcp/sigma_atm");
- tdcp_params.sigma_carrier = _server.getParam<double>(prefix + _unique_name + "/gnss/tdcp/sigma_carrier");
- tdcp_params.multi_freq = _server.getParam<bool> (prefix + _unique_name + "/gnss/tdcp/multi_freq");
+ tdcp_structure = _server.getParam<std::string> (prefix + _unique_name + "/gnss/tdcp/structure");
+ remove_outliers_tdcp = _server.getParam<bool> (prefix + _unique_name + "/gnss/tdcp/remove_outliers");
+ tdcp_params.batch = _server.getParam<bool> (prefix + _unique_name + "/gnss/tdcp/batch");
+ gnss_opt.carrier_opt.corr_iono = _server.getParam<bool> (prefix + _unique_name + "/gnss/tdcp/corr_iono");
+ gnss_opt.carrier_opt.corr_tropo = _server.getParam<bool> (prefix + _unique_name + "/gnss/tdcp/corr_tropo");
+ gnss_opt.carrier_opt.corr_clock = _server.getParam<bool> (prefix + _unique_name + "/gnss/tdcp/corr_clock");
+ tdcp_params.loss_function = _server.getParam<bool> (prefix + _unique_name + "/gnss/tdcp/loss_function");
+ tdcp_params.time_window = _server.getParam<double> (prefix + _unique_name + "/gnss/tdcp/time_window");
+ tdcp_params.sigma_atm = _server.getParam<double> (prefix + _unique_name + "/gnss/tdcp/sigma_atm");
+ tdcp_params.sigma_carrier = _server.getParam<double> (prefix + _unique_name + "/gnss/tdcp/sigma_carrier");
+ tdcp_params.multi_freq = _server.getParam<bool> (prefix + _unique_name + "/gnss/tdcp/multi_freq");
if (tdcp_params.batch)
{
tdcp_params.min_common_sats = _server.getParam<int> (prefix + _unique_name + "/gnss/tdcp/min_common_sats");
diff --git a/include/gnss/sensor/sensor_gnss.h b/include/gnss/sensor/sensor_gnss.h
index f0f1489059fa1a6e3a0e8d3cc420c2219c7276c4..f1f19ef786b3e291e5bf3933c06ec1a547d0f5dd 100644
--- a/include/gnss/sensor/sensor_gnss.h
+++ b/include/gnss/sensor/sensor_gnss.h
@@ -103,7 +103,7 @@ class SensorGnss : public SensorBase
protected:
ParamsSensorGnssPtr params_;
bool ENU_defined_, t_ENU_MAP_initialized_, R_ENU_MAP_initialized_;
- Eigen::Matrix3d R_ENU_ECEF_;
+ Eigen::Quaterniond q_ENU_ECEF_;
Eigen::Vector3d t_ENU_ECEF_;
public:
@@ -122,9 +122,9 @@ class SensorGnss : public SensorBase
StateBlockPtr getEnuMapPitch();
StateBlockConstPtr getEnuMapYaw() const;
StateBlockPtr getEnuMapYaw();
- const Eigen::Matrix3d& getREnuEcef() const;
- const Eigen::Vector3d& gettEnuEcef() const;
- Eigen::Matrix3d getREnuMap() const;
+ Eigen::Quaterniond getQEnuEcef() const;
+ Eigen::Vector3d gettEnuEcef() const;
+ Eigen::Quaterniond getQEnuMap() const;
Eigen::Vector3d gettEnuMap() const;
bool isEnuDefined() const;
bool isEnuMapInitialized() const;
@@ -145,16 +145,24 @@ class SensorGnss : public SensorBase
void setEnuMapTranslationInitialized(const bool& _init);
void setEnuMapRotationInitialized(const bool& _init);
void fixEnuMap();
+ void resetEnu();
// compute
template<typename T>
Eigen::Matrix<T,3,3> computeREnuMap(const T& _r,const T& _p,const T& _y) const;
+ template<typename T>
+ Eigen::Quaternion<T> computeQEnuMap(const T& _r,const T& _p,const T& _y) const;
void initializeEnuMap(const Eigen::VectorXd& _pose_MAP_frameENU, const Eigen::Vector3d& _t_ECEF_antenaENU,
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;
};
+}
+
+#include <core/math/rotations.h>
+
+namespace wolf {
inline bool SensorGnss::isEnuDefined() const
{
@@ -226,12 +234,12 @@ inline StateBlockPtr SensorGnss::getEnuMapYaw()
return getStateBlock('y');
}
-inline const Eigen::Matrix3d& SensorGnss::getREnuEcef() const
+inline Eigen::Quaterniond SensorGnss::getQEnuEcef() const
{
- return R_ENU_ECEF_;
+ return q_ENU_ECEF_;
}
-inline const Eigen::Vector3d& SensorGnss::gettEnuEcef() const
+inline Eigen::Vector3d SensorGnss::gettEnuEcef() const
{
return t_ENU_ECEF_;
}
@@ -244,14 +252,18 @@ inline Eigen::Vector3d SensorGnss::gettEnuMap() const
template<typename T>
inline Eigen::Matrix<T,3,3> SensorGnss::computeREnuMap(const T& _r,const T& _p,const T& _y) const
{
- return Eigen::Matrix<T,3,3>(Eigen::AngleAxis<T>(_r, Eigen::Matrix<T,3,1>::UnitX()) *
- Eigen::AngleAxis<T>(_p, Eigen::Matrix<T,3,1>::UnitY()) *
- Eigen::AngleAxis<T>(_y, Eigen::Matrix<T,3,1>::UnitZ()));
+ return e2R((Eigen::Matrix<T,3,1>() << _r, _p, _y).finished());
+}
+
+template<typename T>
+inline Eigen::Quaternion<T> SensorGnss::computeQEnuMap(const T& _r,const T& _p,const T& _y) const
+{
+ return e2q((Eigen::Matrix<T,3,1>() << _r, _p, _y).finished());
}
-inline Eigen::Matrix3d SensorGnss::getREnuMap() const
+inline Eigen::Quaterniond SensorGnss::getQEnuMap() const
{
- return computeREnuMap(getEnuMapRoll() ->getState()(0),
+ return computeQEnuMap(getEnuMapRoll() ->getState()(0),
getEnuMapPitch()->getState()(0),
getEnuMapYaw() ->getState()(0));
}
@@ -288,6 +300,11 @@ inline void SensorGnss::fixEnuMap()
getEnuMapYaw()->fix();
}
+inline void SensorGnss::resetEnu()
+{
+ ENU_defined_ = false;
+}
+
} // namespace wolf
#endif //SENSOR_GPS_H_
diff --git a/src/capture/capture_gnss_fix.cpp b/src/capture/capture_gnss_fix.cpp
index 5ddd2bb8a87e12e512a92a7313576f6893463ec3..50a2fa24b3b403170285ea78edf3cbe19e48ab29 100644
--- a/src/capture/capture_gnss_fix.cpp
+++ b/src/capture/capture_gnss_fix.cpp
@@ -20,16 +20,42 @@
//
//--------LICENSE_END--------
#include "gnss/capture/capture_gnss_fix.h"
+#include <core/state_block/state_block_derived.h>
namespace wolf {
-CaptureGnssFix::CaptureGnssFix(const TimeStamp& _ts, SensorBasePtr _sensor_ptr, const Eigen::Vector3d& _position, const Eigen::Matrix3d& _covariance, bool _ecef_coordinates) :
+CaptureGnssFix::CaptureGnssFix(const TimeStamp& _ts,
+ SensorBasePtr _sensor_ptr,
+ const Eigen::Vector4d& _fix,
+ const Eigen::Matrix4d& _covariance,
+ bool _ecef_coordinates) :
CaptureBase("CaptureGnssFix", _ts, _sensor_ptr),
- position_ECEF_(_ecef_coordinates ? _position : GnssUtils::latLonAltToEcef(_position)),
- covariance_ECEF_(_ecef_coordinates ?_covariance : GnssUtils::enuToEcefCov(_position, _covariance))
+ fix_ECEF_(_fix),
+ covariance_ECEF_(_covariance)
{
- //
+ // Convert to ECEF from latlonalt
+ if (not _ecef_coordinates)
+ {
+ fix_ECEF_.head<3>() = GnssUtils::latLonAltToEcef(_fix.head<3>());
+ covariance_ECEF_.topLeftCorner<3,3>() = GnssUtils::enuToEcefCov(_fix.head<3>(),
+ _covariance.topLeftCorner<3,3>());
+ }
+
+ // Clock bias
+ assert(_sensor_ptr->getStateBlock('T') != nullptr and _sensor_ptr->isStateBlockDynamic('T'));
+ addStateBlock('T', std::make_shared<StateParams1>(Vector1d(),true), nullptr);
+
+ // interconstellation clock bias
+ assert(_sensor_ptr->getStateBlock('G') != nullptr);
+ if(_sensor_ptr->isStateBlockDynamic('G'))
+ addStateBlock('G', std::make_shared<StateParams1>(Vector1d(),true), nullptr);
+ assert(_sensor_ptr->getStateBlock('E') != nullptr);
+ if(_sensor_ptr->isStateBlockDynamic('E'))
+ addStateBlock('E', std::make_shared<StateParams1>(Vector1d(),true), nullptr);
+ assert(_sensor_ptr->getStateBlock('M') != nullptr);
+ if(_sensor_ptr->isStateBlockDynamic('M'))
+ addStateBlock('M', std::make_shared<StateParams1>(Vector1d(),true), nullptr);
}
CaptureGnssFix::~CaptureGnssFix()
diff --git a/src/processor/processor_gnss_fix.cpp b/src/processor/processor_gnss_fix.cpp
index 6066c1f0aea1e80fbfb698fda952de48cd9bf872..de9eed7707736db191d2360ca619b765208934cf 100644
--- a/src/processor/processor_gnss_fix.cpp
+++ b/src/processor/processor_gnss_fix.cpp
@@ -139,12 +139,14 @@ FeatureBasePtr ProcessorGnssFix::emplaceFeature(CaptureBasePtr _capture)
{
WOLF_DEBUG("ProcessorGnssFix: creating feature from incoming navsatfix. Type:", _capture->getType());
auto fix_capture = std::static_pointer_cast<CaptureGnssFix>(_capture);
- pos_output.time = fix_capture->getGpsTimeStamp().getSeconds(); // time_t time;
- pos_output.sec = 1e-9 * fix_capture->getGpsTimeStamp().getSeconds(); // double sec;
- pos_output.pos = fix_capture->getPositionEcef(); // Eigen::Vector3d pos; // position (m)
- pos_output.vel = Eigen::Vector3d::Zero(); // Eigen::Vector3d vel; // velocity (m/s)
- pos_output.pos_covar = fix_capture->getCovarianceEcef(); // Eigen::Matrix3d pos_covar; // position covariance (m^2)
- pos_output.type = 0; // int type; // coordinates used (0:xyz-ecef,1:enu-baseline)
+ pos_output.time = fix_capture->getGpsTimeStamp().getSeconds(); // time_t
+ pos_output.sec = 1e-9 * fix_capture->getGpsTimeStamp().getSeconds(); // double
+ pos_output.pos = fix_capture->getPositionEcef(); // Vector3d - position (m)
+ pos_output.vel = Eigen::Vector3d::Zero(); // Vector3d - velocity (m/s)
+ pos_output.sol_cov = fix_capture->getFixCovarianceEcef(); // Matrix4d - solution covariance [x, y, z, dt] (m², s²)
+ pos_output.rcv_bias(0) = fix_capture->getClockBias(); // Vector4d - receiver clock bias to time systems (s) 0: receiver clock bias (s), 1: glo-gps time offset (s), 2: gal-gps time offset (s), 3: bds-gps time offset (s)
+ pos_output.rcv_bias_var(0) = fix_capture->getClockBiasVariance(); // Matrix4d - receiver clock bias variances 0: receiver clock bias (s), 1: glo-gps time offset (s), 2: gal-gps time offset (s), 3: bds-gps time offset (s)
+ pos_output.type = 0; // int - coordinates used (0:xyz-ecef,1:enu-baseline)
}
else
throw std::runtime_error("ProcessorGnssFix::emplaceFeature wrong capture type");
@@ -238,7 +240,7 @@ void ProcessorGnssFix::handleEnuMap(FeatureBasePtr feature)
if (!sensor_gnss_->isEnuDefined())
{
WOLF_INFO("Defining ecef enu with first fix");
- sensor_gnss_->setEcefEnu(feature->getMeasurement());
+ sensor_gnss_->setEcefEnu(feature->getMeasurement().head<3>());
}
// Store the first capture that established a factor (for later initialization ENU-MAP)
@@ -246,7 +248,7 @@ void ProcessorGnssFix::handleEnuMap(FeatureBasePtr feature)
not sensor_gnss_->isEnuMapFixed())
{
first_frame_state_ = feature->getCapture()->getFrame()->getState();
- first_pos_ = feature->getMeasurement();
+ first_pos_ = feature->getMeasurement().head<3>();
}
// Initialize ENU-MAP if: ENU defined and ENU-MAP not initialized (and not fixed) and far enough
@@ -255,7 +257,7 @@ void ProcessorGnssFix::handleEnuMap(FeatureBasePtr feature)
sensor_gnss_->isEnuDefined() and
not sensor_gnss_->isEnuMapInitialized() and
not sensor_gnss_->isEnuMapFixed() and
- (first_pos_-feature->getMeasurement()).norm() > params_gnss_->enu_map_init_dist_min)
+ (first_pos_-feature->getMeasurement().head<3>()).norm() > params_gnss_->enu_map_init_dist_min)
{
assert(first_frame_state_.count('P') and
first_frame_state_.count('O') and
@@ -264,14 +266,14 @@ void ProcessorGnssFix::handleEnuMap(FeatureBasePtr feature)
sensor_gnss_->initializeEnuMap(first_frame_state_.vector("PO"),
first_pos_,
feature->getCapture()->getFrame()->getState().vector("PO"),
- feature->getMeasurement());
+ feature->getMeasurement().head<3>());
// Set as not-initialized if factors not separated enough ( < enu_map_init_dist_max)
- if ((first_pos_ - feature->getMeasurement()).norm() < params_gnss_->enu_map_init_dist_max)
+ if ((first_pos_ - feature->getMeasurement().head<3>()).norm() < params_gnss_->enu_map_init_dist_max)
sensor_gnss_->setEnuMapInitialized(false);
}
// Fix ENU-MAP if separated enough from first pos ( > enu_map_fix_dist )
- if ((first_pos_ - feature->getMeasurement()).norm() > params_gnss_->enu_map_fix_dist)
+ if ((first_pos_ - feature->getMeasurement().head<3>()).norm() > params_gnss_->enu_map_fix_dist)
sensor_gnss_->fixEnuMap();
}
diff --git a/src/processor/processor_gnss_tdcp.cpp b/src/processor/processor_gnss_tdcp.cpp
index 1ed2b6855410743a787dc6f6edadd9ae0b6ab6ab..73067dee69b8147c870c91caa852917a82eb9edd 100644
--- a/src/processor/processor_gnss_tdcp.cpp
+++ b/src/processor/processor_gnss_tdcp.cpp
@@ -239,7 +239,7 @@ bool ProcessorGnssTdcp::voteForKeyFrame() const
// }
//
// // Distance criterion: From the last KF with gnssfix capture
-// Eigen::Vector2d v_origin_current = (sensor_gnss_->getREnuMap().transpose() * sensor_gnss_->getREnuEcef() * incoming_capture_->getData()).head<2>();
+// Eigen::Vector2d v_origin_current = (sensor_gnss_->getQEnuMap().conjugate() * sensor_gnss_->getQEnuEcef() * incoming_capture_->getData()).head<2>();
// Eigen::Vector2d v_lastKF_origin = incoming_capture_->getOriginFrame()->getP()->getState() - last_KF_->getP()->getState();
// //std::cout << "params_tdcp_->dist_traveled" << params_tdcp_->dist_traveled << std::endl;
// //std::cout << "v_origin_current: " << v_origin_current.transpose() << std::endl;
diff --git a/src/processor/processor_tracker_gnss.cpp b/src/processor/processor_tracker_gnss.cpp
index 3ac5d6817e29641352a7e7b8ca5d8bafd79838a1..4e135a955c213daa27f307f545ff6d9a9b848a16 100644
--- a/src/processor/processor_tracker_gnss.cpp
+++ b/src/processor/processor_tracker_gnss.cpp
@@ -339,7 +339,7 @@ void ProcessorTrackerGnss::establishFactors()
// initialize frame state with antenna position in map coordinates
// (since we don't have orientation for removing extrinsics)
if (params_tracker_gnss_->init_frames and fix_last_.success)
- last_ptr_->getFrame()->getP()->setState(sensor_gnss_->getREnuMap().transpose() * (sensor_gnss_->getREnuEcef() * fix_last_.pos + sensor_gnss_->gettEnuEcef() - sensor_gnss_->gettEnuMap()));
+ last_ptr_->getFrame()->getP()->setState(sensor_gnss_->getQEnuMap().conjugate() * (sensor_gnss_->getQEnuEcef() * fix_last_.pos + sensor_gnss_->gettEnuEcef() - sensor_gnss_->gettEnuMap()));
FactorBasePtrList new_factors;
@@ -395,6 +395,7 @@ void ProcessorTrackerGnss::establishFactors()
ftr_sat,
sensor_gnss_,
shared_from_this(),
+ params_tracker_gnss_->apply_sagnac_correction,
params_->apply_loss_function);
new_factors.push_back(new_fac);
@@ -578,6 +579,7 @@ void ProcessorTrackerGnss::establishFactors()
ftr_k,
sensor_gnss_,
shared_from_this(),
+ params_tracker_gnss_->apply_sagnac_correction,
params_tracker_gnss_->tdcp_params.loss_function,
factor_status);
new_factors.push_back(new_fac);
@@ -653,7 +655,7 @@ void ProcessorTrackerGnss::removeOutliers(FactorBasePtrList fac_list, CaptureBas
if (cap == last_ptr_ and fix_last_.stat != 0 and params_tracker_gnss_->remove_outliers_with_fix)
{
WOLF_DEBUG("OUTLIERS COMPUTED USING fix_last");
- x = sensor_gnss_->getREnuMap().transpose() * (sensor_gnss_->getREnuEcef() * fix_last_.pos + sensor_gnss_->gettEnuEcef() - sensor_gnss_->gettEnuMap());
+ x = sensor_gnss_->getQEnuMap().conjugate() * (sensor_gnss_->getQEnuEcef() * fix_last_.pos + sensor_gnss_->gettEnuEcef() - sensor_gnss_->gettEnuMap());
o << 0,0,0,1;
clock_bias << CLIGHT * fix_last_.rcv_bias(0);
clock_bias_glo << CLIGHT * fix_last_.rcv_bias(1);
diff --git a/src/sensor/sensor_gnss.cpp b/src/sensor/sensor_gnss.cpp
index 6ae5a98afc6c312e908f59d99e6d47401dbdaa54..8ee07e4c8dbea5533baf64a0ce58eb451fc2c154 100644
--- a/src/sensor/sensor_gnss.cpp
+++ b/src/sensor/sensor_gnss.cpp
@@ -39,7 +39,7 @@ SensorGnss::SensorGnss(const Eigen::VectorXd& _extrinsics,
ENU_defined_(false),
t_ENU_MAP_initialized_(false),
R_ENU_MAP_initialized_(false),
- R_ENU_ECEF_(Eigen::Matrix3d::Identity()),
+ q_ENU_ECEF_(Eigen::Quaterniond::Identity()),
t_ENU_ECEF_(Eigen::Vector3d::Zero())
{
assert(_extrinsics.size() == 3 && "Bad extrinsics size");
@@ -96,13 +96,15 @@ void SensorGnss::setEcefEnu(const Eigen::Vector3d& _ENU, bool _ECEF_coordinates)
WOLF_WARN_COND(ENU_defined_,"setEnuEcef: ENU already defined!");
assert(!(ENU_defined_ && params_->ENU_mode=="ECEF") && "ENU cannot be redefined if set to ECEF");
+ Eigen::Matrix3d R_ENU_ECEF;
if (_ECEF_coordinates)
- GnssUtils::computeEnuEcefFromEcef(_ENU, R_ENU_ECEF_, t_ENU_ECEF_);
+ GnssUtils::computeEnuEcefFromEcef(_ENU, R_ENU_ECEF, t_ENU_ECEF_);
else
GnssUtils::computeEnuEcefFromEcef(GnssUtils::latLonAltToEcef(_ENU, not params_->latlon_in_degrees),
- R_ENU_ECEF_,
+ R_ENU_ECEF,
t_ENU_ECEF_);
+ q_ENU_ECEF_ = R2q(R_ENU_ECEF);
ENU_defined_ = true;
WOLF_INFO("SensorGnss: ECEF-ENU set.")
@@ -113,7 +115,7 @@ void SensorGnss::setEnuEcef(const Eigen::Matrix3d& _R_ENU_ECEF, const Eigen::Vec
WOLF_WARN_COND(ENU_defined_,"setEnuEcef: ENU already defined!");
assert(!(ENU_defined_ && params_->ENU_mode=="ECEF") && "ENU cannot be redefined if set to ECEF");
- R_ENU_ECEF_ = _R_ENU_ECEF;
+ q_ENU_ECEF_ = R2q(_R_ENU_ECEF);
t_ENU_ECEF_ = _t_ENU_ECEF;
ENU_defined_ = true;
}
@@ -124,7 +126,7 @@ void SensorGnss::initializeEnuMap(const Eigen::VectorXd& _pose_MAP_frameENU, con
assert(ENU_defined_ && "initializing ENU-MAP before setting ENU");
// compute fix vector (from 1 to 2) in ENU coordinates
- Eigen::Vector3d v_ENU = R_ENU_ECEF_ * (_t_ECEF_antena2 - _t_ECEF_antenaENU);
+ Eigen::Vector3d v_ENU = q_ENU_ECEF_ * (_t_ECEF_antena2 - _t_ECEF_antenaENU);
// 2d
if (getProblem()->getDim() == 2)
@@ -136,28 +138,23 @@ void SensorGnss::initializeEnuMap(const Eigen::VectorXd& _pose_MAP_frameENU, con
Eigen::Vector2d t_MAP_antena2 = _pose_MAP_frame2.head<2>() + Eigen::Rotation2D<double>(_pose_MAP_frame2(2)) * getP()->getState().head<2>();
Eigen::Vector2d v_MAP = t_MAP_antena2 - t_MAP_antenaENU;
- // ENU-MAP Rotation
- Eigen::Matrix2d R_ENU_MAP;
- // get it if already initialized
- if (R_ENU_MAP_initialized_)
- R_ENU_MAP = getREnuMap().topLeftCorner<2,2>();
+ // ENU-MAP Rotation (considering yaw only)
// initialize yaw if not initialized
- else
+ if (not R_ENU_MAP_initialized_)
{
double theta_ENU = atan2(v_ENU(1),v_ENU(0));
double theta_MAP = atan2(v_MAP(1),v_MAP(0));
double yaw_ENU_MAP = theta_ENU-theta_MAP;
setEnuMapYawState(yaw_ENU_MAP);
- R_ENU_MAP = computeREnuMap(getEnuMapRoll()->getState()(0),
- getEnuMapPitch()->getState()(0),
- getEnuMapYaw()->getState()(0)).topLeftCorner<2,2>();
WOLF_INFO("SensorGnss: ENU-MAP rotation initialized.");
}
+ auto R_ENU_MAP = Eigen::Rotation2Dd(getEnuMapYaw()->getState()(0));
+
// ENU-MAP translation
Eigen::Vector3d t_ENU_MAP(Eigen::Vector3d::Zero());
- t_ENU_MAP.head<2>() = -R_ENU_MAP * t_MAP_antenaENU;
+ t_ENU_MAP.head<2>() = -(R_ENU_MAP * t_MAP_antenaENU);
setEnuMapTranslationState(t_ENU_MAP);
WOLF_INFO("SensorGnss: ENU-MAP translation initialized.");
@@ -202,7 +199,7 @@ void SensorGnss::initializeEnuMapYaw(const Eigen::VectorXd& _pose_MAP_frame1, co
Eigen::Vector2d t_MAP_antena1 = _pose_MAP_frame1.head<2>() + Eigen::Rotation2D<double>(_pose_MAP_frame1(2)) * getP()->getState().head<2>();
Eigen::Vector2d t_MAP_antena2 = _pose_MAP_frame2.head<2>() + Eigen::Rotation2D<double>(_pose_MAP_frame2(2)) * getP()->getState().head<2>();
Eigen::Vector2d v_MAP_antena1_antena2 = t_MAP_antena2 - t_MAP_antena1;
- Eigen::Vector3d v_ENU_antena1_antena2 = R_ENU_ECEF_ * _v_ECEF_antena1_antena2;
+ Eigen::Vector3d v_ENU_antena1_antena2 = q_ENU_ECEF_ * _v_ECEF_antena1_antena2;
double theta_ENU = atan2(v_ENU_antena1_antena2(1),v_ENU_antena1_antena2(0));
double theta_MAP = atan2(v_MAP_antena1_antena2(1),v_MAP_antena1_antena2(0));
@@ -246,7 +243,7 @@ Eigen::Vector3d SensorGnss::computeFrameAntennaPosEcef(const FrameBasePtr& frm)
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()));
+ return q_ENU_ECEF_.conjugate() * (-t_ENU_ECEF_ + gettEnuMap() + getQEnuMap() * (frm->getP()->getState() + Eigen::Quaterniond(Eigen::Vector4d(frm->getO()->getState())) * this->getP()->getState()));
}
} // namespace wolf
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index eced537f0d3e20ff77e2764e857919f6b04a629f..5842b3926d47a765f056e881965b1037c39165d2 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -11,6 +11,12 @@ add_subdirectory(gtest)
# FactorGnssFix2d test
wolf_add_gtest(gtest_factor_gnss_fix_2d gtest_factor_gnss_fix_2d.cpp)
+# FactorGnssFix3d test
+wolf_add_gtest(gtest_factor_gnss_fix_3d gtest_factor_gnss_fix_3d.cpp)
+
+# FactorGnssFix3dWithClock test
+wolf_add_gtest(gtest_factor_gnss_fix_3d_with_clock gtest_factor_gnss_fix_3d_with_clock.cpp)
+
# FactorGnssPseudoRange test
wolf_add_gtest(gtest_factor_gnss_pseudo_range gtest_factor_gnss_pseudo_range.cpp)
diff --git a/test/gtest_factor_gnss_fix_2d.cpp b/test/gtest_factor_gnss_fix_2d.cpp
index a5a439cfc4d7ec232122da70552d75fecfc3a875..33d8ec421880be992014891c42d441a6e62b561a 100644
--- a/test/gtest_factor_gnss_fix_2d.cpp
+++ b/test/gtest_factor_gnss_fix_2d.cpp
@@ -19,13 +19,10 @@
// along with this program. If not, see <http://www.gnu.org/licenses/>.
//
//--------LICENSE_END--------
-/**
- * \file gtest_factor_gnss_fix_2d.cpp
- *
- * Created on: Aug 1, 2018
- * \author: jvallve
- */
+#include "gnss/internal/config.h"
+#include <core/math/rotations.h>
+#include <core/math/SE3.h>
#include <core/ceres_wrapper/solver_ceres.h>
#include <core/utils/utils_gtest.h>
@@ -33,207 +30,224 @@
#include "gnss/sensor/sensor_gnss.h"
#include "gnss/processor/processor_gnss_fix.h"
#include "gnss/capture/capture_gnss_fix.h"
+#include "gnss/feature/feature_gnss_fix.h"
#include "gnss/factor/factor_gnss_fix_2d.h"
using namespace Eigen;
using namespace wolf;
-void resetStates(SensorGnssPtr gnss_sensor_ptr, FrameBasePtr frame_ptr,
- const Vector1d& o_enu_map,
- const Vector3d& t_base_antena,
- const Vector3d& t_enu_map,
- const Vector3d& t_map_base,
- const Vector1d& o_map_base)
-{
- // ENU-MAP
- gnss_sensor_ptr->getEnuMapRoll()->setState(Eigen::Vector1d::Zero());
- gnss_sensor_ptr->getEnuMapPitch()->setState(Eigen::Vector1d::Zero());
- gnss_sensor_ptr->getEnuMapYaw()->setState(o_enu_map);
- gnss_sensor_ptr->getEnuMapTranslation()->setState(t_enu_map);
- // Antena
- gnss_sensor_ptr->getP()->setState(t_base_antena);
- // Frame
- frame_ptr->getP()->setState(t_map_base.head(2));
- frame_ptr->getO()->setState(o_map_base);
-}
+int N = 10;
-void fixAllStates(SensorGnssPtr gnss_sensor_ptr, FrameBasePtr frame_ptr)
+class FactorGnssFix2dTest : public testing::Test
{
- // ENU-MAP
- gnss_sensor_ptr->getEnuMapRoll()->fix();
- gnss_sensor_ptr->getEnuMapPitch()->fix();
- gnss_sensor_ptr->getEnuMapYaw()->fix();
- gnss_sensor_ptr->getEnuMapTranslation()->fix();
- // Antena
- gnss_sensor_ptr->getP()->fix();
- // Frame
- frame_ptr->getP()->fix();
- frame_ptr->getO()->fix();
- }
-
-void computeParamSizes(const SolverCeresPtr& solver_ceres, int& num_params_reduced, int& num_param_blocks_reduced )
-{
- num_param_blocks_reduced = 0;
- num_params_reduced = 0;
+ public:
+
+ // groundtruth transformations
+ Vector3d t_ecef_enu, t_enu_map, t_map_base, t_base_antena, t_ecef_antena;
+ Vector3d rpy_enu_map;
+ Quaterniond q_ecef_enu, q_enu_map, q_map_base;
+ Vector1d o_map_base;
+ double clock_bias;
+
+ // WOLF
+ ProblemPtr problem;
+ SolverCeresPtr solver_ceres;
+ SensorGnssPtr gnss_sensor;
+ FrameBasePtr frame;
+ CaptureGnssFixPtr cap_gnss;
+ FeatureBasePtr feat_gnss;
+ FactorGnssFix2dPtr fac_gnss_fix_2d;
+
+ void randomSetup(bool random_rpy = true)
+ {
+ ASSERT_TRUE(frame);
+ ASSERT_TRUE(cap_gnss);
+
+ if (fac_gnss_fix_2d)
+ feat_gnss->remove();
+
+ ASSERT_TRUE(frame);
+ ASSERT_TRUE(cap_gnss);
+
+ // groundtruth transformations
+ if (random_rpy)
+ rpy_enu_map = Vector3d::Random() * M_PI * 0.1;
+ t_enu_map = Vector3d::Random() * 20;
+ t_map_base = Vector3d::Random() * 50;
+ t_map_base(2) = 0; //z=0
+ o_map_base = Vector1d::Random() * M_PI;
+ t_base_antena = Vector3d::Random() * 3;// Antena extrinsics
+ t_ecef_enu = Vector3d::Random().cwiseAbs() * 1e3;
+ q_ecef_enu = Quaterniond::UnitRandom();
+ q_enu_map = e2q(rpy_enu_map);
+ q_map_base = e2q((Vector3d() << 0, 0, o_map_base(0)).finished());
+ t_ecef_antena = q_ecef_enu * (q_enu_map * (q_map_base * t_base_antena + t_map_base) + t_enu_map ) + t_ecef_enu;
+ clock_bias = 0.5;
+
+ // capture
+ cap_gnss->setPositionEcef(t_ecef_antena);
+ cap_gnss->getStateBlock('T')->setState(Vector1d(clock_bias));
+
+ // feature
+ GnssUtils::ComputePosOutput pos_output;
+ pos_output.pos = t_ecef_antena;
+ pos_output.sol_cov = cap_gnss->getFixCovarianceEcef();
+ pos_output.rcv_bias(0) = cap_gnss->getClockBias();
+ feat_gnss = FeatureBase::emplace<FeatureGnssFix>(cap_gnss, pos_output);
+
+ // factor
+ fac_gnss_fix_2d = FactorBase::emplace<FactorGnssFix2d>(feat_gnss, feat_gnss, gnss_sensor, nullptr, false);
+
+ // STATES
+ // ENU-MAP
+ gnss_sensor->setEnuMapRollState(rpy_enu_map(0));
+ gnss_sensor->setEnuMapPitchState(rpy_enu_map(1));
+ gnss_sensor->setEnuMapYawState(rpy_enu_map(2));
+ gnss_sensor->setEnuMapTranslationState(t_enu_map);
+ // ECEF-ENU
+ Vector3d t_enu_ecef;
+ Quaterniond q_enu_ecef;
+ SE3::inverse(t_ecef_enu, q_ecef_enu, t_enu_ecef, q_enu_ecef);
+ gnss_sensor->resetEnu();
+ gnss_sensor->setEnuEcef(q2R(q_enu_ecef), t_enu_ecef);
+ // Antena
+ gnss_sensor->getP()->setState(t_base_antena);
+ // Frame
+ frame->getP()->setState(t_map_base.head(2));
+ frame->getO()->setState(o_map_base);
+ }
- std::vector<double*> param_blocks;
- solver_ceres->getCeresProblem()->GetParameterBlocks(¶m_blocks);
+ void checkStates()
+ {
+ // clock bias
+ ASSERT_NEAR(cap_gnss->getStateBlock('T')->getState()(0), clock_bias, Constants::EPS);
+ // ENU-MAP
+ ASSERT_NEAR(gnss_sensor->getEnuMapRoll()->getState()(0), rpy_enu_map(0), Constants::EPS);
+ ASSERT_NEAR(gnss_sensor->getEnuMapPitch()->getState()(0), rpy_enu_map(1), Constants::EPS);
+ ASSERT_NEAR(gnss_sensor->getEnuMapYaw()->getState()(0), rpy_enu_map(2), Constants::EPS);
+ ASSERT_MATRIX_APPROX(gnss_sensor->getEnuMapTranslation()->getState().head<2>(), t_enu_map.head<2>(), Constants::EPS);
+ // Antena
+ ASSERT_MATRIX_APPROX(gnss_sensor->getP()->getState().head<2>(), t_base_antena.head<2>(),Constants::EPS);
+ // Frame
+ ASSERT_MATRIX_APPROX(frame->getP()->getState(), t_map_base.head(2), Constants::EPS);
+ ASSERT_MATRIX_APPROX(frame->getO()->getState(), o_map_base, Constants::EPS);
+ }
- for (auto pb : param_blocks)
- {
- std::vector<ceres::ResidualBlockId> residual_blocks;
- solver_ceres->getCeresProblem()->GetResidualBlocksForParameterBlock(pb,&residual_blocks);
+ void SetUp() override
+ {
+ // WOLF
+ problem = Problem::create("PO", 2);
+ solver_ceres = std::make_shared<SolverCeres>(problem);
+ solver_ceres->getSolverOptions().max_num_iterations = 100;
+ solver_ceres->getSolverOptions().gradient_tolerance = 1e-8;
+ solver_ceres->getSolverOptions().function_tolerance = 1e-8;
+
+ // sensor
+ gnss_sensor = std::static_pointer_cast<SensorGnss>(problem->installSensor("SensorGnss",
+ "gnss",
+ t_base_antena,
+ std::make_shared<ParamsSensorGnss>()));
+
+ // frame
+ frame = problem->emplaceFrame(TimeStamp(0), Vector3d::Zero());
+ problem->keyFrameCallback(frame, nullptr);
+
+ // capture
+ cap_gnss = CaptureBase::emplace<CaptureGnssFix>(frame,
+ TimeStamp(0),
+ gnss_sensor,
+ Vector4d::Zero(),
+ 1e-6*Matrix4d::Identity());
+ }
- if (not solver_ceres->getCeresProblem()->IsParameterBlockConstant(pb) and
- not residual_blocks.empty())
+ void fixAllStates()
{
- num_param_blocks_reduced ++;
- num_params_reduced += solver_ceres->getCeresProblem()->ParameterBlockLocalSize(pb);
+ cap_gnss->fix();
+ // ENU-MAP
+ gnss_sensor->getEnuMapRoll()->fix();
+ gnss_sensor->getEnuMapPitch()->fix();
+ gnss_sensor->getEnuMapYaw()->fix();
+ gnss_sensor->getEnuMapTranslation()->fix();
+ // Antena
+ gnss_sensor->getP()->fix();
+ // Frame
+ frame->getP()->fix();
+ frame->getO()->fix();
}
- }
-}
+};
-// groundtruth transformations
-Vector1d o_enu_map = (Vector1d() << 2.6).finished();
-Vector3d t_enu_map = (Vector3d() << 12, -34, 4).finished();
-Vector3d t_map_base = (Vector3d() << 32, -34, 0).finished(); // z=0
-Vector1d o_map_base = (Vector1d() << -0.4).finished();
-Vector3d t_base_antena = (Vector3d() << 3,-2,8).finished();// Antena extrinsics
-Vector3d t_ecef_enu = (Vector3d() << 100,30,50).finished();
-Matrix3d R_ecef_enu = (AngleAxis<double>(1.3, Vector3d::UnitX()) *
- AngleAxis<double>(-2.2, Vector3d::UnitY()) *
- AngleAxis<double>(-1.8, Vector3d::UnitZ())).toRotationMatrix();
-Matrix3d R_enu_map = AngleAxis<double>(o_enu_map(0), Vector3d::UnitZ()).toRotationMatrix();
-Matrix3d R_map_base = AngleAxis<double>(o_map_base(0), Vector3d::UnitZ()).toRotationMatrix();
-Vector3d t_ecef_antena = R_ecef_enu * (R_enu_map * (R_map_base * t_base_antena + t_map_base) + t_enu_map ) + t_ecef_enu;
-
-// WOLF
-ProblemPtr problem_ptr = Problem::create("PO", 2);
-SolverCeresPtr solver_ceres = std::make_shared<SolverCeres>(problem_ptr);
-SensorGnssPtr gnss_sensor_ptr = std::static_pointer_cast<SensorGnss>(problem_ptr->installSensor("SensorGnss", "gnss", t_base_antena, std::make_shared<ParamsSensorGnss>()));
-FrameBasePtr frame_ptr;
////////////////////////////////////////////////////////
-TEST(FactorGnssFix2dTest, configure_tree)
+TEST_F(FactorGnssFix2dTest, configure_tree)
{
- solver_ceres->getSolverOptions().max_num_iterations = 100;
-
- // Configure sensor and processor
- gnss_sensor_ptr->setEnuMapTranslationState(t_enu_map);
- gnss_sensor_ptr->setEnuMapYawState(o_enu_map(0));
- gnss_sensor_ptr->getEnuMapRoll()->fix();
- gnss_sensor_ptr->getEnuMapPitch()->fix();
- gnss_sensor_ptr->setEnuEcef(R_ecef_enu.transpose(), R_ecef_enu.transpose()*(-t_ecef_enu));
-
- ParamsProcessorGnssFixPtr gnss_params_ptr = std::make_shared<ParamsProcessorGnssFix>();
- gnss_params_ptr->time_tolerance = 1.0;
- gnss_params_ptr->voting_active = true;
- gnss_params_ptr->apply_loss_function = false;
- problem_ptr->installProcessor("ProcessorGnssFix", "gnss fix", gnss_sensor_ptr, gnss_params_ptr);
-
- // Emplace a frame (FIXED)
- Vector3d frame_pose = (Vector3d() << t_map_base(0), t_map_base(1), o_map_base(0)).finished();
- frame_ptr = problem_ptr->emplaceFrame( TimeStamp(0), frame_pose);
- problem_ptr->keyFrameCallback(frame_ptr, nullptr);
-
- // Create & process GNSS Fix capture
- CaptureGnssFixPtr cap_gnss_ptr = std::make_shared<CaptureGnssFix>(TimeStamp(0), gnss_sensor_ptr, t_ecef_antena, 1e-3*Matrix3d::Identity());
- gnss_sensor_ptr->process(cap_gnss_ptr);
-
- // Checks
- EXPECT_TRUE(problem_ptr->check(1));
+ EXPECT_TRUE(problem->check(1));
}
-/*
- * Test with one GNSS fix capture.
+/* Test with one GNSS fix capture.
*
* Estimating: MAP_BASE position.
* Fixed: ENU_MAP, MAP_BASE orientation, BASE_ANTENA.
*/
-TEST(FactorGnssFix2dTest, gnss_1_map_base_position)
+TEST_F(FactorGnssFix2dTest, gnss_1_map_base_position)
{
- problem_ptr->print();
-
- // --------------------------- Reset states
- resetStates(gnss_sensor_ptr, frame_ptr, o_enu_map, t_base_antena, t_enu_map, t_map_base, o_map_base);
-
- // --------------------------- fixing/unfixing states
- fixAllStates(gnss_sensor_ptr, frame_ptr);
- frame_ptr->getP()->unfix();
-
- // --------------------------- distort: map base position
- Vector3d frm_dist = frame_ptr->getState().vector("PO");
- frm_dist(0) += 0.18;
- frm_dist(1) += -0.58;
- frame_ptr->setState(frm_dist, "PO", {2,1});
-
- // --------------------------- update solver
- solver_ceres->update();
-
- // --------------------------- check problem parameters
- int num_params_reduced, num_param_blocks_reduced;
- computeParamSizes(solver_ceres, num_params_reduced, num_param_blocks_reduced);
+ for (auto i = 0; i<N; i++)
+ {
+ randomSetup();
- EXPECT_EQ(num_param_blocks_reduced, 1);
- EXPECT_EQ(num_params_reduced, 2);
+ // --------------------------- fixing/unfixing states
+ fixAllStates();
+ frame->getP()->unfix();
- // --------------------------- solve
- std::string report = solver_ceres->solve(SolverManager::ReportVerbosity::FULL);
+ // --------------------------- perturb: map base position
+ frame->getP()->perturb(0.5);
- //std::cout << report << std::endl;
+ // --------------------------- solve
+ std::string report = solver_ceres->solve(SolverManager::ReportVerbosity::FULL);
+ //std::cout << report << std::endl;
- // --------------------------- check summary parameters & residuals
- EXPECT_EQ(solver_ceres->getSummary().num_parameter_blocks_reduced, 1);
- EXPECT_EQ(solver_ceres->getSummary().num_parameters_reduced, 2);
- EXPECT_EQ(solver_ceres->getSummary().num_residual_blocks_reduced, 1);
- EXPECT_EQ(solver_ceres->getSummary().num_residuals_reduced, 3);
+ // --------------------------- check summary parameters & residuals
+ EXPECT_EQ(solver_ceres->getSummary().num_parameter_blocks_reduced, 1);
+ EXPECT_EQ(solver_ceres->getSummary().num_parameters_reduced, 2);
+ EXPECT_EQ(solver_ceres->getSummary().num_residual_blocks_reduced, 1);
+ EXPECT_EQ(solver_ceres->getSummary().num_residuals_reduced, 3);
- // --------------------------- check solver solution
- EXPECT_MATRIX_APPROX(frame_ptr->getState().at('P'), t_map_base.head(2), 1e-6);
+ // --------------------------- check solver solution
+ checkStates();
+ }
}
-/*
- * Test with one GNSS fix capture.
+/* Test with one GNSS fix capture.
*
* Estimating: MAP_BASE orientation.
* Fixed: ENU_MAP, MAP_BASE position, BASE_ANTENA.
*/
-TEST(FactorGnssFix2dTest, gnss_1_map_base_orientation)
+TEST_F(FactorGnssFix2dTest, gnss_1_map_base_orientation)
{
- // --------------------------- Reset states
- resetStates(gnss_sensor_ptr, frame_ptr, o_enu_map, t_base_antena, t_enu_map, t_map_base, o_map_base);
-
- // --------------------------- fixing/unfixing states
- fixAllStates(gnss_sensor_ptr, frame_ptr);
- frame_ptr->getO()->unfix();
-
- // --------------------------- distort: map base orientation
- Vector1d frm_dist = frame_ptr->getO()->getState();
- frm_dist(0) += 0.18;
- frame_ptr->getO()->setState(frm_dist);
-
- // --------------------------- update solver
- solver_ceres->update();
+ for (auto i = 0; i<N; i++)
+ {
+ randomSetup();
- // --------------------------- check problem parameters
- int num_params_reduced, num_param_blocks_reduced;
- computeParamSizes(solver_ceres, num_params_reduced, num_param_blocks_reduced);
+ // --------------------------- fixing/unfixing states
+ fixAllStates();
+ frame->getO()->unfix();
- EXPECT_EQ(num_param_blocks_reduced, 1);
- EXPECT_EQ(num_params_reduced, 1);
+ // --------------------------- perturb: map base position
+ frame->getO()->perturb(0.5);
- // --------------------------- solve
- std::string report = solver_ceres->solve(SolverManager::ReportVerbosity::QUIET);
+ // --------------------------- solve
+ std::string report = solver_ceres->solve(SolverManager::ReportVerbosity::FULL);
+ //std::cout << report << std::endl;
- // --------------------------- check summary parameters & residuals
- EXPECT_EQ(solver_ceres->getSummary().num_parameter_blocks_reduced, 1);
- EXPECT_EQ(solver_ceres->getSummary().num_parameters_reduced, 1);
- EXPECT_EQ(solver_ceres->getSummary().num_residual_blocks_reduced, 1);
- EXPECT_EQ(solver_ceres->getSummary().num_residuals_reduced, 3);
+ // --------------------------- check summary parameters & residuals
+ EXPECT_EQ(solver_ceres->getSummary().num_parameter_blocks_reduced, 1);
+ EXPECT_EQ(solver_ceres->getSummary().num_parameters_reduced, 1);
+ EXPECT_EQ(solver_ceres->getSummary().num_residual_blocks_reduced, 1);
+ EXPECT_EQ(solver_ceres->getSummary().num_residuals_reduced, 3);
- // --------------------------- check solver solution
- EXPECT_MATRIX_APPROX(frame_ptr->getO()->getState(), o_map_base, 1e-6);
+ // --------------------------- check solver solution
+ checkStates();
+ }
}
/*
@@ -242,132 +256,100 @@ TEST(FactorGnssFix2dTest, gnss_1_map_base_orientation)
* Estimating: ENU_MAP yaw.
* Fixed: ENU_MAP position, MAP_BASE and BASE_ANTENA.
*/
-TEST(FactorGnssFix2dTest, gnss_1_enu_map_yaw)
+TEST_F(FactorGnssFix2dTest, gnss_1_enu_map_yaw)
{
- // --------------------------- Reset states
- resetStates(gnss_sensor_ptr, frame_ptr, o_enu_map, t_base_antena, t_enu_map, t_map_base, o_map_base);
-
- // --------------------------- fixing/unfixing states
- fixAllStates(gnss_sensor_ptr, frame_ptr);
- gnss_sensor_ptr->getEnuMapYaw()->unfix();
-
- // --------------------------- distort: yaw enu_map
- Vector1d o_enu_map_dist = gnss_sensor_ptr->getEnuMapYaw()->getState();
- o_enu_map_dist(0) += 0.18;
- gnss_sensor_ptr->getEnuMapYaw()->setState(o_enu_map_dist);
-
- // --------------------------- update solver
- solver_ceres->update();
- EXPECT_TRUE(solver_ceres->check());
-
- // --------------------------- check problem parameters
- int num_params_reduced, num_param_blocks_reduced;
- computeParamSizes(solver_ceres, num_params_reduced, num_param_blocks_reduced);
+ for (auto i = 0; i<N; i++)
+ {
+ randomSetup();
- EXPECT_EQ(num_param_blocks_reduced, 1);
- EXPECT_EQ(num_params_reduced, 1);
+ // --------------------------- fixing/unfixing states
+ fixAllStates();
+ gnss_sensor->getEnuMapYaw()->unfix();
- // --------------------------- solve
- std::string report = solver_ceres->solve(SolverManager::ReportVerbosity::FULL);
+ // --------------------------- perturb: map base position
+ gnss_sensor->getEnuMapYaw()->perturb(0.5);
- // --------------------------- check summary parameters & residuals
- EXPECT_EQ(solver_ceres->getSummary().num_parameter_blocks_reduced, 1);
- EXPECT_EQ(solver_ceres->getSummary().num_parameters_reduced, 1);
- EXPECT_EQ(solver_ceres->getSummary().num_residual_blocks_reduced, 1);
- EXPECT_EQ(solver_ceres->getSummary().num_residuals_reduced, 3);
+ // --------------------------- solve
+ std::string report = solver_ceres->solve(SolverManager::ReportVerbosity::FULL);
+ //std::cout << report << std::endl;
- // --------------------------- check solver solution
- EXPECT_MATRIX_APPROX(gnss_sensor_ptr->getEnuMapYaw()->getState(), o_enu_map, 1e-6);
+ // --------------------------- check summary parameters & residuals
+ EXPECT_EQ(solver_ceres->getSummary().num_parameter_blocks_reduced, 1);
+ EXPECT_EQ(solver_ceres->getSummary().num_parameters_reduced, 1);
+ EXPECT_EQ(solver_ceres->getSummary().num_residual_blocks_reduced, 1);
+ EXPECT_EQ(solver_ceres->getSummary().num_residuals_reduced, 3);
- problem_ptr->print(4,1,1,1);
+ // --------------------------- check solver solution
+ checkStates();
+ }
}
-/*
- * Test with one GNSS fix capture.
+/* Test with one GNSS fix capture.
*
* Estimating: ENU_MAP position.
* Fixed: ENU_MAP yaw, MAP_BASE and BASE_ANTENA.
*/
-TEST(FactorGnssFix2dTest, gnss_1_enu_map_position)
+TEST_F(FactorGnssFix2dTest, gnss_1_enu_map_position)
{
- // --------------------------- Reset states
- resetStates(gnss_sensor_ptr, frame_ptr, o_enu_map, t_base_antena, t_enu_map, t_map_base, o_map_base);
-
- // --------------------------- fixing/unfixing states
- fixAllStates(gnss_sensor_ptr, frame_ptr);
- gnss_sensor_ptr->getEnuMapTranslation()->unfix();// enu-map yaw translation
-
- // --------------------------- distort: position enu_map
- Vector3d t_enu_map_dist = gnss_sensor_ptr->getEnuMapTranslation()->getState();
- t_enu_map_dist(0) += 0.86;
- t_enu_map_dist(1) += -0.34;
- gnss_sensor_ptr->getEnuMapTranslation()->setState(t_enu_map_dist);
-
- // --------------------------- update solver
- solver_ceres->update();
+ for (auto i = 0; i<N; i++)
+ {
+ // observable x and y if enu and map aligned
+ rpy_enu_map = Vector3d::Zero();
+ randomSetup(false);
- // --------------------------- check problem parameters
- int num_params_reduced, num_param_blocks_reduced;
- computeParamSizes(solver_ceres, num_params_reduced, num_param_blocks_reduced);
+ // --------------------------- fixing/unfixing states
+ fixAllStates();
+ gnss_sensor->getEnuMapTranslation()->unfix();
- EXPECT_EQ(num_param_blocks_reduced, 1);
- EXPECT_EQ(num_params_reduced, 3);
+ // --------------------------- perturb: map base position
+ gnss_sensor->getEnuMapTranslation()->perturb(0.5);
- // --------------------------- solve
- std::string report = solver_ceres->solve(SolverManager::ReportVerbosity::QUIET);
+ // --------------------------- solve
+ std::string report = solver_ceres->solve(SolverManager::ReportVerbosity::FULL);
+ //std::cout << report << std::endl;
- // --------------------------- check summary parameters & residuals
- EXPECT_EQ(solver_ceres->getSummary().num_parameter_blocks_reduced, 1);
- EXPECT_EQ(solver_ceres->getSummary().num_parameters_reduced, 3);
- EXPECT_EQ(solver_ceres->getSummary().num_residual_blocks_reduced, 1);
- EXPECT_EQ(solver_ceres->getSummary().num_residuals_reduced, 3);
+ // --------------------------- check summary parameters & residuals
+ EXPECT_EQ(solver_ceres->getSummary().num_parameter_blocks_reduced, 1);
+ EXPECT_EQ(solver_ceres->getSummary().num_parameters_reduced, 3);
+ EXPECT_EQ(solver_ceres->getSummary().num_residual_blocks_reduced, 1);
+ EXPECT_EQ(solver_ceres->getSummary().num_residuals_reduced, 3);
- // --------------------------- check solver solution
- EXPECT_MATRIX_APPROX(gnss_sensor_ptr->getEnuMapTranslation()->getState().head(2), t_enu_map.head(2), 1e-6);
+ // --------------------------- check solver solution
+ checkStates();
+ }
}
-/*
- * Test with one GNSS fix capture.
+/* Test with one GNSS fix capture.
*
* Estimating: BASE_ANTENA (2 first components that are observable).
* Fixed: ENU_MAP, MAP_BASE.
*/
-TEST(FactorGnssFix2dTest, gnss_1_base_antena)
+TEST_F(FactorGnssFix2dTest, gnss_1_base_antena)
{
- // --------------------------- Reset states
- resetStates(gnss_sensor_ptr, frame_ptr, o_enu_map, t_base_antena, t_enu_map, t_map_base, o_map_base);
-
- // --------------------------- fixing/unfixing states
- fixAllStates(gnss_sensor_ptr, frame_ptr);
- gnss_sensor_ptr->getP()->unfix();
-
- // --------------------------- distort: base_antena
- Vector3d base_antena_dist = gnss_sensor_ptr->getP()->getState();
- base_antena_dist(0) += 1.68;
- base_antena_dist(0) += -0.48;
- gnss_sensor_ptr->getP()->setState(base_antena_dist);
-
- // --------------------------- update solver
- solver_ceres->update();
+ for (auto i = 0; i<N; i++)
+ {
+ randomSetup();
- // --------------------------- check problem parameters
- int num_params_reduced, num_param_blocks_reduced;
- computeParamSizes(solver_ceres, num_params_reduced, num_param_blocks_reduced);
+ // --------------------------- fixing/unfixing states
+ fixAllStates();
+ gnss_sensor->getP()->unfix();
- EXPECT_EQ(num_param_blocks_reduced, 1);
- EXPECT_EQ(num_params_reduced, 3);
+ // --------------------------- perturb: map base position
+ gnss_sensor->getP()->perturb(0.5);
- // --------------------------- solve
- std::string report = solver_ceres->solve(SolverManager::ReportVerbosity::QUIET);
+ // --------------------------- solve
+ std::string report = solver_ceres->solve(SolverManager::ReportVerbosity::FULL);
+ //std::cout << report << std::endl;
- // --------------------------- check summary parameters & residuals
- EXPECT_EQ(solver_ceres->getSummary().num_parameter_blocks_reduced, 1);
- EXPECT_EQ(solver_ceres->getSummary().num_parameters_reduced, 3);
- EXPECT_EQ(solver_ceres->getSummary().num_residual_blocks_reduced, 1);
- EXPECT_EQ(solver_ceres->getSummary().num_residuals_reduced, 3);
+ // --------------------------- check summary parameters & residuals
+ EXPECT_EQ(solver_ceres->getSummary().num_parameter_blocks_reduced, 1);
+ EXPECT_EQ(solver_ceres->getSummary().num_parameters_reduced, 3);
+ EXPECT_EQ(solver_ceres->getSummary().num_residual_blocks_reduced, 1);
+ EXPECT_EQ(solver_ceres->getSummary().num_residuals_reduced, 3);
- // --------------------------- check solver solution
- EXPECT_MATRIX_APPROX(gnss_sensor_ptr->getP()->getState().head(2), t_base_antena.head(2), 1e-6);
+ // --------------------------- check solver solution
+ checkStates();
+ }
}
int main(int argc, char **argv)
diff --git a/test/gtest_factor_gnss_fix_3d.cpp b/test/gtest_factor_gnss_fix_3d.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..0f50ce292a46cfd20815d99141ae1844778f860e
--- /dev/null
+++ b/test/gtest_factor_gnss_fix_3d.cpp
@@ -0,0 +1,350 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+
+#include "gnss/internal/config.h"
+#include <core/math/rotations.h>
+#include <core/math/SE3.h>
+#include <core/ceres_wrapper/solver_ceres.h>
+#include <core/utils/utils_gtest.h>
+
+#include "core/problem/problem.h"
+#include "gnss/sensor/sensor_gnss.h"
+#include "gnss/processor/processor_gnss_fix.h"
+#include "gnss/capture/capture_gnss_fix.h"
+#include "gnss/feature/feature_gnss_fix.h"
+#include "gnss/factor/factor_gnss_fix_3d.h"
+
+using namespace Eigen;
+using namespace wolf;
+
+int N = 10;
+
+class FactorGnssFix3dTest : public testing::Test
+{
+ public:
+
+ // groundtruth transformations
+ Vector3d t_ecef_enu, t_enu_map, t_map_base, t_base_antena, t_ecef_antena;
+ Vector3d rpy_enu_map;
+ Quaterniond q_ecef_enu, q_enu_map, q_map_base;
+ double clock_bias;
+
+ // WOLF
+ ProblemPtr problem;
+ SolverCeresPtr solver_ceres;
+ SensorGnssPtr gnss_sensor;
+ FrameBasePtr frame;
+ CaptureGnssFixPtr cap_gnss;
+ FeatureBasePtr feat_gnss;
+ FactorGnssFix3dPtr fac_gnss_fix_3d;
+
+ void randomSetup()
+ {
+ ASSERT_TRUE(frame);
+ ASSERT_TRUE(cap_gnss);
+
+ if (fac_gnss_fix_3d)
+ feat_gnss->remove();
+
+ ASSERT_TRUE(frame);
+ ASSERT_TRUE(cap_gnss);
+
+ // groundtruth transformations
+ rpy_enu_map = Vector3d::Random() * M_PI * 0.1;
+ t_enu_map = Vector3d::Random() * 20;
+ t_map_base = Vector3d::Random() * 50;
+ t_base_antena = Vector3d::Random() * 3;// Antena extrinsics
+ t_ecef_enu = Vector3d::Random().cwiseAbs() * 1e3;
+ q_ecef_enu = Quaterniond::UnitRandom();
+ q_enu_map = e2q(rpy_enu_map);
+ q_map_base = Quaterniond::UnitRandom();
+ t_ecef_antena = q_ecef_enu * (q_enu_map * (q_map_base * t_base_antena + t_map_base) + t_enu_map ) + t_ecef_enu;
+ clock_bias = Vector1d::Random()(0);
+
+ // Fill groundtruth ----------------
+ // ENU-MAP
+ gnss_sensor->setEnuMapRollState(rpy_enu_map(0));
+ gnss_sensor->setEnuMapPitchState(rpy_enu_map(1));
+ gnss_sensor->setEnuMapYawState(rpy_enu_map(2));
+ gnss_sensor->setEnuMapTranslationState(t_enu_map);
+ // ECEF-ENU
+ Vector3d t_enu_ecef;
+ Quaterniond q_enu_ecef;
+ SE3::inverse(t_ecef_enu, q_ecef_enu, t_enu_ecef, q_enu_ecef);
+ gnss_sensor->resetEnu();
+ gnss_sensor->setEnuEcef(q2R(q_enu_ecef), t_enu_ecef);
+ // Antena
+ gnss_sensor->getP()->setState(t_base_antena);
+ // Frame
+ frame->getP()->setState(t_map_base);
+ frame->getO()->setState(q_map_base.coeffs());
+ // capture
+ cap_gnss->setPositionEcef(t_ecef_antena);
+ cap_gnss->getStateBlock('T')->setState(Vector1d(clock_bias));
+
+ // Fake measurement ---------------------
+ // feature
+ GnssUtils::ComputePosOutput pos_output;
+ pos_output.pos = t_ecef_antena;
+ pos_output.sol_cov = cap_gnss->getFixCovarianceEcef();
+ pos_output.rcv_bias(0) = cap_gnss->getClockBias();
+ feat_gnss = FeatureBase::emplace<FeatureGnssFix>(cap_gnss, pos_output);
+ // factor
+ fac_gnss_fix_3d = FactorBase::emplace<FactorGnssFix3d>(feat_gnss, feat_gnss, gnss_sensor, nullptr, false);
+ }
+
+ void checkStates()
+ {
+ // clock bias
+ ASSERT_NEAR(cap_gnss->getStateBlock('T')->getState()(0), clock_bias, Constants::EPS);
+ // ENU-MAP
+ ASSERT_NEAR(gnss_sensor->getEnuMapRoll()->getState()(0), rpy_enu_map(0), Constants::EPS);
+ ASSERT_NEAR(gnss_sensor->getEnuMapPitch()->getState()(0), rpy_enu_map(1), Constants::EPS);
+ ASSERT_NEAR(gnss_sensor->getEnuMapYaw()->getState()(0), rpy_enu_map(2), Constants::EPS);
+ ASSERT_MATRIX_APPROX(gnss_sensor->getEnuMapTranslation()->getState(), t_enu_map, Constants::EPS);
+ // Antena
+ ASSERT_MATRIX_APPROX(gnss_sensor->getP()->getState(), t_base_antena,Constants::EPS);
+ // Frame
+ ASSERT_MATRIX_APPROX(frame->getP()->getState(), t_map_base, Constants::EPS);
+ ASSERT_QUATERNION_VECTOR_APPROX(frame->getO()->getState(), q_map_base.coeffs(), Constants::EPS);
+ }
+
+ void SetUp() override
+ {
+ // WOLF
+ problem = Problem::create("PO", 3);
+ solver_ceres = std::make_shared<SolverCeres>(problem);
+ solver_ceres->getSolverOptions().max_num_iterations = 100;
+ solver_ceres->getSolverOptions().gradient_tolerance = 1e-8;
+ solver_ceres->getSolverOptions().function_tolerance = 1e-8;
+
+ // sensor
+ gnss_sensor = std::static_pointer_cast<SensorGnss>(problem->installSensor("SensorGnss",
+ "gnss",
+ t_base_antena,
+ std::make_shared<ParamsSensorGnss>()));
+
+ // frame
+ frame = problem->emplaceFrame(TimeStamp(0));
+ problem->keyFrameCallback(frame, nullptr);
+
+ // capture
+ cap_gnss = CaptureBase::emplace<CaptureGnssFix>(frame,
+ TimeStamp(0),
+ gnss_sensor,
+ Vector4d::Zero(),
+ 1e-6*Matrix4d::Identity());
+ }
+
+ void fixAllStates()
+ {
+ cap_gnss->fix();
+ // ENU-MAP
+ gnss_sensor->getEnuMapRoll()->fix();
+ gnss_sensor->getEnuMapPitch()->fix();
+ gnss_sensor->getEnuMapYaw()->fix();
+ gnss_sensor->getEnuMapTranslation()->fix();
+ // Antena
+ gnss_sensor->getP()->fix();
+ // Frame
+ frame->getP()->fix();
+ frame->getO()->fix();
+ }
+};
+
+
+////////////////////////////////////////////////////////
+
+TEST_F(FactorGnssFix3dTest, configure_tree)
+{
+ EXPECT_TRUE(problem->check(1));
+}
+
+/* Test with one GNSS fix capture.
+ *
+ * Estimating: MAP_BASE position.
+ * Fixed: ENU_MAP, MAP_BASE orientation, BASE_ANTENA.
+ */
+TEST_F(FactorGnssFix3dTest, gnss_1_map_base_position)
+{
+ for (auto i = 0; i<N; i++)
+ {
+ randomSetup();
+
+ // --------------------------- fixing/unfixing states
+ fixAllStates();
+ frame->getP()->unfix();
+
+ // --------------------------- perturb: map base position
+ frame->getP()->perturb(0.5);
+
+ // --------------------------- solve
+ std::string report = solver_ceres->solve(SolverManager::ReportVerbosity::FULL);
+ //std::cout << report << std::endl;
+
+ // --------------------------- check summary parameters & residuals
+ EXPECT_EQ(solver_ceres->getSummary().num_parameter_blocks_reduced, 1);
+ EXPECT_EQ(solver_ceres->getSummary().num_parameters_reduced, 3);
+ EXPECT_EQ(solver_ceres->getSummary().num_residual_blocks_reduced, 1);
+ EXPECT_EQ(solver_ceres->getSummary().num_residuals_reduced, 3);
+
+ // --------------------------- check solver solution
+ checkStates();
+ }
+}
+
+/* Test with one GNSS fix capture.
+ *
+ * Estimating: MAP_BASE orientation.
+ * Fixed: ENU_MAP, MAP_BASE position, BASE_ANTENA.
+ */
+TEST_F(FactorGnssFix3dTest, gnss_1_map_base_orientation)
+{
+ for (auto i = 0; i<N; i++)
+ {
+ randomSetup();
+
+ // --------------------------- fixing/unfixing states
+ fixAllStates();
+ frame->getO()->unfix();
+
+ // --------------------------- perturb: map base position
+ frame->getO()->perturb(0.5);
+
+ // --------------------------- solve
+ std::string report = solver_ceres->solve(SolverManager::ReportVerbosity::FULL);
+ //std::cout << report << std::endl;
+
+ // --------------------------- check summary parameters & residuals
+ EXPECT_EQ(solver_ceres->getSummary().num_parameter_blocks_reduced, 1);
+ EXPECT_EQ(solver_ceres->getSummary().num_parameters_reduced, 4);
+ EXPECT_EQ(solver_ceres->getSummary().num_residual_blocks_reduced, 1);
+ EXPECT_EQ(solver_ceres->getSummary().num_residuals_reduced, 3);
+
+ // --------------------------- check solver solution
+ // NOT OBSERVABLE checkStates();
+ }
+}
+
+/*
+ * Test with one GNSS fix capture.
+ *
+ * Estimating: ENU_MAP yaw.
+ * Fixed: ENU_MAP position, MAP_BASE and BASE_ANTENA.
+ */
+TEST_F(FactorGnssFix3dTest, gnss_1_enu_map_yaw)
+{
+ for (auto i = 0; i<N; i++)
+ {
+ randomSetup();
+
+ // --------------------------- fixing/unfixing states
+ fixAllStates();
+ gnss_sensor->getEnuMapYaw()->unfix();
+
+ // --------------------------- perturb: map base position
+ gnss_sensor->getEnuMapYaw()->perturb(0.5);
+
+ // --------------------------- solve
+ std::string report = solver_ceres->solve(SolverManager::ReportVerbosity::FULL);
+ //std::cout << report << std::endl;
+
+ // --------------------------- check summary parameters & residuals
+ EXPECT_EQ(solver_ceres->getSummary().num_parameter_blocks_reduced, 1);
+ EXPECT_EQ(solver_ceres->getSummary().num_parameters_reduced, 1);
+ EXPECT_EQ(solver_ceres->getSummary().num_residual_blocks_reduced, 1);
+ EXPECT_EQ(solver_ceres->getSummary().num_residuals_reduced, 3);
+
+ // --------------------------- check solver solution
+ checkStates();
+ }
+}
+
+/* Test with one GNSS fix capture.
+ *
+ * Estimating: ENU_MAP position.
+ * Fixed: ENU_MAP yaw, MAP_BASE and BASE_ANTENA.
+ */
+TEST_F(FactorGnssFix3dTest, gnss_1_enu_map_position)
+{
+ for (auto i = 0; i<N; i++)
+ {
+ randomSetup();
+
+ // --------------------------- fixing/unfixing states
+ fixAllStates();
+ gnss_sensor->getEnuMapTranslation()->unfix();
+
+ // --------------------------- perturb: map base position
+ gnss_sensor->getEnuMapTranslation()->perturb(0.5);
+
+ // --------------------------- solve
+ std::string report = solver_ceres->solve(SolverManager::ReportVerbosity::FULL);
+ //std::cout << report << std::endl;
+ // --------------------------- check summary parameters & residuals
+ EXPECT_EQ(solver_ceres->getSummary().num_parameter_blocks_reduced, 1);
+ EXPECT_EQ(solver_ceres->getSummary().num_parameters_reduced, 3);
+ EXPECT_EQ(solver_ceres->getSummary().num_residual_blocks_reduced, 1);
+ EXPECT_EQ(solver_ceres->getSummary().num_residuals_reduced, 3);
+
+ // --------------------------- check solver solution
+ checkStates();
+ }
+}
+
+/* Test with one GNSS fix capture.
+ *
+ * Estimating: BASE_ANTENA (2 first components that are observable).
+ * Fixed: ENU_MAP, MAP_BASE.
+ */
+TEST_F(FactorGnssFix3dTest, gnss_1_base_antena)
+{
+ for (auto i = 0; i<N; i++)
+ {
+ randomSetup();
+
+ // --------------------------- fixing/unfixing states
+ fixAllStates();
+ gnss_sensor->getP()->unfix();
+
+ // --------------------------- perturb: map base position
+ gnss_sensor->getP()->perturb(0.5);
+
+ // --------------------------- solve
+ std::string report = solver_ceres->solve(SolverManager::ReportVerbosity::FULL);
+ //std::cout << report << std::endl;
+ // --------------------------- check summary parameters & residuals
+ EXPECT_EQ(solver_ceres->getSummary().num_parameter_blocks_reduced, 1);
+ EXPECT_EQ(solver_ceres->getSummary().num_parameters_reduced, 3);
+ EXPECT_EQ(solver_ceres->getSummary().num_residual_blocks_reduced, 1);
+ EXPECT_EQ(solver_ceres->getSummary().num_residuals_reduced, 3);
+
+ // --------------------------- check solver solution
+ checkStates();
+ }
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/test/gtest_factor_gnss_fix_3d_with_clock.cpp b/test/gtest_factor_gnss_fix_3d_with_clock.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..ce429743eda288a4bfb0d00ae1604a04fd3fc168
--- /dev/null
+++ b/test/gtest_factor_gnss_fix_3d_with_clock.cpp
@@ -0,0 +1,363 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+
+#include "gnss/internal/config.h"
+#include <core/math/rotations.h>
+#include <core/math/SE3.h>
+#include <core/ceres_wrapper/solver_ceres.h>
+#include <core/utils/utils_gtest.h>
+
+#include "core/problem/problem.h"
+#include "gnss/sensor/sensor_gnss.h"
+#include "gnss/processor/processor_gnss_fix.h"
+#include "gnss/capture/capture_gnss_fix.h"
+#include "gnss/feature/feature_gnss_fix.h"
+#include "gnss/factor/factor_gnss_fix_3d_with_clock.h"
+
+using namespace Eigen;
+using namespace wolf;
+
+int N = 10;
+
+class FactorGnssFix3dWithClockTest : public testing::Test
+{
+ public:
+
+ // groundtruth transformations
+ Vector3d t_ecef_enu, t_enu_map, t_map_base, t_base_antena, t_ecef_antena;
+ Vector3d rpy_enu_map;
+ Quaterniond q_ecef_enu, q_enu_map, q_map_base;
+ double clock_bias;
+
+ // WOLF
+ ProblemPtr problem;
+ SolverCeresPtr solver_ceres;
+ SensorGnssPtr gnss_sensor;
+ FrameBasePtr frame;
+ CaptureGnssFixPtr cap_gnss;
+ FeatureBasePtr feat_gnss;
+ FactorGnssFix3dWithClockPtr fac_gnss_fix_3d;
+
+ void randomSetup()
+ {
+ ASSERT_TRUE(frame);
+ ASSERT_TRUE(cap_gnss);
+
+ if (fac_gnss_fix_3d)
+ feat_gnss->remove();
+
+ ASSERT_TRUE(frame);
+ ASSERT_TRUE(cap_gnss);
+
+ // groundtruth transformations
+ rpy_enu_map = Vector3d::Random() * M_PI * 0.2;
+ t_enu_map = Vector3d::Random() * 20;
+ t_map_base = Vector3d::Random() * 50;
+ t_base_antena = Vector3d::Random() * 3;// Antena extrinsics
+ t_ecef_enu = Vector3d::Random().cwiseAbs() * 1e3;
+ q_ecef_enu = Quaterniond::UnitRandom();
+ q_enu_map = e2q(rpy_enu_map);
+ q_map_base = Quaterniond::UnitRandom();
+ t_ecef_antena = q_ecef_enu * (q_enu_map * (q_map_base * t_base_antena + t_map_base) + t_enu_map ) + t_ecef_enu;
+ clock_bias = Vector1d::Random()(0);
+
+ // Fill groundtruth --------
+ // ENU-MAP
+ gnss_sensor->setEnuMapRollState(rpy_enu_map(0));
+ gnss_sensor->setEnuMapPitchState(rpy_enu_map(1));
+ gnss_sensor->setEnuMapYawState(rpy_enu_map(2));
+ gnss_sensor->setEnuMapTranslationState(t_enu_map);
+ // ECEF-ENU
+ Vector3d t_enu_ecef;
+ Quaterniond q_enu_ecef;
+ SE3::inverse(t_ecef_enu, q_ecef_enu, t_enu_ecef, q_enu_ecef);
+ gnss_sensor->resetEnu();
+ gnss_sensor->setEnuEcef(q2R(q_enu_ecef), t_enu_ecef);
+ // Antena
+ gnss_sensor->getP()->setState(t_base_antena);
+ // Frame
+ frame->getP()->setState(t_map_base);
+ frame->getO()->setState(q_map_base.coeffs());
+ // capture
+ cap_gnss->setPositionEcef(t_ecef_antena);
+ cap_gnss->setClockBias(clock_bias);
+ cap_gnss->getStateBlock('T')->setState(Vector1d(clock_bias));
+
+ // Create fake measurement ----------------
+ // feature
+ GnssUtils::ComputePosOutput pos_output;
+ pos_output.pos = t_ecef_antena;
+ pos_output.sol_cov = Matrix4d::Identity() * 1e-6;
+ pos_output.rcv_bias(0) = cap_gnss->getClockBias();
+ feat_gnss = FeatureBase::emplace<FeatureGnssFix>(cap_gnss, pos_output);
+ // factor
+ fac_gnss_fix_3d = FactorBase::emplace<FactorGnssFix3dWithClock>(feat_gnss, feat_gnss, gnss_sensor, nullptr, false);
+ }
+
+ void checkStates()
+ {
+ // clock bias
+ ASSERT_NEAR(cap_gnss->getStateBlock('T')->getState()(0), clock_bias, Constants::EPS);
+ // ENU-MAP
+ ASSERT_NEAR(gnss_sensor->getEnuMapRoll()->getState()(0), rpy_enu_map(0), Constants::EPS*10);
+ ASSERT_NEAR(gnss_sensor->getEnuMapPitch()->getState()(0), rpy_enu_map(1), Constants::EPS*10);
+ ASSERT_NEAR(gnss_sensor->getEnuMapYaw()->getState()(0), rpy_enu_map(2), Constants::EPS*10);
+ ASSERT_MATRIX_APPROX(gnss_sensor->getEnuMapTranslation()->getState(), t_enu_map, Constants::EPS);
+ // Antena
+ ASSERT_MATRIX_APPROX(gnss_sensor->getP()->getState(), t_base_antena,Constants::EPS);
+ // Frame
+ ASSERT_MATRIX_APPROX(frame->getP()->getState(), t_map_base, Constants::EPS);
+ ASSERT_QUATERNION_VECTOR_APPROX(frame->getO()->getState(), q_map_base.coeffs(), Constants::EPS);
+ }
+
+ void SetUp() override
+ {
+ // WOLF
+ problem = Problem::create("PO", 3);
+ solver_ceres = std::make_shared<SolverCeres>(problem);
+ solver_ceres->getSolverOptions().max_num_iterations = 100;
+ solver_ceres->getSolverOptions().gradient_tolerance = 1e-8;
+ solver_ceres->getSolverOptions().function_tolerance = 1e-9;
+
+ // sensor
+ gnss_sensor = std::static_pointer_cast<SensorGnss>(problem->installSensor("SensorGnss",
+ "gnss",
+ t_base_antena,
+ std::make_shared<ParamsSensorGnss>()));
+
+ // frame
+ frame = problem->emplaceFrame(TimeStamp(0));
+ problem->keyFrameCallback(frame, nullptr);
+
+ // capture
+ cap_gnss = CaptureBase::emplace<CaptureGnssFix>(frame,
+ TimeStamp(0),
+ gnss_sensor,
+ Vector4d::Zero(),
+ 1e-6*Matrix4d::Identity());
+ }
+
+ void fixAllStates()
+ {
+ cap_gnss->fix();
+ // ENU-MAP
+ gnss_sensor->getEnuMapRoll()->fix();
+ gnss_sensor->getEnuMapPitch()->fix();
+ gnss_sensor->getEnuMapYaw()->fix();
+ gnss_sensor->getEnuMapTranslation()->fix();
+ // Antena
+ gnss_sensor->getP()->fix();
+ // Frame
+ frame->getP()->fix();
+ frame->getO()->fix();
+ }
+};
+
+
+////////////////////////////////////////////////////////
+
+TEST_F(FactorGnssFix3dWithClockTest, configure_tree)
+{
+ EXPECT_TRUE(problem->check(1));
+}
+
+/* Test with one GNSS fix capture.
+ *
+ * Estimating: MAP_BASE position.
+ * Fixed: ENU_MAP, MAP_BASE orientation, BASE_ANTENA.
+ */
+TEST_F(FactorGnssFix3dWithClockTest, gnss_1_map_base_position)
+{
+ for (auto i = 0; i<N; i++)
+ {
+ randomSetup();
+
+ // --------------------------- fixing/unfixing states
+ fixAllStates();
+ cap_gnss->getStateBlock('T')->unfix();
+ frame->getP()->unfix();
+
+ // --------------------------- perturb: map base position
+ cap_gnss->getStateBlock('T')->perturb(0.2);
+ frame->getP()->perturb(0.5);
+
+ // --------------------------- solve
+ std::string report = solver_ceres->solve(SolverManager::ReportVerbosity::FULL);
+ //std::cout << report << std::endl;
+
+ // --------------------------- check summary parameters & residuals
+ EXPECT_EQ(solver_ceres->getSummary().num_parameter_blocks_reduced, 2);
+ EXPECT_EQ(solver_ceres->getSummary().num_parameters_reduced, 4);
+ EXPECT_EQ(solver_ceres->getSummary().num_residual_blocks_reduced, 1);
+ EXPECT_EQ(solver_ceres->getSummary().num_residuals_reduced, 4);
+
+ // --------------------------- check solver solution
+ checkStates();
+ }
+}
+
+/* Test with one GNSS fix capture.
+ *
+ * Estimating: MAP_BASE orientation.
+ * Fixed: ENU_MAP, MAP_BASE position, BASE_ANTENA.
+ */
+TEST_F(FactorGnssFix3dWithClockTest, gnss_1_map_base_orientation)
+{
+ for (auto i = 0; i<N; i++)
+ {
+ randomSetup();
+
+ // --------------------------- fixing/unfixing states
+ fixAllStates();
+ cap_gnss->getStateBlock('T')->unfix();
+ frame->getO()->unfix();
+
+ // --------------------------- perturb: map base position
+ cap_gnss->getStateBlock('T')->perturb(0.2);
+ frame->getO()->perturb(0.5);
+
+ // --------------------------- solve
+ std::string report = solver_ceres->solve(SolverManager::ReportVerbosity::FULL);
+ //std::cout << report << std::endl;
+
+ // --------------------------- check summary parameters & residuals
+ EXPECT_EQ(solver_ceres->getSummary().num_parameter_blocks_reduced, 2);
+ EXPECT_EQ(solver_ceres->getSummary().num_parameters_reduced, 5);
+ EXPECT_EQ(solver_ceres->getSummary().num_residual_blocks_reduced, 1);
+ EXPECT_EQ(solver_ceres->getSummary().num_residuals_reduced, 4);
+
+ // --------------------------- check solver solution
+ // NOT OBSERVABLE checkStates();
+ }
+}
+
+/*
+ * Test with one GNSS fix capture.
+ *
+ * Estimating: ENU_MAP yaw.
+ * Fixed: ENU_MAP position, MAP_BASE and BASE_ANTENA.
+ */
+TEST_F(FactorGnssFix3dWithClockTest, gnss_1_enu_map_yaw)
+{
+ for (auto i = 0; i<N; i++)
+ {
+ randomSetup();
+
+ // --------------------------- fixing/unfixing states
+ fixAllStates();
+ cap_gnss->getStateBlock('T')->unfix();
+ gnss_sensor->getEnuMapYaw()->unfix();
+
+ // --------------------------- perturb: map base position
+ cap_gnss->getStateBlock('T')->perturb(0.2);
+ gnss_sensor->getEnuMapYaw()->perturb(0.2);
+
+ // --------------------------- solve
+ std::string report = solver_ceres->solve(SolverManager::ReportVerbosity::FULL);
+ //std::cout << report << std::endl;
+
+ // --------------------------- check summary parameters & residuals
+ EXPECT_EQ(solver_ceres->getSummary().num_parameter_blocks_reduced, 2);
+ EXPECT_EQ(solver_ceres->getSummary().num_parameters_reduced, 2);
+ EXPECT_EQ(solver_ceres->getSummary().num_residual_blocks_reduced, 1);
+ EXPECT_EQ(solver_ceres->getSummary().num_residuals_reduced, 4);
+
+ // --------------------------- check solver solution
+ checkStates();
+ }
+}
+
+/* Test with one GNSS fix capture.
+ *
+ * Estimating: ENU_MAP position.
+ * Fixed: ENU_MAP yaw, MAP_BASE and BASE_ANTENA.
+ */
+TEST_F(FactorGnssFix3dWithClockTest, gnss_1_enu_map_position)
+{
+ for (auto i = 0; i<N; i++)
+ {
+ randomSetup();
+
+ // --------------------------- fixing/unfixing states
+ fixAllStates();
+ cap_gnss->getStateBlock('T')->unfix();
+ gnss_sensor->getEnuMapTranslation()->unfix();
+
+ // --------------------------- perturb: map base position
+ cap_gnss->getStateBlock('T')->perturb(0.2);
+ gnss_sensor->getEnuMapTranslation()->perturb(0.5);
+
+ // --------------------------- solve
+ std::string report = solver_ceres->solve(SolverManager::ReportVerbosity::FULL);
+ //std::cout << report << std::endl;
+
+ // --------------------------- check summary parameters & residuals
+ EXPECT_EQ(solver_ceres->getSummary().num_parameter_blocks_reduced, 2);
+ EXPECT_EQ(solver_ceres->getSummary().num_parameters_reduced, 4);
+ EXPECT_EQ(solver_ceres->getSummary().num_residual_blocks_reduced, 1);
+ EXPECT_EQ(solver_ceres->getSummary().num_residuals_reduced, 4);
+
+ // --------------------------- check solver solution
+ checkStates();
+ }
+}
+
+/* Test with one GNSS fix capture.
+ *
+ * Estimating: BASE_ANTENA (2 first components that are observable).
+ * Fixed: ENU_MAP, MAP_BASE.
+ */
+TEST_F(FactorGnssFix3dWithClockTest, gnss_1_base_antena)
+{
+ for (auto i = 0; i<N; i++)
+ {
+ randomSetup();
+
+ // --------------------------- fixing/unfixing states
+ fixAllStates();
+ cap_gnss->getStateBlock('T')->unfix();
+ gnss_sensor->getP()->unfix();
+
+ // --------------------------- perturb: map base position
+ cap_gnss->getStateBlock('T')->perturb(0.2);
+ gnss_sensor->getP()->perturb(0.5);
+
+ // --------------------------- solve
+ std::string report = solver_ceres->solve(SolverManager::ReportVerbosity::FULL);
+ // std::cout << report << std::endl;
+
+ // --------------------------- check summary parameters & residuals
+ EXPECT_EQ(solver_ceres->getSummary().num_parameter_blocks_reduced, 2);
+ EXPECT_EQ(solver_ceres->getSummary().num_parameters_reduced, 4);
+ EXPECT_EQ(solver_ceres->getSummary().num_residual_blocks_reduced, 1);
+ EXPECT_EQ(solver_ceres->getSummary().num_residuals_reduced, 4);
+
+ // --------------------------- check solver solution
+ checkStates();
+ }
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/test/gtest_factor_gnss_pseudo_range.cpp b/test/gtest_factor_gnss_pseudo_range.cpp
index 91fbaa28c0a8768f7088e902f71c50ddfec242de..4aaf5603423847dcb0fa401d11c47473cac319a5 100644
--- a/test/gtest_factor_gnss_pseudo_range.cpp
+++ b/test/gtest_factor_gnss_pseudo_range.cpp
@@ -60,17 +60,11 @@ void randomGroundtruth()
{
// ECEF-ENU
t_ecef_enu = Vector3d::Random() * 1e3;
- Vector3d rpy_ecef_enu = M_PI*Vector3d::Random();
- R_ecef_enu = (AngleAxis<double>(rpy_ecef_enu(0), Vector3d::UnitX()) *
- AngleAxis<double>(rpy_ecef_enu(1), Vector3d::UnitY()) *
- AngleAxis<double>(rpy_ecef_enu(2), Vector3d::UnitZ())).toRotationMatrix();
-
+ R_ecef_enu = q2R(Quaterniond::UnitRandom());
// ENU-MAP
t_enu_map = Vector3d::Random() * 1e3;
rpy_enu_map = M_PI*Vector3d::Random();
- R_enu_map = (AngleAxis<double>(rpy_enu_map(0), Vector3d::UnitX()) *
- AngleAxis<double>(rpy_enu_map(1), Vector3d::UnitY()) *
- AngleAxis<double>(rpy_enu_map(2), Vector3d::UnitZ())).toRotationMatrix();
+ R_enu_map = e2R(rpy_enu_map);
// MAP-BASE
t_map_base = Vector3d::Random() * 1e2;
@@ -109,6 +103,8 @@ void setUpProblem()
frm->remove();
solver->getSolverOptions().max_num_iterations = 100;
+ solver->getSolverOptions().gradient_tolerance = 1e-8;
+ solver->getSolverOptions().function_tolerance = 1e-8;
// Sensor
// ENU-MAP
@@ -139,10 +135,10 @@ void setUpProblem()
ftr4 = FeatureBase::emplace<FeatureGnssSatellite>(cap, obsd_t(), sat4, range4); // obsd_t data is not used in pseudo range factors
// factors
- fac1 = FactorBase::emplace<FactorGnssPseudoRange>(ftr1, ftr1, gnss_sensor, nullptr, false);
- fac2 = FactorBase::emplace<FactorGnssPseudoRange>(ftr2, ftr2, gnss_sensor, nullptr, false);
- fac3 = FactorBase::emplace<FactorGnssPseudoRange>(ftr3, ftr3, gnss_sensor, nullptr, false);
- fac4 = FactorBase::emplace<FactorGnssPseudoRange>(ftr4, ftr4, gnss_sensor, nullptr, false);
+ fac1 = FactorBase::emplace<FactorGnssPseudoRange>(ftr1, ftr1, gnss_sensor, nullptr, false, false); // without sagnac correction
+ fac2 = FactorBase::emplace<FactorGnssPseudoRange>(ftr2, ftr2, gnss_sensor, nullptr, false, false); // without sagnac correction
+ fac3 = FactorBase::emplace<FactorGnssPseudoRange>(ftr3, ftr3, gnss_sensor, nullptr, false, false); // without sagnac correction
+ fac4 = FactorBase::emplace<FactorGnssPseudoRange>(ftr4, ftr4, gnss_sensor, nullptr, false, false); // without sagnac correction
// ASSERTS
// ENU-MAP
diff --git a/test/gtest_factor_gnss_tdcp.cpp b/test/gtest_factor_gnss_tdcp.cpp
index 7818ab4a9bcbebcb10f9bb14e4d1abc42fdb6883..c54857558b22d006bbab8b7c590909ed2bb47964 100644
--- a/test/gtest_factor_gnss_tdcp.cpp
+++ b/test/gtest_factor_gnss_tdcp.cpp
@@ -33,6 +33,9 @@ using namespace Eigen;
using namespace wolf;
using namespace GnssUtils;
+int N = 10;
+double TestEPS = 1e-4;
+
// groundtruth transformations
Vector3d t_ecef_enu, t_enu_map, t_map_base_r, t_map_base_k, t_base_antena; // Antena extrinsics
Vector3d t_ecef_antena_r, t_ecef_antena_k;
@@ -65,17 +68,13 @@ void randomGroundtruth()
{
// ECEF-ENU
t_ecef_enu = Vector3d::Random() * 1e3;
- Vector3d rpy_ecef_enu = M_PI*Vector3d::Random();
- R_ecef_enu = (AngleAxis<double>(rpy_ecef_enu(0), Vector3d::UnitX()) *
- AngleAxis<double>(rpy_ecef_enu(1), Vector3d::UnitY()) *
- AngleAxis<double>(rpy_ecef_enu(2), Vector3d::UnitZ())).toRotationMatrix();
+ R_ecef_enu = q2R(Quaterniond::UnitRandom());
// ENU-MAP
t_enu_map = Vector3d::Random() * 1e3;
- rpy_enu_map = M_PI*Vector3d::Random();
- R_enu_map = (AngleAxis<double>(rpy_enu_map(0), Vector3d::UnitX()) *
- AngleAxis<double>(rpy_enu_map(1), Vector3d::UnitY()) *
- AngleAxis<double>(rpy_enu_map(2), Vector3d::UnitZ())).toRotationMatrix();
+ rpy_enu_map = M_PI*Vector3d::Random() * 0.1;
+ rpy_enu_map(1)*=0.5; //pitch in [-90,90]
+ R_enu_map = e2R(rpy_enu_map);
// MAP-BASE
t_map_base_k = Vector3d::Random() * 1e2;
@@ -124,6 +123,8 @@ void setUpProblem()
frm_k->remove();
solver->getSolverOptions().max_num_iterations = 100;
+ solver->getSolverOptions().gradient_tolerance = 1e-8;
+ solver->getSolverOptions().function_tolerance = 1e-8;
// Sensor
// ENU-MAP
@@ -132,6 +133,7 @@ void setUpProblem()
gnss_sensor->setEnuMapPitchState(rpy_enu_map(1));
gnss_sensor->setEnuMapYawState(rpy_enu_map(2));
// ECEF-ENU
+ gnss_sensor->resetEnu();
gnss_sensor->setEnuEcef(R_ecef_enu.transpose(), R_ecef_enu.transpose()*(-t_ecef_enu));
// Extrinsics
gnss_sensor->getP()->setState(t_base_antena);
@@ -171,28 +173,28 @@ void setUpProblem()
ftr4_k = FeatureBase::emplace<FeatureGnssSatellite>(cap_k, obsd_t(), sat4_k, range4_k);
// factors
- fac1 = FactorBase::emplace<FactorGnssTdcp>(ftr1_r, 0.1, ftr1_r, ftr1_k, gnss_sensor, nullptr, false);
- fac2 = FactorBase::emplace<FactorGnssTdcp>(ftr2_r, 0.1, ftr2_r, ftr2_k, gnss_sensor, nullptr, false);
- fac3 = FactorBase::emplace<FactorGnssTdcp>(ftr3_r, 0.1, ftr3_r, ftr3_k, gnss_sensor, nullptr, false);
- fac4 = FactorBase::emplace<FactorGnssTdcp>(ftr4_r, 0.1, ftr4_r, ftr4_k, gnss_sensor, nullptr, false);
+ fac1 = FactorBase::emplace<FactorGnssTdcp>(ftr1_r, 1e-3, ftr1_r, ftr1_k, gnss_sensor, nullptr, false, false); // without sagnac correction
+ fac2 = FactorBase::emplace<FactorGnssTdcp>(ftr2_r, 1e-3, ftr2_r, ftr2_k, gnss_sensor, nullptr, false, false); // without sagnac correction
+ fac3 = FactorBase::emplace<FactorGnssTdcp>(ftr3_r, 1e-3, ftr3_r, ftr3_k, gnss_sensor, nullptr, false, false); // without sagnac correction
+ fac4 = FactorBase::emplace<FactorGnssTdcp>(ftr4_r, 1e-3, ftr4_r, ftr4_k, gnss_sensor, nullptr, false, false); // without sagnac correction
// ASSERTS
// ENU-MAP
ASSERT_FLOAT_EQ(gnss_sensor->getEnuMapRoll()->getState()(0), rpy_enu_map(0));
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);
+ ASSERT_MATRIX_APPROX(gnss_sensor->getEnuMapTranslation()->getState(), t_enu_map, TestEPS);
// Antena
- ASSERT_MATRIX_APPROX(gnss_sensor->getP()->getState(), t_base_antena, 1e-3);
+ ASSERT_MATRIX_APPROX(gnss_sensor->getP()->getState(), t_base_antena, TestEPS);
// Frame r
- ASSERT_MATRIX_APPROX(frm_r->getP()->getState(), t_map_base_r, 1e-3);
- ASSERT_MATRIX_APPROX(frm_r->getO()->getState(), q_map_base_r.coeffs(), 1e-3);
+ ASSERT_MATRIX_APPROX(frm_r->getP()->getState(), t_map_base_r, TestEPS);
+ ASSERT_QUATERNION_VECTOR_APPROX(frm_r->getO()->getState(), q_map_base_r.coeffs(), TestEPS);
// Frame k
- ASSERT_MATRIX_APPROX(frm_k->getP()->getState(), t_map_base_k, 1e-3);
- ASSERT_MATRIX_APPROX(frm_k->getO()->getState(), q_map_base_k.coeffs(), 1e-3);
+ ASSERT_MATRIX_APPROX(frm_k->getP()->getState(), t_map_base_k, TestEPS);
+ ASSERT_QUATERNION_VECTOR_APPROX(frm_k->getO()->getState(), q_map_base_k.coeffs(), TestEPS);
// clock drift
- ASSERT_MATRIX_APPROX(clock_drift_r, cap_r->getStateBlock('T')->getState(), 1e-3);
- ASSERT_MATRIX_APPROX(clock_drift_k, cap_k->getStateBlock('T')->getState(), 1e-3);
+ ASSERT_MATRIX_APPROX(clock_drift_r, cap_r->getStateBlock('T')->getState(), TestEPS);
+ ASSERT_MATRIX_APPROX(clock_drift_k, cap_k->getStateBlock('T')->getState(), TestEPS);
}
void fixAllStates()
@@ -215,7 +217,7 @@ void fixAllStates()
////////////////////////////////////////////////////////
TEST(FactorGnssTdcpTest, observe_clock_drift_r)
{
- for (auto i = 0; i < 100; i++)
+ for (auto i = 0; i < N; i++)
{
// setup random problem
randomGroundtruth();
@@ -237,13 +239,13 @@ TEST(FactorGnssTdcpTest, observe_clock_drift_r)
std::string report = solver->solve(SolverManager::ReportVerbosity::FULL);
//std::cout << report << std::endl;
- ASSERT_MATRIX_APPROX(clock_drift_r, cap_r->getStateBlock('T')->getState(), 1e-3);
+ ASSERT_MATRIX_APPROX(clock_drift_r, cap_r->getStateBlock('T')->getState(), TestEPS);
}
}
TEST(FactorGnssTdcpTest, observe_clock_drift_k)
{
- for (auto i = 0; i < 100; i++)
+ for (auto i = 0; i < N; i++)
{
// setup random problem
randomGroundtruth();
@@ -265,13 +267,13 @@ TEST(FactorGnssTdcpTest, observe_clock_drift_k)
std::string report = solver->solve(SolverManager::ReportVerbosity::FULL);
//std::cout << report << std::endl;
- ASSERT_MATRIX_APPROX(clock_drift_k, cap_k->getStateBlock('T')->getState(), 1e-3);
+ ASSERT_MATRIX_APPROX(clock_drift_k, cap_k->getStateBlock('T')->getState(), TestEPS);
}
}
TEST(FactorGnssTdcpTest, observe_frame_p_r)
{
- for (auto i = 0; i < 100; i++)
+ for (auto i = 0; i < N; i++)
{
// setup random problem
randomGroundtruth();
@@ -291,13 +293,13 @@ TEST(FactorGnssTdcpTest, observe_frame_p_r)
std::string report = solver->solve(SolverManager::ReportVerbosity::FULL);
//std::cout << report << std::endl;
- ASSERT_MATRIX_APPROX(frm_r->getP()->getState(), t_map_base_r, 1e-3);
+ ASSERT_MATRIX_APPROX(frm_r->getP()->getState(), t_map_base_r, TestEPS);
}
}
TEST(FactorGnssTdcpTest, observe_frame_p_k)
{
- for (auto i = 0; i < 100; i++)
+ for (auto i = 0; i < N; i++)
{
// setup random problem
randomGroundtruth();
@@ -317,13 +319,13 @@ TEST(FactorGnssTdcpTest, observe_frame_p_k)
std::string report = solver->solve(SolverManager::ReportVerbosity::FULL);
//std::cout << report << std::endl;
- ASSERT_MATRIX_APPROX(frm_k->getP()->getState(), t_map_base_k, 1e-3);
+ ASSERT_MATRIX_APPROX(frm_k->getP()->getState(), t_map_base_k, TestEPS);
}
}
TEST(FactorGnssTdcpTest, observe_frame_p_clock_k)
{
- for (auto i = 0; i < 100; i++)
+ for (auto i = 0; i < N; i++)
{
// setup random problem
randomGroundtruth();
@@ -344,14 +346,14 @@ TEST(FactorGnssTdcpTest, observe_frame_p_clock_k)
std::string report = solver->solve(SolverManager::ReportVerbosity::FULL);
//std::cout << report << std::endl;
- ASSERT_MATRIX_APPROX(frm_k->getP()->getState(), t_map_base_k, 1e-3);
- ASSERT_MATRIX_APPROX(clock_drift_k, cap_k->getStateBlock('T')->getState(), 1e-3);
+ ASSERT_MATRIX_APPROX(frm_k->getP()->getState(), t_map_base_k, TestEPS);
+ ASSERT_MATRIX_APPROX(clock_drift_k, cap_k->getStateBlock('T')->getState(), TestEPS);
}
}
TEST(FactorGnssTdcpTest, observe_frame_p_clock_r)
{
- for (auto i = 0; i < 100; i++)
+ for (auto i = 0; i < N; i++)
{
// setup random problem
randomGroundtruth();
@@ -372,14 +374,14 @@ TEST(FactorGnssTdcpTest, observe_frame_p_clock_r)
std::string report = solver->solve(SolverManager::ReportVerbosity::FULL);
//std::cout << report << std::endl;
- ASSERT_MATRIX_APPROX(frm_r->getP()->getState(), t_map_base_r, 1e-3);
- ASSERT_MATRIX_APPROX(clock_drift_r, cap_r->getStateBlock('T')->getState(), 1e-3);
+ ASSERT_MATRIX_APPROX(frm_r->getP()->getState(), t_map_base_r, TestEPS);
+ ASSERT_MATRIX_APPROX(clock_drift_r, cap_r->getStateBlock('T')->getState(), TestEPS);
}
}
TEST(FactorGnssTdcpTest, observe_enumap_p)
{
- for (auto i = 0; i < 100; i++)
+ for (auto i = 0; i < N; i++)
{
// setup random problem
randomGroundtruth();
@@ -399,13 +401,13 @@ TEST(FactorGnssTdcpTest, observe_enumap_p)
std::string report = solver->solve(SolverManager::ReportVerbosity::FULL);
//std::cout << report << std::endl;
- ASSERT_MATRIX_APPROX(gnss_sensor->getEnuMapTranslation()->getState(), t_enu_map, 1e-3);
+ ASSERT_MATRIX_APPROX(gnss_sensor->getEnuMapTranslation()->getState(), t_enu_map, TestEPS);
}
}
-TEST(FactorGnssTdcpTest, observe_enumap_o)
+TEST(FactorGnssTdcpTest, observe_enumap_roll)
{
- for (auto i = 0; i < 100; i++)
+ for (auto i = 0; i < N; i++)
{
// setup random problem
randomGroundtruth();
@@ -414,12 +416,68 @@ TEST(FactorGnssTdcpTest, observe_enumap_o)
// fix/unfix
fixAllStates();
gnss_sensor->getEnuMapRoll()->unfix();
- gnss_sensor->getEnuMapPitch()->unfix();
- gnss_sensor->getEnuMapYaw()->unfix();
// perturb
gnss_sensor->getEnuMapRoll()->perturb(1);
+
+ // Only 3 factors
+ fac4->setStatus(FAC_INACTIVE);
+
+ // solve
+ std::string report = solver->solve(SolverManager::ReportVerbosity::FULL);
+ //std::cout << report << std::endl;
+
+ ASSERT_MATRIX_APPROX((Vector3d() << gnss_sensor->getEnuMapRoll()->getState(),
+ gnss_sensor->getEnuMapPitch()->getState(),
+ gnss_sensor->getEnuMapYaw()->getState()).finished(),
+ rpy_enu_map,
+ TestEPS);
+ }
+}
+
+TEST(FactorGnssTdcpTest, observe_enumap_pitch)
+{
+ for (auto i = 0; i < N; i++)
+ {
+ // setup random problem
+ randomGroundtruth();
+ setUpProblem();
+
+ // fix/unfix
+ fixAllStates();
+ gnss_sensor->getEnuMapPitch()->unfix();
+
+ // perturb
gnss_sensor->getEnuMapPitch()->perturb(1);
+
+ // Only 3 factors
+ fac4->setStatus(FAC_INACTIVE);
+
+ // solve
+ std::string report = solver->solve(SolverManager::ReportVerbosity::FULL);
+ //std::cout << report << std::endl;
+
+ ASSERT_MATRIX_APPROX((Vector3d() << gnss_sensor->getEnuMapRoll()->getState(),
+ gnss_sensor->getEnuMapPitch()->getState(),
+ gnss_sensor->getEnuMapYaw()->getState()).finished(),
+ rpy_enu_map,
+ TestEPS);
+ }
+}
+
+TEST(FactorGnssTdcpTest, observe_enumap_yaw)
+{
+ for (auto i = 0; i < N; i++)
+ {
+ // setup random problem
+ randomGroundtruth();
+ setUpProblem();
+
+ // fix/unfix
+ fixAllStates();
+ gnss_sensor->getEnuMapYaw()->unfix();
+
+ // perturb
gnss_sensor->getEnuMapYaw()->perturb(1);
// Only 3 factors
@@ -429,7 +487,11 @@ TEST(FactorGnssTdcpTest, observe_enumap_o)
std::string report = solver->solve(SolverManager::ReportVerbosity::FULL);
//std::cout << report << std::endl;
- ASSERT_MATRIX_APPROX(gnss_sensor->getEnuMapTranslation()->getState(), t_enu_map, 1e-3);
+ ASSERT_MATRIX_APPROX((Vector3d() << gnss_sensor->getEnuMapRoll()->getState(),
+ gnss_sensor->getEnuMapPitch()->getState(),
+ gnss_sensor->getEnuMapYaw()->getState()).finished(),
+ rpy_enu_map,
+ TestEPS);
}
}