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