diff --git a/CMakeLists.txt b/CMakeLists.txt index 4f6d43d2d4c233fef99abf227ec9c41e058f9c84..5765ab7eadbe19ab1479b8dc5c2df49aa7edde26 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -97,16 +97,16 @@ ENDIF() option(_WOLF_TRACE "Enable wolf tracing macro" ON) -# option(BUILD_EXAMPLES "Build examples" OFF) +# option(BUILD_DEMOS "Build examples" OFF) set(BUILD_TESTS true) -set(BUILD_EXAMPLES false) +set(BUILD_DEMOS false) # Does this has any other interest # but for the examples ? # yes, for the tests ! -IF(BUILD_EXAMPLES OR BUILD_TESTS) +IF(BUILD_DEMOS OR BUILD_TESTS) set(_WOLF_ROOT_DIR ${CMAKE_SOURCE_DIR}) -ENDIF(BUILD_EXAMPLES OR BUILD_TESTS) +ENDIF(BUILD_DEMOS OR BUILD_TESTS) #find dependencies. @@ -115,57 +115,10 @@ FIND_PACKAGE(Eigen3 3.2.92 REQUIRED) FIND_PACKAGE(Threads REQUIRED) -FIND_PACKAGE(Ceres QUIET) #Ceres is not required -IF(Ceres_FOUND) - MESSAGE("Ceres Library FOUND: Ceres related sources will be built.") -ENDIF(Ceres_FOUND) - -FIND_PACKAGE(faramotics QUIET) #faramotics is not required -IF(faramotics_FOUND) - FIND_PACKAGE(GLUT REQUIRED) - FIND_PACKAGE(pose_state_time REQUIRED) - MESSAGE("Faramotics Library FOUND: Faramotics related sources will be built.") -ENDIF(faramotics_FOUND) - -# Cereal -FIND_PACKAGE(cereal QUIET) -IF(cereal_FOUND) - MESSAGE("cereal Library FOUND: cereal related sources will be built.") -ENDIF(cereal_FOUND) - +FIND_PACKAGE(Ceres REQUIRED) #Ceres is not required # YAML with yaml-cpp FIND_PACKAGE(YamlCpp REQUIRED) -IF(YAMLCPP_FOUND) - MESSAGE("yaml-cpp Library FOUND: yaml-cpp related sources will be built.") -ELSEIF(YAMLCPP_FOUND) - MESSAGE("yaml-cpp Library NOT FOUND!") -ENDIF(YAMLCPP_FOUND) - -#GLOG -INCLUDE (${PROJECT_SOURCE_DIR}/cmake_modules/FindGlog.cmake) -IF(GLOG_FOUND) - MESSAGE("glog Library FOUND: glog related sources will be built.") - MESSAGE(STATUS ${GLOG_INCLUDE_DIR}) - MESSAGE(STATUS ${GLOG_LIBRARY}) -ELSEIF(GLOG_FOUND) - MESSAGE("glog Library NOT FOUND!") -ENDIF(GLOG_FOUND) - -# SuiteSparse doesn't have find*.cmake: -FIND_PATH( - Suitesparse_INCLUDE_DIRS - NAMES SuiteSparse_config.h - PATHS /usr/include/suitesparse /usr/local/include/suitesparse) -MESSAGE("Found suitesparse_INCLUDE_DIRS:" ${Suitesparse_INCLUDE_DIRS}) - -IF(Suitesparse_INCLUDE_DIRS) - SET(Suitesparse_FOUND TRUE) - MESSAGE("Suitesparse FOUND: wolf_solver will be built.") -ELSE (Suitesparse_INCLUDE_DIRS) - SET(Suitesparse_FOUND FALSE) - MESSAGE(FATAL_ERROR "Suitesparse NOT FOUND") -ENDIF (Suitesparse_INCLUDE_DIRS) # Define the directory where will be the configured config.h SET(WOLF_CONFIG_DIR ${PROJECT_BINARY_DIR}/conf/core/internal) @@ -184,10 +137,11 @@ configure_file(${CMAKE_CURRENT_SOURCE_DIR}/internal/config.h.in "${WOLF_CONFIG_D message("WOLF CONFIG ${WOLF_CONFIG_DIR}/config.h") message("CONFIG DIRECTORY ${PROJECT_BINARY_DIR}") include_directories("${PROJECT_BINARY_DIR}/conf") + # include spdlog (logging library) FIND_PATH(SPDLOG_INCLUDE_DIR spdlog.h /usr/local/include/spdlog /usr/include/spdlog) IF (SPDLOG_INCLUDE_DIR) - INCLUDE_DIRECTORIES(${SPDLOG_INCLUDE_DIR}) + # INCLUDE_DIRECTORIES(${SPDLOG_INCLUDE_DIR}) MESSAGE(STATUS "Found spdlog: ${SPDLOG_INCLUDE_DIR}") ELSE (SPDLOG_INCLUDE_DIR) MESSAGE(FATAL_ERROR "Could not find spdlog") @@ -196,32 +150,10 @@ ENDIF (SPDLOG_INCLUDE_DIR) INCLUDE_DIRECTORIES(${EIGEN_INCLUDE_DIRS}) INCLUDE_DIRECTORIES("include") INCLUDE_DIRECTORIES(${YAMLCPP_INCLUDE_DIR}) - -IF(Ceres_FOUND) - INCLUDE_DIRECTORIES(${CERES_INCLUDE_DIRS}) -ENDIF(Ceres_FOUND) - -IF(faramotics_FOUND) - INCLUDE_DIRECTORIES(${faramotics_INCLUDE_DIRS}) -ENDIF(faramotics_FOUND) - -# cereal -IF(cereal_FOUND) - INCLUDE_DIRECTORIES(${cereal_INCLUDE_DIRS}) -ENDIF(cereal_FOUND) - -IF(Suitesparse_FOUND) - INCLUDE_DIRECTORIES(${Suitesparse_INCLUDE_DIRS}) -ENDIF(Suitesparse_FOUND) - -IF(GLOG_FOUND) -INCLUDE_DIRECTORIES(${GLOG_INCLUDE_DIR}) -ENDIF(GLOG_FOUND) - +INCLUDE_DIRECTORIES(${CERES_INCLUDE_DIRS}) #HEADERS - SET(HDRS_COMMON include/core/common/factory.h include/core/common/node_base.h @@ -513,23 +445,21 @@ IF (Ceres_FOUND) TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${CERES_LIBRARIES}) ENDIF(Ceres_FOUND) -IF (GLOG_FOUND) - TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${GLOG_LIBRARY}) -ENDIF (GLOG_FOUND) +# IF (GLOG_FOUND) +# TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${GLOG_LIBRARY}) +# ENDIF (GLOG_FOUND) #check if this is done correctly -IF (GLOG_FOUND) - IF(BUILD_TESTS) - MESSAGE("Building tests.") - add_subdirectory(test) - ENDIF(BUILD_TESTS) -ENDIF (GLOG_FOUND) +IF(BUILD_TESTS) + MESSAGE("Building tests.") + add_subdirectory(test) +ENDIF(BUILD_TESTS) -IF(BUILD_EXAMPLES) - #Build examples +IF(BUILD_DEMOS) + #Build demos MESSAGE("Building demos.") ADD_SUBDIRECTORY(demos) -ENDIF(BUILD_EXAMPLES) +ENDIF(BUILD_DEMOS) #install library @@ -593,10 +523,14 @@ INSTALL(FILES ${PROJECT_NAME}.found configure_file("${CMAKE_SOURCE_DIR}/cmake_modules/wolfConfig.cmake" "${CMAKE_BINARY_DIR}/wolfConfig.cmake" @ONLY) +configure_file("${CMAKE_SOURCE_DIR}/cmake_modules/FindYamlCpp.cmake" + "${CMAKE_BINARY_DIR}/FindYamlCpp.cmake" @ONLY) + INSTALL(FILES "${WOLF_CONFIG_DIR}/config.h" DESTINATION include/iri-algorithms/wolf/plugin_core/core/internal) INSTALL(FILES "${CMAKE_BINARY_DIR}/wolfConfig.cmake" DESTINATION "lib/cmake/${PROJECT_NAME}") +INSTALL(FILES "${CMAKE_BINARY_DIR}/FindYamlCpp.cmake" DESTINATION "lib/cmake/${PROJECT_NAME}") INSTALL(DIRECTORY ${SPDLOG_INCLUDE_DIRS} DESTINATION "include/iri-algorithms/") diff --git a/PluginsInfo.md b/PluginsInfo.md index 822109172503bf13012fb4a5b333459f7a1b8e78..33e19f05fa1e96f0699f16b16fd4e13fc6a80240 100644 --- a/PluginsInfo.md +++ b/PluginsInfo.md @@ -5,8 +5,8 @@ ## Installing wolf(core) ``` -git clone https://gitlab.iri.upc.edu/mobile_robotics/wolf_projects/wolf_lib/plugins/core.git -cd core +git clone https://gitlab.iri.upc.edu/mobile_robotics/wolf_projects/wolf_lib/wolf.git +cd wolf mkdir build & cd build cmake .. make @@ -32,8 +32,8 @@ If you want to use the core, you just need to have it installed and in your CMak `find_package(wolf REQUIRED)`. If wolf is indeed installed, this will define two variables -`${wolf_INCLUDE_DIR}` which will contain the path to the wolf include directory -and `${wolf_LIBRARY}` which will contain the path to the wolf library. +`${wolf_INCLUDE_DIRS}` which will contain the path to the wolf include directory +and `${wolf_LIBRARIES}` which will contain the path to the wolf library. ## Using wolf plugins @@ -41,13 +41,13 @@ If you also want to use some wolf plugin, you just follow the same procedure, ch `find_package(wolf<plugin name> REQUIRED)`. If the pluging is indeed installed, this will define two variables -`${wolf<plugin name>_INCLUDE_DIR}` which will contain the path to the plugin's include directory -and `${wolf<plugin name>_LIBRARY}` which will contain the path to the plugin's library. +`${wolf<plugin name>_INCLUDE_DIRS}` which will contain the path to the plugin's include directory +and `${wolf<plugin name>_LIBRARIES}` which will contain the path to the plugin's library. -As an example, suppose that we want to use the _vision_ plugin. First, we will clone and install it +As an example, suppose that we want to use the _laser_ plugin. First, we will clone and install it ``` -git clone https://gitlab.iri.upc.edu/mobile_robotics/wolf_projects/wolf_lib/plugins/vision.git -cd vision +git clone https://gitlab.iri.upc.edu/mobile_robotics/wolf_projects/wolf_lib/plugins/laser.git +cd laser mkdir build && cd build cmake .. make @@ -56,15 +56,12 @@ sudo make install Then, in the CMakeLists.txt of the application we are developing we will put the following line ``` -find_package(wolfvision REQUIRED) +find_package(wolflaser REQUIRED) ``` if the plugin has been correctly installed, and thus find_package succeeds, then it will define two variables -- ${wolfvision_INCLUDE_DIR} which is the path to the includes. It should have the value `/usr/local/include/iri-algorithms/wolf/plugin_vision` -- ${wolfvision_LIBRARY} which is the path to the compiled (& linked) library. It should have the value `/usr/local/lib/iri-algorithms/libwolfvision.so` - -**IMPORTANT NOTE** For the time being, each plugin is only responsible for finding their own includes and library. This means that for instance wolfvision, which obviously requires -the _core_ plugin, will not find its own dependencies. It is the responsibility of the programmer to do `find_package(wolf REQUIRED)` to get the includes and library. In the future this will change and each plugin will be responsible for finding all the necessary dependencies. +- ${wolflaser_INCLUDE_DIRS} which is the path to the includes. It should have the value `/usr/local/include/iri-algorithms/wolf/plugin_laser;/usr/local/include/iri-algorithms` +- ${wolfvision_LIBRARIES} which is the path to the required libraries. It should have the value `/usr/local/lib/iri-algorithms/libwolflaser.so;/usr/local/lib/iri-algorithms/liblaser_scan_utils.so` # Creating your plugin diff --git a/cmake_modules/wolfConfig.cmake b/cmake_modules/wolfConfig.cmake index a72b121bde57c392cf9f13fe4a5ed73e309ddf98..90e622c1b964cfb78b1664b1ed9794f8004bff47 100644 --- a/cmake_modules/wolfConfig.cmake +++ b/cmake_modules/wolfConfig.cmake @@ -1,33 +1,33 @@ #edit the following line to add the librarie's header files FIND_PATH( - wolf_INCLUDE_DIR + wolf_INCLUDE_DIRS NAMES wolf.found PATHS /usr/local/include/iri-algorithms/wolf/plugin_core) -IF(wolf_INCLUDE_DIR) - MESSAGE("Found wolf include dirs: ${wolf_INCLUDE_DIR}") -ELSE(wolf_INCLUDE_DIR) +IF(wolf_INCLUDE_DIRS) + MESSAGE("Found wolf include dirs: ${wolf_INCLUDE_DIRS}") +ELSE(wolf_INCLUDE_DIRS) MESSAGE("Couldn't find wolf include dirs") -ENDIF(wolf_INCLUDE_DIR) +ENDIF(wolf_INCLUDE_DIRS) FIND_LIBRARY( - wolf_LIBRARY + wolf_LIBRARIES NAMES libwolf.so libwolf.dylib PATHS /usr/local/lib/iri-algorithms) -IF(wolf_LIBRARY) - MESSAGE("Found wolf lib: ${wolf_LIBRARY}") -ELSE(wolf_LIBRARY) +IF(wolf_LIBRARIES) + MESSAGE("Found wolf lib: ${wolf_LIBRARIES}") +ELSE(wolf_LIBRARIES) MESSAGE("Couldn't find wolf lib") -ENDIF(wolf_LIBRARY) +ENDIF(wolf_LIBRARIES) -IF (wolf_INCLUDE_DIR AND wolf_LIBRARY) +IF (wolf_INCLUDE_DIRS AND wolf_LIBRARIES) SET(wolf_FOUND TRUE) - ELSE(wolf_INCLUDE_DIR AND wolf_LIBRARY) + ELSE(wolf_INCLUDE_DIRS AND wolf_LIBRARIES) set(wolf_FOUND FALSE) -ENDIF (wolf_INCLUDE_DIR AND wolf_LIBRARY) +ENDIF (wolf_INCLUDE_DIRS AND wolf_LIBRARIES) IF (wolf_FOUND) IF (NOT wolf_FIND_QUIETLY) - MESSAGE(STATUS "Found wolf: ${wolf_LIBRARY}") + MESSAGE(STATUS "Found wolf: ${wolf_LIBRARIES}") ENDIF (NOT wolf_FIND_QUIETLY) ELSE (wolf_FOUND) IF (wolf_FIND_REQUIRED) @@ -38,7 +38,7 @@ ENDIF (wolf_FOUND) macro(wolf_report_not_found REASON_MSG) set(wolf_FOUND FALSE) - unset(wolf_INCLUDE_DIR) + unset(wolf_INCLUDE_DIRS) unset(wolf_LIBRARIES) # Reset the CMake module path to its state when this script was called. @@ -62,4 +62,31 @@ if(NOT wolf_FOUND) wolf_report_not_found("Something went wrong while setting up wolf.") endif(NOT wolf_FOUND) # Set the include directories for wolf (itself). -set(wolf_FOUND TRUE) \ No newline at end of file +set(wolf_FOUND TRUE) + +# Now we gather all the required dependencies for Wolf + +get_filename_component(WOLF_CURRENT_CONFIG_DIR + "${CMAKE_CURRENT_LIST_FILE}" PATH) + +SET(BACKUP_MODULE_PATH ${CMAKE_MODULE_PATH}) +LIST(APPEND CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} ${WOLF_CURRENT_CONFIG_DIR}) + +FIND_PACKAGE(Threads REQUIRED) +list(APPEND wolf_LIBRARIES ${CMAKE_THREAD_LIBS_INIT}) + +FIND_PACKAGE(Ceres REQUIRED) +list(APPEND wolf_INCLUDE_DIRS ${CERES_INCLUDE_DIRS}) +list(APPEND wolf_LIBRARIES ${CERES_LIBRARIES}) + +# YAML with yaml-cpp +FIND_PACKAGE(YamlCpp REQUIRED) +list(APPEND wolf_INCLUDE_DIRS ${YAMLCPP_INCLUDE_DIRS}) +list(APPEND wolf_LIBRARIES ${YAMLCPP_LIBRARY}) + +if(NOT Eigen_FOUND) + FIND_PACKAGE(Eigen3 REQUIRED) + list(APPEND wolf_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIRS}) +endif() + +SET(CMAKE_MODULE_PATH ${BACKUP_MODULE_PATH}) \ No newline at end of file diff --git a/doc/doxygen.conf b/doc/doxygen.conf index dc3608694f177138711f769ceadb7ef004665ad4..f0009526c6f9fcb8a61ae84916690a62190e32c6 100644 --- a/doc/doxygen.conf +++ b/doc/doxygen.conf @@ -429,7 +429,7 @@ EXTRACT_PACKAGE = NO # included in the documentation. # The default value is: NO. -EXTRACT_STATIC = NO +EXTRACT_STATIC = YES # If the EXTRACT_LOCAL_CLASSES tag is set to YES classes (and structs) defined # locally in source files will be included in the documentation. If set to NO @@ -756,7 +756,8 @@ WARN_LOGFILE = # Note: If this tag is empty the current directory is searched. INPUT = ../doc/main.dox \ - ../src + ../src \ + ../include/core # This tag can be used to specify the character encoding of the source files # that doxygen parses. Internally doxygen uses the UTF-8 encoding. Doxygen uses diff --git a/hello_wolf/processor_range_bearing.cpp b/hello_wolf/processor_range_bearing.cpp index b75801bde13cff3a003e0ba746bec5fb864bb813..cd7ba019179d7bfb7aa61723073028ccc3b17024 100644 --- a/hello_wolf/processor_range_bearing.cpp +++ b/hello_wolf/processor_range_bearing.cpp @@ -25,15 +25,15 @@ void ProcessorRangeBearing::process(CaptureBasePtr _capture) // 1. get KF FrameBasePtr kf(nullptr); - if ( !kf_pack_buffer_.empty() ) + if ( !buffer_pack_kf_.empty() ) { // KeyFrame Callback received - PackKeyFramePtr pack = kf_pack_buffer_.selectPack( _capture->getTimeStamp(), params_->time_tolerance ); + PackKeyFramePtr pack = buffer_pack_kf_.selectPack( _capture->getTimeStamp(), params_->time_tolerance ); if (pack!=nullptr) kf = pack->key_frame; - kf_pack_buffer_.removeUpTo( _capture->getTimeStamp() ); + buffer_pack_kf_.removeUpTo( _capture->getTimeStamp() ); assert( kf && "Callback KF is not close enough to _capture!"); } diff --git a/include/core/frame/frame_base.h b/include/core/frame/frame_base.h index 8d251380532bfc842028405d1bf5d8f08299401f..23f3d0975565dfd2d9f123d4b78c291925bba810 100644 --- a/include/core/frame/frame_base.h +++ b/include/core/frame/frame_base.h @@ -83,6 +83,7 @@ class FrameBase : public NodeBase, public std::enable_shared_from_this<FrameBase bool isKeyOrAux() const; // set type + void setNonEstimated(); void setKey(); void setAux(); diff --git a/include/core/problem/problem.h b/include/core/problem/problem.h index 6152670ea86394f733230805ecf0708f6b243932..6f37d56a539008cc34ba37b6227756d392900677 100644 --- a/include/core/problem/problem.h +++ b/include/core/problem/problem.h @@ -63,7 +63,7 @@ class Problem : public std::enable_shared_from_this<Problem> public: static ProblemPtr create(const std::string& _frame_structure, SizeEigen _dim); // USE THIS AS A CONSTRUCTOR! - static ProblemPtr autoSetup(const std::string& _frame_structure, SizeEigen _dim, const std::string& _yaml_file); + static ProblemPtr autoSetup(paramsServer &_server); virtual ~Problem(); // Properties ----------------------------------------- diff --git a/include/core/processor/processor_base.h b/include/core/processor/processor_base.h index 72912c21db54fbc92baea6a06e891f3a2df975f0..412d3b3289d9f2ef3a00151c71d2da66dde94649 100644 --- a/include/core/processor/processor_base.h +++ b/include/core/processor/processor_base.h @@ -40,14 +40,14 @@ WOLF_PTR_TYPEDEFS(PackKeyFrame); * * Object and functions to manage a buffer of KFPack objects. */ -class PackKeyFrameBuffer +class BufferPackKeyFrame { public: typedef std::map<TimeStamp,PackKeyFramePtr>::iterator Iterator; // buffer iterator - PackKeyFrameBuffer(void); - ~PackKeyFrameBuffer(void); + BufferPackKeyFrame(void); + ~BufferPackKeyFrame(void); /**\brief Select a Pack from the buffer * @@ -142,11 +142,10 @@ class ProcessorBase : public NodeBase, public std::enable_shared_from_this<Proce protected: unsigned int processor_id_; ProcessorParamsBasePtr params_; - PackKeyFrameBuffer kf_pack_buffer_; + BufferPackKeyFrame buffer_pack_kf_; private: SensorBaseWPtr sensor_ptr_; - static unsigned int processor_id_count_; public: @@ -284,27 +283,27 @@ inline void ProcessorBase::setTimeTolerance(Scalar _time_tolerance) params_->time_tolerance = _time_tolerance; } -inline PackKeyFrameBuffer::PackKeyFrameBuffer(void) +inline BufferPackKeyFrame::BufferPackKeyFrame(void) { } -inline PackKeyFrameBuffer::~PackKeyFrameBuffer(void) +inline BufferPackKeyFrame::~BufferPackKeyFrame(void) { } -inline void PackKeyFrameBuffer::clear() +inline void BufferPackKeyFrame::clear() { container_.clear(); } -inline bool PackKeyFrameBuffer::empty() +inline bool BufferPackKeyFrame::empty() { return container_.empty(); } -inline SizeStd PackKeyFrameBuffer::size(void) +inline SizeStd BufferPackKeyFrame::size(void) { return container_.size(); } diff --git a/include/core/processor/processor_tracker_landmark.h b/include/core/processor/processor_tracker_landmark.h index 2d216b24d5d1cdc773394e461e2792ec3ca9c510..f539a4cab26725b612f0b17a9b4fbe1af5d65cdd 100644 --- a/include/core/processor/processor_tracker_landmark.h +++ b/include/core/processor/processor_tracker_landmark.h @@ -160,17 +160,17 @@ class ProcessorTrackerLandmark : public ProcessorTracker const CaptureBasePtr& _capture, FeatureBasePtrList& _features_out) = 0; - /** \brief Emplace a landmark for each new feature of new_features_last_ + /** \brief Emplaces a landmark for each new feature of new_features_last_ **/ virtual void emplaceNewLandmarks(); - /** \brief Emplace one landmark + /** \brief Emplaces one landmark * * Implement in derived classes to build the type of landmark you need for this tracker. */ virtual LandmarkBasePtr emplaceLandmark(FeatureBasePtr _feature_ptr) = 0; - /** \brief Emplace a new factor + /** \brief Emplaces a new factor * \param _feature_ptr pointer to the Feature to constrain * \param _landmark_ptr LandmarkBase pointer to the Landmark constrained. * diff --git a/include/core/processor/track_matrix.h b/include/core/processor/track_matrix.h index ca7e71d1a7de39fbb496a3f9eb00ab276815ed28..2615ad411adbe9c32efea2bf543cb24657e5ed97 100644 --- a/include/core/processor/track_matrix.h +++ b/include/core/processor/track_matrix.h @@ -63,12 +63,12 @@ typedef map<size_t, pair<FeatureBasePtr, FeatureBasePtr> > TrackMatches; // mat * * these fields of FeatureBase are initialized each time a feature is added to the track matrix: * - * add(Cap, track_id, f) will set f.capture_ptr = C and f.traci_id = traci_id. + * add(Cap, track_id, f) will set f.capture_ptr = C and f.track_id = track_id. * * so e.g. given a feature f, * * getTrack (f->trackId()) ; // returns all the track where feature f is. - * getSnapshot(f->getCapture()) ; // returns all the features in the same capture of f. + * getSnapshot(f->getCapture()) ; // returns all the features in the same capture of f. * */ @@ -98,15 +98,23 @@ class TrackMatrix FeatureBasePtr feature (const SizeStd& _track_id, CaptureBasePtr _cap); CaptureBasePtr firstCapture(const SizeStd& _track_id); + // tracks across captures that belong to keyframe +// SizeStd numKeyframeTracks(); + Track trackAtKeyframes(size_t _track_id); +// bool markKeyframe(CaptureBasePtr _capture); +// bool unmarkKeyframe(CaptureBasePtr _capture); + private: static SizeStd track_id_count_; - // Along track: maps of Feature pointers indexed by time stamp. + // tracks across all Captures map<SizeStd, Track > tracks_; // map indexed by track_Id of ( maps indexed by TimeStamp of ( features ) ) +// // tracks across captures that belong to keyframe +// map<size_t, Track > tracks_kf_; // map indexed by track_Id of ( maps indexed by TimeStamp of ( features ) ) // Across track: maps of Feature pointers indexed by track_Id. - map<SizeStd, Snapshot > snapshots_; // map indexed by capture_Id of ( maps indexed by track_Id of ( features ) ) + map<CaptureBasePtr, Snapshot > snapshots_; // map indexed by capture_ptr of ( maps indexed by track_Id of ( features ) ) }; } /* namespace wolf */ diff --git a/include/core/utils/converter.h b/include/core/utils/converter.h index 96e64575a653cef6f9f01d0c0d9b372e0540a170..ffb955d456f515aa0871a54eea4de84df94af187 100644 --- a/include/core/utils/converter.h +++ b/include/core/utils/converter.h @@ -8,11 +8,21 @@ #include <iostream> #include <array> +/** + @file + */ namespace utils{ + //Typically we want to convert from/to list-type structures. In order to be general + //we define a list type which is used throughout the converter. In this case this type + //is implemented with std::vector template <typename A> using list = std::vector<A>; // template <typename A> // class toString<A>{}; + /** @Brief Splits a comma-separated string into its pieces + * @param val is just the string of comma separated values + * @return <b>{std::vector<std::string>}</b> vector whose i-th component is the i-th comma separated value + */ static inline std::vector<std::string> splitter(std::string val){ std::vector<std::string> cont = std::vector<std::string>(); std::stringstream ss(val); @@ -22,6 +32,11 @@ namespace utils{ } return cont; } + /** @Brief Returns all the substrings of @val that match @exp + * @param val String to be matched + * @param exp Regular expression + * @return <b>{std::vector<std::string>}</b> Collection of matching subtrings + */ static inline std::vector<std::string> getMatches(std::string val, std::regex exp){ std::smatch res; auto v = std::vector<std::string>(); @@ -32,10 +47,10 @@ namespace utils{ } return v; } - static inline std::vector<std::string> pairSplitter(std::string val){ - std::regex exp("(\\{[^\\{:]:.*?\\})"); - return getMatches(val, exp); - } + /** @Brief Given a string representation of a matrix extracts the dimensions and the values + * @param matrix is a string either of the form "[[N,M],[a1,a2,a3,...]]" or "[a1,a2,a3,...]" + * @return <b>{std::array<std::string, 2>}</b> pair ([N,M],[a1,a2,a3,...]) or just ([a1,a2,a3...]) + */ static inline std::array<std::string,2> splitMatrixStringRepresentation(std::string matrix){ std::regex rgx("\\[\\[((?:[0-9]+,?)+)\\],((?:(?:[0-9]+\\.)?[0-9]+,?)+)\\]"); std::regex rgxStatic("\\[((?:(?:[0-9]+\\.)?[0-9]+,?)+)\\]"); @@ -52,10 +67,21 @@ namespace utils{ } return values; } + /** @Brief Splits a dictionary-like string of the form {k1:v1},{k2:v2},... It is tightly coupled with splitMapStringRepresentation + * @param val is just a dictionary-like string + * @return <b>{std::vector<std::string>}</b> Collection of the strings of the form {k_i:v_i} + */ + static inline std::vector<std::string> pairSplitter(std::string val){ + std::regex exp("\\{[^\\{:]:.*?\\}"); + return getMatches(val, exp); + } + /** @Brief Splits a dictionary-like string of the form [{k1:v1},{k2:v2},...] + * @param str_map just a dictionary-like string + * @return <b>{std::string}</b> String {k1:v1},{k2:v2},... (notice the removed brackets) + */ static inline std::string splitMapStringRepresentation(std::string str_map){ std::smatch mmatches; std::regex rgxM("\\[((?:(?:\\{[^\\{:]+:[^:\\}]+\\}),?)*)\\]"); - // auto v = std::vector<std::string>(); std::string result = ""; if(std::regex_match(str_map, mmatches, rgxM)) { // v = splitter(mmatches[1].str()); @@ -71,7 +97,7 @@ namespace wolf{ template<typename T> struct converter{ static T convert(std::string val){ - assert(1 == 0 && "There is no general convert for arbitrary T !!!"); + throw std::runtime_error("There is no general convert for arbitrary T !!! String provided: "+ val); } }; template<typename A> @@ -81,7 +107,7 @@ struct converter<utils::list<A>>{ utils::list<A> result = utils::list<A>(); if(std::regex_match(val, rgxP)) { std::string aux = val.substr(1,val.size()-2); - auto l = utils::getMatches(aux, std::regex("([^,]+),?")); + auto l = utils::getMatches(aux, std::regex("([^,]+)")); for(auto it : l){ result.push_back(converter<A>::convert(it)); } @@ -134,8 +160,7 @@ struct converter<std::string>{ } template<typename T> static std::string convert(T val){ - assert(1 == 0 && "There is no general convert to string for arbitrary T !!!"); - return ""; + throw std::runtime_error("There is no general convert to string for arbitrary T !!! String provided: " + val); } static std::string convert(int val){ return std::to_string(val); diff --git a/include/core/yaml/parser_yaml.hpp b/include/core/yaml/parser_yaml.hpp index 78ff43caa595ff4f8db37b0b860b3aef4ebf66fd..c721a163ac59abb487cfd01edbd3235f95d875d7 100644 --- a/include/core/yaml/parser_yaml.hpp +++ b/include/core/yaml/parser_yaml.hpp @@ -1,6 +1,7 @@ #ifndef PARSER_YAML_HPP #define PARSER_YAML_HPP #include "yaml-cpp/yaml.h" +#include "core/utils/converter.h" #include <vector> #include <regex> #include <map> @@ -8,17 +9,20 @@ #include <algorithm> #include <numeric> -using namespace std; namespace { // std::string remove_ws( const std::string& str ){ // std::string str_no_ws ; // for( char c : str ) if( !std::isspace(c) ) str_no_ws += c ; // return str_no_ws ; // } - string parseSequence(YAML::Node n){ + /** @Brief Generates a std::string [v1,v2,v3,...] representing the YAML sequence node + * @param n a YAML::Node + * @return <b>{std::string}</b> [v1,v2,v3,...] + */ + std::string parseSequence(YAML::Node n){ assert(n.Type() != YAML::NodeType::Map && "Trying to parse as a Sequence a Map node"); if(n.Type() == YAML::NodeType::Scalar) return n.Scalar(); - string aux = "["; + std::string aux = "["; bool first = true; for(auto it : n){ if(first) { @@ -31,15 +35,19 @@ namespace { aux = aux + "]"; return aux; } + /** @Brief Transforms a std::map<std::string,std::string> to its std::string representation [{k1:v1},{k2:v2},{k3:v3},...] + * @param _map just a std::map<std::string,std::string> + * @return <b>{std::string}</b> [{k1:v1},{k2:v2},{k3:v3},...] + */ std::string mapToString(std::map<std::string,std::string> _map){ std::string result = ""; - auto v = std::vector<string>(); - std::transform(_map.begin(), _map.end(), back_inserter(v), [](const std::pair<string,string> p){return "{" + p.first + ":" + p.second + "}";}); - auto concat = [](string ac, string str)-> string { + auto v = std::vector<std::string>(); + std::transform(_map.begin(), _map.end(), back_inserter(v), [](const std::pair<std::string,std::string> p){return "{" + p.first + ":" + p.second + "}";}); + auto concat = [](std::string ac, std::string str)-> std::string { return ac + str + ","; }; - string aux = ""; - string accumulated = std::accumulate(v.begin(), v.end(), aux, concat); + std::string aux = ""; + std::string accumulated = std::accumulate(v.begin(), v.end(), aux, concat); if(accumulated.size() > 1) accumulated = accumulated.substr(0,accumulated.size() - 1); else accumulated = ""; return "[" + accumulated + "]"; @@ -47,134 +55,147 @@ namespace { } class parserYAML { struct ParamsInitSensor{ - string _type; - string _name; + std::string _type; + std::string _name; YAML::Node n; }; struct ParamsInitProcessor{ - string _type; - string _name; - string _name_assoc_sensor; + std::string _type; + std::string _name; + std::string _name_assoc_sensor; YAML::Node n; }; - map<string, string> _params; - string _active_name; - vector<ParamsInitSensor> _paramsSens; - vector<ParamsInitProcessor> _paramsProc; - vector<string> _files; - string _file; + std::map<std::string, std::string> _params; + std::string _active_name; + std::vector<ParamsInitSensor> _paramsSens; + std::vector<ParamsInitProcessor> _paramsProc; + std::vector<std::string> _files; + std::string _file; bool _relative_path; - string _path_root; - vector<array<string, 3>> _callbacks; + std::string _path_root; + std::vector<std::array<std::string, 3>> _callbacks; + YAML::Node problem; + std::string generatePath(std::string); public: parserYAML(){ - _params = map<string, string>(); + _params = std::map<std::string, std::string>(); _active_name = ""; - _paramsSens = vector<ParamsInitSensor>(); - _paramsProc = vector<ParamsInitProcessor>(); + _paramsSens = std::vector<ParamsInitSensor>(); + _paramsProc = std::vector<ParamsInitProcessor>(); _file = ""; - _files = vector<string>(); + _files = std::vector<std::string>(); _path_root = ""; _relative_path = false; - _callbacks = vector<array<string, 3>>(); + _callbacks = std::vector<std::array<std::string, 3>>(); } - parserYAML(string file){ - _params = map<string, string>(); + parserYAML(std::string file){ + _params = std::map<std::string, std::string>(); _active_name = ""; - _paramsSens = vector<ParamsInitSensor>(); - _paramsProc = vector<ParamsInitProcessor>(); - _files = vector<string>(); + _paramsSens = std::vector<ParamsInitSensor>(); + _paramsProc = std::vector<ParamsInitProcessor>(); + _files = std::vector<std::string>(); _file = file; _path_root = ""; _relative_path = false; - _callbacks = vector<array<string, 3>>(); + _callbacks = std::vector<std::array<std::string, 3>>(); } - parserYAML(string file, string path_root){ - _params = map<string, string>(); + parserYAML(std::string file, std::string path_root){ + _params = std::map<std::string, std::string>(); _active_name = ""; - _paramsSens = vector<ParamsInitSensor>(); - _paramsProc = vector<ParamsInitProcessor>(); - _files = vector<string>(); + _paramsSens = std::vector<ParamsInitSensor>(); + _paramsProc = std::vector<ParamsInitProcessor>(); + _files = std::vector<std::string>(); _file = file; - _path_root = path_root; - _relative_path = true; - _callbacks = vector<array<string, 3>>(); + if(path_root != ""){ + std::regex r("/$"); + if(not std::regex_match(path_root, r)) _path_root = path_root + "/"; + else _path_root = path_root; + _relative_path = true; + }else{ + _relative_path = false; + } + _callbacks = std::vector<std::array<std::string, 3>>(); } ~parserYAML(){ // } - void walkTree(string file); - void walkTree(string file, vector<string>& tags); - void walkTree(string file, vector<string>& tags, string hdr); - void walkTreeR(YAML::Node n, vector<string>& tags, string hdr); - void updateActiveName(string tag); - void parseFirstLevel(string file); - string tagsToString(vector<string>& tags); - vector<array<string, 2>> sensorsSerialization(); - vector<array<string, 3>> processorsSerialization(); - vector<string> getFiles(); - vector<array<string, 3>> getCallbacks(); - map<string,string> getParams(); + void walkTree(std::string file); + void walkTree(std::string file, std::vector<std::string>& tags); + void walkTree(std::string file, std::vector<std::string>& tags, std::string hdr); + void walkTreeR(YAML::Node n, std::vector<std::string>& tags, std::string hdr); + void updateActiveName(std::string tag); + void parseFirstLevel(std::string file); + std::string tagsToString(std::vector<std::string>& tags); + std::vector<std::array<std::string, 2>> sensorsSerialization(); + std::vector<std::array<std::string, 3>> processorsSerialization(); + std::vector<std::string> getFiles(); + std::vector<std::array<std::string, 3>> getCallbacks(); + std::vector<std::array<std::string, 2>> getProblem(); + std::map<std::string,std::string> getParams(); void parse(); - map<string, string> fetchAsMap(YAML::Node); + std::map<std::string, std::string> fetchAsMap(YAML::Node); }; -string parserYAML::tagsToString(vector<std::string> &tags){ - string hdr = ""; +std::string parserYAML::generatePath(std::string path){ + std::regex r("^/.*"); + if(std::regex_match(path, r)){ + std::cout << "Generating " + path << std::endl; + return path; + }else{ + std::cout << "Generating " + _path_root + path << std::endl; + return _path_root + path; + } +} +std::string parserYAML::tagsToString(std::vector<std::string> &tags){ + std::string hdr = ""; for(auto it : tags){ hdr = hdr + "/" + it; } return hdr; } -void parserYAML::walkTree(string file){ +void parserYAML::walkTree(std::string file){ YAML::Node n; - // cout << "RELATIVE? " << _relative_path << " path root " << _path_root << " file " << file << endl; - if(not _relative_path) n = YAML::LoadFile(file); - else n = YAML::LoadFile(_path_root + file); - vector<string> hdrs = vector<string>(); + n = YAML::LoadFile(generatePath(file)); + std::vector<std::string> hdrs = std::vector<std::string>(); walkTreeR(n, hdrs, ""); } -void parserYAML::walkTree(string file, vector<string>& tags){ +void parserYAML::walkTree(std::string file, std::vector<std::string>& tags){ YAML::Node n; - // cout << "RELATIVE? " << _relative_path << " path root " << _path_root << " file " << file << endl; - if(not _relative_path) n = YAML::LoadFile(file); - else n = YAML::LoadFile(_path_root + file); + n = YAML::LoadFile(generatePath(file)); walkTreeR(n, tags, ""); } -void parserYAML::walkTree(string file, vector<string>& tags, string hdr){ +void parserYAML::walkTree(std::string file, std::vector<std::string>& tags, std::string hdr){ YAML::Node n; - // cout << "RELATIVE? " << _relative_path << " path root " << _path_root << " file " << file << endl; - if(not _relative_path) n = YAML::LoadFile(file); - else n = YAML::LoadFile(_path_root + file); + n = YAML::LoadFile(generatePath(file)); walkTreeR(n, tags, hdr); } -void parserYAML::walkTreeR(YAML::Node n, vector<string>& tags, string hdr){ +/** @Brief Recursively walks the YAML tree while filling a map with the values oarsed from the file +* @param YAML node to be parsed +* @param tags represents the path from the root of the YAML tree to the current node +* @param hdr is the name of the current YAML node +*/ +void parserYAML::walkTreeR(YAML::Node n, std::vector<std::string>& tags, std::string hdr){ switch (n.Type()) { case YAML::NodeType::Scalar : { - regex r("^@.*"); - if(regex_match(n.Scalar(), r)){ - string str = n.Scalar(); - // cout << "SUBSTR " << str.substr(1,str.size() - 1); + std::regex r("^@.*"); + if(std::regex_match(n.Scalar(), r)){ + std::string str = n.Scalar(); walkTree(str.substr(1,str.size() - 1), tags, hdr); }else{ - // std::copy(tags.begin(), tags.end(), std::ostream_iterator<string>(std::cout, "¬")); - // cout << "«»" << n.Scalar() << endl; - _params.insert(pair<string,string>(hdr, n.Scalar())); + _params.insert(std::pair<std::string,std::string>(hdr, n.Scalar())); } break; } case YAML::NodeType::Sequence : { - // cout << tags[tags.size() - 1] << "«»" << kv << endl; - // std::vector<double> vi = n.as<std::vector<double>>(); - string aux = parseSequence(n); - _params.insert(pair<string,string>(hdr, aux)); + std::string aux = parseSequence(n); + _params.insert(std::pair<std::string,std::string>(hdr, aux)); break; } case YAML::NodeType::Map : { for(const auto& kv : n){ //If the key's value starts with a $ (i.e. $key) then its value is parsed as a literal map, //otherwise the parser recursively parses the map. - regex r("^\\$.*"); - if(not regex_match(kv.first.as<string>(), r)){ + std::regex r("^\\$.*"); + if(not std::regex_match(kv.first.as<std::string>(), r)){ /* If key=="follow" then the parser will assume that the value is a path and will parse the (expected) yaml file at the specified path. Note that this does not increase the header depth. @@ -197,40 +218,43 @@ void parserYAML::walkTreeR(YAML::Node n, vector<string>& tags, string hdr){ - $follow: "@some_path" | - var: 1.2 | */ - regex rr("follow"); - if(not regex_match(kv.first.as<string>(), rr)) { - tags.push_back(kv.first.as<string>()); - if(tags.size() == 2) this->updateActiveName(kv.first.as<string>()); - walkTreeR(kv.second, tags, hdr +"/"+ kv.first.as<string>()); + std::regex rr("follow"); + if(not std::regex_match(kv.first.as<std::string>(), rr)) { + tags.push_back(kv.first.as<std::string>()); + if(tags.size() == 2) this->updateActiveName(kv.first.as<std::string>()); + walkTreeR(kv.second, tags, hdr +"/"+ kv.first.as<std::string>()); tags.pop_back(); if(tags.size() == 1) this->updateActiveName(""); }else{ - walkTree(kv.second.as<string>(), tags, hdr); + walkTree(kv.second.as<std::string>(), tags, hdr); } }else{ - string key = kv.first.as<string>(); + std::string key = kv.first.as<std::string>(); key = key.substr(1,key.size() - 1); auto fm = fetchAsMap(kv.second); - _params.insert(pair<string,string>(hdr + "/" + key, mapToString(fm))); + _params.insert(std::pair<std::string,std::string>(hdr + "/" + key, mapToString(fm))); } } break; } default: - assert(1 == 0 && "Unsupported node Type at walkTreeR"); + assert(1 == 0 && "Unsupported node Type at walkTreeR."); break; } } -void parserYAML::updateActiveName(string tag){ +void parserYAML::updateActiveName(std::string tag){ this->_active_name = tag; } -void parserYAML::parseFirstLevel(string file){ +/** @Brief Parse the sensors, processors, callbacks and files elements of the YAML file. We assume that these elements + are defined at the top level of the YAML file. + * @param file is the path to the YAML file */ +void parserYAML::parseFirstLevel(std::string file){ YAML::Node n; - // cout << "RELATIVE? " << _relative_path << " path root " << _path_root << " file " << file << endl; - if(not _relative_path) n = YAML::LoadFile(file); - else n = YAML::LoadFile(_path_root + file); + n = YAML::LoadFile(generatePath(file)); + YAML::Node n_config = n["config"]; assert(n_config.Type() == YAML::NodeType::Map && "trying to parse config node but found a non-Map node"); + this->problem = n_config["problem"]; for(const auto& kv : n_config["sensors"]){ ParamsInitSensor pSensor = {kv["type"].Scalar(), kv["name"].Scalar(), kv}; _paramsSens.push_back(pSensor); @@ -243,76 +267,80 @@ void parserYAML::parseFirstLevel(string file){ _callbacks.push_back({{kv[0].as<std::string>(), kv[1].as<std::string>(), kv[2].as<std::string>()}}); } YAML::Node n_files = n["files"]; - assert(n_files.Type() == YAML::NodeType::Sequence && "trying to parse files node but found a non-Sequence node"); for(const auto& kv : n_files){ _files.push_back(kv.Scalar()); } + _params.insert(std::pair<std::string,std::string>("files", wolf::converter<std::string>::convert(_files))); } -vector<array<string, 2>> parserYAML::sensorsSerialization(){ - vector<array<string, 2>> aux = vector<array<string, 2>>(); +std::vector<std::array<std::string, 2>> parserYAML::sensorsSerialization(){ + std::vector<std::array<std::string, 2>> aux = std::vector<std::array<std::string, 2>>(); for(auto it : this->_paramsSens) aux.push_back({{it._type,it._name}}); return aux; } -vector<array<string, 3>> parserYAML::processorsSerialization(){ - vector<array<string, 3>> aux = vector<array<string, 3>>(); +std::vector<std::array<std::string, 3>> parserYAML::processorsSerialization(){ + std::vector<std::array<std::string, 3>> aux = std::vector<std::array<std::string, 3>>(); for(auto it : this->_paramsProc) aux.push_back({{it._type,it._name,it._name_assoc_sensor}}); return aux; } -vector<string> parserYAML::getFiles(){ +std::vector<std::string> parserYAML::getFiles(){ return this->_files; } -vector<array<string, 3>> parserYAML::getCallbacks(){ +std::vector<std::array<std::string, 3>> parserYAML::getCallbacks(){ return this->_callbacks; } -map<string,string> parserYAML::getParams(){ - map<string,string> rtn = _params; +std::vector<std::array<std::string, 2>> parserYAML::getProblem(){ + return std::vector<std::array<std::string, 2>>(); +} +std::map<std::string,std::string> parserYAML::getParams(){ + std::map<std::string,std::string> rtn = _params; return rtn; } void parserYAML::parse(){ this->parseFirstLevel(this->_file); + + if(this->problem.Type() != YAML::NodeType::Undefined){ + std::vector<std::string> tags = std::vector<std::string>(); + this->walkTreeR(this->problem, tags , "problem"); + } for(auto it : _paramsSens){ - vector<string> tags = vector<string>(); - // this->walkTreeR(it.n , tags , it._type + "/" + it._name); + std::vector<std::string> tags = std::vector<std::string>(); this->walkTreeR(it.n , tags , it._name); } for(auto it : _paramsProc){ - vector<string> tags = vector<string>(); - // this->walkTreeR(it.n , tags , it._type + "/" + it._name); + std::vector<std::string> tags = std::vector<std::string>(); this->walkTreeR(it.n , tags , it._name); } } -map<string, string> parserYAML::fetchAsMap(YAML::Node n){ +std::map<std::string, std::string> parserYAML::fetchAsMap(YAML::Node n){ assert(n.Type() == YAML::NodeType::Map && "trying to fetch as Map a non-Map node"); - auto m = map<string, string>(); + auto m = std::map<std::string, std::string>(); for(const auto& kv : n){ - string key = kv.first.as<string>(); + std::string key = kv.first.as<std::string>(); switch (kv.second.Type()) { case YAML::NodeType::Scalar : { - string value = kv.second.Scalar(); - m.insert(pair<string,string>(key, value)); + std::string value = kv.second.Scalar(); + m.insert(std::pair<std::string,std::string>(key, value)); break; } case YAML::NodeType::Sequence : { - // cout << tags[tags.size() - 1] << "«»" << kv << endl; - // std::vector<double> vi = n.as<std::vector<double>>(); - string aux = parseSequence(kv.second); - m.insert(pair<string,string>(key, aux)); + std::string aux = parseSequence(kv.second); + m.insert(std::pair<std::string,std::string>(key, aux)); break; } case YAML::NodeType::Map : { m = fetchAsMap(kv.second); - auto rtn = vector<string>(); - std::transform(m.begin(), m.end(), back_inserter(rtn), [](const std::pair<string,string> v){return "{" + v.first + ":" + v.second + "}";}); - auto concat = [](string ac, string str)-> string { + auto rtn = std::vector<std::string>(); + std::transform(m.begin(), m.end(), back_inserter(rtn), [](const std::pair<std::string,std::string> v){return "{" + v.first + ":" + v.second + "}";}); + auto concat = [](std::string ac, std::string str)-> std::string { return ac + str + ","; }; - string aux = ""; - string accumulated = std::accumulate(rtn.begin(), rtn.end(), aux, concat); + std::string aux = ""; + std::string accumulated = std::accumulate(rtn.begin(), rtn.end(), aux, concat); if(accumulated.size() > 1) accumulated = accumulated.substr(0,accumulated.size() - 1); else accumulated = ""; - m.insert(pair<string,string>(key, "[" + accumulated + "]")); + m.insert(std::pair<std::string,std::string>(key, "[" + accumulated + "]")); break; } default: diff --git a/src/frame/frame_base.cpp b/src/frame/frame_base.cpp index 04bbaa38fff74496753765f6fd6628dc60d6b68b..0bdb038d95318698cccbd722e81c719c03d1d97b 100644 --- a/src/frame/frame_base.cpp +++ b/src/frame/frame_base.cpp @@ -155,6 +155,20 @@ void FrameBase::removeStateBlocks() } } +void FrameBase::setNonEstimated() +{ + // unregister if previously estimated + if (isKeyOrAux()) + removeStateBlocks(); + + type_ = NON_ESTIMATED; + if (getTrajectory()) + { + getTrajectory()->sortFrame(shared_from_this()); + getTrajectory()->updateLastFrames(); + } +} + void FrameBase::setKey() { // register if previously not estimated @@ -163,8 +177,11 @@ void FrameBase::setKey() // WOLF_DEBUG("Set Key", this->id()); type_ = KEY; - getTrajectory()->sortFrame(shared_from_this()); - getTrajectory()->updateLastFrames(); + if (getTrajectory()) + { + getTrajectory()->sortFrame(shared_from_this()); + getTrajectory()->updateLastFrames(); + } } void FrameBase::setAux() @@ -174,8 +191,11 @@ void FrameBase::setAux() // WOLF_DEBUG("Set Auxiliary", this->id()); type_ = AUXILIARY; - getTrajectory()->sortFrame(shared_from_this()); - getTrajectory()->updateLastFrames(); + if (getTrajectory()) + { + getTrajectory()->sortFrame(shared_from_this()); + getTrajectory()->updateLastFrames(); + } } void FrameBase::fix() diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp index 056db02dc4c5022e4bfb0b4d505c7533f71571da..cab9b32cd98474de4dc98e087c2f77cc005c2ea1 100644 --- a/src/problem/problem.cpp +++ b/src/problem/problem.cpp @@ -11,7 +11,6 @@ #include "core/sensor/sensor_factory.h" #include "core/processor/processor_factory.h" #include "core/state_block/state_block.h" -#include "core/yaml/parser_yaml.hpp" #include "core/utils/params_server.hpp" #include "core/utils/loader.hpp" @@ -20,6 +19,8 @@ // C++ includes #include <algorithm> +#include <map> +#include <vector> namespace wolf { @@ -72,34 +73,35 @@ ProblemPtr Problem::create(const std::string& _frame_structure, SizeEigen _dim) p->setup(); return p->shared_from_this(); } -ProblemPtr Problem::autoSetup(const std::string& _frame_structure, SizeEigen _dim, const std::string& _yaml_file) -{ - auto p = Problem::create(_frame_structure, _dim); - // string file = "/home/jcasals/catkin_ws/src/wolf_ros_wrapper/src/params.yaml"; - parserYAML parser = parserYAML(_yaml_file); - parser.parse(); - paramsServer server = paramsServer(parser.getParams(), parser.sensorsSerialization(), parser.processorsSerialization()); +ProblemPtr Problem::autoSetup(paramsServer &_server) +{ + // parserYAML parser = parserYAML(_yaml_file, _root_path); + // parser.parse(); + // paramsServer server = paramsServer(parser.getParams(), parser.sensorsSerialization(), parser.processorsSerialization()); + std::string frame_structure = _server.getParam<std::string>("problem/frame structure", "PO"); + int dim = _server.getParam<int>("problem/dimension", "2"); + auto p = Problem::create(frame_structure, dim); // cout << "PRINTING SERVER MAP" << endl; - // server.print(); + // _server.print(); // cout << "-----------------------------------" << endl; - auto loaders = vector<Loader*>(); - for(auto it : parser.getFiles()) { - cout << "LOADING " << it << endl; + auto loaders = std::vector<Loader*>(); + for(auto it : _server.getParam<std::vector<std::string>>("files")) { + std::cout << "LOADING " << it << std::endl; auto l = new LoaderRaw(it); l->load(); loaders.push_back(l); } //TODO: To be fixed. This prior should be set in here, but now it is set externally. // setPrior(Eigen::Vector3s::Zero(), 0.1*Eigen::Matrix3s::Identity(), TimeStamp(), Scalar(0.1)); - auto sensorMap = map<string, SensorBasePtr>(); - auto procesorMap = map<string, ProcessorBasePtr>(); - for(auto s : server.getSensors()){ - cout << s._name << " " << s._type << endl; - sensorMap.insert(pair<string, SensorBasePtr>(s._name,p->installSensor(s._type, s._name, server))); + auto sensorMap = std::map<std::string, SensorBasePtr>(); + auto procesorMap = std::map<std::string, ProcessorBasePtr>(); + for(auto s : _server.getSensors()){ + // cout << s._name << " " << s._type << endl; + sensorMap.insert(std::pair<std::string, SensorBasePtr>(s._name,p->installSensor(s._type, s._name, _server))); } - for(auto s : server.getProcessors()){ - cout << s._name << " " << s._type << " " << s._name_assoc_sensor << endl; - procesorMap.insert(pair<string, ProcessorBasePtr>(s._name,p->installProcessor(s._type, s._name, s._name_assoc_sensor, server))); + for(auto s : _server.getProcessors()){ + // cout << s._name << " " << s._type << " " << s._name_assoc_sensor << endl; + procesorMap.insert(std::pair<std::string, ProcessorBasePtr>(s._name,p->installProcessor(s._type, s._name, s._name_assoc_sensor, _server))); } return p; } diff --git a/src/processor/processor_base.cpp b/src/processor/processor_base.cpp index fb9c7f921c6869976b5d5ce01fb3ebb7ce5950a7..dd79d5b10033732892cf187a898f6a862b1f872d 100644 --- a/src/processor/processor_base.cpp +++ b/src/processor/processor_base.cpp @@ -54,7 +54,7 @@ void ProcessorBase::keyFrameCallback(FrameBasePtr _keyframe_ptr, const Scalar& _ { WOLF_DEBUG("P", isMotion() ? "M " : "T ", getName(), ": KF", _keyframe_ptr->id(), " callback received with ts = ", _keyframe_ptr->getTimeStamp()); if (_keyframe_ptr != nullptr) - kf_pack_buffer_.add(_keyframe_ptr,_time_tol_other); + buffer_pack_kf_.add(_keyframe_ptr,_time_tol_other); } void ProcessorBase::remove() @@ -96,25 +96,25 @@ void ProcessorBase::link(SensorBasePtr _sen_ptr) } ///////////////////////////////////////////////////////////////////////////////////////// -void PackKeyFrameBuffer::removeUpTo(const TimeStamp& _time_stamp) +void BufferPackKeyFrame::removeUpTo(const TimeStamp& _time_stamp) { - PackKeyFrameBuffer::Iterator post = container_.upper_bound(_time_stamp); + BufferPackKeyFrame::Iterator post = container_.upper_bound(_time_stamp); container_.erase(container_.begin(), post); // erasing by range } -void PackKeyFrameBuffer::add(const FrameBasePtr& _key_frame, const Scalar& _time_tolerance) +void BufferPackKeyFrame::add(const FrameBasePtr& _key_frame, const Scalar& _time_tolerance) { TimeStamp time_stamp = _key_frame->getTimeStamp(); PackKeyFramePtr kfpack = std::make_shared<PackKeyFrame>(_key_frame, _time_tolerance); container_.emplace(time_stamp, kfpack); } -PackKeyFramePtr PackKeyFrameBuffer::selectPack(const TimeStamp& _time_stamp, const Scalar& _time_tolerance) +PackKeyFramePtr BufferPackKeyFrame::selectPack(const TimeStamp& _time_stamp, const Scalar& _time_tolerance) { if (container_.empty()) return nullptr; - PackKeyFrameBuffer::Iterator post = container_.upper_bound(_time_stamp); + BufferPackKeyFrame::Iterator post = container_.upper_bound(_time_stamp); // remove packs corresponding to removed KFs (keeping the next iterator in post) while (post != container_.end() && post->second->key_frame->isRemoving()) @@ -129,7 +129,7 @@ PackKeyFramePtr PackKeyFrameBuffer::selectPack(const TimeStamp& _time_stamp, con if (prev_exists) { - PackKeyFrameBuffer::Iterator prev = std::prev(post); + BufferPackKeyFrame::Iterator prev = std::prev(post); bool prev_ok = checkTimeTolerance(prev->first, prev->second->time_tolerance, _time_stamp, _time_tolerance); @@ -152,12 +152,12 @@ PackKeyFramePtr PackKeyFrameBuffer::selectPack(const TimeStamp& _time_stamp, con return nullptr; } -PackKeyFramePtr PackKeyFrameBuffer::selectPack(const CaptureBasePtr _capture, const Scalar& _time_tolerance) +PackKeyFramePtr BufferPackKeyFrame::selectPack(const CaptureBasePtr _capture, const Scalar& _time_tolerance) { return selectPack(_capture->getTimeStamp(), _time_tolerance); } -PackKeyFramePtr PackKeyFrameBuffer::selectFirstPackBefore(const TimeStamp& _time_stamp, const Scalar& _time_tolerance) +PackKeyFramePtr BufferPackKeyFrame::selectFirstPackBefore(const TimeStamp& _time_stamp, const Scalar& _time_tolerance) { // remove packs corresponding to removed KFs while (!container_.empty() && container_.begin()->second->key_frame->isRemoving()) @@ -181,12 +181,12 @@ PackKeyFramePtr PackKeyFrameBuffer::selectFirstPackBefore(const TimeStamp& _time } -PackKeyFramePtr PackKeyFrameBuffer::selectFirstPackBefore(const CaptureBasePtr _capture, const Scalar& _time_tolerance) +PackKeyFramePtr BufferPackKeyFrame::selectFirstPackBefore(const CaptureBasePtr _capture, const Scalar& _time_tolerance) { return selectFirstPackBefore(_capture->getTimeStamp(), _time_tolerance); } -void PackKeyFrameBuffer::print(void) +void BufferPackKeyFrame::print(void) { std::cout << "[ "; for (auto iter : container_) @@ -196,7 +196,7 @@ void PackKeyFrameBuffer::print(void) std::cout << "]" << std::endl; } -bool PackKeyFrameBuffer::checkTimeTolerance(const TimeStamp& _time_stamp1, const Scalar& _time_tolerance1, +bool BufferPackKeyFrame::checkTimeTolerance(const TimeStamp& _time_stamp1, const Scalar& _time_tolerance1, const TimeStamp& _time_stamp2, const Scalar& _time_tolerance2) { Scalar time_diff = std::fabs(_time_stamp1 - _time_stamp2); diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp index a24cd7ed2bb9ff2336cbfe394bb32a61a245db93..0b646d71b9e670d8d81725b2a8e80b1f02187da8 100644 --- a/src/processor/processor_motion.cpp +++ b/src/processor/processor_motion.cpp @@ -92,7 +92,7 @@ void ProcessorMotion::process(CaptureBasePtr _incoming_ptr) PackKeyFramePtr pack = computeProcessingStep(); if (pack) - kf_pack_buffer_.removeUpTo( pack->key_frame->getTimeStamp() ); + buffer_pack_kf_.removeUpTo( pack->key_frame->getTimeStamp() ); switch(processing_step_) { @@ -656,11 +656,11 @@ PackKeyFramePtr ProcessorMotion::computeProcessingStep() throw std::runtime_error("ProcessorMotion received data before being initialized."); } - PackKeyFramePtr pack = kf_pack_buffer_.selectFirstPackBefore(last_ptr_, params_motion_->time_tolerance); + PackKeyFramePtr pack = buffer_pack_kf_.selectFirstPackBefore(last_ptr_, params_motion_->time_tolerance); if (pack) { - if (kf_pack_buffer_.checkTimeTolerance(pack->key_frame->getTimeStamp(), pack->time_tolerance, origin_ptr_->getTimeStamp(), params_motion_->time_tolerance)) + if (buffer_pack_kf_.checkTimeTolerance(pack->key_frame->getTimeStamp(), pack->time_tolerance, origin_ptr_->getTimeStamp(), params_motion_->time_tolerance)) { WOLF_WARN("||*||"); WOLF_INFO(" ... It seems you missed something!"); diff --git a/src/processor/processor_tracker.cpp b/src/processor/processor_tracker.cpp index 7acb2066ac89a2c1655f2de957c35d39b279e961..b1c67806c6ab4a7ff2a82f0c10a53ab6a3953adf 100644 --- a/src/processor/processor_tracker.cpp +++ b/src/processor/processor_tracker.cpp @@ -51,8 +51,8 @@ void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr) { case FIRST_TIME_WITH_PACK : { - PackKeyFramePtr pack = kf_pack_buffer_.selectPack( incoming_ptr_, params_tracker_->time_tolerance); - kf_pack_buffer_.removeUpTo( pack->key_frame->getTimeStamp() ); + PackKeyFramePtr pack = buffer_pack_kf_.selectPack( incoming_ptr_, params_tracker_->time_tolerance); + buffer_pack_kf_.removeUpTo( pack->key_frame->getTimeStamp() ); WOLF_DEBUG( "PT ", getName(), ": KF" , pack->key_frame->id() , " callback unpacked with ts= " , pack->key_frame->getTimeStamp().get() ); @@ -96,7 +96,7 @@ void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr) case SECOND_TIME_WITH_PACK : { // No-break case only for debug. Next case will be executed too. - PackKeyFramePtr pack = kf_pack_buffer_.selectPack( incoming_ptr_, params_tracker_->time_tolerance); + PackKeyFramePtr pack = buffer_pack_kf_.selectPack( incoming_ptr_, params_tracker_->time_tolerance); WOLF_DEBUG( "PT ", getName(), ": KF" , pack->key_frame->id() , " callback unpacked with ts= " , pack->key_frame->getTimeStamp().get() ); } // @suppress("No break at end of case") case SECOND_TIME_WITHOUT_PACK : @@ -123,8 +123,8 @@ void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr) } case RUNNING_WITH_PACK : { - PackKeyFramePtr pack = kf_pack_buffer_.selectPack( last_ptr_ , params_tracker_->time_tolerance); - kf_pack_buffer_.removeUpTo( pack->key_frame->getTimeStamp() ); + PackKeyFramePtr pack = buffer_pack_kf_.selectPack( last_ptr_ , params_tracker_->time_tolerance); + buffer_pack_kf_.removeUpTo( pack->key_frame->getTimeStamp() ); WOLF_DEBUG( "PT ", getName(), ": KF" , pack->key_frame->id() , " callback unpacked with ts= " , pack->key_frame->getTimeStamp().get() ); @@ -173,9 +173,6 @@ void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr) FrameBasePtr frm = getProblem()->emplaceFrame(NON_ESTIMATED, incoming_ptr_->getTimeStamp()); incoming_ptr_->link(frm); - // Set state to the keyframe - last_ptr_->getFrame()->setState(getProblem()->getState(last_ptr_->getTimeStamp())); - // Establish factors establishFactors(); @@ -202,9 +199,6 @@ void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr) FrameBasePtr frm = getProblem()->emplaceFrame(NON_ESTIMATED, incoming_ptr_->getTimeStamp()); frm->addCapture(incoming_ptr_); - // Set state to the keyframe - last_ptr_->getFrame()->setState(getProblem()->getState(last_ptr_->getTimeStamp())); - // Establish factors establishFactors(); @@ -261,7 +255,7 @@ void ProcessorTracker::computeProcessingStep() { case FIRST_TIME : - if (kf_pack_buffer_.selectPack(incoming_ptr_, params_tracker_->time_tolerance)) + if (buffer_pack_kf_.selectPack(incoming_ptr_, params_tracker_->time_tolerance)) processing_step_ = FIRST_TIME_WITH_PACK; else // ! last && ! pack(incoming) processing_step_ = FIRST_TIME_WITHOUT_PACK; @@ -269,7 +263,7 @@ void ProcessorTracker::computeProcessingStep() case SECOND_TIME : - if (kf_pack_buffer_.selectPack(last_ptr_, params_tracker_->time_tolerance)) + if (buffer_pack_kf_.selectPack(last_ptr_, params_tracker_->time_tolerance)) processing_step_ = SECOND_TIME_WITH_PACK; else processing_step_ = SECOND_TIME_WITHOUT_PACK; @@ -278,7 +272,7 @@ void ProcessorTracker::computeProcessingStep() case RUNNING : default : - if (kf_pack_buffer_.selectPack(last_ptr_, params_tracker_->time_tolerance)) + if (buffer_pack_kf_.selectPack(last_ptr_, params_tracker_->time_tolerance)) { if (last_ptr_->getFrame()->isKey()) { diff --git a/src/processor/track_matrix.cpp b/src/processor/track_matrix.cpp index e38d33f316d801f29e2f190932984c70e4216c83..b6c9cd176ffe3fd7468d60710c434ff29fe8e1db 100644 --- a/src/processor/track_matrix.cpp +++ b/src/processor/track_matrix.cpp @@ -32,8 +32,8 @@ Track TrackMatrix::track(const SizeStd& _track_id) Snapshot TrackMatrix::snapshot(CaptureBasePtr _capture) { - if (_capture && snapshots_.count(_capture->id()) > 0) - return snapshots_.at(_capture->id()); + if (_capture && snapshots_.count(_capture) > 0) + return snapshots_.at(_capture); else return Snapshot(); } @@ -51,7 +51,7 @@ void TrackMatrix::add(const SizeStd& _track_id, const FeatureBasePtr& _ftr) _ftr->setTrackId(_track_id); tracks_[_track_id].emplace(_ftr->getCapture()->getTimeStamp(), _ftr); - snapshots_[_ftr->getCapture()->id()].emplace(_track_id, _ftr); // will create new snapshot if _cap_id is not present + snapshots_[_ftr->getCapture()].emplace(_track_id, _ftr); // will create new snapshot if _cap_id is not present } void TrackMatrix::add(const FeatureBasePtr& _ftr_existing,const FeatureBasePtr& _ftr_new) @@ -66,10 +66,11 @@ void TrackMatrix::remove(const SizeStd& _track_id) { for (auto const& pair_time_ftr : tracks_.at(_track_id)) { - SizeStd cap_id = pair_time_ftr.second->getCapture()->id(); - snapshots_.at(cap_id).erase(_track_id); - if (snapshots_.at(cap_id).empty()) - snapshots_.erase(cap_id); + CaptureBasePtr cap = pair_time_ftr.second->getCapture(); + snapshots_.at(cap).erase(_track_id); + if (snapshots_.at(cap).empty()) + snapshots_.erase(cap); + } // Remove track @@ -80,10 +81,10 @@ void TrackMatrix::remove(const SizeStd& _track_id) void TrackMatrix::remove(CaptureBasePtr _cap) { // remove snapshot features from all tracks - if (snapshots_.count(_cap->id())) + if (snapshots_.count(_cap)) { TimeStamp ts = _cap->getTimeStamp(); - for (auto const& pair_trkid_ftr : snapshots_.at(_cap->id())) + for (auto const& pair_trkid_ftr : snapshots_.at(_cap)) { SizeStd trk_id = pair_trkid_ftr.first; tracks_.at(trk_id).erase(ts); @@ -92,7 +93,7 @@ void TrackMatrix::remove(CaptureBasePtr _cap) } // remove snapshot - snapshots_.erase(_cap->id()); + snapshots_.erase(_cap); } } @@ -103,13 +104,13 @@ void TrackMatrix::remove(FeatureBasePtr _ftr) { if (auto cap = _ftr->getCapture()) { - tracks_ .at(_ftr->trackId()).erase(cap->getTimeStamp()); + tracks_ .at(_ftr->trackId()).erase(cap->getTimeStamp()); if (tracks_.at(_ftr->trackId()).empty()) tracks_.erase(_ftr->trackId()); - snapshots_.at(cap->id()) .erase(_ftr->trackId()); - if (snapshots_.at(cap->id()).empty()) - snapshots_.erase(cap->id()); + snapshots_.at(cap) .erase(_ftr->trackId()); + if (snapshots_.at(cap).empty()) + snapshots_.erase(cap); } } } @@ -155,8 +156,8 @@ vector<FeatureBasePtr> TrackMatrix::trackAsVector(const SizeStd& _track_id) std::list<FeatureBasePtr> TrackMatrix::snapshotAsList(CaptureBasePtr _cap) { std::list<FeatureBasePtr> lst; - if (snapshots_.count(_cap->id())) - for (auto const& pair_trkid_ftr : snapshots_.at(_cap->id())) + if (snapshots_.count(_cap)) + for (auto const& pair_trkid_ftr : snapshots_.at(_cap)) lst.push_back(pair_trkid_ftr.second); return lst; } @@ -198,4 +199,23 @@ CaptureBasePtr TrackMatrix::firstCapture(const SizeStd& _track_id) return firstFeature(_track_id)->getCapture(); } +Track TrackMatrix::trackAtKeyframes(size_t _track_id) +{ + // We assemble a track_kf on the fly by checking each capture's frame. + if (tracks_.count(_track_id)) + { + Track track_kf; + for (auto& pair_ts_ftr : tracks_.at(_track_id)) + { + auto& ts = pair_ts_ftr.first; + auto& ftr = pair_ts_ftr.second; + if (ftr && ftr->getCapture() && ftr->getCapture()->getFrame() && ftr->getCapture()->getFrame()->isKey()) + track_kf[ts] = ftr; + } + return track_kf; + } + else + return Track(); +} + } diff --git a/src/sensor/sensor_base.cpp b/src/sensor/sensor_base.cpp index 4f1bba034ee2da85ec3b06aad5ec29984de023b5..1be0920fb059789e99b16f3a84d11b3a33b49699 100644 --- a/src/sensor/sensor_base.cpp +++ b/src/sensor/sensor_base.cpp @@ -410,7 +410,6 @@ void SensorBase::setProblem(ProblemPtr _problem) void SensorBase::link(HardwareBasePtr _hw_ptr) { assert(this->getHardware() == nullptr && "linking an already linked sensor"); - if(_hw_ptr) { this->setHardware(_hw_ptr); diff --git a/test/gtest_converter.cpp b/test/gtest_converter.cpp index b49f30a2d3e3f519dcc9e525aa8652830c809b51..d8378df4616f90af772fe967e06ba892ecb516b5 100644 --- a/test/gtest_converter.cpp +++ b/test/gtest_converter.cpp @@ -20,6 +20,13 @@ TEST(Converter, ParseToVector) ASSERT_EQ(v[8],11); vector<string> vs {"a","b","c"}; ASSERT_EQ(converter<string>::convert(vs), "[a,b,c]"); + string v2 = "[first,second,third,fourth]"; + vector<string> vv = converter<vector<string>>::convert(v2); + ASSERT_EQ(vv.size(),4); + ASSERT_EQ(vv[0],"first"); + ASSERT_EQ(vv[1],"second"); + ASSERT_EQ(vv[2],"third"); + ASSERT_EQ(vv[3],"fourth"); } TEST(Converter, ParseToEigenMatrix) @@ -42,7 +49,11 @@ TEST(Converter, ParseToMap) map<string, vector<int>> m = {{"x",vector<int>{1,2}}, {"y",vector<int>{}}, {"z",vector<int>{3}}}; ASSERT_EQ(converter<string>::convert(m), "[{x:[1,2]},{y:[]},{z:[3]}]"); } - +TEST(Converter, noGeneralConvert) +{ + class DUMMY{}; + EXPECT_THROW(([=]{converter<DUMMY>::convert("Should fail");}()), std::runtime_error); +} int main(int argc, char **argv) { testing::InitGoogleTest(&argc, argv); diff --git a/test/gtest_pack_KF_buffer.cpp b/test/gtest_pack_KF_buffer.cpp index 6597f0f53207c98759e882a96221a2ac0fcb00d0..1b1f1ffb61ded8028dfbe7814f6f813f76793334 100644 --- a/test/gtest_pack_KF_buffer.cpp +++ b/test/gtest_pack_KF_buffer.cpp @@ -26,7 +26,7 @@ class BufferPackKeyFrameTest : public testing::Test { public: - PackKeyFrameBuffer pack_kf_buffer; + BufferPackKeyFrame pack_kf_buffer; FrameBasePtr f10, f20, f21, f28; Scalar tt10, tt20, tt21, tt28; TimeStamp timestamp; diff --git a/test/gtest_param_server.cpp b/test/gtest_param_server.cpp index 3e83f8f89955a57342a0ec5247afb63303de69ce..dc5f5a5aa40962abba31b3e21ccdaa3aeb2d928a 100644 --- a/test/gtest_param_server.cpp +++ b/test/gtest_param_server.cpp @@ -18,7 +18,7 @@ parserYAML parse(string _file, string _path_root) TEST(ParamsServer, Default) { - auto parser = parse("/test/yaml/params1.yaml", wolf_root); + auto parser = parse("test/yaml/params1.yaml", wolf_root); auto params = parser.getParams(); paramsServer server = paramsServer(params, parser.sensorsSerialization(), parser.processorsSerialization()); EXPECT_EQ(server.getParam<double>("should_not_exist", "2.6"), 2.6); diff --git a/test/gtest_parser_yaml.cpp b/test/gtest_parser_yaml.cpp index 97451a6060ccd3adb43fd990c929c9cfee7b8dbc..d3ba3a40587e1e732cccf4d1a6c0d39e58a0207f 100644 --- a/test/gtest_parser_yaml.cpp +++ b/test/gtest_parser_yaml.cpp @@ -17,7 +17,7 @@ parserYAML parse(string _file, string _path_root) TEST(ParserYAML, RegularParse) { - auto parser = parse("/test/yaml/params1.yaml", wolf_root); + auto parser = parse("test/yaml/params1.yaml", wolf_root); auto params = parser.getParams(); // for(auto it : params) // cout << it.first << " %% " << it.second << endl; @@ -26,16 +26,23 @@ TEST(ParserYAML, RegularParse) } TEST(ParserYAML, ParseMap) { - auto parser = parse("/test/yaml/params2.yaml", wolf_root); + auto parser = parse("test/yaml/params2.yaml", wolf_root); auto params = parser.getParams(); EXPECT_EQ(params["processor1/mymap"], "[{k1:v1},{k2:v2},{k3:[v3,v4,v5]}]"); } TEST(ParserYAML, JumpFile) { - auto parser = parse("/test/yaml/params3.yaml", wolf_root); + auto parser = parse("test/yaml/params3.yaml", wolf_root); auto params = parser.getParams(); - EXPECT_EQ(params["my_proc_test/max_buff_length"], "100"); - EXPECT_EQ(params["my_proc_test/jump/voting_active"], "false"); + EXPECT_EQ(params["my_proc_test/extern params/max_buff_length"], "100"); + EXPECT_EQ(params["my_proc_test/extern params/voting_active"], "false"); +} +TEST(ParserYAML, ProblemConfig) +{ + auto parser = parse("test/yaml/params2.yaml", wolf_root); + auto params = parser.getParams(); + EXPECT_EQ(params["problem/frame structure"], "POV"); + EXPECT_EQ(params["problem/dimension"], "2"); } int main(int argc, char **argv) { diff --git a/test/gtest_track_matrix.cpp b/test/gtest_track_matrix.cpp index 83ffdbd5eb3a630e66d9d5914957ad83ffa0e308..8d7cafc5f699abc2f0fa9d275f57d5b4b3677821 100644 --- a/test/gtest_track_matrix.cpp +++ b/test/gtest_track_matrix.cpp @@ -19,6 +19,7 @@ class TrackMatrixTest : public testing::Test Eigen::Vector2s m; Eigen::Matrix2s m_cov = Eigen::Matrix2s::Identity()*0.01; + FrameBasePtr F0, F1, F2, F3, F4; CaptureBasePtr C0, C1, C2, C3, C4; FeatureBasePtr f0, f1, f2, f3, f4; @@ -32,6 +33,14 @@ class TrackMatrixTest : public testing::Test C3 = CaptureBase::emplace<CaptureBase>(nullptr, "BASE", 3.0); C4 = CaptureBase::emplace<CaptureBase>(nullptr, "BASE", 4.0); + // unlinked frames + // Some warnings will be thrown "linking with nullptr" for emplacing without providing frame pointer + F0 = FrameBase::emplace<FrameBase>(nullptr, 0.0, nullptr); + F1 = FrameBase::emplace<FrameBase>(nullptr, 1.0, nullptr); + F2 = FrameBase::emplace<FrameBase>(nullptr, 2.0, nullptr); + F3 = FrameBase::emplace<FrameBase>(nullptr, 3.0, nullptr); + F4 = FrameBase::emplace<FrameBase>(nullptr, 4.0, nullptr); + // unlinked features // Some warnings will be thrown "linking with nullptr" for emplacing without providing capture pointer f0 = FeatureBase::emplace<FeatureBase>(nullptr, "BASE", m, m_cov); @@ -39,6 +48,17 @@ class TrackMatrixTest : public testing::Test f2 = FeatureBase::emplace<FeatureBase>(nullptr, "BASE", m, m_cov); f3 = FeatureBase::emplace<FeatureBase>(nullptr, "BASE", m, m_cov); f4 = FeatureBase::emplace<FeatureBase>(nullptr, "BASE", m, m_cov); + + // F0 and F4 are keyframes + F0->setKey(); + F4->setKey(); + + // link captures + C0->link(F0); + C1->link(F1); + C2->link(F2); + C3->link(F3); + C4->link(F4); } }; @@ -46,12 +66,20 @@ TEST_F(TrackMatrixTest, newTrack) { f0->link(C0); f1->link(C1); + f2->link(C1); + f3->link(C1); track_matrix.newTrack(f0); ASSERT_EQ(track_matrix.numTracks(), (unsigned int) 1); track_matrix.newTrack(f1); ASSERT_EQ(track_matrix.numTracks(), (unsigned int) 2); + + track_matrix.newTrack(f2); + ASSERT_EQ(track_matrix.numTracks(), (unsigned int) 3); + + track_matrix.newTrack(f3); + ASSERT_EQ(track_matrix.numTracks(), (unsigned int) 4); } TEST_F(TrackMatrixTest, add) @@ -61,17 +89,19 @@ TEST_F(TrackMatrixTest, add) f2->link(C2); track_matrix.newTrack(f0); + ASSERT_EQ(track_matrix.numTracks(), (unsigned int) 1); track_matrix.add(f0->trackId(), f1); - /* C0 C1 C2 snapshots + /* KC0 C1 snapshots * * f0---f1 trk 0 */ ASSERT_EQ(track_matrix.trackSize(f1->trackId()), (unsigned int) 2); ASSERT_EQ(f1->trackId(), f0->trackId()); + ASSERT_EQ(track_matrix.numTracks(), (unsigned int) 1); track_matrix.add(f0->trackId(), f2); - /* C0 C1 C2 snapshots + /* KC0 C1 C2 snapshots * * f0---f1---f2 trk 0 */ @@ -84,11 +114,12 @@ TEST_F(TrackMatrixTest, add2) f0->link(C0); f1->link(C1); f2->link(C2); + f3->link(C1); track_matrix.newTrack(f0); track_matrix.add(f0, f1); - /* C0 C1 C2 snapshots + /* KC0 C1 snapshots * * f0---f1 trk 0 */ @@ -96,12 +127,23 @@ TEST_F(TrackMatrixTest, add2) ASSERT_EQ(f1->trackId(), f0->trackId()); track_matrix.add(f1, f2); - /* C0 C1 C2 snapshots + /* KC0 C1 C2 snapshots * * f0---f1---f2 trk 0 */ ASSERT_EQ(track_matrix.trackSize(f2->trackId()), (unsigned int) 3); ASSERT_EQ(f2->trackId(), f0->trackId()); + ASSERT_EQ(track_matrix.numTracks(), (unsigned int) 1); + + track_matrix.newTrack(f3); + /* KC0 C1 C2 snapshots + * + * f0---f1---f2 trk 0 + * f3 trk 1 + */ + ASSERT_EQ(track_matrix.trackSize(f3->trackId()), (unsigned int) 1); + ASSERT_NE(f3->trackId(), f0->trackId()); + ASSERT_EQ(track_matrix.numTracks(), (unsigned int) 2); } TEST_F(TrackMatrixTest, numTracks) @@ -205,8 +247,18 @@ TEST_F(TrackMatrixTest, remove_ftr) ASSERT_EQ(track_matrix.snapshot(C1).at(f0->trackId()), f1); track_matrix.remove(f1); + /* C0 C1 C2 snapshots + * + * f2 trk 1 + */ ASSERT_EQ(track_matrix.numTracks(), (unsigned int) 1); ASSERT_EQ(track_matrix.firstFeature(f2->trackId()), f2); + + track_matrix.remove(f2); + /* C0 C1 C2 snapshots + * + */ + ASSERT_EQ(track_matrix.numTracks(), (unsigned int) 0); } TEST_F(TrackMatrixTest, remove_trk) @@ -403,6 +455,36 @@ TEST_F(TrackMatrixTest, matches) ASSERT_EQ(pairs.size(), (unsigned int) 0); } +TEST_F(TrackMatrixTest, trackAtKeyframes) +{ + f0->link(C0); + f1->link(C1); + f2->link(C2); + f3->link(C1); + f4->link(C4); + + track_matrix.newTrack(f0); + track_matrix.add(f0->trackId(), f1); + track_matrix.add(f0->trackId(), f2); + track_matrix.add(f0->trackId(), f4); + track_matrix.newTrack(f3); + /* KC0 C1 C2 C3 KC4 snapshots + * + * f0---f1---f2--------f4 trk 0 + * | + * f3 trk 1 + */ + + wolf::Track trk_kf_0 = track_matrix.trackAtKeyframes(f0->trackId()); + ASSERT_EQ(trk_kf_0.size(), 2); + ASSERT_EQ(trk_kf_0[0.0], f0); + ASSERT_EQ(trk_kf_0[4.0], f4); + + wolf::Track trk_kf_1 = track_matrix.trackAtKeyframes(f3->trackId()); + ASSERT_EQ(trk_kf_1.size(), 0); +} + + int main(int argc, char **argv) { testing::InitGoogleTest(&argc, argv); diff --git a/test/params1.yaml b/test/params1.yaml deleted file mode 100644 index 4ad74198329b61d86b023f17f9a7016224d5489a..0000000000000000000000000000000000000000 --- a/test/params1.yaml +++ /dev/null @@ -1 +0,0 @@ - follow: "/test/yaml/params3.1.yaml" \ No newline at end of file diff --git a/test/params3.yaml b/test/params3.yaml deleted file mode 100644 index 78489d218fad394f90364a1e1d758daa0270e7cd..0000000000000000000000000000000000000000 --- a/test/params3.yaml +++ /dev/null @@ -1 +0,0 @@ - jump: "@/test/yaml/params3.1.yaml" \ No newline at end of file diff --git a/test/yaml/params1.yaml b/test/yaml/params1.yaml index 940d3e5854a5ec612f5223251243e643c7bef79b..d7d066b53a333d6beb27b20d7eef7468b29f505e 100644 --- a/test/yaml/params1.yaml +++ b/test/yaml/params1.yaml @@ -1,4 +1,7 @@ config: + problem: + frame structure: "POV" + dimension: 3 sensors: - type: "ODOM 2D" @@ -24,7 +27,7 @@ config: type: "ODOM 2D" name: "my_proc_test" sensor name: "odom" - follow: "/test/yaml/params3.1.yaml" + follow: "test/yaml/params3.1.yaml" files: - "/home/jcasals/workspace/wip/wolf/lib/libsensor_odo.so" - "/home/jcasals/workspace/wip/wolf/lib/librange_bearing.so" \ No newline at end of file diff --git a/test/yaml/params2.yaml b/test/yaml/params2.yaml index d58014fbabc36387be1a96cc4244cff954ac2820..7830e8cb85f9fef0acea46e81fa293ed7528fc5c 100644 --- a/test/yaml/params2.yaml +++ b/test/yaml/params2.yaml @@ -1,4 +1,7 @@ config: + problem: + frame structure: "POV" + dimension: 2 sensors: - type: "ODOM 2D" diff --git a/test/yaml/params3.yaml b/test/yaml/params3.yaml index ac82cf0dd818e6c6dec00c61ff86b75c38a3fc11..3e0d7a40f9fce04d9f6976518fe3bb900dabdbf9 100644 --- a/test/yaml/params3.yaml +++ b/test/yaml/params3.yaml @@ -1,31 +1,19 @@ config: - sensors: + # problem: + # frame structure: "POV" + # dimension: 2 + sensors: - type: "ODOM 2D" name: "odom" intrinsic: k_disp_to_disp: 0.1 - k_rot_to_rot: 0.1 + k_rot_to_rot: 0.1 extrinsic: pos: [1,2,3] - - - type: "RANGE BEARING" - name: "rb" processors: - - - type: "ODOM 2D" - name: "processor1" - sensor name: "odom" - - - type: "RANGE BEARING" - name: "rb_processor" - sensor name: "rb" - type: "ODOM 2D" name: "my_proc_test" sensor name: "odom" - follow: "/test/yaml/params3.1.yaml" - jump: "@/test/yaml/params3.1.yaml" -files: - - "/home/jcasals/workspace/wip/wolf/lib/libsensor_odo.so" - - "/home/jcasals/workspace/wip/wolf/lib/librange_bearing.so" \ No newline at end of file + extern params: "@test/yaml/params3.1.yaml" \ No newline at end of file