diff --git a/CMakeLists.txt b/CMakeLists.txt
index 89e8ce8771d7987d3a40afbd1f7a78a6f8aecc7a..c31312a7033c003e32aa91b03f704f41c283441d 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -118,7 +118,6 @@ SET(HDRS_FACTOR
include/gnss/factor/factor_gnss_fix_2d.h
include/gnss/factor/factor_gnss_fix_3d.h
include/gnss/factor/factor_gnss_pseudo_range.h
- include/gnss/factor/factor_gnss_single_diff_2d.h
include/gnss/factor/factor_gnss_tdcp.h
include/gnss/factor/factor_gnss_tdcp_2d.h
include/gnss/factor/factor_gnss_tdcp_3d.h
diff --git a/include/gnss/capture/capture_gnss.h b/include/gnss/capture/capture_gnss.h
index 1a8980534f1e38c6aa0b6479369fbe4db77c078f..ea0d900399a23860bcb53e3615af830834008d77 100644
--- a/include/gnss/capture/capture_gnss.h
+++ b/include/gnss/capture/capture_gnss.h
@@ -43,25 +43,43 @@ class CaptureGnss : public CaptureBase
CaptureGnss(const TimeStamp& _ts, SensorBasePtr _sensor_ptr, GnssUtils::SnapshotPtr _snapshot);
~CaptureGnss() override;
- GnssUtils::SnapshotPtr getSnapshot() const;
- GnssUtils::ObservationsPtr getObservations() const;
- GnssUtils::NavigationPtr getNavigation() const;
- GnssUtils::Satellites& getSatellites();
+ GnssUtils::SnapshotConstPtr getSnapshot() const;
+ GnssUtils::SnapshotPtr getSnapshot();
+ GnssUtils::ObservationsConstPtr getObservations() const;
+ GnssUtils::ObservationsPtr getObservations();
+ GnssUtils::NavigationConstPtr getNavigation() const;
+ GnssUtils::NavigationPtr getNavigation();
const GnssUtils::Satellites& getSatellites() const;
+ GnssUtils::Satellites& getSatellites();
};
-inline GnssUtils::SnapshotPtr CaptureGnss::getSnapshot() const
+inline GnssUtils::SnapshotConstPtr CaptureGnss::getSnapshot() const
{
return snapshot_;
}
-inline GnssUtils::ObservationsPtr CaptureGnss::getObservations() const
+inline GnssUtils::SnapshotPtr CaptureGnss::getSnapshot()
+{
+ return snapshot_;
+}
+
+inline GnssUtils::ObservationsConstPtr CaptureGnss::getObservations() const
+{
+ return snapshot_->getObservations();
+}
+
+inline GnssUtils::ObservationsPtr CaptureGnss::getObservations()
{
return snapshot_->getObservations();
}
-inline GnssUtils::NavigationPtr CaptureGnss::getNavigation() const
+inline GnssUtils::NavigationConstPtr CaptureGnss::getNavigation() const
+{
+ return snapshot_->getNavigation();
+}
+
+inline GnssUtils::NavigationPtr CaptureGnss::getNavigation()
{
return snapshot_->getNavigation();
}
diff --git a/include/gnss/capture/capture_gnss_tdcp.h b/include/gnss/capture/capture_gnss_tdcp.h
index 73af63d7ccc75ce4ec45616638c60a54b43e722f..73dce5e723f89ecbf2ef88dc0fd13e7db692a944 100644
--- a/include/gnss/capture/capture_gnss_tdcp.h
+++ b/include/gnss/capture/capture_gnss_tdcp.h
@@ -56,7 +56,8 @@ class CaptureGnssTdcp : public CaptureBase
const Eigen::Vector3d& getData() const;
const Eigen::Matrix3d& getDataCovariance() const;
void getDataAndCovariance(Eigen::Vector3d& data, Eigen::Matrix3d& data_cov) const;
- FrameBasePtr getOriginFrame() const;
+ FrameBaseConstPtr getOriginFrame() const;
+ FrameBasePtr getOriginFrame();
const TimeStamp& getGpsTimeStamp() const;
void setGpsTimeStamp(const TimeStamp &_ts_GPST);
};
@@ -77,12 +78,17 @@ inline void CaptureGnssTdcp::getDataAndCovariance(Eigen::Vector3d& data, Eigen::
data_cov = data_covariance_;
}
-inline FrameBasePtr CaptureGnssTdcp::getOriginFrame() const
+inline FrameBaseConstPtr CaptureGnssTdcp::getOriginFrame() const
{
return origin_frame_ptr_;
}
-inline const wolf::TimeStamp& CaptureGnssTdcp::getGpsTimeStamp() const
+inline FrameBasePtr CaptureGnssTdcp::getOriginFrame()
+{
+ return origin_frame_ptr_;
+}
+
+inline const TimeStamp& CaptureGnssTdcp::getGpsTimeStamp() const
{
return ts_GPST_;
}
diff --git a/include/gnss/factor/factor_gnss_single_diff_2d.h b/include/gnss/factor/factor_gnss_single_diff_2d.h
deleted file mode 100644
index f85495b058a4c7a33f8982f85f38d5b1e99c5b0b..0000000000000000000000000000000000000000
--- a/include/gnss/factor/factor_gnss_single_diff_2d.h
+++ /dev/null
@@ -1,126 +0,0 @@
-//--------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--------
-
-#ifndef FACTOR_GNSS_SINGLE_DIFF_2d_H_
-#define FACTOR_GNSS_SINGLE_DIFF_2d_H_
-
-//Wolf includes
-#include "core/common/wolf.h"
-#include "core/factor/factor_autodiff.h"
-#include "core/frame/frame_base.h"
-#include "gnss/sensor/sensor_gnss.h"
-
-namespace wolf {
-
-WOLF_PTR_TYPEDEFS(FactorGnssSingleDiff2d);
-
-class FactorGnssSingleDiff2d : public FactorAutodiff<FactorGnssSingleDiff2d, 3, 2, 1, 2, 1, 3, 1, 1, 1>
-{
- protected:
- SensorGnssPtr sensor_gnss_ptr_;
-
- public:
-
- FactorGnssSingleDiff2d(FeatureBasePtr& _ftr_ptr, const FrameBasePtr& _frame_other_ptr, const SensorGnssPtr& _sensor_gnss_ptr, const ProcessorBasePtr& _processor_ptr, bool _apply_loss_function, FactorStatus _status = FAC_ACTIVE) :
- FactorAutodiff<FactorGnssSingleDiff2d, 3, 2, 1, 2, 1, 3, 1, 1, 1>("GNSS SINGLE DIFFERENCES 2d",
- 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");
- }
-
- virtual ~FactorGnssSingleDiff2d() = default;
-
- template<typename T>
- bool 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;
-
-};
-
-template<typename T>
-inline bool FactorGnssSingleDiff2d::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
-{
- 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::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::Map<Eigen::Matrix<T,3,1> > residuals_ECEF(_residuals);
-
- Eigen::Matrix<T,3,3> R_ENU_ECEF = sensor_gnss_ptr_->getREnuEcef().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>());
-
- // 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);
-
- // 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);
-
- //std::cout << "frame1: " << _x1[0] << " " << _x1[1] << " " << _o1[0] << std::endl;
- //std::cout << "frame2: " << _x2[0] << " " << _x2[1] << " " << _o2[0] << std::endl;
- //std::cout << "antena: " << _x_antena[0] << " " << _x_antena[1] << " " << _x_antena[2] << std::endl;
- //std::cout << "RPY: " << _roll[0] << " " << _pitch[1] << " " << _yaw[2] << std::endl;
- //std::cout << "measured_diff_2d: " << measured_diff_2d[0] << " " << measured_diff_2d[1] << std::endl;
- //std::cout << "expected_diff_2d: " << expected_diff_2d[0] << " " << expected_diff_2d[1] << std::endl;
-
- return true;
-}
-
-} // namespace wolf
-
-#endif
diff --git a/include/gnss/processor/processor_gnss_tdcp.h b/include/gnss/processor/processor_gnss_tdcp.h
index 0f1bbf1d0aed54d393109a3d63f7769a3a3aea5d..caca1debbd9e2add16bf14dfd522e97ba96a2546 100644
--- a/include/gnss/processor/processor_gnss_tdcp.h
+++ b/include/gnss/processor/processor_gnss_tdcp.h
@@ -96,6 +96,7 @@ class ProcessorGnssTdcp : public ProcessorBase
WOLF_PROCESSOR_CREATE(ProcessorGnssTdcp, ParamsProcessorGnssTdcp);
+ FrameBaseConstPtr getLastKF() const;
FrameBasePtr getLastKF();
protected:
@@ -144,7 +145,12 @@ class ProcessorGnssTdcp : public ProcessorBase
};
-inline wolf::FrameBasePtr ProcessorGnssTdcp::getLastKF()
+inline FrameBaseConstPtr ProcessorGnssTdcp::getLastKF() const
+{
+ return last_KF_;
+}
+
+inline FrameBasePtr ProcessorGnssTdcp::getLastKF()
{
return last_KF_;
}
diff --git a/include/gnss/sensor/sensor_gnss.h b/include/gnss/sensor/sensor_gnss.h
index 03fc7a2d446f4b568939e1686ccb021f234f1129..fece2683601328edd6d6f86b0efc5ba64ce36dc8 100644
--- a/include/gnss/sensor/sensor_gnss.h
+++ b/include/gnss/sensor/sensor_gnss.h
@@ -110,10 +110,14 @@ class SensorGnss : public SensorBase
~SensorGnss() override;
// Gets
- StateBlockPtr getEnuMapTranslation() const;
- StateBlockPtr getEnuMapRoll() const;
- StateBlockPtr getEnuMapPitch() const;
- StateBlockPtr getEnuMapYaw() const;
+ StateBlockConstPtr getEnuMapTranslation() const;
+ StateBlockPtr getEnuMapTranslation();
+ StateBlockConstPtr getEnuMapRoll() const;
+ StateBlockPtr getEnuMapRoll();
+ StateBlockConstPtr getEnuMapPitch() const;
+ StateBlockPtr getEnuMapPitch();
+ StateBlockConstPtr getEnuMapYaw() const;
+ StateBlockPtr getEnuMapYaw();
const Eigen::Matrix3d& getREnuEcef() const;
const Eigen::Vector3d& gettEnuEcef() const;
Eigen::Matrix3d getREnuMap() const;
@@ -178,22 +182,42 @@ inline bool SensorGnss::isEnuModeAuto() const
return params_->ENU_mode == "auto";
}
-inline StateBlockPtr SensorGnss::getEnuMapTranslation() const
+inline StateBlockConstPtr SensorGnss::getEnuMapTranslation() const
{
return getStateBlock('t');
}
-inline StateBlockPtr SensorGnss::getEnuMapRoll() const
+inline StateBlockPtr SensorGnss::getEnuMapTranslation()
+{
+ return getStateBlock('t');
+}
+
+inline StateBlockConstPtr SensorGnss::getEnuMapRoll() const
+{
+ return getStateBlock('r');
+}
+
+inline StateBlockPtr SensorGnss::getEnuMapRoll()
{
return getStateBlock('r');
}
-inline StateBlockPtr SensorGnss::getEnuMapPitch() const
+inline StateBlockConstPtr SensorGnss::getEnuMapPitch() const
{
return getStateBlock('p');
}
-inline StateBlockPtr SensorGnss::getEnuMapYaw() const
+inline StateBlockPtr SensorGnss::getEnuMapPitch()
+{
+ return getStateBlock('p');
+}
+
+inline StateBlockConstPtr SensorGnss::getEnuMapYaw() const
+{
+ return getStateBlock('y');
+}
+
+inline StateBlockPtr SensorGnss::getEnuMapYaw()
{
return getStateBlock('y');
}
diff --git a/src/processor/processor_gnss_single_diff.cpp b/src/processor/processor_gnss_single_diff.cpp
deleted file mode 100644
index 99c73f2fc8b8fb824739dde680a1264dbe8e842b..0000000000000000000000000000000000000000
--- a/src/processor/processor_gnss_single_diff.cpp
+++ /dev/null
@@ -1,179 +0,0 @@
-//--------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/factor/factor_gnss_single_diff_2d.h"
-#include "gnss/feature/feature_gnss_single_diff.h"
-#include "gnss/processor/processor_gnss_single_diff.h"
-#include "gnss/capture/capture_gnss_single_diff.h"
-
-namespace wolf
-{
-
-ProcessorGnssSingleDiff::ProcessorGnssSingleDiff(ParamsProcessorGnssSingleDiffPtr _params_gnss) :
- ProcessorBase("ProcessorGnssSingleDiff", 0, _params_gnss),
- params_gnss_(_params_gnss)
-{
- //
-}
-
-ProcessorGnssSingleDiff::~ProcessorGnssSingleDiff()
-{
- //
-}
-
-void ProcessorGnssSingleDiff::processCapture(CaptureBasePtr _capture)
-{
- // TODO: keep captures in a buffer and deal with KFpacks
-
- //WOLF_DEBUG("ProcessorGnssSingleDiff::process()");
- incoming_capture_ = std::static_pointer_cast<CaptureGnssSingleDiff>(_capture);
-
- // discard capture with null or non-key origin frame
- if (incoming_capture_->getOriginFrame() == nullptr || !incoming_capture_->getOriginFrame()->isKey())
- {
- WOLF_WARN("process single difference with null frame origin, skipping...");
- return;
- }
-
- if (last_KF_ == nullptr)
- last_KF_ = incoming_capture_->getOriginFrame();
-
- // NEW KF? ------------------------------------------------
- FrameBasePtr new_frame = nullptr;
-
- // ALREADY CREATED KF
- PackKeyFramePtr KF_pack = buffer_pack_kf_.selectPack( incoming_capture_, params_->time_tolerance);
- if (KF_pack && KF_pack->key_frame != incoming_capture_->getOriginFrame())
- {
- new_frame = KF_pack->key_frame;
- WOLF_DEBUG( "PR ",getName()," - capture ", incoming_capture_->id(), " appended to existing KF " , KF_pack->key_frame->id() , " TS: ", KF_pack->key_frame->getTimeStamp());
- }
- // MAKE KF
- else if (voteForKeyFrame() && permittedKeyFrame())
- {
- new_frame = getProblem()->emplaceKeyFrame( incoming_capture_->getTimeStamp());
- getProblem()->keyFrameCallback(new_frame, shared_from_this(), params_->time_tolerance);
- WOLF_DEBUG( "PR ",getName()," - capture ", incoming_capture_->id(), " appended to new KF " , new_frame->id() , " TS: ", new_frame->getTimeStamp());
- }
-
- // ESTABLISH FACTOR ------------------------------------------------
- if (new_frame)
- {
- // LINK CAPTURE
- _capture->link(new_frame); // Add incoming Capture to the new Frame
-
- // EMPLACE FEATURE
- auto ftr = FeatureBase::emplace<FeatureGnssSingleDiff>(incoming_capture_, incoming_capture_->getData(),incoming_capture_->getDataCovariance());
-
- // ADD FACTOR
- emplaceFactor(ftr);
-
- // store last KF
- last_KF_ = new_frame;
- }
-
- // INITIALIZE ENU_MAP IF 4 NECESSARY CONDITIONS ------------------------------------------------
- // 1 - ENU-ECEF defined
- // 2 - not initialized
- // 3 - current capture in key-frame with factor
- // 4 - frames constained by the factor separated enough ( > enu_map_init_dist_min)
- if ( sensor_gnss_->isEnuDefined() &&
- !sensor_gnss_->isEnuMapInitialized() &&
- new_frame != nullptr &&
- incoming_capture_->getFrame() != nullptr && incoming_capture_->getFrame()->isKey() &&
- incoming_capture_->getData().norm() > params_gnss_->enu_map_init_dist_min)
- {
- WOLF_DEBUG("initializing enu map");
- sensor_gnss_->initializeEnuMapYaw(incoming_capture_->getOriginFrame()->getState(),
- incoming_capture_->getFrame()->getState(),
- incoming_capture_->getData());
- }
-}
-
-FactorBasePtr ProcessorGnssSingleDiff::emplaceFactor(FeatureBasePtr _ftr)
-{
- //WOLF_DEBUG("creating the factor...");
- // 2d
- if (getProblem()->getDim() == 2)
- return FactorBase::emplace<FactorGnssSingleDiff2d>(_ftr, _ftr, incoming_capture_->getOriginFrame(), sensor_gnss_, shared_from_this(), params_->apply_loss_function);
- // 3d TODO
- else
- std::runtime_error("Single Differences in 3d not implemented yet.");
-
- return nullptr;
-}
-
-bool ProcessorGnssSingleDiff::voteForKeyFrame() const
-{
- if (last_KF_==nullptr)
- return true;
-
- std::cout << "params_gnss_->time_th = " << params_gnss_->time_th << std::endl;
- std::cout << "(last_KF_->getTimeStamp() - incoming_capture_->getTimeStamp()) = " << (last_KF_->getTimeStamp() - incoming_capture_->getTimeStamp()) << std::endl;
-
- // Depending on time since the last KF with gnssfix capture
- if ((incoming_capture_->getTimeStamp() - last_KF_->getTimeStamp()) > params_gnss_->time_th)
- return true;
-
- // Distance criterion
- std::cout << "params_gnss_->dist_traveled = " << params_gnss_->dist_traveled << std::endl;
- Eigen::Vector2d v_current_origin = (sensor_gnss_->getREnuMap().transpose() * sensor_gnss_->getREnuEcef() * incoming_capture_->getData()).head<2>();
- std::cout << "v_current_origin: " << v_current_origin.transpose() << std::endl;
- Eigen::Vector2d v_origin_last_KF = last_KF_->getP()->getState() - incoming_capture_->getOriginFrame()->getP()->getState();
- std::cout << "v_origin_last_KF: " << v_origin_last_KF.transpose() << std::endl;
- std::cout << "v_current_origin + v_origin_last_KF: " << (v_current_origin + v_origin_last_KF).transpose() << std::endl;
- if ((v_current_origin + v_origin_last_KF).norm() > params_gnss_->dist_traveled)
- return true;
-
- // TODO: more alternatives?
-
- // otherwise
- return false;
-}
-
-bool ProcessorGnssSingleDiff::storeKeyFrame(FrameBasePtr _frame_ptr)
-{
- return true;
-}
-bool ProcessorGnssSingleDiff::storeCapture(CaptureBasePtr _cap_ptr)
-{
- return false;
-}
-void ProcessorGnssSingleDiff::configure(SensorBasePtr _sensor)
-{
- sensor_gnss_ = std::static_pointer_cast<SensorGnss>(_sensor);
-};
-
-ProcessorBasePtr ProcessorGnssSingleDiff::create(const std::string& _unique_name, const ParamsProcessorBasePtr _params)
-{
- ProcessorGnssSingleDiffPtr prc = std::make_shared<ProcessorGnssSingleDiff>(std::static_pointer_cast<ParamsProcessorGnssSingleDiff>(_params));
- prc->setName(_unique_name);
- return prc;
-}
-
-} // namespace wolf
-
-
-// Register in the FactorySensor
-#include "core/processor/factory_processor.h"
-namespace wolf {
-WOLF_REGISTER_PROCESSOR(ProcessorGnssSingleDiff)
-} // namespace wolf
diff --git a/src/processor/processor_gnss_tdcp.cpp b/src/processor/processor_gnss_tdcp.cpp
index c087f70747614b859de670898ce9cd9dd655ca6f..1ed2b6855410743a787dc6f6edadd9ae0b6ab6ab 100644
--- a/src/processor/processor_gnss_tdcp.cpp
+++ b/src/processor/processor_gnss_tdcp.cpp
@@ -144,11 +144,15 @@ void ProcessorGnssTdcp::processKeyFrame(FrameBasePtr _keyframe)
}
// Iterate over all KF of the trajectory with GNSS captures
- for (auto KF_it = getProblem()->getTrajectory()->rbegin();
- KF_it != getProblem()->getTrajectory()->rend();
- KF_it++)
+ auto frame_map = getProblem()->getTrajectory()->getFrameMap();
+ for (auto frame_rev_iter = frame_map.rbegin();
+ frame_rev_iter != frame_map.rend();
+ ++frame_rev_iter)
{
- auto KF_ref = *KF_it;
+ auto KF_ref = frame_rev_iter->second;
+ if (not KF_ref)
+ break;
+
if (_keyframe->getTimeStamp() < KF_ref->getTimeStamp())
continue;
diff --git a/src/processor/processor_tracker_gnss.cpp b/src/processor/processor_tracker_gnss.cpp
index ace52948ce6e00adc5961081c00ee02fe8760781..802a7ca5f1c385da8335b9512121a0ebce5f154b 100644
--- a/src/processor/processor_tracker_gnss.cpp
+++ b/src/processor/processor_tracker_gnss.cpp
@@ -410,21 +410,21 @@ void ProcessorTrackerGnss::establishFactors()
WOLF_DEBUG("TDCP BATCH frame ", last_ptr_->getFrame()->id());
FactorBasePtr last_fac_ptr = nullptr;
- for (auto KF_rit = getProblem()->getTrajectory()->rbegin();
- KF_rit != getProblem()->getTrajectory()->rend();
- KF_rit++)
+ auto frame_map = getProblem()->getTrajectory()->getFrameMap();
+ for (auto frame_rev_iter = frame_map.rbegin();
+ frame_rev_iter != frame_map.rend();
+ ++frame_rev_iter)
{
- FrameBasePtr KF = (*KF_rit);
-
- WOLF_DEBUG("TDCP BATCH ref frame ", KF->id());
+ auto ref_KF = frame_rev_iter->second;
+ WOLF_DEBUG("TDCP BATCH ref frame ", ref_KF->id());
// discard non-key frames, last-last pair and frames without CaptureGnss
- if (KF == last_ptr_->getFrame() or
- KF->getCaptureOf(getSensor(),"CaptureGnss") == nullptr)
+ if (ref_KF == last_ptr_->getFrame() or
+ ref_KF->getCaptureOf(getSensor(),"CaptureGnss") == nullptr)
continue;
// static cast
- auto ref_cap_gnss = std::static_pointer_cast<CaptureGnss>((*KF_rit)->getCaptureOf(getSensor(),"CaptureGnss"));
+ auto ref_cap_gnss = std::static_pointer_cast<CaptureGnss>(ref_KF->getCaptureOf(getSensor(),"CaptureGnss"));
auto last_cap_gnss = std::static_pointer_cast<CaptureGnss>(last_ptr_);
// dt
@@ -469,7 +469,7 @@ void ProcessorTrackerGnss::establishFactors()
//std::cout << std::endl;
// reference ECEF position
- Eigen::Vector3d x_r = sensor_gnss_->computeFrameAntennaPosEcef(KF);
+ Eigen::Vector3d x_r = sensor_gnss_->computeFrameAntennaPosEcef(ref_KF);
// compute TDCP batch
auto tdcp_output = GnssUtils::Tdcp(ref_cap_gnss->getSnapshot(),
@@ -527,7 +527,8 @@ void ProcessorTrackerGnss::establishFactors()
{
// current feature
auto ftr_k = std::dynamic_pointer_cast<FeatureGnssSatellite>(ftr_k_pair.second);
- assert(ftr_k != nullptr);
+ if(not ftr_k)
+ continue;
// check valid measurement
assert(std::abs(ftr_k->getObservation().L[0]) > 1e-12);
@@ -543,7 +544,8 @@ void ProcessorTrackerGnss::establishFactors()
{
// cast reference feature
auto ftr_r = std::dynamic_pointer_cast<FeatureGnssSatellite>(ts_ftr_r_it->second);
- assert(ftr_r != nullptr);
+ if(not ftr_r)
+ continue;
// dt
double dt = ftr_k->getCapture()->getTimeStamp() - ts_ftr_r_it->first;