diff --git a/.gitlab-ci.yml b/.gitlab-ci.yml index 4565017604fc766e0e797371c16e38e508fbe49f..6f5980d7ced970ffae4aae48459b800545a4158b 100644 --- a/.gitlab-ci.yml +++ b/.gitlab-ci.yml @@ -6,6 +6,25 @@ stages: - deploy_ros ############ YAML ANCHORS ############ +.print_variables_template: &print_variables_definition + # Print variables + - echo $CI_COMMIT_BRANCH + - echo $WOLF_IMU_BRANCH + - echo $WOLF_GNSS_BRANCH + - echo $WOLF_LASER_BRANCH + - echo $WOLF_VISION_BRANCH + - echo $WOLF_APRILTAG_BRANCH + - echo $WOLF_BODYDYNAMICS_BRANCH + - echo $GNSSUTILS_BRANCH + - echo $LASERSCANUTILS_BRANCH + - echo $DEPLOY_CI_ROS + - echo $WOLF_ROS_IMU_BRANCH + - echo $WOLF_ROS_GNSS_BRANCH + - echo $WOLF_ROS_LASER_BRANCH + - echo $WOLF_ROS_VISION_BRANCH + - echo $WOLF_ROS_APRILTAG_BRANCH + - echo $WOLF_ROS_BODYDYNAMICS_BRANCH + .preliminaries_template: &preliminaries_definition ## Install ssh-agent if not already installed, it is required by Docker. ## (change apt-get to yum if you use an RPM-based image) @@ -80,6 +99,7 @@ license_header: stage: license image: labrobotica/wolf_deps:20.04 before_script: + - *print_variables_definition - *preliminaries_definition script: - *license_header_definition @@ -89,6 +109,7 @@ build_and_test:bionic: stage: build_and_test image: labrobotica/wolf_deps:18.04 script: + - *print_variables_definition - *build_and_test_definition ############ UBUNTU 20.04 TESTS ############ @@ -96,6 +117,7 @@ build_and_test:focal: stage: build_and_test image: labrobotica/wolf_deps:20.04 script: + - *print_variables_definition - *build_and_test_definition ############ DEPLOY PLUGINS ANY BRANCY EXCEPT FOR main ############ @@ -117,6 +139,7 @@ deploy_gnss: - if: $CI_COMMIT_BRANCH != "main" variables: WOLF_CORE_BRANCH: $CI_COMMIT_BRANCH + GNSSUTILS_BRANCH: $GNSSUTILS_BRANCH DEPLOY_CI_ROS: "false" GNSSUTILS_BRANCH: $GNSSUTILS_BRANCH trigger: @@ -142,6 +165,7 @@ deploy_laser: - if: $CI_COMMIT_BRANCH != "main" variables: WOLF_CORE_BRANCH: $CI_COMMIT_BRANCH + LASERSCANUTILS_BRANCH: $LASERSCANUTILS_BRANCH DEPLOY_CI_ROS: "false" trigger: project: mobile_robotics/wolf_projects/wolf_lib/plugins/laser @@ -154,6 +178,7 @@ deploy_apriltag: - if: $CI_COMMIT_BRANCH != "main" variables: WOLF_CORE_BRANCH: $CI_COMMIT_BRANCH + WOLF_VISION_BRANCH: $WOLF_VISION_BRANCH DEPLOY_CI_ROS: "false" trigger: project: mobile_robotics/wolf_projects/wolf_lib/plugins/apriltag @@ -166,6 +191,7 @@ deploy_bodydynamics: - if: $CI_COMMIT_BRANCH != "main" variables: WOLF_CORE_BRANCH: $CI_COMMIT_BRANCH + WOLF_IMU_BRANCH: $WOLF_IMU_BRANCH DEPLOY_CI_ROS: "false" trigger: project: mobile_robotics/wolf_projects/wolf_lib/plugins/bodydynamics @@ -178,7 +204,7 @@ deploy_imu_main: rules: - if: $CI_COMMIT_BRANCH == "main" variables: - WOLF_CORE_BRANCH: $CI_COMMIT_BRANCH + WOLF_CORE_BRANCH: main DEPLOY_CI_ROS: "false" trigger: project: mobile_robotics/wolf_projects/wolf_lib/plugins/imu @@ -190,7 +216,8 @@ deploy_gnss_main: rules: - if: $CI_COMMIT_BRANCH == "main" variables: - WOLF_CORE_BRANCH: $CI_COMMIT_BRANCH + WOLF_CORE_BRANCH: main + GNSSUTILS_BRANCH: $GNSSUTILS_BRANCH DEPLOY_CI_ROS: "false" trigger: project: mobile_robotics/wolf_projects/wolf_lib/plugins/gnss @@ -202,7 +229,7 @@ deploy_vision_main: rules: - if: $CI_COMMIT_BRANCH == "main" variables: - WOLF_CORE_BRANCH: $CI_COMMIT_BRANCH + WOLF_CORE_BRANCH: main DEPLOY_CI_ROS: "false" trigger: project: mobile_robotics/wolf_projects/wolf_lib/plugins/vision @@ -214,7 +241,8 @@ deploy_laser_main: rules: - if: $CI_COMMIT_BRANCH == "main" variables: - WOLF_CORE_BRANCH: $CI_COMMIT_BRANCH + WOLF_CORE_BRANCH: main + LASERSCANUTILS_BRANCH: $LASERSCANUTILS_BRANCH DEPLOY_CI_ROS: "false" trigger: project: mobile_robotics/wolf_projects/wolf_lib/plugins/laser @@ -226,7 +254,8 @@ deploy_apriltag_main: rules: - if: $CI_COMMIT_BRANCH == "main" variables: - WOLF_CORE_BRANCH: $CI_COMMIT_BRANCH + WOLF_CORE_BRANCH: main + WOLF_VISION_BRANCH: main DEPLOY_CI_ROS: "false" trigger: project: mobile_robotics/wolf_projects/wolf_lib/plugins/apriltag @@ -238,7 +267,8 @@ deploy_bodydynamics_main: rules: - if: $CI_COMMIT_BRANCH == "main" variables: - WOLF_CORE_BRANCH: $CI_COMMIT_BRANCH + WOLF_CORE_BRANCH: main + WOLF_IMU_BRANCH: main DEPLOY_CI_ROS: "false" trigger: project: mobile_robotics/wolf_projects/wolf_lib/plugins/bodydynamics diff --git a/demos/hello_wolf/factor_bearing.h b/demos/hello_wolf/factor_bearing.h index d06ace3a796002e87d33a2e9546ad280bea49416..f790a24839bb6c63f1a09703fe283a6f48c11949 100644 --- a/demos/hello_wolf/factor_bearing.h +++ b/demos/hello_wolf/factor_bearing.h @@ -36,18 +36,28 @@ namespace wolf using namespace Eigen; -class FactorBearing : public FactorAutodiff<FactorBearing, 1, 2, 1, 2> +class FactorBearing : public FactorAutodiff<FactorBearing, 1, 2, 1, 2, 1, 2> { public: - FactorBearing(const LandmarkBasePtr& _landmark_other_ptr, + FactorBearing(const FeatureBasePtr& _feature_ptr, + const LandmarkBasePtr& _landmark_other_ptr, const ProcessorBasePtr& _processor_ptr, bool _apply_loss_function, FactorStatus _status) : - FactorAutodiff<FactorBearing, 1, 2, 1, 2>("BEARING", TOP_LMK, nullptr, nullptr, nullptr, - _landmark_other_ptr, _processor_ptr, - _apply_loss_function, _status, - getCapture()->getFrame()->getP(), - getCapture()->getFrame()->getO(), - _landmark_other_ptr->getP()) + FactorAutodiff<FactorBearing, 1, 2, 1, 2, 1, 2>("BEARING", + TOP_LMK, + _feature_ptr, + nullptr, + nullptr, + nullptr, + _landmark_other_ptr, + _processor_ptr, + _apply_loss_function, + _status, + _feature_ptr->getCapture()->getFrame()->getP(), + _feature_ptr->getCapture()->getFrame()->getO(), + _feature_ptr->getCapture()->getSensor()->getP(), + _feature_ptr->getCapture()->getSensor()->getO(), + _landmark_other_ptr->getP()) { // } @@ -58,9 +68,11 @@ class FactorBearing : public FactorAutodiff<FactorBearing, 1, 2, 1, 2> } template<typename T> - bool operator ()(const T* const _p1, - const T* const _o1, - const T* const _p2, + bool operator ()(const T* const _p_w_r, + const T* const _o_w_r, + const T* const _p_r_s, // sensor position + const T* const _o_r_s, // sensor orientation + const T* const _lmk, T* _res) const; }; @@ -73,24 +85,30 @@ namespace wolf { template<typename T> -inline bool FactorBearing::operator ()(const T* const _p1, const T* const _o1, - const T* const _p2, T* _res) const +inline bool FactorBearing::operator ()( const T* const _p_w_r, + const T* const _o_w_r, + const T* const _p_r_s, // sensor position + const T* const _o_r_s, // sensor orientation + const T* const _lmk, + T* _res) const { // 1. produce a transformation matrix to transform from robot frame to world frame - Transform<T, 2, Isometry> H_w_r = Translation<T,2>(_p1[0], _p1[1]) * Rotation2D<T>(*_o1) ; // Robot frame = robot-to-world transform + Transform<T, 2, Isometry> H_w_r = Translation<T,2>(_p_w_r[0], _p_w_r[1]) * Rotation2D<T>(*_o_w_r) ; // Robot frame = robot-to-world transform + Transform<T, 2, Isometry> H_r_s = Translation<T,2>(_p_r_s[0], _p_r_s[1]) * Rotation2D<T>(*_o_r_s) ; // Robot frame = robot-to-world transform + // Map input pointers into meaningful Eigen elements - Map<const Matrix<T, 2, 1>> point_w(_p2); + Map<const Matrix<T, 2, 1>> point_w(_lmk); Map<const Matrix<T, 1, 1>> res(_res); - // 2. Transform world point to robot-referenced point - Matrix<T, 2, 1> point_r = H_w_r.inverse() * point_w; + // 2. Transform world point to sensor-referenced point + Matrix<T, 2, 1> point_s = (H_w_r * H_r_s).inverse() * point_w; // 3. Get the expected bearing of the point - T bearing = atan2(point_r(1), point_r(0)); + T bearing = atan2(point_s(1), point_s(0)); // 4. Get the measured range-and-bearing to the point - Matrix<T, 2, 1> range_bearing = getMeasurement(); + Matrix<T, 2, 1> range_bearing = getMeasurement().cast<T>(); // 5. Get the bearing error by comparing against the bearing measurement T er = range_bearing(1) - bearing; @@ -100,7 +118,7 @@ inline bool FactorBearing::operator ()(const T* const _p1, const T* const _o1, er -= T(2*M_PI); // 6. Compute the residual by scaling according to the standard deviation of the bearing part - *_res = er * T(getMeasurementSquareRootInformationUpper()(1,1)); + *_res = er * getMeasurementSquareRootInformationUpper()(1,1); return true; } diff --git a/include/core/common/factory.h b/include/core/common/factory.h index d533ff0da8b627dd0d12ca9a32637956da871af5..1e18985f9a047dea575af0852b24eac6ff6305bd 100644 --- a/include/core/common/factory.h +++ b/include/core/common/factory.h @@ -307,6 +307,8 @@ class Factory static bool unregisterCreator(const std::string& _type); static TypeBasePtr create(const std::string& _type, TypeInput... _input); std::string getClass() const; + static void printAddress(); + static void printCallbacks(); private: CallbackMap callbacks_; @@ -321,9 +323,15 @@ class Factory void operator=(Factory const&) = delete; private: Factory() { } - ~Factory() { } + ~Factory(); }; +template<class TypeBase, typename... TypeInput> +inline Factory<TypeBase, TypeInput...>::~Factory<TypeBase, TypeInput...>() +{ + std::cout << " Factory destructor " << this->getClass() << std::endl; +} + template<class TypeBase, typename... TypeInput> inline bool Factory<TypeBase, TypeInput...>::registerCreator(const std::string& _type, CreatorCallback createFn) { @@ -358,7 +366,7 @@ inline typename Factory<TypeBase, TypeInput...>::TypeBasePtr Factory<TypeBase, T template<class TypeBase, typename... TypeInput> inline Factory<TypeBase, TypeInput...>& Factory<TypeBase, TypeInput...>::get() { - static Factory instance_; + static Factory<TypeBase, TypeInput...> instance_; return instance_; } @@ -368,6 +376,21 @@ inline std::string Factory<TypeBase, TypeInput...>::getClass() const return "Factory<class TypeBase>"; } +template<class TypeBase, typename... TypeInput> +inline void Factory<TypeBase, TypeInput...>::printAddress() +{ + std::cout << get().getClass() << " address: " << &get() << std::endl; +} + +template<class TypeBase, typename... TypeInput> +inline void Factory<TypeBase, TypeInput...>::printCallbacks() +{ + std::cout << get().getClass() << " callbacks size: " << get().callbacks_.size() << std::endl; + for (auto cb: get().callbacks_){ + std::cout << "\t" << cb.first << std::endl; + } +} + } // namespace wolf namespace wolf diff --git a/include/core/frame/frame_base.h b/include/core/frame/frame_base.h index 8d426eacab111c81b435e48e630d39a802e02a6f..8d64663585af0982d7ac3209a3fcb808c753080a 100644 --- a/include/core/frame/frame_base.h +++ b/include/core/frame/frame_base.h @@ -154,20 +154,20 @@ class FrameBase : public NodeBase, public HasStateBlocks, public std::enable_sha CaptureBaseConstPtrList getCapturesOfType(const std::string& type) const; CaptureBasePtrList getCapturesOfType(const std::string& type); - CaptureBaseConstPtr getCaptureOf(const SensorBaseConstPtr _sensor_ptr) const; - CaptureBasePtr getCaptureOf(const SensorBaseConstPtr _sensor_ptr); + CaptureBaseConstPtr getCaptureOf(SensorBaseConstPtr _sensor_ptr) const; + CaptureBasePtr getCaptureOf(SensorBaseConstPtr _sensor_ptr); - CaptureBaseConstPtr getCaptureOf(const SensorBasePtr _sensor_ptr, const std::string& type) const; - CaptureBasePtr getCaptureOf(const SensorBasePtr _sensor_ptr, const std::string& type); + CaptureBaseConstPtr getCaptureOf(SensorBaseConstPtr _sensor_ptr, const std::string& type) const; + CaptureBasePtr getCaptureOf(SensorBasePtr _sensor_ptr, const std::string& type); - CaptureBaseConstPtrList getCapturesOf(const SensorBasePtr _sensor_ptr) const; - CaptureBasePtrList getCapturesOf(const SensorBasePtr _sensor_ptr); + CaptureBaseConstPtrList getCapturesOf(SensorBaseConstPtr _sensor_ptr) const; + CaptureBasePtrList getCapturesOf(SensorBasePtr _sensor_ptr); - FactorBaseConstPtr getFactorOf(const ProcessorBasePtr _processor_ptr) const; - FactorBasePtr getFactorOf(const ProcessorBasePtr _processor_ptr); + FactorBaseConstPtr getFactorOf(ProcessorBaseConstPtr _processor_ptr) const; + FactorBasePtr getFactorOf(ProcessorBasePtr _processor_ptr); - FactorBaseConstPtr getFactorOf(const ProcessorBasePtr _processor_ptr, const std::string& type) const; - FactorBasePtr getFactorOf(const ProcessorBasePtr _processor_ptr, const std::string& type); + FactorBaseConstPtr getFactorOf(ProcessorBaseConstPtr _processor_ptr, const std::string& type) const; + FactorBasePtr getFactorOf(ProcessorBasePtr _processor_ptr, const std::string& type); unsigned int getHits() const; diff --git a/include/core/problem/problem.h b/include/core/problem/problem.h index 69e55ea8556dc03a323e0aa62e90d125b93015a0..9dfb7cae3e62adac06662e9414c75764045148e2 100644 --- a/include/core/problem/problem.h +++ b/include/core/problem/problem.h @@ -37,13 +37,11 @@ struct ParamsProcessorBase; //wolf includes #include "core/common/wolf.h" +#include "core/utils/params_server.h" #include "core/frame/frame_base.h" #include "core/state_block/state_block.h" -#include "core/utils/params_server.h" -#include "core/sensor/factory_sensor.h" -#include "core/processor/factory_processor.h" -#include <core/processor/motion_provider.h> #include "core/state_block/state_composite.h" +#include "core/processor/motion_provider.h" // std includes #include <mutex> diff --git a/include/core/processor/motion_provider.h b/include/core/processor/motion_provider.h index 9a6e84d9c8949b4c9d5c5e69cdf3ec071d66fa57..d7bd26d00df45c7f14d0ff5ae01966e271af03ef 100644 --- a/include/core/processor/motion_provider.h +++ b/include/core/processor/motion_provider.h @@ -80,7 +80,7 @@ class MotionProvider void setStatePriority(int); public: - const StateStructure& getStateStructure ( ) { return state_structure_; }; + const StateStructure& getStateStructure ( ) const { return state_structure_; }; void setStateStructure(std::string _state_structure) { state_structure_ = _state_structure; }; void addToProblem(ProblemPtr _prb_ptr, MotionProviderPtr _motion_ptr); diff --git a/include/core/processor/processor_base.h b/include/core/processor/processor_base.h index fc93357dd46a948c4d225e982b8ade7bf4682af9..52eab3b6b239922b5afbfe08e84361acd2c0e98e 100644 --- a/include/core/processor/processor_base.h +++ b/include/core/processor/processor_base.h @@ -92,6 +92,7 @@ class Buffer public: typedef typename std::map<TimeStamp,T>::iterator Iterator; // buffer iterator + typedef typename std::map<TimeStamp,T>::const_iterator ConstIterator; // buffer iterator Buffer(){}; ~Buffer(void){}; @@ -101,27 +102,28 @@ public: * Select from the buffer the closest element (w.r.t. time stamp), * respecting a defined time tolerances */ - T select(const TimeStamp& _time_stamp, const double& _time_tolerance); + T select(const TimeStamp& _time_stamp, const double& _time_tolerance) const; /**\brief Select an element iterator from the buffer * * Select from the buffer the iterator pointing to the closest element (w.r.t. time stamp), * respecting a defined time tolerances */ + ConstIterator selectIterator(const TimeStamp& _time_stamp, const double& _time_tolerance) const; Iterator selectIterator(const TimeStamp& _time_stamp, const double& _time_tolerance); - T selectFirstBefore(const TimeStamp& _time_stamp, const double& _time_tolerance); + T selectFirstBefore(const TimeStamp& _time_stamp, const double& _time_tolerance) const; - T selectLastAfter(const TimeStamp& _time_stamp, const double& _time_tolerance); + T selectLastAfter(const TimeStamp& _time_stamp, const double& _time_tolerance) const; - T selectFirst(); + T selectFirst() const; - T selectLast(); + T selectLast() const; /**\brief Buffer size * */ - SizeStd size(void); + SizeStd size(void) const; /**\brief Add a element to the buffer * @@ -132,6 +134,7 @@ public: * * elements are ordered from most recent to oldest */ + const std::map<TimeStamp,T>& getContainer() const; std::map<TimeStamp,T>& getContainer(); /**\brief Remove all elements in the buffer with a time stamp older than the specified @@ -152,7 +155,7 @@ public: /**\brief is the buffer empty ? * */ - bool empty(); + bool empty() const; protected: /**\brief Check time tolerance @@ -160,14 +163,19 @@ protected: * Check if the time distance between two time stamps is smaller than * the time tolerance. */ - static bool checkTimeTolerance(const TimeStamp& _time_stamp1, const TimeStamp& _time_stamp2, const double& _time_tolerance); + static bool checkTimeTolerance(const TimeStamp& _time_stamp1, + const TimeStamp& _time_stamp2, + const double& _time_tolerance); /**\brief Check time tolerance * * Check if the time distance between two time stamps is smaller than * the minimum time tolerance of the two frames. */ - static bool doubleCheckTimeTolerance(const TimeStamp& _time_stamp1, const double& _time_tolerance1, const TimeStamp& _time_stamp2, const double& _time_tolerance2); + static bool doubleCheckTimeTolerance(const TimeStamp& _time_stamp1, + const double& _time_tolerance1, + const TimeStamp& _time_stamp2, + const double& _time_tolerance2); protected: @@ -454,6 +462,42 @@ std::shared_ptr<classType> ProcessorBase::emplace(SensorBasePtr _sen_ptr, T&&... ///////////////////////////////////////////////////////////////////////////////////////// +template <typename T> +typename Buffer<T>::ConstIterator Buffer<T>::selectIterator(const TimeStamp& _time_stamp, const double& _time_tolerance) const +{ + Buffer<T>::ConstIterator post = container_.upper_bound(_time_stamp); + + bool prev_exists = (post != container_.begin()); + bool post_exists = (post != container_.end()); + + bool post_ok = post_exists && checkTimeTolerance(post->first, _time_stamp, _time_tolerance); + + if (prev_exists) + { + Buffer<T>::ConstIterator prev = std::prev(post); + + bool prev_ok = checkTimeTolerance(prev->first, _time_stamp, _time_tolerance); + + if (prev_ok && !post_ok) + return prev; + + else if (!prev_ok && post_ok) + return post; + + else if (prev_ok && post_ok) + { + if (std::fabs(post->first - _time_stamp) < std::fabs(prev->first - _time_stamp)) + return post; + else + return prev; + } + } + else if (post_ok) + return post; + + return container_.end(); +} + template <typename T> typename Buffer<T>::Iterator Buffer<T>::selectIterator(const TimeStamp& _time_stamp, const double& _time_tolerance) { @@ -491,12 +535,12 @@ typename Buffer<T>::Iterator Buffer<T>::selectIterator(const TimeStamp& _time_st } template <typename T> -T Buffer<T>::select(const TimeStamp& _time_stamp, const double& _time_tolerance) +T Buffer<T>::select(const TimeStamp& _time_stamp, const double& _time_tolerance) const { if (container_.empty()) return nullptr; - Buffer<T>::Iterator it = selectIterator(_time_stamp, _time_tolerance); + auto it = selectIterator(_time_stamp, _time_tolerance); // end is returned from selectIterator if an element of the buffer complying with the time stamp // and time tolerance has not been found @@ -508,7 +552,7 @@ T Buffer<T>::select(const TimeStamp& _time_stamp, const double& _time_tolerance) } template <typename T> -T Buffer<T>::selectFirstBefore(const TimeStamp& _time_stamp, const double& _time_tolerance) +T Buffer<T>::selectFirstBefore(const TimeStamp& _time_stamp, const double& _time_tolerance) const { // There is no element if (container_.empty()) @@ -529,7 +573,7 @@ T Buffer<T>::selectFirstBefore(const TimeStamp& _time_stamp, const double& _time template <typename T> -T Buffer<T>::selectLastAfter(const TimeStamp& _time_stamp, const double& _time_tolerance) +T Buffer<T>::selectLastAfter(const TimeStamp& _time_stamp, const double& _time_tolerance) const { // There is no element if (container_.empty()) @@ -549,7 +593,7 @@ T Buffer<T>::selectLastAfter(const TimeStamp& _time_stamp, const double& _time_t } template <typename T> -T Buffer<T>::selectFirst() +T Buffer<T>::selectFirst() const { // There is no element if (container_.empty()) @@ -560,7 +604,7 @@ T Buffer<T>::selectFirst() } template <typename T> -T Buffer<T>::selectLast() +T Buffer<T>::selectLast() const { // There is no element if (container_.empty()) @@ -576,6 +620,12 @@ void Buffer<T>::emplace(const TimeStamp& _time_stamp, const T& _element) container_.emplace(_time_stamp, _element); } +template <typename T> +const std::map<TimeStamp,T>& Buffer<T>::getContainer() const +{ + return container_; +} + template <typename T> std::map<TimeStamp,T>& Buffer<T>::getContainer() { @@ -589,13 +639,13 @@ inline void Buffer<T>::clear() } template <typename T> -inline bool Buffer<T>::empty() +inline bool Buffer<T>::empty() const { return container_.empty(); } template <typename T> -inline SizeStd Buffer<T>::size(void) +inline SizeStd Buffer<T>::size(void) const { return container_.size(); } @@ -628,8 +678,8 @@ inline bool Buffer<T>::doubleCheckTimeTolerance(const TimeStamp& _time_stamp1, template <typename T> inline bool Buffer<T>::checkTimeTolerance(const TimeStamp& _time_stamp1, - const TimeStamp& _time_stamp2, - const double& _time_tolerance) + const TimeStamp& _time_stamp2, + const double& _time_tolerance) { double time_diff = std::fabs(_time_stamp1 - _time_stamp2); bool pass = time_diff <= _time_tolerance; diff --git a/include/core/processor/processor_tracker.h b/include/core/processor/processor_tracker.h index d93c9baa367b8ba65374a4b64d21bab194ce1d2a..b0683aba04b3e5d8fe041775199823436c728655 100644 --- a/include/core/processor/processor_tracker.h +++ b/include/core/processor/processor_tracker.h @@ -136,7 +136,7 @@ class ProcessorTracker : public ProcessorBase ParamsProcessorTrackerPtr _params_tracker); ~ProcessorTracker() override; - StateStructure getStateStructure() const; + const StateStructure& getStateStructure() const; virtual CaptureBaseConstPtr getOrigin() const; virtual CaptureBasePtr getOrigin(); @@ -313,7 +313,7 @@ inline FeatureBasePtrList& ProcessorTracker::getNewFeaturesListIncoming() return new_features_incoming_; } -inline StateStructure ProcessorTracker::getStateStructure ( ) const +inline const StateStructure& ProcessorTracker::getStateStructure ( ) const { return state_structure_; } diff --git a/src/frame/frame_base.cpp b/src/frame/frame_base.cpp index f295cdbca724a4e4ee89b8d2aa5c52ae487b537a..aae21b6eb2fa9d5552e8212259299b71e0e0c440 100644 --- a/src/frame/frame_base.cpp +++ b/src/frame/frame_base.cpp @@ -230,7 +230,7 @@ CaptureBasePtrList FrameBase::getCapturesOfType(const std::string& type) return captures; } -CaptureBaseConstPtr FrameBase::getCaptureOf(const SensorBasePtr _sensor_ptr, const std::string& type) const +CaptureBaseConstPtr FrameBase::getCaptureOf(SensorBaseConstPtr _sensor_ptr, const std::string& type) const { for (auto capture_ptr : getCaptureList()) if (capture_ptr->getSensor() == _sensor_ptr && capture_ptr->getType() == type) @@ -238,7 +238,7 @@ CaptureBaseConstPtr FrameBase::getCaptureOf(const SensorBasePtr _sensor_ptr, con return nullptr; } -CaptureBasePtr FrameBase::getCaptureOf(const SensorBasePtr _sensor_ptr, const std::string& type) +CaptureBasePtr FrameBase::getCaptureOf(SensorBasePtr _sensor_ptr, const std::string& type) { for (auto capture_ptr : getCaptureList()) if (capture_ptr->getSensor() == _sensor_ptr && capture_ptr->getType() == type) @@ -246,7 +246,7 @@ CaptureBasePtr FrameBase::getCaptureOf(const SensorBasePtr _sensor_ptr, const st return nullptr; } -CaptureBaseConstPtr FrameBase::getCaptureOf(const SensorBaseConstPtr _sensor_ptr) const +CaptureBaseConstPtr FrameBase::getCaptureOf(SensorBaseConstPtr _sensor_ptr) const { if (_sensor_ptr) for (auto capture_ptr : getCaptureList()) @@ -255,7 +255,7 @@ CaptureBaseConstPtr FrameBase::getCaptureOf(const SensorBaseConstPtr _sensor_ptr return nullptr; } -CaptureBasePtr FrameBase::getCaptureOf(const SensorBaseConstPtr _sensor_ptr) +CaptureBasePtr FrameBase::getCaptureOf(SensorBaseConstPtr _sensor_ptr) { if (_sensor_ptr) for (auto capture_ptr : getCaptureList()) @@ -264,7 +264,7 @@ CaptureBasePtr FrameBase::getCaptureOf(const SensorBaseConstPtr _sensor_ptr) return nullptr; } -CaptureBaseConstPtrList FrameBase::getCapturesOf(const SensorBasePtr _sensor_ptr) const +CaptureBaseConstPtrList FrameBase::getCapturesOf(SensorBaseConstPtr _sensor_ptr) const { CaptureBaseConstPtrList captures; for (auto capture_ptr : getCaptureList()) @@ -273,7 +273,7 @@ CaptureBaseConstPtrList FrameBase::getCapturesOf(const SensorBasePtr _sensor_ptr return captures; } -CaptureBasePtrList FrameBase::getCapturesOf(const SensorBasePtr _sensor_ptr) +CaptureBasePtrList FrameBase::getCapturesOf(SensorBasePtr _sensor_ptr) { CaptureBasePtrList captures; for (auto capture_ptr : getCaptureList()) @@ -282,7 +282,7 @@ CaptureBasePtrList FrameBase::getCapturesOf(const SensorBasePtr _sensor_ptr) return captures; } -FactorBaseConstPtr FrameBase::getFactorOf(const ProcessorBasePtr _processor_ptr, const std::string& type) const +FactorBaseConstPtr FrameBase::getFactorOf(ProcessorBaseConstPtr _processor_ptr, const std::string& type) const { for (auto factor_ptr : getConstrainedByList()) if (factor_ptr->getProcessor() == _processor_ptr && factor_ptr->getType() == type) @@ -295,7 +295,7 @@ FactorBaseConstPtr FrameBase::getFactorOf(const ProcessorBasePtr _processor_ptr, return nullptr; } -FactorBasePtr FrameBase::getFactorOf(const ProcessorBasePtr _processor_ptr, const std::string& type) +FactorBasePtr FrameBase::getFactorOf(ProcessorBasePtr _processor_ptr, const std::string& type) { for (auto factor_ptr : getConstrainedByList()) if (factor_ptr->getProcessor() == _processor_ptr && factor_ptr->getType() == type) @@ -308,7 +308,7 @@ FactorBasePtr FrameBase::getFactorOf(const ProcessorBasePtr _processor_ptr, cons return nullptr; } -FactorBaseConstPtr FrameBase::getFactorOf(const ProcessorBasePtr _processor_ptr) const +FactorBaseConstPtr FrameBase::getFactorOf(ProcessorBaseConstPtr _processor_ptr) const { for (auto factor_ptr : getConstrainedByList()) if (factor_ptr->getProcessor() == _processor_ptr) @@ -321,7 +321,7 @@ FactorBaseConstPtr FrameBase::getFactorOf(const ProcessorBasePtr _processor_ptr) return nullptr; } -FactorBasePtr FrameBase::getFactorOf(const ProcessorBasePtr _processor_ptr) +FactorBasePtr FrameBase::getFactorOf(ProcessorBasePtr _processor_ptr) { for (auto factor_ptr : getConstrainedByList()) if (factor_ptr->getProcessor() == _processor_ptr) diff --git a/src/map/map_base.cpp b/src/map/map_base.cpp index 88b5f411dc6b718cab9f5e1a2a70071abffa4f8e..66367305a0b9ad82e9ef5987c3bdacb9e9a1eeea 100644 --- a/src/map/map_base.cpp +++ b/src/map/map_base.cpp @@ -23,7 +23,7 @@ // wolf #include "core/map/map_base.h" #include "core/landmark/landmark_base.h" -//#include "core/common/factory.h" +#include "core/common/factory.h" // YAML #include <yaml-cpp/yaml.h> diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp index 75f23c36e4d86da038bf21c7fc17577c05a637d1..12ec2c4f0b14c5800be2fa617902c466bca7e6cf 100644 --- a/src/problem/problem.cpp +++ b/src/problem/problem.cpp @@ -21,40 +21,18 @@ //--------LICENSE_END-------- // wolf includes #include "core/problem/problem.h" -#include "core/hardware/hardware_base.h" -#include "core/trajectory/trajectory_base.h" -#include "core/map/map_base.h" #include "core/map/factory_map.h" -#include "core/sensor/sensor_base.h" #include "core/sensor/factory_sensor.h" #include "core/processor/factory_processor.h" -#include "core/processor/processor_motion.h" -#include "core/processor/processor_tracker.h" -#include "core/capture/capture_pose.h" #include "core/capture/capture_void.h" -#include "core/factor/factor_base.h" #include "core/factor/factor_block_absolute.h" #include "core/factor/factor_quaternion_absolute.h" -#include "core/state_block/state_block.h" #include "core/state_block/state_quaternion.h" #include "core/state_block/state_angle.h" #include "core/tree_manager/factory_tree_manager.h" #include "core/tree_manager/tree_manager_base.h" - -#include "core/utils/params_server.h" #include "core/utils/loader.h" -#include "core/utils/check_log.h" -#include "core/math/covariance.h" -#include "core/state_block/factory_state_block.h" - -// C++ includes -#include <algorithm> -#include <map> -#include <sstream> -#include <stdexcept> -#include <string> -#include <vector> -#include <unordered_set> + namespace wolf { diff --git a/src/processor/processor_tracker.cpp b/src/processor/processor_tracker.cpp index 512d941b7669c13e22a87aee23c5a8f84ed44329..3d3794e75b1a87461832695c26341b8f40fc11f5 100644 --- a/src/processor/processor_tracker.cpp +++ b/src/processor/processor_tracker.cpp @@ -216,10 +216,6 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr) last_ptr_->getFrame()->setState(getProblem()->getState(last_ptr_->getTimeStamp())); last_ptr_->getFrame()->link(getProblem()); - // // make F; append incoming to new F - // FrameBasePtr frm = FrameBase::createNonKeyFrame<FrameBase>(incoming_ptr_->getTimeStamp()); - // incoming_ptr_->link(frm); - // Establish factors establishFactors(); @@ -232,7 +228,7 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr) // make F; append incoming to new F FrameBasePtr frame = std::make_shared<FrameBase>(incoming_ptr_->getTimeStamp(), getProblem()->getFrameStructure(), - last_ptr_->getFrame()->getState()); + getProblem()->getState(incoming_ptr_->getTimeStamp())); incoming_ptr_ ->link(frame); origin_ptr_ = last_ptr_; last_ptr_ = incoming_ptr_; @@ -250,7 +246,7 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr) // Replace last frame for a new one in incoming FrameBasePtr frame = std::make_shared<FrameBase>(incoming_ptr_->getTimeStamp(), getProblem()->getFrameStructure(), - last_ptr_->getFrame()->getState()); + getProblem()->getState(incoming_ptr_->getTimeStamp())); incoming_ptr_->link(frame); last_ptr_->unlink(); // unlink last (destroying the frame) instead of frame destruction that would implicitly destroy last diff --git a/src/yaml/processor_odom_3d_yaml.cpp b/src/yaml/processor_odom_3d_yaml.cpp index a8dd748ea29aa2a63eb72003b5c37465d62c3d17..771d19b7414b2a4a7342c8afea09d08d19f1b750 100644 --- a/src/yaml/processor_odom_3d_yaml.cpp +++ b/src/yaml/processor_odom_3d_yaml.cpp @@ -31,7 +31,7 @@ // wolf #include "core/processor/processor_odom_3d.h" -#include "core/common/factory.h" +#include "core/processor/factory_processor.h" // yaml-cpp library #include <yaml-cpp/yaml.h> diff --git a/src/yaml/sensor_odom_2d_yaml.cpp b/src/yaml/sensor_odom_2d_yaml.cpp index 68a44ff61adaabb99cb16cc1bc1c72017de6d813..dcb0cf0c10e428a6dfce876a1765e8c0d245e1a2 100644 --- a/src/yaml/sensor_odom_2d_yaml.cpp +++ b/src/yaml/sensor_odom_2d_yaml.cpp @@ -33,6 +33,7 @@ // wolf #include "core/sensor/sensor_odom_2d.h" +#include "core/sensor/factory_sensor.h" // yaml-cpp library #include <yaml-cpp/yaml.h> diff --git a/src/yaml/sensor_odom_3d_yaml.cpp b/src/yaml/sensor_odom_3d_yaml.cpp index 02e0ff3d8e5296398a5f9cfc52b66fd21cf15d85..0cb47d441d9b8a0786a9fcf8d00952a682072c4a 100644 --- a/src/yaml/sensor_odom_3d_yaml.cpp +++ b/src/yaml/sensor_odom_3d_yaml.cpp @@ -31,7 +31,7 @@ // wolf #include "core/sensor/sensor_odom_3d.h" -#include "core/common/factory.h" +#include "core/sensor/factory_sensor.h" // yaml-cpp library #include <yaml-cpp/yaml.h> diff --git a/src/yaml/sensor_pose_yaml.cpp b/src/yaml/sensor_pose_yaml.cpp index 102729bd91dcfca4f51a567f47d790ce1de6c6b7..bfbe151c43315c21e5f636e2632fa9ceb614c9d4 100644 --- a/src/yaml/sensor_pose_yaml.cpp +++ b/src/yaml/sensor_pose_yaml.cpp @@ -31,7 +31,7 @@ // wolf #include "core/sensor/sensor_pose.h" -#include "core/common/factory.h" +#include "core/sensor/factory_sensor.h" // yaml-cpp library #include <yaml-cpp/yaml.h> diff --git a/test/gtest_factor_diff_drive.cpp b/test/gtest_factor_diff_drive.cpp index 8283b62b8ef35e591dad8533256a517ad65f7718..c37bed03e30da209b9b644d49b92011afeeb5f93 100644 --- a/test/gtest_factor_diff_drive.cpp +++ b/test/gtest_factor_diff_drive.cpp @@ -626,7 +626,7 @@ TEST(FactorDiffDrive, preintegrate_and_solve_sensor_intrinsics) // right turn 90 deg in N steps --> ends up in (1.5, -1.5, -pi/2) int N = 6; - Vector3d data; + Vector2d data; Matrix2d data_cov; data_cov.setIdentity(); data(0) = 0.50*intr->ticks_per_wheel_revolution / N; diff --git a/test/gtest_processor_odom_3d.cpp b/test/gtest_processor_odom_3d.cpp index a5e0beabb5447222a27bd02138144997d3389a00..23c0afdf5a4bcf4ef2586d7e63b692ae4c953906 100644 --- a/test/gtest_processor_odom_3d.cpp +++ b/test/gtest_processor_odom_3d.cpp @@ -149,7 +149,7 @@ TEST(ProcessorOdom3d, deltaPlusDelta_Jac) MatrixXd J_D(7,7), J_d(7,7); - JAC_NUMERIC(prc_ptr, D, d, dt, J_D, J_d, Constants::EPS); + JAC_NUMERIC(prc_ptr, D, d, dt, J_D, J_d, 1000*Constants::EPS); WOLF_DEBUG("J_D:\n ", J_D); WOLF_DEBUG("J_d:\n ", J_d);