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/include/gnss/capture/capture_gnss_fix.h b/include/gnss/capture/capture_gnss_fix.h
index c894d3e7f887ee2deaed7241f9a3fc819f0e8ace..cfbf8900d360798b371e7982bb3084f0d6979ed1 100644
--- a/include/gnss/capture/capture_gnss_fix.h
+++ b/include/gnss/capture/capture_gnss_fix.h
@@ -37,38 +37,66 @@ 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& _ts,
+ SensorBasePtr _sensor_ptr,
+ const Eigen::Vector4d& _fix,
+ const Eigen::Matrix4d& _covariance,
+ bool _ecef_coordinates = true);
~CaptureGnssFix() override;
- const Eigen::Vector3d& getPositionEcef() const;
- const Eigen::Matrix3d& getCovarianceEcef() const;
+ 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& position, Eigen::Matrix3d& cov) const;
- const TimeStamp& getGpsTimeStamp() const;
+ TimeStamp getGpsTimeStamp() const;
void setGpsTimeStamp(const TimeStamp& _ts_GPST);
};
-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_;
}
diff --git a/include/gnss/factor/factor_gnss_fix_2d.h b/include/gnss/factor/factor_gnss_fix_2d.h
index 26a8735be406bfdadb7c62f52dd0c3507adf21ed..0a59456b207a4fa63dc7cd84a70748b72257516e 100644
--- a/include/gnss/factor/factor_gnss_fix_2d.h
+++ b/include/gnss/factor/factor_gnss_fix_2d.h
@@ -96,7 +96,6 @@ inline bool FactorGnssFix2d::operator ()(const T* const _x,
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;
@@ -106,7 +105,7 @@ inline bool FactorGnssFix2d::operator ()(const T* const _x,
//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 << "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 * (R_ENU_ECEF * getMeasurement().head<3>() + 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;
@@ -117,7 +116,7 @@ inline bool FactorGnssFix2d::operator ()(const T* const _x,
// 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);
+ residuals_ECEF = (getMeasurementSquareRootInformationUpper().topLeftCorner<3,3>() * R_ENU_ECEF.transpose() * 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..c3b3929e94fedc5db62a509bdb1e5cf826f86978 100644
--- a/include/gnss/factor/factor_gnss_fix_3d.h
+++ b/include/gnss/factor/factor_gnss_fix_3d.h
@@ -95,10 +95,9 @@ inline bool FactorGnssFix3d::operator ()(const T* const _x,
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::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);
+ Eigen::Matrix<T,3,1> fix_map = R_map_ENU * (R_ENU_ECEF * getMeasurement().head<3>() + 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;
@@ -106,7 +105,7 @@ inline bool FactorGnssFix3d::operator ()(const T* const _x,
// 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);
+ residuals_ECEF = (getMeasurementSquareRootInformationUpper().topLeftCorner<3,3>() * R_ENU_ECEF.transpose() * R_map_ENU.transpose()) * (antena_map - fix_map);
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..fe582423e8f35cc62a928caea6ba3bf74a8a038f
--- /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::Matrix<T,3,3> R_ECEF_ENU = sensor_gnss_ptr_->getREnuEcef().cast<T>().transpose();
+ Eigen::Matrix<T,3,1> t_ENU_ECEF = sensor_gnss_ptr_->gettEnuEcef().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::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.head<3>() = R_ECEF_ENU * (R_ENU_map * (q_map_base * t_base_antena + t_map_base) + t_ENU_map - t_ENU_ECEF);
+ fix_expected(3) = _clock_bias;
+
+ // Compute residual
+ residuals_ECEF = getMeasurementSquareRootInformationUpper() * (antena_ecef - getMeasurement());
+
+ return true;
+}
+
+} // namespace wolf
\ No newline at end of file
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_gnss_fix.h b/include/gnss/processor/processor_gnss_fix.h
index 1fa78ddd321ef08d9c1d557a7bd2ea7a5fac703a..a2bdf9edc92a504082fc1b5fbecabec311c10687 100644
--- a/include/gnss/processor/processor_gnss_fix.h
+++ b/include/gnss/processor/processor_gnss_fix.h
@@ -23,8 +23,8 @@
#define WOLF_PROCESSOR_GNSS_FIX_H
// Wolf includes
+#include "gnss/internal/config.h"
#include "core/processor/processor_base.h"
-#include "gnss/capture/capture_gnss.h"
#include "gnss/sensor/sensor_gnss.h"
// Std includes
@@ -127,9 +127,8 @@ class ProcessorGnssFix : public ProcessorBase
SensorGnssPtr sensor_gnss_;
ParamsProcessorGnssFixPtr params_gnss_;
CaptureBasePtr last_KF_capture_, incoming_capture_;
- FeatureGnssFixPtr last_KF_feature_;
+ FeatureBasePtr last_KF_feature_, incoming_feature_;
FrameBasePtr last_KF_;
- GnssUtils::ComputePosOutput incoming_pos_out_;
Eigen::Vector3d first_pos_;
VectorComposite first_frame_state_;
@@ -155,43 +154,42 @@ class ProcessorGnssFix : public ProcessorBase
*/
void processKeyFrame(FrameBasePtr _keyframe_ptr) override {};
+
/** \brief trigger in capture
*
* Returns true if processCapture() should be called after the provided capture arrived.
*/
- bool triggerInCapture(CaptureBasePtr) const override;
+ bool triggerInCapture(CaptureBasePtr) const override {return true;};
/** \brief trigger in key-frame
*
* The ProcessorTracker only processes incoming captures, then it returns false.
*/
- bool triggerInKeyFrame(FrameBasePtr _keyframe_ptr) const override {return false;}
+ bool triggerInKeyFrame(FrameBasePtr _keyframe_ptr) const override {return false;};
/** \brief store key frame
*
* Returns true if the key frame should be stored
*/
- bool storeKeyFrame(FrameBasePtr) override;
+ bool storeKeyFrame(FrameBasePtr) override {return true;};
/** \brief store capture
*
* Returns true if the capture should be stored
*/
- bool storeCapture(CaptureBasePtr) override;
+ bool storeCapture(CaptureBasePtr) override {return false;};
bool voteForKeyFrame() const override;
private:
+ void processStoredFrames();
+ void handleEnuMap(FeatureBasePtr incoming_feature);
+ FeatureBasePtr emplaceFeature(CaptureBasePtr _capture);
FactorBasePtr emplaceFactor(FeatureBasePtr _ftr_ptr);
bool detectOutlier(FactorBasePtr ctr_ptr);
};
-inline bool ProcessorGnssFix::triggerInCapture(CaptureBasePtr) const
-{
- return true;
-}
-
} // namespace wolf
#endif //WOLF_PROCESSOR_GNSS_FIX_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 31f95fd152bb034143384d3d96ee03b5029c6479..de9eed7707736db191d2360ca619b765208934cf 100644
--- a/src/processor/processor_gnss_fix.cpp
+++ b/src/processor/processor_gnss_fix.cpp
@@ -19,12 +19,12 @@
// along with this program. If not, see <http://www.gnu.org/licenses/>.
//
//--------LICENSE_END--------
+#include "gnss/processor/processor_gnss_fix.h"
#include "gnss/capture/capture_gnss.h"
#include "gnss/capture/capture_gnss_fix.h"
#include "gnss/factor/factor_gnss_fix_2d.h"
#include "gnss/factor/factor_gnss_fix_3d.h"
#include "gnss/feature/feature_gnss_fix.h"
-#include "gnss/processor/processor_gnss_fix.h"
namespace wolf
{
@@ -41,59 +41,150 @@ ProcessorGnssFix::~ProcessorGnssFix()
//
}
+void ProcessorGnssFix::processStoredFrames()
+{
+ // clean buffers (useless entries)
+ if (not buffer_frame_.empty() and not buffer_capture_.empty())
+ {
+ buffer_frame_ .removeUpToLower(buffer_capture_.getContainer().begin()->first-params_->time_tolerance);
+ buffer_capture_.removeUpToLower(buffer_frame_ .getContainer().begin()->first-params_->time_tolerance);
+ }
+
+ // iterate over stored frames
+ auto frm_it = buffer_frame_.getContainer().begin();
+ while (frm_it != buffer_frame_.getContainer().end())
+ {
+ // Search for any stored capture within time tolerance of frame
+ auto capture = buffer_capture_.select(frm_it->first, params_->time_tolerance);
+
+ // Capture found
+ if (capture)
+ {
+ // safety check: capture stored already linked
+ if (capture->getFrame())
+ {
+ WOLF_WARN("ProcessorGnssFix::processStoredFrames: any stored CaptureGnss was already linked to a frame");
+ buffer_capture_.getContainer().erase(capture->getTimeStamp());
+ frm_it++;
+ continue;
+ }
+ // link capture
+ capture->link(frm_it->second);
+
+ // get feature
+ FeatureGnssFixPtr feat_fix;
+ auto feature_list = capture->getFeatureList();
+ for (auto feat : feature_list)
+ {
+ feat_fix = std::dynamic_pointer_cast<FeatureGnssFix>(feat);
+ if (feat_fix)
+ break;
+ }
+ if (not feat_fix)
+ {
+ WOLF_WARN("ProcessorGnssFix::processStoredFrames: A stored CaptureGnss doesn't have any FrameGnssFix");
+ buffer_capture_.getContainer().erase(capture->getTimeStamp());
+ frm_it++;
+ continue;
+ }
+
+ // emplace factor
+ auto fac = emplaceFactor(feat_fix);
+
+ // outlier rejection (only can be evaluated if ENU defined and ENU-MAP well initialized: fixed)
+ if (params_gnss_->remove_outliers and detectOutlier(fac))
+ {
+ feat_fix->remove();
+ buffer_capture_.getContainer().erase(capture->getTimeStamp());
+ frm_it++;
+ continue;
+ }
+
+ // Handle ENU definition, ENU-MAP initialization etc.
+ handleEnuMap(feat_fix);
+
+ // store last KF (if more recent)
+ if (not last_KF_capture_ or last_KF_capture_->getTimeStamp() < capture->getTimeStamp())
+ {
+ last_KF_capture_ = capture;
+ last_KF_feature_ = feat_fix;
+ }
+
+ // remove capture from buffer
+ buffer_capture_.getContainer().erase(capture->getTimeStamp());
+
+ // remove frame from buffer
+ frm_it = buffer_frame_.getContainer().erase(frm_it);
+ }
+ else
+ frm_it++;
+ }
+}
+
+FeatureBasePtr ProcessorGnssFix::emplaceFeature(CaptureBasePtr _capture)
+{
+ GnssUtils::ComputePosOutput pos_output;
+ if (std::dynamic_pointer_cast<CaptureGnss>(_capture))
+ {
+ WOLF_DEBUG("ProcessorGnssFix: creating feature from incoming raw measurement. Type:", _capture->getType());
+ auto raw_capture = std::static_pointer_cast<CaptureGnss>(_capture);
+ pos_output = GnssUtils::computePos(*(raw_capture->getObservations()), *(raw_capture->getNavigation()), params_gnss_->compute_pos_opt);
+ if (!pos_output.success) // error
+ {
+ WOLF_DEBUG("computePos failed with msg: ", pos_output.msg);
+ return nullptr;
+ }
+ }
+ else if (std::dynamic_pointer_cast<CaptureGnssFix>(_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
+ 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");
+
+ return FeatureBase::emplace<FeatureGnssFix>(_capture, pos_output);
+}
+
void ProcessorGnssFix::processCapture(CaptureBasePtr _capture)
{
- // TODO: keep captures in a buffer and deal with KFpacks
WOLF_DEBUG("PR ", getName()," process() capture type: ", _capture->getType());
- incoming_capture_ = _capture;
- bool KF_created = false;
+ // Check capture type
+ if (not std::dynamic_pointer_cast<CaptureGnss>(_capture) and not std::dynamic_pointer_cast<CaptureGnssFix>(_capture))
+ throw std::runtime_error("ProcessorGnssFix::processCapture capture of bad type. Accepted types: CaptureGnss and CaptureGnssFix");
- // Check type of capture: GNSS raw
- bool israw = false;
- if (std::dynamic_pointer_cast<CaptureGnss>(_capture)!=nullptr)
- israw = true;
- // if not raw nor Fix, not processable
- else if (std::dynamic_pointer_cast<CaptureGnssFix>(_capture)==nullptr)
- return;
+ // First process stored frames
+ processStoredFrames();
- // Only process raw if fix_from_raw
- if (israw and !params_gnss_->fix_from_raw)
+ // Only process raw if fix_from_raw and fix if not fix_from_raw
+ bool israw = std::dynamic_pointer_cast<CaptureGnss>(_capture)!=nullptr;
+ if (israw and not params_gnss_->fix_from_raw)
return;
-
- // Only process fix if not fix_from_raw
- if (!israw and params_gnss_->fix_from_raw)
+ if (not israw and params_gnss_->fix_from_raw)
return;
+ // Process incoming capture
+ incoming_capture_ = _capture;
+ bool KF_created = false;
FrameBasePtr new_frame = nullptr;
FactorBasePtr new_fac = nullptr;
- // emplace features
- if (israw)
- {
- auto raw_capture = std::static_pointer_cast<CaptureGnss>(incoming_capture_);
- incoming_pos_out_ = GnssUtils::computePos(*(raw_capture->getObservations()), *(raw_capture->getNavigation()), params_gnss_->compute_pos_opt);
- if (!incoming_pos_out_.success) // error
- {
- WOLF_DEBUG("computePos failed with msg: ", incoming_pos_out_.msg);
- return;
- }
- }
- else
- {
- WOLF_DEBUG("ProcessorGnssFix: creating feature from incoming navsatfix. Type:", incoming_capture_->getType());
- auto fix_capture = std::static_pointer_cast<CaptureGnssFix>(incoming_capture_);
- incoming_pos_out_.time = fix_capture->getGpsTimeStamp().getSeconds(); // time_t time;
- incoming_pos_out_.sec = 1e-9 * fix_capture->getGpsTimeStamp().getSeconds(); // double sec;
- incoming_pos_out_.pos = fix_capture->getPositionEcef(); // Eigen::Vector3d pos; // position (m)
- incoming_pos_out_.vel = Eigen::Vector3d::Zero(); // Eigen::Vector3d vel; // velocity (m/s)
- incoming_pos_out_.pos_covar = fix_capture->getCovarianceEcef(); // Eigen::Matrix3d pos_covar; // position covariance (m^2)
- incoming_pos_out_.type = 0; // int type; // coordinates used (0:xyz-ecef,1:enu-baseline)
- }
- auto incoming_feature = FeatureBase::emplace<FeatureGnssFix>(incoming_capture_, incoming_pos_out_);
+ // emplace feature
+ incoming_feature_ = emplaceFeature(incoming_capture_);
+ if (not incoming_feature_)
+ return;
// ALREADY CREATED KF
- FrameBasePtr keyframe = buffer_frame_.select( incoming_capture_->getTimeStamp(), params_gnss_->time_tolerance);
+ FrameBasePtr keyframe = buffer_frame_.select(incoming_capture_->getTimeStamp(), params_gnss_->time_tolerance);
if (keyframe and last_KF_capture_ and keyframe == last_KF_capture_->getFrame())
keyframe = nullptr;
if (keyframe)
@@ -108,70 +199,82 @@ void ProcessorGnssFix::processCapture(CaptureBasePtr _capture)
new_frame = getProblem()->emplaceFrame(incoming_capture_->getTimeStamp());
KF_created = true;
}
- // OTHERWISE return
+ // OTHERWISE store capture
else
+ {
+ buffer_capture_.emplace(_capture->getTimeStamp(), _capture);
return;
+ }
- // ESTABLISH FACTOR
// link capture
incoming_capture_->link(new_frame);
+
// emplace factor
- new_fac = emplaceFactor(incoming_feature);
+ new_fac = emplaceFactor(incoming_feature_);
// outlier rejection (only can be evaluated if ENU defined and ENU-MAP well initialized: fixed)
- if (params_gnss_->remove_outliers and
- sensor_gnss_->isEnuDefined() and
- sensor_gnss_->isEnuMapFixed() and
- detectOutlier(new_fac))
+ if (params_gnss_->remove_outliers and detectOutlier(new_fac))
{
- new_frame->remove();
+ if (KF_created)
+ new_frame->remove();
+ else
+ incoming_feature_->remove();
return;
}
+ // Handle ENU definition, ENU-MAP initialization etc.
+ handleEnuMap(incoming_feature_);
+
// store last KF
last_KF_capture_ = incoming_capture_;
- last_KF_feature_ = incoming_feature;
+ last_KF_feature_ = incoming_feature_;
+
+ // Notify if KF created
+ if (KF_created)
+ getProblem()->keyFrameCallback(new_frame, shared_from_this());
+}
+void ProcessorGnssFix::handleEnuMap(FeatureBasePtr feature)
+{
// Define ENU (if not defined)
if (!sensor_gnss_->isEnuDefined())
{
- WOLF_DEBUG("Defining ecef enu");
- sensor_gnss_->setEcefEnu(incoming_feature->getMeasurement());
+ WOLF_INFO("Defining ecef enu with first fix");
+ sensor_gnss_->setEcefEnu(feature->getMeasurement().head<3>());
}
- // Store the first capture that established a factor (for initializing ENU-MAP)
+ // Store the first capture that established a factor (for later initialization ENU-MAP)
if (first_frame_state_.empty() and
not sensor_gnss_->isEnuMapFixed())
{
- first_frame_state_ = incoming_capture_->getFrame()->getState();
- first_pos_ = incoming_feature->getMeasurement();
+ first_frame_state_ = feature->getCapture()->getFrame()->getState();
+ first_pos_ = feature->getMeasurement().head<3>();
}
+
// Initialize ENU-MAP if: ENU defined and ENU-MAP not initialized (and not fixed) and far enough
if (params_gnss_->init_enu_map and
not first_frame_state_.empty() and
sensor_gnss_->isEnuDefined() and
not sensor_gnss_->isEnuMapInitialized() and
not sensor_gnss_->isEnuMapFixed() and
- (first_pos_-incoming_pos_out_.pos).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 incoming_capture_->getFrame() != nullptr);
+ assert(first_frame_state_.count('P') and
+ first_frame_state_.count('O') and
+ feature->getCapture()->getFrame() != nullptr);
sensor_gnss_->initializeEnuMap(first_frame_state_.vector("PO"),
first_pos_,
- incoming_capture_->getFrame()->getState().vector("PO"),
- incoming_feature->getMeasurement());
+ feature->getCapture()->getFrame()->getState().vector("PO"),
+ feature->getMeasurement().head<3>());
// Set as not-initialized if factors not separated enough ( < enu_map_init_dist_max)
- if ((first_pos_ - incoming_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_ - incoming_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();
-
- // Notify if KF created
- if (KF_created)
- getProblem()->keyFrameCallback(new_frame, shared_from_this());
}
FactorBasePtr ProcessorGnssFix::emplaceFactor(FeatureBasePtr _ftr)
@@ -216,8 +319,8 @@ bool ProcessorGnssFix::voteForKeyFrame() const
sensor_gnss_->isEnuDefined() and
not sensor_gnss_->isEnuMapInitialized() and
not sensor_gnss_->isEnuMapFixed() and
- (first_pos_-incoming_pos_out_.pos).norm() > params_gnss_->enu_map_init_dist_min and
- (first_pos_-incoming_pos_out_.pos).norm() < params_gnss_->enu_map_init_dist_max)
+ (first_pos_-incoming_feature_->getMeasurement()).norm() > params_gnss_->enu_map_init_dist_min and
+ (first_pos_-incoming_feature_->getMeasurement()).norm() < params_gnss_->enu_map_init_dist_max)
{
WOLF_DEBUG("KF because of enu map not initialized");
return true;
@@ -225,9 +328,9 @@ bool ProcessorGnssFix::voteForKeyFrame() const
// Distance criterion (ENU defined and ENU-MAP initialized)
if (last_KF_capture_ != nullptr and
- (incoming_pos_out_.pos - last_KF_feature_->getMeasurement()).norm() > params_gnss_->dist_traveled)
+ (incoming_feature_->getMeasurement() - last_KF_feature_->getMeasurement()).norm() > params_gnss_->dist_traveled)
{
- WOLF_DEBUG("KF because of distance criterion: ", (incoming_pos_out_.pos - last_KF_feature_->getMeasurement()).norm());
+ WOLF_DEBUG("KF because of distance criterion: ", (incoming_feature_->getMeasurement() - last_KF_feature_->getMeasurement()).norm());
return true;
}
@@ -239,6 +342,9 @@ bool ProcessorGnssFix::detectOutlier(FactorBasePtr fac)
{
WOLF_DEBUG( "PR ", getName()," rejectOutlier...");
+ if (not sensor_gnss_->isEnuDefined() or not sensor_gnss_->isEnuMapFixed())
+ return false;
+
// Cast feature
auto gnss_ftr = std::static_pointer_cast<FeatureGnssFix>(fac->getFeature());
@@ -273,14 +379,6 @@ bool ProcessorGnssFix::detectOutlier(FactorBasePtr fac)
return false;
}
-bool ProcessorGnssFix::storeKeyFrame(FrameBasePtr _frame_ptr)
-{
- return true;
-}
-bool ProcessorGnssFix::storeCapture(CaptureBasePtr _cap_ptr)
-{
- return false;
-}
void ProcessorGnssFix::configure(SensorBasePtr _sensor)
{
sensor_gnss_ = std::static_pointer_cast<SensorGnss>(_sensor);
diff --git a/test/gtest_factor_gnss_fix_2d.cpp b/test/gtest_factor_gnss_fix_2d.cpp
index a95688fd9c9c73b8456f3d81e2ebc036edf9eee9..fca8420cebb3aea6705ee24f502e356916ceb800 100644
--- a/test/gtest_factor_gnss_fix_2d.cpp
+++ b/test/gtest_factor_gnss_fix_2d.cpp
@@ -25,7 +25,7 @@
* Created on: Aug 1, 2018
* \author: jvallve
*/
-
+#include "gnss/internal/config.h"
#include <core/ceres_wrapper/solver_ceres.h>
#include <core/utils/utils_gtest.h>
@@ -38,16 +38,19 @@
using namespace Eigen;
using namespace wolf;
-void resetStates(SensorGnssPtr gnss_sensor_ptr, FrameBasePtr frame_ptr,
+void resetStates(SensorGnssPtr gnss_sensor_ptr, FrameBasePtr frame_ptr, CaptureBasePtr cap_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)
+ const Vector1d& o_map_base,
+ const double clock_bias)
{
+ // clock bias
+ cap_ptr->getStateBlock('T')->setState(Vector1d(clock_bias));
// ENU-MAP
- gnss_sensor_ptr->getEnuMapRoll()->setState(Eigen::Vector1d::Zero());
- gnss_sensor_ptr->getEnuMapPitch()->setState(Eigen::Vector1d::Zero());
+ gnss_sensor_ptr->getEnuMapRoll()->setState(Vector1d::Zero());
+ gnss_sensor_ptr->getEnuMapPitch()->setState(Vector1d::Zero());
gnss_sensor_ptr->getEnuMapYaw()->setState(o_enu_map);
gnss_sensor_ptr->getEnuMapTranslation()->setState(t_enu_map);
// Antena
@@ -106,12 +109,14 @@ Matrix3d R_ecef_enu = (AngleAxis<double>(1.3, Vector3d::UnitX()) *
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;
+double clock_bias = 0.5;
// 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;
+CaptureGnssFixPtr cap_gnss_ptr;
////////////////////////////////////////////////////////
@@ -138,7 +143,10 @@ TEST(FactorGnssFix2dTest, configure_tree)
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());
+ cap_gnss_ptr = std::make_shared<CaptureGnssFix>(TimeStamp(0),
+ gnss_sensor_ptr,
+ (Vector4d() << t_ecef_antena, clock_bias).finished(),
+ 1e-3*Matrix4d::Identity());
gnss_sensor_ptr->process(cap_gnss_ptr);
// Checks
@@ -153,8 +161,10 @@ TEST(FactorGnssFix2dTest, configure_tree)
*/
TEST(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);
+ resetStates(gnss_sensor_ptr, frame_ptr, cap_gnss_ptr, o_enu_map, t_base_antena, t_enu_map, t_map_base, o_map_base, clock_bias);
// --------------------------- fixing/unfixing states
fixAllStates(gnss_sensor_ptr, frame_ptr);
@@ -200,7 +210,7 @@ TEST(FactorGnssFix2dTest, gnss_1_map_base_position)
TEST(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);
+ resetStates(gnss_sensor_ptr, frame_ptr, cap_gnss_ptr, o_enu_map, t_base_antena, t_enu_map, t_map_base, o_map_base, clock_bias);
// --------------------------- fixing/unfixing states
fixAllStates(gnss_sensor_ptr, frame_ptr);
@@ -243,7 +253,7 @@ TEST(FactorGnssFix2dTest, gnss_1_map_base_orientation)
TEST(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);
+ resetStates(gnss_sensor_ptr, frame_ptr, cap_gnss_ptr, o_enu_map, t_base_antena, t_enu_map, t_map_base, o_map_base, clock_bias);
// --------------------------- fixing/unfixing states
fixAllStates(gnss_sensor_ptr, frame_ptr);
@@ -289,7 +299,7 @@ TEST(FactorGnssFix2dTest, gnss_1_enu_map_yaw)
TEST(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);
+ resetStates(gnss_sensor_ptr, frame_ptr, cap_gnss_ptr, o_enu_map, t_base_antena, t_enu_map, t_map_base, o_map_base, clock_bias);
// --------------------------- fixing/unfixing states
fixAllStates(gnss_sensor_ptr, frame_ptr);
@@ -333,7 +343,7 @@ TEST(FactorGnssFix2dTest, gnss_1_enu_map_position)
TEST(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);
+ resetStates(gnss_sensor_ptr, frame_ptr, cap_gnss_ptr, o_enu_map, t_base_antena, t_enu_map, t_map_base, o_map_base, clock_bias);
// --------------------------- fixing/unfixing states
fixAllStates(gnss_sensor_ptr, frame_ptr);