Merge branch 'devel' into 298-params-and-factory-renaming

CMakeLists.txt

@@ -246,6 +246,7 @@ SET(HDRS_FACTOR
   include/core/factor/factor_pose_3d.h
   include/core/factor/factor_quaternion_absolute.h
   include/core/factor/factor_relative_2d_analytic.h
+  include/core/factor/factor_relative_pose_2d_with_extrinsics.h
   )
 SET(HDRS_FEATURE
   include/core/feature/feature_base.h
@@ -285,7 +286,11 @@ SET(HDRS_SOLVER
   include/core/solver/solver_manager.h
   include/core/solver/factory_solver.h
   )
-
+SET(HDRS_TREE_MANAGER
+  include/core/tree_manager/factory_tree_manager.h
+  include/core/tree_manager/tree_manager_base.h
+  include/core/tree_manager/tree_manager_sliding_window.h
+  )
 SET(HDRS_YAML
   include/core/yaml/parser_yaml.h
   include/core/yaml/yaml_conversion.h
@@ -371,6 +376,9 @@ SET(SRCS_SENSOR
 SET(SRCS_SOLVER
   src/solver/solver_manager.cpp
   )
+SET(SRCS_TREE_MANAGER
+  src/tree_manager/tree_manager_sliding_window.cpp
+  )
 SET(SRCS_YAML
   src/yaml/parser_yaml.cpp
   src/yaml/processor_odom_3d_yaml.cpp
@@ -400,8 +408,6 @@ ELSE(Ceres_FOUND)
  SET(SRCS_WRAPPER)
 ENDIF(Ceres_FOUND)
 
-#SUBDIRECTORIES
-add_subdirectory(hello_wolf)
 IF (cereal_FOUND)
 ADD_SUBDIRECTORY(serialization/cereal)
 ENDIF(cereal_FOUND)
@@ -431,6 +437,7 @@ ADD_LIBRARY(${PROJECT_NAME}
   ${SRCS_SOLVER}
   ${SRCS_STATE_BLOCK}
   ${SRCS_TRAJECTORY}
+  ${SRCS_TREE_MANAGER}
   ${SRCS_UTILS}
   ${SRCS_WRAPPER}
   ${SRCS_YAML}
@@ -471,44 +478,46 @@ INSTALL(TARGETS ${PROJECT_NAME} EXPORT ${PROJECT_NAME}Targets
 
 install(EXPORT ${PROJECT_NAME}Targets DESTINATION lib/cmake/${PROJECT_NAME})
 #install headers
-INSTALL(FILES ${HDRS_MATH}
-  DESTINATION include/iri-algorithms/wolf/plugin_core/core/math)
-INSTALL(FILES ${HDRS_UTILS}
-  DESTINATION include/iri-algorithms/wolf/plugin_core/core/utils)
-INSTALL(FILES ${HDRS_PROBLEM}
-  DESTINATION include/iri-algorithms/wolf/plugin_core/core/problem)
-INSTALL(FILES ${HDRS_HARDWARE}
-  DESTINATION include/iri-algorithms/wolf/plugin_core/core/hardware)
-INSTALL(FILES ${HDRS_TRAJECTORY}
-  DESTINATION include/iri-algorithms/wolf/plugin_core/core/trajectory)
-INSTALL(FILES ${HDRS_MAP}
-  DESTINATION include/iri-algorithms/wolf/plugin_core/core/map)
-INSTALL(FILES ${HDRS_FRAME}
-  DESTINATION include/iri-algorithms/wolf/plugin_core/core/frame)
-INSTALL(FILES ${HDRS_STATE_BLOCK}
-  DESTINATION include/iri-algorithms/wolf/plugin_core/core/state_block)
-INSTALL(FILES ${HDRS_COMMON}
-  DESTINATION include/iri-algorithms/wolf/plugin_core/core/common)
 INSTALL(FILES ${HDRS_CAPTURE}
    DESTINATION include/iri-algorithms/wolf/plugin_core/core/capture)
+INSTALL(FILES ${HDRS_COMMON}
+  DESTINATION include/iri-algorithms/wolf/plugin_core/core/common)
 INSTALL(FILES ${HDRS_FACTOR}
   DESTINATION include/iri-algorithms/wolf/plugin_core/core/factor)
 INSTALL(FILES ${HDRS_FEATURE}
   DESTINATION include/iri-algorithms/wolf/plugin_core/core/feature)
-INSTALL(FILES ${HDRS_SENSOR}
-  DESTINATION include/iri-algorithms/wolf/plugin_core/core/sensor)
-INSTALL(FILES ${HDRS_PROCESSOR}
-  DESTINATION include/iri-algorithms/wolf/plugin_core/core/processor)
+INSTALL(FILES ${HDRS_FRAME}
+  DESTINATION include/iri-algorithms/wolf/plugin_core/core/frame)
+INSTALL(FILES ${HDRS_HARDWARE}
+  DESTINATION include/iri-algorithms/wolf/plugin_core/core/hardware)
 INSTALL(FILES ${HDRS_LANDMARK}
   DESTINATION include/iri-algorithms/wolf/plugin_core/core/landmark)
-INSTALL(FILES ${HDRS_WRAPPER}
-  DESTINATION include/iri-algorithms/wolf/plugin_core/core/ceres_wrapper)
-INSTALL(FILES ${HDRS_SOLVER_SUITESPARSE}
-  DESTINATION include/iri-algorithms/wolf/plugin_core/core/solver_suitesparse)
-INSTALL(FILES ${HDRS_SOLVER}
-  DESTINATION include/iri-algorithms/wolf/plugin_core/core/solver)
+INSTALL(FILES ${HDRS_MAP}
+  DESTINATION include/iri-algorithms/wolf/plugin_core/core/map)
+INSTALL(FILES ${HDRS_MATH}
+  DESTINATION include/iri-algorithms/wolf/plugin_core/core/math)
+INSTALL(FILES ${HDRS_PROBLEM}
+  DESTINATION include/iri-algorithms/wolf/plugin_core/core/problem)
+INSTALL(FILES ${HDRS_PROCESSOR}
+  DESTINATION include/iri-algorithms/wolf/plugin_core/core/processor)
+INSTALL(FILES ${HDRS_SENSOR}
+  DESTINATION include/iri-algorithms/wolf/plugin_core/core/sensor)
 INSTALL(FILES ${HDRS_SERIALIZATION}
   DESTINATION include/iri-algorithms/wolf/plugin_core/core/serialization)
+INSTALL(FILES ${HDRS_SOLVER}
+  DESTINATION include/iri-algorithms/wolf/plugin_core/core/solver)
+INSTALL(FILES ${HDRS_SOLVER_SUITESPARSE}
+  DESTINATION include/iri-algorithms/wolf/plugin_core/core/solver_suitesparse)
+INSTALL(FILES ${HDRS_STATE_BLOCK}
+  DESTINATION include/iri-algorithms/wolf/plugin_core/core/state_block)
+INSTALL(FILES ${HDRS_TRAJECTORY}
+  DESTINATION include/iri-algorithms/wolf/plugin_core/core/trajectory)
+INSTALL(FILES ${HDRS_TREE_MANAGER}
+  DESTINATION include/iri-algorithms/wolf/plugin_core/core/tree_manager)
+INSTALL(FILES ${HDRS_UTILS}
+  DESTINATION include/iri-algorithms/wolf/plugin_core/core/utils)
+INSTALL(FILES ${HDRS_WRAPPER}
+  DESTINATION include/iri-algorithms/wolf/plugin_core/core/ceres_wrapper)
 INSTALL(FILES ${HDRS_YAML}
   DESTINATION include/iri-algorithms/wolf/plugin_core/core/yaml)
 

demos/CMakeLists.txt

+#Add hello_wolf demo
+add_subdirectory(hello_wolf)

demos/demo_map_yaml.cpp

-/**
- * \file test_map_yaml.cpp
- *
- *  Created on: Jul 27, 2016
- *      \author: jsola
- */
-
-#include "core/common/wolf.h"
-#include "core/problem/problem.h"
-#include "core/map/map_base.h"
-#include "core/landmark/landmark_polyline_2d.h"
-#include "core/landmark/landmark_AHP.h"
-#include "core/state_block/state_block.h"
-#include "core/yaml/yaml_conversion.h"
-
-#include <iostream>
-using namespace wolf;
-
-void print(MapBase& _map)
-{
-    for (auto lmk_ptr : _map.getLandmarkList())
-    {
-        std::cout << "Lmk ID:    " << lmk_ptr->id();
-        std::cout << "\nLmk type:  " << lmk_ptr->getType();
-        if (lmk_ptr->getType() == "POLYLINE 2d")
-        {
-            LandmarkPolyline2dPtr poly_ptr = std::static_pointer_cast<LandmarkPolyline2d>(lmk_ptr);
-            std::cout << "\npos:       " << poly_ptr->getP()->getState().transpose() << " -- fixed: " << poly_ptr->getP()->isFixed();
-            std::cout << "\nori:       " << poly_ptr->getO()->getState().transpose() << " -- fixed: " << poly_ptr->getO()->isFixed();
-            std::cout << "\nn points:  " << poly_ptr->getNPoints();
-            std::cout << "\nFirst idx: " << poly_ptr->getFirstId();
-            std::cout << "\nFirst def: " << poly_ptr->isFirstDefined();
-            std::cout << "\nLast  def: " << poly_ptr->isLastDefined();
-            for (int idx = poly_ptr->getFirstId(); idx <= poly_ptr->getLastId(); idx++)
-                std::cout << "\n  point: " << idx << ": " << poly_ptr->getPointStateBlock(idx)->getState().transpose();
-            break;
-        }
-        else if (lmk_ptr->getType() == "AHP")
-        {
-            LandmarkAHPPtr ahp_ptr = std::static_pointer_cast<LandmarkAHP>(lmk_ptr);
-            std::cout << "\npos:       " << ahp_ptr->getP()->getState().transpose() << " -- fixed: " << ahp_ptr->getP()->isFixed();
-            std::cout << "\ndescript:  " << ahp_ptr->getCvDescriptor().t();
-            break;
-        }
-        else
-            break;
-
-        std::cout << std::endl;
-    }
-}
-
-int main()
-{
-    using namespace Eigen;
-
-    std::cout << "\nTesting Lmk creator and node saving..." << std::endl;
-    Vector4d v;
-    v << 1, 2, 3, 4;
-    cv::Mat d = (cv::Mat_<int>(8,1) << 1, 2, 3, 4, 5, 6, 7, 8);
-    LandmarkAHP lmk_1(v, nullptr, nullptr, d);
-    std::cout << "Pos 1 = " << lmk_1.getP()->getState().transpose() << std::endl;
-    std::cout << "Des 1 = " << lmk_1.getCvDescriptor().t() << std::endl;
-
-    YAML::Node n = lmk_1.saveToYaml();
-    std::cout << "Pos n = " << n["position"].as<VectorXd>().transpose() << std::endl;
-    std::cout << "Des n = " << n["descriptor"].as<VectorXd>().transpose() << std::endl;
-
-    LandmarkAHP lmk_2 = *(std::static_pointer_cast<LandmarkAHP>(LandmarkAHP::create(n)));
-    std::cout << "Pos 2 = " << lmk_2.getP()->getState().transpose() << std::endl;
-    std::cout << "Des 2 = " << lmk_2.getCvDescriptor().t() << std::endl;
-
-    std::string filename;
-
-    std::string wolf_root       = _WOLF_ROOT_DIR;
-    std::string wolf_config     = wolf_root + "/src/examples";
-    std::cout << "\nWolf directory for configuration files: " << wolf_config << std::endl;
-
-    ProblemPtr problem = Problem::create("PO", 2);
-    filename = wolf_config + "/map_polyline_example.yaml";
-    std::cout << "Reading map from file: " << filename << std::endl;
-    problem->loadMap(filename);
-
-    std::cout << "Printing map..." << std::endl;
-    print(*(problem->getMap()));
-
-    filename = wolf_config + "/map_polyline_example_write.yaml";
-    std::cout << "Writing map to file: " << filename << std::endl;
-    std::string thisfile = __FILE__;
-    problem->getMap()->save(filename, "Example generated by test file " + thisfile);
-
-    std::cout << "Clearing map... " << std::endl;
-    problem->getMap()->getLandmarkList().clear();
-
-    std::cout << "Re-reading map from file: " << filename << std::endl;
-    problem->loadMap(filename);
-
-    std::cout << "Printing map..." << std::endl;
-    print(*(problem->getMap()));
-
-    return 0;
-}

hello_wolf/capture_range_bearing.cpp → demos/hello_wolf/capture_range_bearing.cpp

@@ -11,7 +11,7 @@ namespace wolf
 {
 
 CaptureRangeBearing::CaptureRangeBearing(const TimeStamp& _ts, const SensorBasePtr& _scanner, const Eigen::VectorXi& _ids, const Eigen::VectorXd& _ranges, const Eigen::VectorXd& _bearings) :
-        CaptureBase("RANGE BEARING", _ts, _scanner),
+        CaptureBase("CaptureRangeBearing", _ts, _scanner),
         ids_(_ids),
         ranges_(_ranges),
         bearings_(_bearings)

hello_wolf/feature_range_bearing.cpp → demos/hello_wolf/feature_range_bearing.cpp

@@ -11,7 +11,7 @@ namespace wolf
 {
 
 FeatureRangeBearing::FeatureRangeBearing(const Eigen::VectorXd& _measurement, const Eigen::MatrixXd& _meas_covariance) :
-        FeatureBase("RANGE BEARING", _measurement, _meas_covariance)
+        FeatureBase("FeatureRangeBearing", _measurement, _meas_covariance)
 {
     //
 }

hello_wolf/hello_wolf_autoconf.cpp → demos/hello_wolf/hello_wolf_autoconf.cpp

@@ -98,7 +98,7 @@ int main()
     WOLF_TRACE("======== CONFIGURE PROBLEM =======");
 
     // Config file to parse. Here is where all the problem is defined:
-    std::string config_file = "hello_wolf/yaml/hello_wolf_config.yaml";
+    std::string config_file = "demos/hello_wolf/yaml/hello_wolf_config.yaml";
     std::string wolf_path   = std::string(_WOLF_ROOT_DIR);
 
     // parse file into params server: each param will be retrievable from this params server:

hello_wolf/landmark_point_2d.cpp → demos/hello_wolf/landmark_point_2d.cpp

@@ -11,7 +11,7 @@ namespace wolf
 {
 
 LandmarkPoint2d::LandmarkPoint2d(int _id, const Eigen::Vector2d& _xy) :
-        LandmarkBase("POINT 2d", std::make_shared<StateBlock>(_xy))
+        LandmarkBase("LandmarkPoint2d", std::make_shared<StateBlock>(_xy))
 {
     setId(_id);
 }

hello_wolf/yaml/hello_wolf_config.yaml → demos/hello_wolf/yaml/hello_wolf_config.yaml

@@ -2,6 +2,11 @@ config:
 
   problem:
   
+    tree_manager:
+      type: "TreeManagerSlidingWindow"
+      n_key_frames: -1
+      fix_first_key_frame: false
+      viral_remove_empty_parent: true
     frame_structure:      "PO"            # keyframes have position and orientation
     dimension:            2               # space is 2d
     prior:
@@ -17,7 +22,7 @@ config:
       name:               "sen odom"
       extrinsic: 
         pose:             [0,0, 0]
-      follow:             "hello_wolf/yaml/sensor_odom_2d.yaml"         # config parameters in this file
+      follow:             "demos/hello_wolf/yaml/sensor_odom_2d.yaml"         # config parameters in this file
 
     - type:               "SensorRangeBearing"
       plugin:             "core"
@@ -25,7 +30,7 @@ config:
       extrinsic:
         pose:             [1,1, 0]
       noise_range_metres_std: 0.2                                       # parser only considers first appearence so the following file parsing will not overwrite this param
-      follow:             hello_wolf/yaml/sensor_range_bearing.yaml     # config parameters in this file
+      follow:             demos/hello_wolf/yaml/sensor_range_bearing.yaml     # config parameters in this file
           
   processors:
     
@@ -33,10 +38,10 @@ config:
       plugin:             "core"
       name:               "prc odom"
       sensor_name:        "sen odom"                                    # attach processor to this sensor
-      follow:             hello_wolf/yaml/processor_odom_2d.yaml        # config parameters in this file
+      follow:             demos/hello_wolf/yaml/processor_odom_2d.yaml        # config parameters in this file
     
     - type:               "ProcessorRangeBearing"
       plugin:             "core"
       name:               "prc rb"
       sensor_name:        "sen rb"                                      # attach processor to this sensor
-      follow:             hello_wolf/yaml/processor_range_bearing.yaml  # config parameters in this file
+      follow:             demos/hello_wolf/yaml/processor_range_bearing.yaml  # config parameters in this file

hello_wolf/yaml/processor_odom_2d.yaml → demos/hello_wolf/yaml/processor_odom_2d.yaml

@@ -10,3 +10,4 @@ keyframe_vote:
   angle_turned:       999
   max_buff_length:    999
   cov_det:            999
+apply_loss_function: true