new tree manager for TDCP sliding window

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)
 

include/gnss/tree_manager/tree_manager_sliding_window_tdcp.h

+#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_ */

src/tree_manager/tree_manager_sliding_window_tdcp.cpp

+#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
+