Remove demo_map_yaml.cpp & fix yaml 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;
-}

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:

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

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

demos/hello_wolf/yaml/processor_range_bearing.yaml

@@ -5,3 +5,5 @@ time_tolerance:       0.1
 keyframe_vote:
   voting_active:      true
   voting_aux_active:  false
+
+apply_loss_function: true

demos/hello_wolf/yaml/sensor_odom_2d.yaml

@@ -2,3 +2,4 @@ type: "SensorOdom2d"              # This must match the KEY used in the SensorFa
 
 k_disp_to_disp:   0.1  # m^2   / m
 k_rot_to_rot:     0.1  # rad^2 / rad
+apply_loss_function: true

demos/hello_wolf/yaml/sensor_range_bearing.yaml

 type:                       "SensorRangeBearing"
 
 noise_range_metres_std:     0.1
-noise_bearing_degrees_std:  0.5  
+noise_bearing_degrees_std:  0.5
+apply_loss_function: true