diff --git a/CMakeLists.txt b/CMakeLists.txt
index e05b69ceb3c2840724a80df0f68f4b0bb516a9a3..8cce494e8ff4dd259249c90a91600a95a6f7411d 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -177,7 +177,8 @@ SET(HDRS_SENSOR
)
SET(HDRS_SOLVER
)
-SET(HDRS_DTASSC
+SET(HDRS_TREE_MANAGER
+ include/gnss/tree_manager/tree_manager_sliding_window_tdcp.h
)
#SOURCES
@@ -207,10 +208,11 @@ SET(SRCS_PROCESSOR
SET(SRCS_SENSOR
src/sensor/sensor_gnss.cpp
)
-SET(SRCS_DTASSC
- )
SET(SRCS_SOLVER
)
+SET(SRCS_TREE_MANAGER
+ src/tree_manager/tree_manager_sliding_window_tdcp.cpp
+ )
SET(SRCS_YAML
)
#OPTIONALS
@@ -250,6 +252,7 @@ ADD_LIBRARY(${PLUGIN_NAME}
${SRCS_SENSOR}
${SRCS_SOLVER}
${SRCS_STATE_BLOCK}
+ ${SRCS_TREE_MANAGER}
${SRCS_UTILS}
${SRCS_WRAPPER}
${SRCS_YAML}
@@ -292,8 +295,6 @@ install(EXPORT ${PLUGIN_NAME}Targets DESTINATION lib/cmake/${PLUGIN_NAME})
#install headers
INSTALL(FILES ${HDRS_STATE_BLOCK}
DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/state_block)
-INSTALL(FILES ${HDRS_DTASSC}
- DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/association)
INSTALL(FILES ${HDRS_CAPTURE}
DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/capture)
INSTALL(FILES ${HDRS_FACTOR}
@@ -312,8 +313,8 @@ INSTALL(FILES ${HDRS_WRAPPER}
# DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/solver_suitesparse)
INSTALL(FILES ${HDRS_SOLVER}
DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/solver)
-INSTALL(FILES ${HDRS_SERIALIZATION}
- DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/serialization)
+INSTALL(FILES ${HDRS_TREE_MANAGER}
+ DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/tree_manager)
INSTALL(FILES ${HDRS_YAML}
DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/yaml)
diff --git a/include/gnss/factor/factor_gnss_tdcp_3d.h b/include/gnss/factor/factor_gnss_tdcp_3d.h
index df9bfe44df8201a3d429eb0032d0b9ec4212000b..fd4f1a0554363c627440f4a99482efc3558ac700 100644
--- a/include/gnss/factor/factor_gnss_tdcp_3d.h
+++ b/include/gnss/factor/factor_gnss_tdcp_3d.h
@@ -19,7 +19,12 @@ class FactorGnssTdcp3d : public FactorAutodiff<FactorGnssTdcp3d, 3, 3, 4, 3, 4,
public:
- FactorGnssTdcp3d(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) :
+ FactorGnssTdcp3d(const 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<FactorGnssTdcp3d, 3, 3, 4, 3, 4, 3, 1, 1, 1>("FactorGnssTdcp3d",
TOP_GEOM,
_ftr_ptr,
@@ -40,6 +45,7 @@ class FactorGnssTdcp3d : public FactorAutodiff<FactorGnssTdcp3d, 3, 3, 4, 3, 4,
_sensor_gnss_ptr->getEnuMapYaw()),
sensor_gnss_ptr_(_sensor_gnss_ptr)
{
+ assert(_ftr_ptr->getMeasurement().size() == 3 && "FactorGnssTdcp3d uses 3d measurements. For FeatureGnssTdcp with also delta clock, use FactorGnssTdcpBatch instead");
WOLF_WARN_COND(!sensor_gnss_ptr_->isEnuDefined(), "Creating a GNSS SingleDiff 3D factor without initializing ENU");
}
diff --git a/include/gnss/factor/factor_gnss_tdcp_batch.h b/include/gnss/factor/factor_gnss_tdcp_batch.h
index 94a08eca6419220d547905c9ee9a5119c4139ac2..3a5786067f56baaf45ac910075351733d672ffd0 100644
--- a/include/gnss/factor/factor_gnss_tdcp_batch.h
+++ b/include/gnss/factor/factor_gnss_tdcp_batch.h
@@ -46,6 +46,7 @@ class FactorGnssTdcpBatch : public FactorAutodiff<FactorGnssTdcpBatch, 4, 3, 4,
_sensor_gnss_ptr->getEnuMapYaw()),
sensor_gnss_ptr_(_sensor_gnss_ptr)
{
+ assert(_ftr_ptr->getMeasurement().size() == 4 && "FactorGnssTdcpBatch uses 4d measurements (pos.displacement, delta clock). For FeatureGnssTdcp with only displacement, use FactorGnssTdcp3d instead");
WOLF_WARN_COND(!sensor_gnss_ptr_->isEnuDefined(), "Creating a GNSS SingleDiff 3D factor without initializing ENU");
}
@@ -84,22 +85,23 @@ inline bool FactorGnssTdcpBatch::operator ()(const T* const _x1,
Eigen::Map<const Eigen::Matrix<T,3,1>> t_MAP_BASE2(_x2);
Eigen::Map<const Eigen::Quaternion<T>> q_MAP_BASE2(_o2);
Eigen::Map<const Eigen::Matrix<T,3,1>> t_BASE_ANTENA(_x_antena);
+ Eigen::Map<Eigen::Matrix<T,3,1> > disp_residuals(_residuals);
Eigen::Map<Eigen::Matrix<T,4,1> > residuals(_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]);
- // 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));
+ // Expected d
+ Eigen::Matrix<T,4,1> exp;
- // position error
- Eigen::Matrix<T,3,1> error_ECEF = expected_ECEF - getMeasurement().head<3>().cast<T>();
+ // Expected displacement in ECEF
+ exp.head(3) = R_ECEF_ENU * R_ENU_MAP * ((q_MAP_BASE2 * t_BASE_ANTENA + t_MAP_BASE2) - (q_MAP_BASE1 * t_BASE_ANTENA + t_MAP_BASE1));
// clock error
- T error_clock = _t2 - _t1;
+ exp(3) = *_t2 - *_t1;
// Compute residual
- residuals = getMeasurementSquareRootInformationUpper().cast<T>() * (expected_ECEF - getMeasurement().cast<T>());
+ residuals = getMeasurementSquareRootInformationUpper().cast<T>() * (exp - getMeasurement().cast<T>());
//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/processor/processor_tracker_gnss.h b/include/gnss/processor/processor_tracker_gnss.h
index c821bffac1a775b2c5d0957cd2d14b8306028e42..9984f8014711a518a86a2d922d23ab92d1666470 100644
--- a/include/gnss/processor/processor_tracker_gnss.h
+++ b/include/gnss/processor/processor_tracker_gnss.h
@@ -26,7 +26,7 @@ struct ParamsProcessorTrackerGnss : public ParamsProcessorTrackerFeature
double enu_map_fix_dist;
int min_sbas_sats;
bool tdcp_enabled;
- bool tdcp_all_against_all;
+ std::string tdcp_structure;
ParamsProcessorTrackerGnss() = default;
ParamsProcessorTrackerGnss(std::string _unique_name, const ParamsServer& _server):
@@ -41,7 +41,6 @@ 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");
- tdcp_all_against_all = _server.getParam<bool> (prefix + _unique_name + "/gnss/tdcp/all_against_all");
// 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)
@@ -65,6 +64,7 @@ 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");
@@ -84,6 +84,9 @@ struct ParamsProcessorTrackerGnss : public ParamsProcessorTrackerFeature
tdcp_params.max_iterations = _server.getParam<int> (prefix + _unique_name + "/gnss/tdcp/max_iterations");
tdcp_params.residual_opt = _server.getParam<int> (prefix + _unique_name + "/gnss/tdcp/residual_opt");
}
+
+ if (tdcp_structure != "all-all" and tdcp_structure != "consecutive" and tdcp_structure != "first-all")
+ throw std::runtime_error("unknown value for '/gnss/tdcp/structure', should be 'all-all', 'consecutive' or 'first-all'");
}
// COMPUTE FIX OPTIONS (RAIM)
@@ -129,7 +132,7 @@ struct ParamsProcessorTrackerGnss : public ParamsProcessorTrackerFeature
+ "gnss/constellations/IRN: " + std::to_string(gnss_opt.IRN) + "\n"
+ "gnss/constellations/LEO: " + std::to_string(gnss_opt.LEO) + "\n"
+ "gnss/tdcp/enabled: " + std::to_string(tdcp_enabled) + "\n"
- + "gnss/tdcp/all_agains_all: " + std::to_string(tdcp_all_against_all) + "\n"
+ + "gnss/tdcp/structure: " + tdcp_structure + "\n"
+ "gnss/tdcp/batch: " + std::to_string(tdcp_params.batch) + "\n"
+ "gnss/tdcp/corr_iono: " + std::to_string(gnss_opt.carrier_opt.corr_iono)+ "\n"
+ "gnss/tdcp/corr_tropo: " + std::to_string(gnss_opt.carrier_opt.corr_tropo)+ "\n"
diff --git a/include/gnss/tree_manager/tree_manager_sliding_window_tdcp.h b/include/gnss/tree_manager/tree_manager_sliding_window_tdcp.h
new file mode 100644
index 0000000000000000000000000000000000000000..9c46154057f16aa5e93d232cb15e01012c6cbe06
--- /dev/null
+++ b/include/gnss/tree_manager/tree_manager_sliding_window_tdcp.h
@@ -0,0 +1,43 @@
+#ifndef INCLUDE_TREE_MANAGER_SLIDING_WINDOW_TDCP_H_
+#define INCLUDE_TREE_MANAGER_SLIDING_WINDOW_TDCP_H_
+
+#include "core/tree_manager/tree_manager_sliding_window.h"
+
+namespace wolf
+{
+
+WOLF_STRUCT_PTR_TYPEDEFS(ParamsTreeManagerSlidingWindowTdcp)
+WOLF_PTR_TYPEDEFS(TreeManagerSlidingWindowTdcp)
+
+struct ParamsTreeManagerSlidingWindowTdcp : public ParamsTreeManagerSlidingWindow
+{
+ ParamsTreeManagerSlidingWindowTdcp() = default;
+ ParamsTreeManagerSlidingWindowTdcp(std::string _unique_name, const wolf::ParamsServer & _server) :
+ ParamsTreeManagerSlidingWindow(_unique_name, _server)
+ {
+ }
+ std::string print() const override
+ {
+ return "\n" + ParamsTreeManagerSlidingWindow::print();
+ }
+};
+
+class TreeManagerSlidingWindowTdcp : public TreeManagerSlidingWindow
+{
+ public:
+ TreeManagerSlidingWindowTdcp(ParamsTreeManagerSlidingWindowTdcpPtr _params);
+ WOLF_TREE_MANAGER_CREATE(TreeManagerSlidingWindowTdcp, ParamsTreeManagerSlidingWindowTdcp)
+
+ ~TreeManagerSlidingWindowTdcp() override{}
+
+ void keyFrameCallback(FrameBasePtr _key_frame) override;
+
+ protected:
+ ParamsTreeManagerSlidingWindowTdcpPtr params_sw_sb_;
+ SensorBasePtr sensor_imu_;
+ Eigen::Matrix6d cov_bias_;
+};
+
+} /* namespace wolf */
+
+#endif /* INCLUDE_TREE_MANAGER_SLIDING_WINDOW_H_ */
diff --git a/src/processor/processor_gnss_tdcp.cpp b/src/processor/processor_gnss_tdcp.cpp
index 3f2c15cfa393a235a4de3520638a0d82de28d80e..80cc5c99d3a9b3c11b4cecb95eb5814782f4cd38 100644
--- a/src/processor/processor_gnss_tdcp.cpp
+++ b/src/processor/processor_gnss_tdcp.cpp
@@ -190,10 +190,10 @@ FactorBasePtr ProcessorGnssTdcp::emplaceFactor(FeatureBasePtr _ftr, FrameBasePtr
//WOLF_DEBUG("creating the factor...");
// 2D
if (getProblem()->getDim() == 2)
- return FactorBase::emplace<FactorGnssTdcp2d>(_ftr, _ftr, _frm_ref, sensor_gnss_, shared_from_this());
+ return FactorBase::emplace<FactorGnssTdcp2d>(_ftr, _ftr, _frm_ref, sensor_gnss_, shared_from_this(), params_->apply_loss_function);
// 3D
else
- return FactorBase::emplace<FactorGnssTdcp3d>(_ftr, _ftr, _frm_ref, sensor_gnss_, shared_from_this());
+ return FactorBase::emplace<FactorGnssTdcp3d>(_ftr, _ftr, _frm_ref, sensor_gnss_, shared_from_this(), params_->apply_loss_function);
return nullptr;
}
diff --git a/src/processor/processor_tracker_gnss.cpp b/src/processor/processor_tracker_gnss.cpp
index ca533fc159fe61c19abe897f6d2e1237acd69d0b..e3dd9e6ef9d4869472e22dbd9b25063d139baf5c 100644
--- a/src/processor/processor_tracker_gnss.cpp
+++ b/src/processor/processor_tracker_gnss.cpp
@@ -4,7 +4,7 @@
#include "gnss/feature/feature_gnss_tdcp.h"
#include "gnss/feature/feature_gnss_fix.h"
#include "gnss/factor/factor_gnss_tdcp.h"
-#include "gnss/factor/factor_gnss_tdcp_3d.h"
+#include "gnss/factor/factor_gnss_tdcp_batch.h"
#include "gnss/factor/factor_gnss_pseudo_range.h"
#include "gnss/factor/factor_gnss_fix_3d.h"
#include "gnss_utils/utils/rcv_position.h"
@@ -387,6 +387,7 @@ void ProcessorTrackerGnss::establishFactors()
if (params_tracker_gnss_->tdcp_params.batch)
{
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();
@@ -439,9 +440,9 @@ void ProcessorTrackerGnss::establishFactors()
//std::cout << std::endl;
// DEBUG: FIND COMMON SATELLITES OBSERVATIONS
- std::set<int> common_sats_debug = GnssUtils::Observations::findCommonObservations(*ref_cap_gnss->getSnapshot()->getObservations(),
- *last_cap_gnss->getSnapshot()->getObservations());
- WOLF_DEBUG("TDCP BATCH common_sats_debug: ", common_sats_debug.size());
+ //std::set<int> common_sats_debug = GnssUtils::Observations::findCommonObservations(*ref_cap_gnss->getSnapshot()->getObservations(),
+ // *last_cap_gnss->getSnapshot()->getObservations());
+ //WOLF_DEBUG("TDCP BATCH common_sats_debug: ", common_sats_debug.size());
//for (auto sat : common_sats_debug)
// std::cout << sat << " ";
//std::cout << std::endl;
@@ -462,25 +463,39 @@ void ProcessorTrackerGnss::establishFactors()
WOLF_DEBUG("TDCP BATCH cov =\n", tdcp_output.cov_d);
// EMPLACE FEATURE
+ auto factor_status = FAC_ACTIVE;
+ if (params_tracker_gnss_->tdcp_structure == "first-all")
+ factor_status = FAC_INACTIVE;
+
auto ftr = FeatureBase::emplace<FeatureGnssTdcp>(last_cap_gnss,
- Eigen::Vector3d(tdcp_output.d.head<3>()),
- Eigen::Matrix3d(tdcp_output.cov_d.topLeftCorner<3,3>()));
+ tdcp_output.d,
+ tdcp_output.cov_d);
// EMPLACE FACTOR
- FactorBase::emplace<FactorGnssTdcp3d>(ftr,
- ftr,
- KF,
- sensor_gnss_,
- shared_from_this());
+ last_fac_ptr = FactorBase::emplace<FactorGnssTdcpBatch>(ftr,
+ ftr,
+ ref_cap_gnss,
+ sensor_gnss_,
+ shared_from_this(),
+ params_->apply_loss_function,
+ factor_status);
+
+ WOLF_DEBUG("TDCP BATCH factor ", last_fac_ptr->id() , " emplaced as ", (last_fac_ptr->getStatus() == FAC_ACTIVE ? "ACTIVE" : "INACTIVE"));
+
+ // only first (with previous frame) factor in "consecutive" structure
+ if (params_tracker_gnss_->tdcp_structure == "consecutive")
+ break;
}
else
{
WOLF_DEBUG("TDCP BATCH failed with msg: ", tdcp_output.msg);
}
-
- // just one factor
- if (not params_tracker_gnss_->tdcp_all_against_all)
- break;
+ }
+ // only last (with first frame) factor in "first-all" structure
+ if (params_tracker_gnss_->tdcp_structure == "first-all" and last_fac_ptr)
+ {
+ last_fac_ptr->setStatus(FAC_ACTIVE);
+ WOLF_DEBUG("TDCP BATCH factor ", last_fac_ptr->id() , " changed to ", (last_fac_ptr->getStatus() == FAC_ACTIVE ? "ACTIVE" : "INACTIVE"));
}
}
// FACTOR per SATELLITE (FactorGnssTdcp)
@@ -530,6 +545,9 @@ void ProcessorTrackerGnss::establishFactors()
WOLF_DEBUG("previous feature at KF: ", ftr_r->getCapture()->getFrame()->id(), " sat: ", ftr_r->satNumber(), " id: ", ftr_r->id());
// emplace tdcp factor
+ auto factor_status = FAC_ACTIVE;
+ if (params_tracker_gnss_->tdcp_structure == "first-all")
+ factor_status = FAC_INACTIVE;
double var_tdcp = dt * std::pow(params_tracker_gnss_->tdcp_params.sigma_atm,2) + std::pow(params_tracker_gnss_->tdcp_params.sigma_carrier,2);
auto new_fac = FactorBase::emplace<FactorGnssTdcp>(ftr_k,
sqrt(var_tdcp),
@@ -537,17 +555,22 @@ void ProcessorTrackerGnss::establishFactors()
ftr_k,
sensor_gnss_,
shared_from_this(),
- params_tracker_gnss_->tdcp_params.loss_function);
+ params_tracker_gnss_->tdcp_params.loss_function,
+ factor_status);
new_factors.push_back(new_fac);
// WOLF_DEBUG( "Factor: track: " , feature_in_last->trackId(),
// " origin: " , feature_in_origin->id() ,
// " from last: " , feature_in_last->id() );
- // just one factor
- if (not params_tracker_gnss_->tdcp_all_against_all)
+ // only first (with previous frame) factor in "consecutive" structure
+ if (params_tracker_gnss_->tdcp_structure == "consecutive")
break;
}
+ // only last (with first frame) factor in "first-all" structure
+ if (params_tracker_gnss_->tdcp_structure == "first-all")
+ new_factors.back()->setStatus(FAC_ACTIVE);
+
WOLF_DEBUG("All TDCP factors emplaced!");
}
}
diff --git a/src/tree_manager/tree_manager_sliding_window_tdcp.cpp b/src/tree_manager/tree_manager_sliding_window_tdcp.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..0a68df7096fa17ca70adb3734046b588d2381967
--- /dev/null
+++ b/src/tree_manager/tree_manager_sliding_window_tdcp.cpp
@@ -0,0 +1,45 @@
+#include "gnss/tree_manager/tree_manager_sliding_window_tdcp.h"
+
+namespace wolf
+{
+
+TreeManagerSlidingWindowTdcp::TreeManagerSlidingWindowTdcp(ParamsTreeManagerSlidingWindowTdcpPtr _params) :
+ TreeManagerSlidingWindow(_params),
+ params_sw_sb_(_params)
+{
+ NodeBase::setType("TreeManagerSlidingWindowTdcp");
+};
+
+void TreeManagerSlidingWindowTdcp::keyFrameCallback(FrameBasePtr _key_frame)
+{
+ assert(getProblem() && "TreeManagerSlidingWindowTdcp::keyFrameCallback: problem not set.");
+
+ // store first frame
+ auto first_frame = getProblem()->getTrajectory()->getFirstFrame();
+
+ // call base sliding window tree manager
+ TreeManagerSlidingWindow::keyFrameCallback(_key_frame);
+
+ // if first frame was removed, activate all factors of new first frame
+ if (first_frame != getProblem()->getTrajectory()->getFirstFrame())
+ {
+ assert(first_frame->isRemoving());
+ for (auto fac : getProblem()->getTrajectory()->getFirstFrame()->getConstrainedByList())
+ if (fac and not fac->isRemoving() and
+ (fac->getType() == "FactorGnssTdcp" or
+ fac->getType() == "FactorGnssTdcp2d" or
+ fac->getType() == "FactorGnssTdcp3d" or
+ fac->getType() == "FactorGnssTdcpBatch"))
+ fac->setStatus(FAC_ACTIVE);
+ }
+}
+
+} /* namespace wolf */
+
+// Register in the FactoryTreeManager
+#include "core/tree_manager/factory_tree_manager.h"
+namespace wolf {
+WOLF_REGISTER_TREE_MANAGER(TreeManagerSlidingWindowTdcp);
+WOLF_REGISTER_TREE_MANAGER_AUTO(TreeManagerSlidingWindowTdcp);
+} // namespace wolf
+