WIP

CMakeLists.txt

@@ -2,7 +2,7 @@
 CMAKE_MINIMUM_REQUIRED(VERSION 2.6)
 
 if(COMMAND cmake_policy)
-  cmake_policy(SET CMP0005 NEW) 
+  cmake_policy(SET CMP0005 NEW)
   cmake_policy(SET CMP0003 NEW)
 endif(COMMAND cmake_policy)
 # MAC OSX RPATH
@@ -207,8 +207,8 @@ IF (SPDLOG_INCLUDE_DIR)
   MESSAGE(STATUS "Found spdlog: ${SPDLOG_INCLUDE_DIR}")
 ELSE (SPDLOG_INCLUDE_DIR)
  MESSAGE(FATAL_ERROR "Could not find spdlog")
-ENDIF (SPDLOG_INCLUDE_DIR)  
-  
+ENDIF (SPDLOG_INCLUDE_DIR)
+
 INCLUDE_DIRECTORIES(${EIGEN_INCLUDE_DIRS})
 include_directories("include")
 include_directories(.)
@@ -289,6 +289,7 @@ SET(HDRS_BASE
   include/base/wolf.h
   include/base/IMU_tools.h
   include/base/local_parametrization_polyline_extreme.h
+  include/base/loader.hpp
   )
 SET(HDRS_CAPTURE
   include/base/capture/capture_base.h
@@ -440,7 +441,7 @@ SET(HDRS_DTASSC
   include/base/association/association_nnls.h
   )
 
-SET(HDRS_CORE 
+SET(HDRS_CORE
   include/base/capture/capture_base.h
   include/base/capture/capture_buffer.h
   include/base/capture/capture_pose.h
@@ -484,9 +485,11 @@ SET(HDRS_CORE
   include/base/trajectory_base.h
   include/base/wolf.h
   )
-
+SET(HDRS_YAML
+  include/base/yaml/parser_yaml.hpp
+  )
 #SOURCES
-SET(SRCS_CORE 
+SET(SRCS_CORE
   src/capture/capture_base.cpp
   src/capture/capture_pose.cpp
   src/capture/capture_void.cpp
@@ -586,13 +589,13 @@ IF (Ceres_FOUND)
    #ceres_wrapper/qr_manager.h
    include/base/ceres_wrapper/cost_function_wrapper.h
    include/base/ceres_wrapper/create_numeric_diff_cost_function.h
-   include/base/ceres_wrapper/local_parametrization_wrapper.h 
+   include/base/ceres_wrapper/local_parametrization_wrapper.h
    )
  SET(SRCS_WRAPPER
    src/solver/solver_manager.cpp
    src/ceres_wrapper/ceres_manager.cpp
    #ceres_wrapper/qr_manager.cpp
-   src/ceres_wrapper/local_parametrization_wrapper.cpp 
+   src/ceres_wrapper/local_parametrization_wrapper.cpp
    )
 ELSE(Ceres_FOUND)
  SET(HDRS_WRAPPER)
@@ -672,7 +675,7 @@ ADD_SUBDIRECTORY(serialization/cereal)
 ENDIF(cereal_FOUND)
 
 IF (Suitesparse_FOUND)
-  #DOES NOTHING?!    
+  #DOES NOTHING?!
   #ADD_SUBDIRECTORY(solver_suitesparse)
 ENDIF(Suitesparse_FOUND)
 # LEAVE YAML FILES ALWAYS IN THE LAST POSITION !!
@@ -684,7 +687,7 @@ IF(YAMLCPP_FOUND)
   SET(HDRS_YAML ${HDRS_YAML}
     include/base/yaml/yaml_conversion.h
     )
-  
+
   # sources
   SET(SRCS ${SRCS}
     src/yaml/processor_odom_3D_yaml.cpp
@@ -705,11 +708,11 @@ IF(YAMLCPP_FOUND)
       )
   ENDIF(vision_utils_FOUND)
 ENDIF(YAMLCPP_FOUND)
-    
+
 # create the shared library
-ADD_LIBRARY(${PROJECT_NAME} 
-  SHARED 
-  ${SRCS_BASE} 
+ADD_LIBRARY(${PROJECT_NAME}
+  SHARED
+  ${SRCS_BASE}
   ${SRCS_CORE}
   ${SRCS}
   ${SRCS_CAPTURE}
@@ -718,7 +721,7 @@ ADD_LIBRARY(${PROJECT_NAME}
   ${SRCS_LANDMARK}
   ${SRCS_PROCESSOR}
   ${SRCS_SENSOR}
-  #${SRCS_DTASSC} 
+  #${SRCS_DTASSC}
   ${SRCS_SOLVER}
   ${SRCS_WRAPPER}
   )
@@ -835,7 +838,7 @@ IF (UNIX)
     COMMENT "distribution clean"
     COMMAND make ARGS clean
     COMMAND rm ARGS -rf ${CMAKE_SOURCE_DIR}/build/*
-    
+
     TARGET  distclean
     )
 ELSE(UNIX)
@@ -875,4 +878,3 @@ ELSE(UNIX)
     COMMENT "packaging only implemented in unix"
     TARGET  uninstall)
 ENDIF(UNIX)
-

cmake_modules/Findwolf.cmake

@@ -4,7 +4,7 @@ FIND_PATH(
     # NAMES wolf.h
     PATHS /usr/local/include/iri-algorithms/wolf)
 #change INCLUDE_DIRS to its parent directory
-get_filename_component(wolf_INCLUDE_DIRS ${wolf_INCLUDE_DIRS} DIRECTORY)
+# get_filename_component(wolf_INCLUDE_DIRS ${wolf_INCLUDE_DIRS} DIRECTORY)
 MESSAGE("Found wolf include dirs: ${wolf_INCLUDE_DIRS}")
 
 FIND_LIBRARY(

cmake_modules/wolfConfig.cmake

@@ -156,7 +156,7 @@ list(APPEND EIGEN_INCLUDE_DIR_HINTS /usr/include/eigen3)
 # match and reject with an explanation below.
 
 find_package(Eigen3 ${wolf_EIGEN_VERSION} QUIET)
-
+message("FOUND EIGEN? "${EIGEN3_FOUND})
 # Flag set with currently found Eigen version.
 set(EIGEN_VERSION @EIGEN_VERSION@)
 if (EIGEN3_FOUND)

hello_plugin/CMakeLists.txt

@@ -2,13 +2,13 @@ INCLUDE_DIRECTORIES(${CMAKE_CURRENT_SOURCE_DIR})
 # INCLUDE_DIRECTORIES(/home/jcasals/workspace/wolf/src)
 # link_directories(/home/jcasals/workspace/wolf/lib)
 
-# ADD_EXECUTABLE(hello_plugin hello_plugin.cpp sensor_odom_2D.cpp processor_odom_2D.cpp)
 ADD_EXECUTABLE(hello_plugin hello_plugin.cpp)
 ADD_EXECUTABLE(params_autoconf params_autoconf.cpp)
 # target_link_libraries(hello_plugin class_loader boost_system console_bridge wolf yaml-cpp ${CERES_LIBRARIES})
 # target_link_libraries(params_autoconf class_loader boost_system console_bridge wolf yaml-cpp )
 target_link_libraries(hello_plugin wolf yaml-cpp ${CERES_LIBRARIES} dl)
 target_link_libraries(params_autoconf wolf yaml-cpp dl)
+
 # These lines always at the end
 SET(HDRS_PLUGIN ${HDRS_PLUGIN}   PARENT_SCOPE    )
 SET(SRCS_PLUGIN ${SRCS_PLUGIN}    PARENT_SCOPE    )

hello_plugin/hello_plugin.cpp

@@ -15,7 +15,7 @@
 #include "../hello_wolf/feature_range_bearing.h"
 #include "../hello_wolf/constraint_range_bearing.h"
 #include "../hello_wolf/landmark_point_2D.h"
-#include "loader.hpp"
+#include "base/loader.hpp"
 #include "base/processor/processor_odom_2D.h"
 
 #include "base/solver/solver_factory.h"

hello_plugin/params.yaml

@@ -26,5 +26,5 @@ config:
       sensorname: "odom"
       follow: "../hello_plugin/params_conf.yaml"
 files:
-  - "/home/jcasals/workspace/wip/wolf/lib/libsensor_odo.so"
+  - "/home/jcasals/workspace/wip/wolf/lib/libsensor_odom.so"
   - "/home/jcasals/workspace/wip/wolf/lib/librange_bearing.so"
\ No newline at end of file

hello_plugin/params_autoconf.cpp

@@ -15,7 +15,7 @@
 #include "../hello_wolf/feature_range_bearing.h"
 #include "../hello_wolf/constraint_range_bearing.h"
 #include "../hello_wolf/landmark_point_2D.h"
-#include "loader.hpp"
+#include "base/loader.hpp"
 #include "base/processor/processor_odom_2D.h"
 
 #include "base/solver/solver_factory.h"
@@ -42,6 +42,7 @@ int main(int argc, char** argv) {
     auto loaders = vector<Loader*>();
     for(auto it : parser.getFiles()) {
         auto l = new LoaderRaw(it);
+        l->load();
         loaders.push_back(l);
     }
     ProblemPtr problem = Problem::create("PO 2D");

hello_wolf/CMakeLists.txt

@@ -3,32 +3,31 @@ INCLUDE_DIRECTORIES(${CMAKE_CURRENT_SOURCE_DIR})
 # Forward var to parent scope
 
 SET(HDRS_PROCESSOR ${HDRS_PROCESSOR}
-    ${CMAKE_CURRENT_SOURCE_DIR}/capture_range_bearing.h 		
-    ${CMAKE_CURRENT_SOURCE_DIR}/constraint_bearing.h 			
-    ${CMAKE_CURRENT_SOURCE_DIR}/constraint_range_bearing.h 		
-    ${CMAKE_CURRENT_SOURCE_DIR}/feature_range_bearing.h 		
-    ${CMAKE_CURRENT_SOURCE_DIR}/landmark_point_2D.h 		
-    ${CMAKE_CURRENT_SOURCE_DIR}/processor_range_bearing.h 	
-    ${CMAKE_CURRENT_SOURCE_DIR}/sensor_range_bearing.h 		
+    ${CMAKE_CURRENT_SOURCE_DIR}/capture_range_bearing.h
+    ${CMAKE_CURRENT_SOURCE_DIR}/constraint_bearing.h
+    ${CMAKE_CURRENT_SOURCE_DIR}/constraint_range_bearing.h
+    ${CMAKE_CURRENT_SOURCE_DIR}/feature_range_bearing.h
+    ${CMAKE_CURRENT_SOURCE_DIR}/landmark_point_2D.h
+    ${CMAKE_CURRENT_SOURCE_DIR}/processor_range_bearing.h
+    ${CMAKE_CURRENT_SOURCE_DIR}/sensor_range_bearing.h
     )
 
 SET(SRCS_PROCESSOR ${SRCS_PROCESSOR}
-#    ${CMAKE_CURRENT_SOURCE_DIR}/hello_wolf.cpp            	
-    ${CMAKE_CURRENT_SOURCE_DIR}/capture_range_bearing.cpp 	
-    ${CMAKE_CURRENT_SOURCE_DIR}/feature_range_bearing.cpp 	
-    ${CMAKE_CURRENT_SOURCE_DIR}/landmark_point_2D.cpp 		
-    ${CMAKE_CURRENT_SOURCE_DIR}/processor_range_bearing.cpp 	
-    ${CMAKE_CURRENT_SOURCE_DIR}/sensor_range_bearing.cpp 		
+#    ${CMAKE_CURRENT_SOURCE_DIR}/hello_wolf.cpp
+    ${CMAKE_CURRENT_SOURCE_DIR}/capture_range_bearing.cpp
+    ${CMAKE_CURRENT_SOURCE_DIR}/feature_range_bearing.cpp
+    ${CMAKE_CURRENT_SOURCE_DIR}/landmark_point_2D.cpp
+    ${CMAKE_CURRENT_SOURCE_DIR}/processor_range_bearing.cpp
+    ${CMAKE_CURRENT_SOURCE_DIR}/sensor_range_bearing.cpp
     )
 
 ADD_EXECUTABLE(hello_wolf hello_wolf.cpp)
 TARGET_LINK_LIBRARIES(hello_wolf ${PROJECT_NAME})
-    
+add_library(sensor_odom SHARED ../src/sensor/sensor_odom_2D.cpp ../src/processor/processor_odom_2D.cpp)
+TARGET_LINK_LIBRARIES(sensor_odom ${PROJECT_NAME})
+add_library(range_bearing SHARED sensor_range_bearing.cpp processor_range_bearing.cpp)
+TARGET_LINK_LIBRARIES(range_bearing ${PROJECT_NAME})
 
-    
-    
 # These lines always at the end
 SET(HDRS_PROCESSOR ${HDRS_PROCESSOR}   PARENT_SCOPE    )
 SET(SRCS_PROCESSOR ${SRCS_PROCESSOR}    PARENT_SCOPE    )
-
-

include/base/yaml/parser_yaml.hpp

@@ -64,6 +64,7 @@ class parserYAML {
     string _file;
     bool _relative_path;
     string _path_root;
+    vector<array<string, 2>> _callbacks;
 public:
     parserYAML(){
         _params = map<string, string>();
@@ -74,6 +75,7 @@ public:
         _files = vector<string>();
         _path_root = "";
         _relative_path = false;
+        _callbacks = vector<array<string, 2>>();
     }
     parserYAML(string file){
         _params = map<string, string>();
@@ -84,6 +86,7 @@ public:
         _file = file;
         _path_root = "";
         _relative_path = false;
+        _callbacks = vector<array<string, 2>>();
     }
     parserYAML(string file, string path_root){
         _params = map<string, string>();
@@ -94,6 +97,7 @@ public:
         _file = file;
         _path_root = path_root;
         _relative_path = true;
+        _callbacks = vector<array<string, 2>>();
     }
     ~parserYAML(){
         //
@@ -108,6 +112,7 @@ public:
     vector<array<string, 2>> sensorsSerialization();
     vector<array<string, 3>> processorsSerialization();
     vector<string> getFiles();
+    vector<array<string, 2>> getCallbacks();
     map<string,string> getParams();
     void parse();
     map<string, string> fetchAsMap(YAML::Node);
@@ -211,6 +216,9 @@ void parserYAML::parseFirstLevel(string file){
         ParamsInitProcessor pProc = {kv["type"].Scalar(), kv["name"].Scalar(), kv["sensorname"].Scalar(), kv};
         _paramsProc.push_back(pProc);
     }
+    for(const auto& kv : n_config["callbacks"]){
+        _callbacks.push_back({{kv[0].as<std::string>(), kv[1].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){
@@ -232,6 +240,9 @@ vector<array<string, 3>> parserYAML::processorsSerialization(){
 vector<string> parserYAML::getFiles(){
     return this->_files;
 }
+vector<array<string, 2>> parserYAML::getCallbacks(){
+    return this->_callbacks;
+}
 map<string,string> parserYAML::getParams(){
     map<string,string> rtn = _params;
     return rtn;