diff --git a/demos/camera_Dinesh_LAAS_params.yaml b/demos/camera_Dinesh_LAAS_params.yaml
deleted file mode 100644
index 739505d12e8a2dd224b99220ee024ddc8efd9508..0000000000000000000000000000000000000000
--- a/demos/camera_Dinesh_LAAS_params.yaml
+++ /dev/null
@@ -1,20 +0,0 @@
-image_width: 640
-image_height: 480
-camera_name: mono
-camera_matrix:
-  rows: 3
-  cols: 3
-  data: [809.135074, 0.000000, 335.684471, 0.000000, 809.410030, 257.352121, 0.000000, 0.000000, 1.000000]
-distortion_model: plumb_bob
-distortion_coefficients:
-  rows: 1
-  cols: 5
-  data: [-0.416913, 0.264210, -0.000221, -0.000326, 0.000000]
-rectification_matrix:
-  rows: 3
-  cols: 3
-  data: [1.000000, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 1.000000]
-projection_matrix:
-  rows: 3
-  cols: 4
-  data: [753.818405, 0.000000, 337.203047, 0.000000, 0.000000, 777.118492, 258.102663, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000]
diff --git a/demos/camera_Dinesh_LAAS_params_notangentrect.yaml b/demos/camera_Dinesh_LAAS_params_notangentrect.yaml
deleted file mode 100644
index dd2f6433f2b60c21b68ebf70b595af981550923c..0000000000000000000000000000000000000000
--- a/demos/camera_Dinesh_LAAS_params_notangentrect.yaml
+++ /dev/null
@@ -1,20 +0,0 @@
-image_width: 640
-image_height: 480
-camera_name: mono
-camera_matrix:
-  rows: 3
-  cols: 3
-  data: [809.135074, 0.000000, 335.684471, 0.000000, 809.410030, 257.352121, 0.000000, 0.000000, 1.000000]
-distortion_model: plumb_bob
-distortion_coefficients:
-  rows: 1
-  cols: 5
-  data: [-0.416913, 0.264210, 0, 0, 0]
-rectification_matrix:
-  rows: 3
-  cols: 3
-  data: [1.000000, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 1.000000]
-projection_matrix:
-  rows: 3
-  cols: 4
-  data: [753.818405, 0.000000, 337.203047, 0.000000, 0.000000, 777.118492, 258.102663, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000]
diff --git a/demos/camera_logitech_c300_640_480.yaml b/demos/camera_logitech_c300_640_480.yaml
deleted file mode 100644
index 5f8a1899b71df3721e6f9722d39bd8e09e34509a..0000000000000000000000000000000000000000
--- a/demos/camera_logitech_c300_640_480.yaml
+++ /dev/null
@@ -1,22 +0,0 @@
-image_width: 640
-image_height: 480
-#camera_name: narrow_stereo
-camera_name: camera
-camera_matrix:
-  rows: 3
-  cols: 3
-  data: [711.687376, 0.000000, 323.306816, 0.000000, 710.933260, 232.005822, 0.000000, 0.000000, 1.000000]
-distortion_model: plumb_bob
-distortion_coefficients:
-  rows: 1
-  cols: 5
-  data: [0.067204, -0.141639, 0, 0, 0]
-#  data: [0.067204, -0.141639, 0.004462, -0.000636, 0.000000]
-rectification_matrix:
-  rows: 3
-  cols: 3
-  data: [1.000000, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 1.000000]
-projection_matrix:
-  rows: 3
-  cols: 4
-  data: [718.941772, 0.000000, 323.016804, 0.000000, 0.000000, 717.174622, 233.475721, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000]
diff --git a/demos/camera_params.yaml b/demos/camera_params.yaml
deleted file mode 100644
index de0c31d0b05a024724840598a490527a62002ca2..0000000000000000000000000000000000000000
--- a/demos/camera_params.yaml
+++ /dev/null
@@ -1,20 +0,0 @@
-image_width: 640
-image_height: 480
-camera_name: narrow_stereo
-camera_matrix:
-  rows: 3
-  cols: 3
-  data: [872.791604, 0, 407.599166, 0, 883.154343, 270.343971, 0, 0, 1]
-distortion_model: plumb_bob
-distortion_coefficients:
-  rows: 1
-  cols: 5
-  data: [-0.284384, -0.030014, -0.01554, -0.003363, 0]
-rectification_matrix:
-  rows: 3
-  cols: 3
-  data: [1, 0, 0, 0, 1, 0, 0, 0, 1]
-projection_matrix:
-  rows: 3
-  cols: 4
-  data: [826.19989, 0, 413.746093, 0, 0, 863.790649, 267.937027, 0, 0, 0, 1, 0]
\ No newline at end of file
diff --git a/demos/camera_params_1280x960_ideal.yaml b/demos/camera_params_1280x960_ideal.yaml
deleted file mode 100644
index 814d3cb3f74d51925e62389366ef0a24d429ee44..0000000000000000000000000000000000000000
--- a/demos/camera_params_1280x960_ideal.yaml
+++ /dev/null
@@ -1,20 +0,0 @@
-image_width: 1280
-image_height: 960
-camera_name: canonical
-camera_matrix:
-  rows: 3
-  cols: 3
-  data: [640, 0, 640, 0, 640, 480, 0, 0, 1]
-distortion_model: none
-distortion_coefficients:
-  rows: 1
-  cols: 5
-  data: [0, 0, 0, 0, 0]
-rectification_matrix:
-  rows: 3
-  cols: 3
-  data: [1, 0, 0, 0, 1, 0, 0, 0, 1]
-projection_matrix:
-  rows: 3
-  cols: 4
-  data: [640, 0, 640, 0, 0, 640, 480, 0, 0, 0, 1, 0]
\ No newline at end of file
diff --git a/demos/camera_params_canonical.yaml b/demos/camera_params_canonical.yaml
deleted file mode 100644
index 2508a0bec574125ae9dea63e558528b7211079d1..0000000000000000000000000000000000000000
--- a/demos/camera_params_canonical.yaml
+++ /dev/null
@@ -1,20 +0,0 @@
-image_width: 2
-image_height: 2
-camera_name: canonical
-camera_matrix:
-  rows: 3
-  cols: 3
-  data: [1, 0, 0, 0, 1, 0, 0, 0, 1]
-distortion_model: none
-distortion_coefficients:
-  rows: 1
-  cols: 5
-  data: [0, 0, 0, 0, 0]
-rectification_matrix:
-  rows: 3
-  cols: 3
-  data: [1, 0, 0, 0, 1, 0, 0, 0, 1]
-projection_matrix:
-  rows: 3
-  cols: 4
-  data: [1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0]
\ No newline at end of file
diff --git a/demos/camera_params_ueye.yaml b/demos/camera_params_ueye.yaml
deleted file mode 100644
index 8703fc4575d1422190e318fb7e0f8a816d37e5cb..0000000000000000000000000000000000000000
--- a/demos/camera_params_ueye.yaml
+++ /dev/null
@@ -1,20 +0,0 @@
-image_width: 640
-image_height: 480
-camera_name: camera
-camera_matrix:
-  rows: 3
-  cols: 3
-  data: [392.796383, 0, 350.175772, 0, 392.779002, 255.209917, 0, 0, 1]
-distortion_model: plumb_bob
-distortion_coefficients:
-  rows: 1
-  cols: 5
-  data: [-0.3, 0.096, 0, 0, 0]
-rectification_matrix:
-  rows: 3
-  cols: 3
-  data: [1, 0, 0, 0, 1, 0, 0, 0, 1]
-projection_matrix:
-  rows: 3
-  cols: 4
-  data: [313.834106, 0, 361.199457, 0, 0, 349.145264, 258.654166, 0, 0, 0, 1, 0]
diff --git a/demos/camera_params_ueye_radial_dist.yaml b/demos/camera_params_ueye_radial_dist.yaml
deleted file mode 100644
index 52d1fbb4dd01de04e6c759f0850015509235f4b4..0000000000000000000000000000000000000000
--- a/demos/camera_params_ueye_radial_dist.yaml
+++ /dev/null
@@ -1,20 +0,0 @@
-image_width: 640
-image_height: 480
-camera_name: narrow_stereo
-camera_matrix:
-  rows: 3
-  cols: 3
-  data: [402.860630, 0.000000, 350.628016, 0.000000, 403.220300, 269.746108, 0.000000, 0.000000, 1.000000]
-distortion_model: plumb_bob
-distortion_coefficients:
-  rows: 1
-  cols: 5
-  data: [-0.276945, 0.095681, 0.000000, 0.000000, -0.013371]
-rectification_matrix:
-  rows: 3
-  cols: 3
-  data: [1.000000, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 1.000000]
-projection_matrix:
-  rows: 3
-  cols: 4
-  data: [323.792358, 0.000000, 363.187868, 0.000000, 0.000000, 357.040344, 276.369440, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000]
diff --git a/demos/camera_params_ueye_sim.yaml b/demos/camera_params_ueye_sim.yaml
deleted file mode 100644
index 97f8f676b6b36629bb76a8bdc8c3fe06250b7b5b..0000000000000000000000000000000000000000
--- a/demos/camera_params_ueye_sim.yaml
+++ /dev/null
@@ -1,20 +0,0 @@
-image_width: 640
-image_height: 480
-camera_name: camera
-camera_matrix:
-  rows: 3
-  cols: 3
-  data: [320, 0, 320, 0, 320, 240, 0, 0, 1]
-distortion_model: plumb_bob
-distortion_coefficients:
-  rows: 1
-  cols: 5
-  data: [0, 0, 0, 0, 0]
-rectification_matrix:
-  rows: 3
-  cols: 3
-  data: [1, 0, 0, 0, 1, 0, 0, 0, 1]
-projection_matrix:
-  rows: 3
-  cols: 4
-  data: [320, 0, 320, 0, 0, 320, 240, 0, 0, 0, 1, 0]
diff --git a/demos/camera_params_vga_ideal.yaml b/demos/camera_params_vga_ideal.yaml
deleted file mode 100644
index c0c21118a4d69765f3d86471a7ff4003afc64dc4..0000000000000000000000000000000000000000
--- a/demos/camera_params_vga_ideal.yaml
+++ /dev/null
@@ -1,20 +0,0 @@
-image_width: 640
-image_height: 480
-camera_name: canonical
-camera_matrix:
-  rows: 3
-  cols: 3
-  data: [320, 0, 320, 0, 320, 240, 0, 0, 1]
-distortion_model: none
-distortion_coefficients:
-  rows: 1
-  cols: 5
-  data: [0, 0, 0, 0, 0]
-rectification_matrix:
-  rows: 3
-  cols: 3
-  data: [1, 0, 0, 0, 1, 0, 0, 0, 1]
-projection_matrix:
-  rows: 3
-  cols: 4
-  data: [320, 0, 320, 0, 0, 320, 240, 0, 0, 0, 1, 0]
\ No newline at end of file
diff --git a/demos/map_apriltag_1.yaml b/demos/map_apriltag_1.yaml
deleted file mode 100644
index d0c6a90707ddd2d15a2f921f244f085ecb337e6f..0000000000000000000000000000000000000000
--- a/demos/map_apriltag_1.yaml
+++ /dev/null
@@ -1,42 +0,0 @@
-map name: "Example of map of Apriltag landmarks"
-
-nlandmarks: 4          # This must match the number of landmarks in the list that follows. Otherwise it is an error.
-
-landmarks:
-
-  - id : 1                    # use same as tag id
-    type: "APRILTAG"          # This must match the KEY used in the LandmarkFactory. Otherwise it is an error.
-    tag id: 1
-    tag width: 0.1
-    position: [0, 0, 0]
-    orientation: [0, 0, 0]    # roll pitch yaw in degrees
-    position fixed: true
-    orientation fixed: true
-
-  - id : 3                    # use same as tag id
-    type: "APRILTAG"          # This must match the KEY used in the LandmarkFactory. Otherwise it is an error.
-    tag id: 3
-    tag width: 0.1
-    position: [1, 1, 0]
-    orientation: [0, 0, 0]    # roll pitch yaw in degrees
-    position fixed: true
-    orientation fixed: true
-
-  - id : 5                    # use same as tag id
-    type: "APRILTAG"          # This must match the KEY used in the LandmarkFactory. Otherwise it is an error.
-    tag id: 5
-    tag width: 0.1
-    position: [1, 0, 0]
-    orientation: [0, 0, 0]    # roll pitch yaw in degrees
-    position fixed: true
-    orientation fixed: true
-
-  - id : 2                    # use same as tag id
-    type: "APRILTAG"          # This must match the KEY used in the LandmarkFactory. Otherwise it is an error.
-    tag id: 2
-    tag width: 0.1
-    position: [0, 1, 0]
-    orientation: [0, 0, 0]    # roll pitch yaw in degrees
-    position fixed: true
-    orientation fixed: true
-
diff --git a/demos/map_polyline_example.yaml b/demos/map_polyline_example.yaml
deleted file mode 100644
index d1cde10c538cfa7e6f5b6a4b117309e36426528d..0000000000000000000000000000000000000000
--- a/demos/map_polyline_example.yaml
+++ /dev/null
@@ -1,57 +0,0 @@
-map name: "Example of map of Polyline2D landmarks"
-
-nlandmarks: 4          # This must match the number of landmarks in the list that follows. Otherwise it is an error.
-
-landmarks:
-
-  - id: 1
-    type: "POLYLINE 2D"   # This must match the KEY used in the LandmarkFactory. Otherwise it is an error.
-    position: [1, 1]
-    orientation: [1]
-    position fixed: true
-    orientation fixed: true
-    classification: 0
-    first_id: 0
-    first_defined: false
-    last_defined: false
-    points:
-        - [1, 1]
-        - [1, 2]
-        - [1, 3]
-
-  - id: 4
-    type: "POLYLINE 2D"   # This must match the KEY used in the LandmarkFactory. Otherwise it is an error.
-    position: [4, 4]
-    orientation: [4]
-    position fixed: true
-    orientation fixed: true
-    classification: 0
-    first_id: -2
-    first_defined: false
-    last_defined: true
-    points:
-        - [4, 1]
-        - [4, 2]
-        - [4, 3]
-        - [4, 4]
-
-        
-  - id: 6
-    type: "POLYLINE 2D"   # This must match the KEY used in the LandmarkFactory. Otherwise it is an error.
-    position: [6, 6]
-    orientation: [6]
-    position fixed: true
-    orientation fixed: true
-    classification: 0
-    first_id: 0
-    first_defined: true
-    last_defined: false
-    points:
-        - [6, 1]
-        - [6, 2]
-        
-  - id: 7
-    type: "AHP"
-    position: [1, 2, 3, 4]
-    descriptor: [1, 0, 1, 0, 1, 1, 0, 0]
-        
\ No newline at end of file
diff --git a/demos/processor_image_feature.yaml b/demos/processor_image_feature.yaml
deleted file mode 100644
index a48c9e4277b483a0f67e4f856f72c0076a2cc2be..0000000000000000000000000000000000000000
--- a/demos/processor_image_feature.yaml
+++ /dev/null
@@ -1,22 +0,0 @@
-processor type: "IMAGE ORB"
-processor name: "ORB feature tracker"
-
-vision_utils:
-    YAML file params: processor_image_vision_utils.yaml
-
-algorithm:
-    maximum new features: 40
-    minimum features for new keyframe: 40
-    minimum response for new features: 80 #0.0005
-    time tolerance: 0.01
-    distance: 20
-    
-noise:
-    pixel noise std: 1 # pixels
-    
-draw: # Used to control drawing options
-    primary draw: true
-    secondary draw: true
-    detection roi: true
-    tracking roi: false
-    
diff --git a/demos/processor_image_vision_utils.yaml b/demos/processor_image_vision_utils.yaml
deleted file mode 100644
index 028f69ba6321802009765d346f2e735c6a018990..0000000000000000000000000000000000000000
--- a/demos/processor_image_vision_utils.yaml
+++ /dev/null
@@ -1,42 +0,0 @@
-sensor:
-  type: "USB_CAM"
-
-detector:
-  type: "ORB"         
-  nfeatures: 100
-  scale factor: 1.2
-  nlevels: 8
-  edge threshold: 8   # 16
-  first level: 0 
-  WTA_K: 2            # See: http://docs.opencv.org/trunk/db/d95/classcv_1_1ORB.html#a180ae17d3300cf2c619aa240d9b607e5
-  score type: 1       #enum { kBytes = 32, HARRIS_SCORE=0, FAST_SCORE=1 };
-  patch size: 15      # 31
-
-descriptor:
-  type: "ORB"         
-  nfeatures: 100
-  scale factor: 1.2
-  nlevels: 8
-  edge threshold: 8   # 16
-  first level: 0 
-  WTA_K: 2            # See: http://docs.opencv.org/trunk/db/d95/classcv_1_1ORB.html#a180ae17d3300cf2c619aa240d9b607e5
-  score type: 1       #enum { kBytes = 32, HARRIS_SCORE=0, FAST_SCORE=1 };
-  patch size: 15      # 31
-
-matcher:
-  type: "FLANNBASED"
-  match type: 1     #  Match type. MATCH = 1, KNNMATCH = 2, RADIUSMATCH = 3
-  roi:
-    width: 20
-    height: 20
-  min normalized score: 0.85 
-    
-algorithm:
-  type: "ACTIVESEARCH"   
-  draw results: false
-  grid horiz cells: 12
-  grid vert cells: 8
-  separation: 10
-  min features to track: 5
-  max new features: 40
-  min response new features: 80
\ No newline at end of file
diff --git a/demos/processor_imu.yaml b/demos/processor_imu.yaml
deleted file mode 100644
index 8e8a6c8cd0b75c0366a7aa83db4f3d54e4687fa6..0000000000000000000000000000000000000000
--- a/demos/processor_imu.yaml
+++ /dev/null
@@ -1,11 +0,0 @@
-processor type: "IMU"              # This must match the KEY used in the SensorFactory. Otherwise it is an error.
-processor name: "Main imu"        # This is ignored. The name provided to the SensorFactory prevails
-unmeasured perturbation std: 0.00001
-time tolerance: 0.0025         # Time tolerance for joining KFs
-keyframe vote:
-    max time span:      2.0   # seconds
-    max buffer length:  20000    # motion deltas
-    dist traveled:      2.0   # meters
-    angle turned:       0.2   # radians (1 rad approx 57 deg, approx 60 deg)
-    voting_active:      false
-    
\ No newline at end of file
diff --git a/demos/processor_imu_no_vote.yaml b/demos/processor_imu_no_vote.yaml
deleted file mode 100644
index 678867b719b191b6ba980a5c712f5164a9f83e28..0000000000000000000000000000000000000000
--- a/demos/processor_imu_no_vote.yaml
+++ /dev/null
@@ -1,11 +0,0 @@
-processor type: "IMU"              # This must match the KEY used in the SensorFactory. Otherwise it is an error.
-processor name: "Main imu"        # This is ignored. The name provided to the SensorFactory prevails
-time tolerance: 0.0025         # Time tolerance for joining KFs
-unmeasured perturbation std: 0.00001
-keyframe vote:
-    max time span:      999999.0   # seconds
-    max buffer length:  999999     # motion deltas
-    dist traveled:      999999.0   # meters
-    angle turned:       999999     # radians (1 rad approx 57 deg, approx 60 deg)
-    voting_active:      false
-    
\ No newline at end of file
diff --git a/demos/processor_imu_t1.yaml b/demos/processor_imu_t1.yaml
deleted file mode 100644
index b68740d245b4922a4a095b2a0ac1b2ce5becbd52..0000000000000000000000000000000000000000
--- a/demos/processor_imu_t1.yaml
+++ /dev/null
@@ -1,11 +0,0 @@
-processor type: "IMU"              # This must match the KEY used in the SensorFactory. Otherwise it is an error.
-processor name: "Main imu"        # This is ignored. The name provided to the SensorFactory prevails
-unmeasured perturbation std: 0.00001
-time tolerance: 0.0025         # Time tolerance for joining KFs
-keyframe vote:
-    max time span:      0.9999   # seconds
-    max buffer length:  10000    # motion deltas
-    dist traveled:      10000   # meters
-    angle turned:       10000   # radians (1 rad approx 57 deg, approx 60 deg)
-    voting_active:      true
-    
\ No newline at end of file
diff --git a/demos/processor_imu_t6.yaml b/demos/processor_imu_t6.yaml
deleted file mode 100644
index 511a917c7500abbb445c7bfb016737c881dc400a..0000000000000000000000000000000000000000
--- a/demos/processor_imu_t6.yaml
+++ /dev/null
@@ -1,11 +0,0 @@
-processor type: "IMU"              # This must match the KEY used in the SensorFactory. Otherwise it is an error.
-processor name: "Main imu"        # This is ignored. The name provided to the SensorFactory prevails
-unmeasured perturbation std: 0.00001
-time tolerance: 0.0025         # Time tolerance for joining KFs
-keyframe vote:
-    max time span:      5.9999   # seconds
-    max buffer length:  10000    # motion deltas
-    dist traveled:      10000   # meters
-    angle turned:       10000   # radians (1 rad approx 57 deg, approx 60 deg)
-    voting_active:      true
-    
\ No newline at end of file
diff --git a/demos/processor_odom_3D.yaml b/demos/processor_odom_3D.yaml
deleted file mode 100644
index f501e333800ec1bbb4b7c751a32aa67a99bec74c..0000000000000000000000000000000000000000
--- a/demos/processor_odom_3D.yaml
+++ /dev/null
@@ -1,8 +0,0 @@
-processor type: "ODOM 3D"              # This must match the KEY used in the SensorFactory. Otherwise it is an error.
-processor name: "Main odometer"        # This is ignored. The name provided to the SensorFactory prevails
-time tolerance:         0.01  # seconds
-keyframe vote:
-    max time span:      0.2   # seconds
-    max buffer length:  10    # motion deltas
-    dist traveled:      0.5   # meters
-    angle turned:       0.1   # radians (1 rad approx 57 deg, approx 60 deg)
\ No newline at end of file
diff --git a/demos/processor_tracker_landmark_apriltag.yaml b/demos/processor_tracker_landmark_apriltag.yaml
deleted file mode 100644
index e8b1fd02dc80ffedefafc44ae3af9898a873cb3b..0000000000000000000000000000000000000000
--- a/demos/processor_tracker_landmark_apriltag.yaml
+++ /dev/null
@@ -1,57 +0,0 @@
-processor type: "TRACKER LANDMARK APRILTAG"
-processor name: "tracker landmark apriltag example"
-
-detector parameters: 
-    quad_decimate:  1.5      # doing quad detection at lower resolution to speed things up (see end of file)
-    quad_sigma:     0.8	     # gaussian blur good for noisy images, may be recommended with quad_decimate. Kernel size adapted (see end of this file) 
-    nthreads:       8       # how many thread during tag detection (does not change much?)
-    debug:          false    # write some debugging images
-    refine_edges:   true    # better edge detection if quad_decimate > 1 (quite inexpensive, almost no diff)
-    ippe_min_ratio:     3.0  	# quite arbitrary, always > 1 (to deactive, set at 0 for example)
-    ippe_max_rep_error: 2.0     # to deactivate, set at something big (100)
-
-tag widths:                    
-    0: 0.055
-    1: 0.055
-    2: 0.055
-    3: 0.055
-
-tag parameters:
-    tag_family:           "tag36h11" 
-    # tag_black_border:     1
-    tag_width_default:    0.165   # used if tag width not specified
-
-    
-noise:
-    std_xy :          0.1 # m 
-    std_z :           0.1 # m 
-    std_rpy_degrees : 5   # degrees
-    std_pix:          20    # pixel error
-    
-vote:
-    voting active:              true
-    min_time_vote:              0 # s
-    max_time_vote:              0 # s
-    min_features_for_keyframe:  12
-    max_features_diff:          17
-    nb_vote_for_every_first:    50
-    enough_info_necessary:      true   # create kf if enough information to uniquely determine the KF position (necessary for apriltag_only slam)
-
-reestimate_last_frame: true   # for a better prior on the new keyframe: use only if no motion processor
-add_3D_cstr: true             # add 3D constraints between the KF so that they do not jump when using apriltag only
-
-
-# Annexes:
-### Quad decimate: the higher, the lower the resolution
-# Does nothing if <= 1.0
-# Only values taken into account:
-# 1.5, 2, 3, 4
-# 1.5 -> ~*2 speed up
-
-# Higher values use a "bad" code according to commentaries in the library, smaller do nothing
-### Gaussian blur table:
-# max sigma          kernel size
-# 0.499              1  (disabled)
-# 0.999              3
-# 1.499              5
-# 1.999              7
diff --git a/demos/sensor_imu.yaml b/demos/sensor_imu.yaml
deleted file mode 100644
index 66b81a5288877362753612f7aa4b9222da009e8d..0000000000000000000000000000000000000000
--- a/demos/sensor_imu.yaml
+++ /dev/null
@@ -1,9 +0,0 @@
-sensor type: "IMU"             # This must match the KEY used in the SensorFactory. Otherwise it is an error.
-sensor name: "Main IMU"        # This is ignored. The name provided to the SensorFactory prevails
-motion variances: 
-    a_noise:                0.053     # standard deviation of Acceleration noise (same for all the axis) in m/s2
-    w_noise:                0.0011    # standard deviation of Gyroscope noise (same for all the axis) in rad/sec
-    ab_initial_stdev:       0.800     # m/s2    - initial bias 
-    wb_initial_stdev:       0.350     # rad/sec - initial bias 
-    ab_rate_stdev:          0.1       # m/s2/sqrt(s)           
-    wb_rate_stdev:          0.0400    # rad/s/sqrt(s)
\ No newline at end of file
diff --git a/demos/sensor_odom_3D.yaml b/demos/sensor_odom_3D.yaml
deleted file mode 100644
index 9ea77803548cfd9d033f54e40b6d92a72710afb8..0000000000000000000000000000000000000000
--- a/demos/sensor_odom_3D.yaml
+++ /dev/null
@@ -1,8 +0,0 @@
-sensor type: "ODOM 3D"              # This must match the KEY used in the SensorFactory. Otherwise it is an error.
-sensor name: "Main odometer"        # This is ignored. The name provided to the SensorFactory prevails
-motion variances: 
-    disp_to_disp:   0.02  # m^2   / m
-    disp_to_rot:    0.02  # rad^2 / m
-    rot_to_rot:     0.01  # rad^2 / rad
-    min_disp_var:   0.01  # m^2
-    min_rot_var:    0.01  # rad^2
diff --git a/demos/sensor_odom_3D_HQ.yaml b/demos/sensor_odom_3D_HQ.yaml
deleted file mode 100644
index 08945ef842e46f28642f1be63ca95850b618a35e..0000000000000000000000000000000000000000
--- a/demos/sensor_odom_3D_HQ.yaml
+++ /dev/null
@@ -1,8 +0,0 @@
-sensor type: "ODOM 3D"              # This must match the KEY used in the SensorFactory. Otherwise it is an error.
-sensor name: "Main odometer"        # This is ignored. The name provided to the SensorFactory prevails
-motion variances: 
-    disp_to_disp:   0.000001  # m^2   / m
-    disp_to_rot:    0.000001  # rad^2 / m
-    rot_to_rot:     0.000001  # rad^2 / rad
-    min_disp_var:   0.00000001  # m^2
-    min_rot_var:    0.00000001  # rad^2
\ No newline at end of file
diff --git a/test/gtest_processor_tracker_landmark_apriltag.cpp b/test/gtest_processor_tracker_landmark_apriltag.cpp
deleted file mode 100644
index cd5cc87dc4d4c6c270eb9d189a8e380b8361f39e..0000000000000000000000000000000000000000
--- a/test/gtest_processor_tracker_landmark_apriltag.cpp
+++ /dev/null
@@ -1,434 +0,0 @@
-#include <core/utils/utils_gtest.h>
-
-#include "base/common/wolf.h"
-#include "base/utils/logging.h"
-
-#include "base/processor/processor_tracker_landmark_apriltag.h"
-#include "base/feature/feature_apriltag.h"
-#include "base/landmark/landmark_apriltag.h"
-#include "base/capture/capture_pose.h"
-#include "base/processor/processor_factory.h"
-
-using namespace Eigen;
-using namespace wolf;
-using std::static_pointer_cast;
-
-
-////////////////////////////////////////////////////////////////
-/*
- * Wrapper class to be able to have setOriginPtr() and setLastPtr() in ProcessorTrackerLandmarkApriltag
- */
-WOLF_PTR_TYPEDEFS(ProcessorTrackerLandmarkApriltag_Wrapper);
-class ProcessorTrackerLandmarkApriltag_Wrapper : public ProcessorTrackerLandmarkApriltag
-{
-    public:
-        ProcessorTrackerLandmarkApriltag_Wrapper(ProcessorParamsTrackerLandmarkApriltagPtr _params_tracker_landmark_apriltag) :
-            ProcessorTrackerLandmarkApriltag(_params_tracker_landmark_apriltag)
-        {
-            setType("TRACKER LANDMARK APRILTAG WRAPPER");
-        };
-        ~ProcessorTrackerLandmarkApriltag_Wrapper(){}
-        void setOriginPtr(const CaptureBasePtr _origin_ptr) { origin_ptr_ = _origin_ptr; }
-        void setLastPtr  (const CaptureBasePtr _last_ptr)   { last_ptr_ = _last_ptr; }
-        void setIncomingPtr  (const CaptureBasePtr _incoming_ptr)   { incoming_ptr_ = _incoming_ptr; }
-        unsigned int getMinFeaturesForKeyframe (){return min_features_for_keyframe_;}
-        Scalar getMinTimeVote (){return min_time_vote_;}
-        void setIncomingDetections(const FeatureBasePtrList _incoming_detections) { detections_incoming_ = _incoming_detections; }
-        void setLastDetections(const FeatureBasePtrList _last_detections) { detections_last_ = _last_detections; }
-
-        // for factory
-        static ProcessorBasePtr create(const std::string& _unique_name, const ProcessorParamsBasePtr _params, const SensorBasePtr sensor_ptr = nullptr)
-        {
-            std::shared_ptr<ProcessorParamsTrackerLandmarkApriltag> prc_apriltag_params_;
-            if (_params)
-                prc_apriltag_params_ = std::static_pointer_cast<ProcessorParamsTrackerLandmarkApriltag>(_params);
-            else
-                prc_apriltag_params_ = std::make_shared<ProcessorParamsTrackerLandmarkApriltag>();
-
-            ProcessorTrackerLandmarkApriltag_WrapperPtr prc_ptr = std::make_shared<ProcessorTrackerLandmarkApriltag_Wrapper>(prc_apriltag_params_);
-            prc_ptr->setName(_unique_name);
-            return prc_ptr;
-        }
-
-};
-namespace wolf{
-// Register in the Factories
-WOLF_REGISTER_PROCESSOR("TRACKER LANDMARK APRILTAG WRAPPER", ProcessorTrackerLandmarkApriltag_Wrapper);
-}
-////////////////////////////////////////////////////////////////
-
-
-
-////////////////////////////////////////////////////////////////
-/*
- * Test class to prepare a little wolf problem to test the class ProcessorTrackerLandmarkApriltag
- *
- * The class ProcessorTrackerLandmarkApriltag is sometimes tested via the wrapper ProcessorTrackerLandmarkApriltag_Wrapper
- */
-// Use the following in case you want to initialize tests with predefined variables or methods.
-class ProcessorTrackerLandmarkApriltag_class : public testing::Test{
-    public:
-        virtual void SetUp()
-        {
-            wolf_root = _WOLF_ROOT_DIR;
-
-            // configure wolf problem
-            problem = Problem::create("PO", 3);
-            sen = problem->installSensor("CAMERA", "camera", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/src/examples/camera_params_canonical.yaml");
-            prc     = problem->installProcessor("TRACKER LANDMARK APRILTAG WRAPPER", "apriltags_wrapper", "camera", wolf_root + "/src/examples/processor_tracker_landmark_apriltag.yaml");
-            prc_apr = std::static_pointer_cast<ProcessorTrackerLandmarkApriltag_Wrapper>(prc);
-
-            // set prior
-            F1 = problem->setPrior((Vector7s()<<0,0,0,0,0,0,1).finished(), Matrix6s::Identity(), 0.0, 0.1);
-
-            // minimal config for the processor to be operative
-            C1 = std::make_shared<CapturePose>(1.0, sen, Vector7s(), Matrix6s());
-            F1->addCapture(C1);
-            prc_apr->setOriginPtr(C1);
-            prc_apr->setLastPtr(C1);
-
-            det.p[0][0] =  1.0;
-            det.p[0][1] = -1.0;
-            det.p[1][0] =  1.0;
-            det.p[1][1] =  1.0;
-            det.p[2][0] = -1.0;
-            det.p[2][1] =  1.0;
-            det.p[3][0] = -1.0;
-            det.p[3][1] = -1.0;
-
-            rep_error1 = 0.01;
-            rep_error2 = 0.1;
-            use_rotation = true;
-        }
-
-    public:
-        ProcessorTrackerLandmarkApriltag_WrapperPtr prc_apr;
-        std::string             wolf_root;
-        ProblemPtr              problem;
-        SensorBasePtr           sen;
-        ProcessorBasePtr        prc;
-        FrameBasePtr            F1;
-        CaptureBasePtr          C1;
-        apriltag_detection_t    det;
-        Scalar                  rep_error1;
-        Scalar                  rep_error2;
-        bool                    use_rotation;
-};
-////////////////////////////////////////////////////////////////
-
-
-
-/////////////////// TESTS START HERE ///////////////////////////
-//                                                            //
-TEST(ProcessorTrackerLandmarkApriltag, Constructor)
-{
-    std::string s1;
-    std::string s2;
-
-    ProcessorParamsTrackerLandmarkApriltagPtr params = std::make_shared<ProcessorParamsTrackerLandmarkApriltag>();
-
-    ProcessorTrackerLandmarkApriltagPtr p;
-
-    params->tag_family_ = "tag16h5";
-    p = std::make_shared<ProcessorTrackerLandmarkApriltag>(params);
-    ASSERT_TRUE(p->getTagFamily() == params->tag_family_);
-
-    params->tag_family_ = "tag25h9";
-    p = std::make_shared<ProcessorTrackerLandmarkApriltag>(params);
-    ASSERT_TRUE(p->getTagFamily() == params->tag_family_);
-
-    params->tag_family_ = "tag36h11";
-    p = std::make_shared<ProcessorTrackerLandmarkApriltag>(params);
-    ASSERT_TRUE(p->getTagFamily() == params->tag_family_);
-
-    params->tag_family_ = "tagCircle21h7";
-    p = std::make_shared<ProcessorTrackerLandmarkApriltag>(params);
-    ASSERT_TRUE(p->getTagFamily() == params->tag_family_);
-
-    params->tag_family_ = "tagStandard41h12";
-    p = std::make_shared<ProcessorTrackerLandmarkApriltag>(params);
-    ASSERT_TRUE(p->getTagFamily() == params->tag_family_);
-
-    // ! At the time of this test rewriting, Apriltag3 bugs on the creation of the 
-    // subsequent family's detector 
-
-    // NOPE
-    // params->tag_family_ = "tagCircle49h12";
-    // p = std::make_shared<ProcessorTrackerLandmarkApriltag>(params);
-    // ASSERT_TRUE(p->getTagFamily() == params->tag_family_);
-
-    // NOPE
-    // params->tag_family_ = "tagCustom48h12";
-    // p = std::make_shared<ProcessorTrackerLandmarkApriltag>(params);
-    // ASSERT_TRUE(p->getTagFamily() == params->tag_family_);
-
-    // NOPE
-    // params->tag_family_ = "tagStandard52h13";
-    // p = std::make_shared<ProcessorTrackerLandmarkApriltag>(params);
-    // ASSERT_TRUE(p->getTagFamily() == params->tag_family_);
-
-    params->tag_family_ = "wrong_family";
-    WOLF_INFO("The following runtime error \"Unrecognized tag family name. Use e.g. \"tag36h11\".\" is expected and does not imply a failed test:");
-    ASSERT_DEATH( { std::make_shared<ProcessorTrackerLandmarkApriltag>(params); }, "" );
-}
-
-TEST_F(ProcessorTrackerLandmarkApriltag_class, voteForKeyFrame)
-{
-    Scalar min_time_vote = prc_apr->getMinTimeVote();
-    unsigned int min_features_for_keyframe = prc_apr->getMinFeaturesForKeyframe();
-    Scalar start_ts = 2.0;
-
-    CaptureBasePtr Ca = std::make_shared<CapturePose>(start_ts, sen, Vector7s(), Matrix6s());
-    CaptureBasePtr Cb = std::make_shared<CapturePose>(start_ts + min_time_vote/2, sen, Vector7s(), Matrix6s());
-    CaptureBasePtr Cc = std::make_shared<CapturePose>(start_ts + 2*min_time_vote, sen, Vector7s(), Matrix6s());
-    CaptureBasePtr Cd = std::make_shared<CapturePose>(start_ts + 2.5*min_time_vote, sen, Vector7s(), Matrix6s());
-    CaptureBasePtr Ce = std::make_shared<CapturePose>(start_ts + 3*min_time_vote, sen, Vector7s(), Matrix6s());
-
-    for (int i=0; i < min_features_for_keyframe; i++){
-        det.id = i;
-        FeatureApriltagPtr f = std::make_shared<FeatureApriltag>((Vector7s()<<0,0,0,0,0,0,1).finished(), Matrix6s::Identity(), i, det, rep_error1, rep_error2, use_rotation);
-        Ca->addFeature(f);
-        Ca->addFeature(f);
-        Cc->addFeature(f);
-        if (i != min_features_for_keyframe-1){
-            Cd->addFeature(f);
-            Ce->addFeature(f);
-        }
-    }
-    F1->addCapture(Ca);
-    F1->addCapture(Cb);
-    F1->addCapture(Cc);
-    F1->addCapture(Cd);
-    F1->addCapture(Ce);
-
-    // CASE 1: Not enough time between origin and incoming
-    prc_apr->setOriginPtr(Ca);
-    prc_apr->setIncomingPtr(Cb);
-    ASSERT_FALSE(prc_apr->voteForKeyFrame());
-
-    // CASE 2: Enough time but still too many features in image to trigger a KF
-    prc_apr->setOriginPtr(Ca);
-    prc_apr->setLastPtr(Cb);
-    prc_apr->setIncomingPtr(Cc);
-    ASSERT_FALSE(prc_apr->voteForKeyFrame());
-
-    // CASE 3: Enough time, enough features in last, not enough features in incoming
-    prc_apr->setOriginPtr(Ca);
-    prc_apr->setLastPtr(Cc);
-    prc_apr->setIncomingPtr(Cd);
-    ASSERT_TRUE(prc_apr->voteForKeyFrame());
-
-    // CASE 4: Enough time, not enough features in last, not enough features in incoming
-    prc_apr->setOriginPtr(Ca);
-    prc_apr->setLastPtr(Cd);
-    prc_apr->setIncomingPtr(Ce);
-    ASSERT_FALSE(prc_apr->voteForKeyFrame());
-
-}
-
-TEST_F(ProcessorTrackerLandmarkApriltag_class, detectNewFeatures)
-{
-    // No detected features
-    FeatureBasePtrList features_out;
-    prc_apr->detectNewFeatures(1, features_out);
-    ASSERT_EQ(features_out.size(), 0);
-
-    // Some detected features TODO
-    FeatureBasePtrList features_in;
-    Eigen::Vector3s pos;
-    Eigen::Vector3s ori; //Euler angles in rad
-    Eigen::Quaternions quat;
-    Eigen::Vector7s pose;
-    Eigen::Matrix6s meas_cov( (prc_apr->getVarVec()).asDiagonal() );
-    int tag_id;
-
-    // feature 0
-    pos << 0,2,0;
-    ori << M_TORAD * 0, M_TORAD * 0, M_TORAD * 0;
-    quat = e2q(ori);
-    pose << pos, quat.coeffs();
-    tag_id = 0;
-    det.id = tag_id;
-    FeatureBasePtr f0 = std::make_shared<FeatureApriltag>(pose, meas_cov, tag_id, det, rep_error1, rep_error2, use_rotation);
-
-    // feature 1
-    pos << 1,2,0;
-    ori << M_TORAD * 0, M_TORAD * 0, M_TORAD * 0;
-    quat = e2q(ori);
-    pose << pos, quat.coeffs();
-    tag_id = 1;
-    det.id = tag_id;
-    FeatureBasePtr f1 = std::make_shared<FeatureApriltag>(pose, meas_cov, tag_id, det, rep_error1, rep_error2, use_rotation);
-
-    // feature 2
-    pos << 0,2,1;
-    ori << M_TORAD * 0, M_TORAD * 0, M_TORAD * 0;
-    quat = e2q(ori);
-    pose << pos, quat.coeffs();
-    tag_id = 2;
-    det.id = tag_id;
-    FeatureBasePtr f2 = std::make_shared<FeatureApriltag>(pose, meas_cov, tag_id, det, rep_error1, rep_error2, use_rotation);
-
-    features_in.push_back(f0);
-    features_in.push_back(f0);
-
-    // We just added twice the same feature in the list.
-    prc_apr->setLastDetections(features_in);
-    // at this point we have 0 detections in last, 2 detections in incoming with same id. We should keep only one in the final list of new detected features
-    prc_apr->detectNewFeatures(2, features_out);
-    ASSERT_EQ(features_out.size(), 1);
-
-    //we add new different features in the list
-    features_in.clear();
-    features_in.push_back(f0);
-    features_in.push_back(f1);
-    //these features are set as the incoming detections due to processing an image
-    prc_apr->setLastDetections(features_in);
-    // at this point we have 0 detections in last, 2 detections in incoming with different ids, thus we should have 2 new detected features (if max_features set to >= 2)
-    prc_apr->detectNewFeatures(2, features_out);
-    ASSERT_EQ(features_out.size(), 2);
-
-    // Put some of the features in the graph with createLandmark() and detect some of them as well as others with detectNewFeatures() running again.
-    WOLF_WARN("call to function createLandmark() in unit test for detectNewFeatures().")
-    C1->addFeature(f0);
-    LandmarkBasePtr lmk0 = prc_apr->createLandmark(f0);
-    C1->addFeature(f1);
-    LandmarkBasePtr lmk1 = prc_apr->createLandmark(f1);
-
-    // Add landmarks to the map
-    LandmarkBasePtrList landmark_list;
-    landmark_list.push_back(lmk0);
-    landmark_list.push_back(lmk1);
-    problem->addLandmarkList(landmark_list);
-    //problem->print(4,1,1,1);
-
-    // Add 1 one more new feature to the detection list
-    features_in.push_back(f2);
-    prc_apr->setLastDetections(features_in);
-    // At this point we have 2 landmarks (for f0 and f1), and 3 detections (f0, f1 and f2).
-    // Hence we should 1 new detected feature : f2
-    features_out.clear();
-    prc_apr->detectNewFeatures(2, features_out);
-    ASSERT_EQ(features_out.size(), 1);
-    ASSERT_EQ(std::static_pointer_cast<FeatureApriltag>(features_out.front())->getTagId(), 2);
-}
-
-TEST_F(ProcessorTrackerLandmarkApriltag_class, createLandmark)
-{
-    Vector7s pose_landmark((Vector7s()<<0,0,0,0,0,0,1).finished());
-    det.id = 1;
-    FeatureApriltagPtr f1 = std::make_shared<FeatureApriltag>(pose_landmark, Matrix6s::Identity(), 1, det, rep_error1, rep_error2, use_rotation);
-
-    C1->addFeature(f1);
-    LandmarkBasePtr lmk = prc_apr->createLandmark(f1);
-    LandmarkApriltagPtr lmk_april = std::static_pointer_cast<LandmarkApriltag>(lmk);
-    ASSERT_TRUE(lmk_april->getType() == "APRILTAG");
-    ASSERT_MATRIX_APPROX(lmk_april->getState(), pose_landmark, 1e-6);
-}
-
-TEST_F(ProcessorTrackerLandmarkApriltag_class, createFactor)
-{
-    det.id = 1;
-    FeatureApriltagPtr f1 = std::make_shared<FeatureApriltag>((Vector7s()<<0,0,0,0,0,0,1).finished(), Matrix6s::Identity(), 1, det, rep_error1, rep_error2, use_rotation);
-
-    C1->addFeature(f1);
-    LandmarkBasePtr lmk = prc_apr->createLandmark(f1);
-    LandmarkApriltagPtr lmk_april = std::static_pointer_cast<LandmarkApriltag>(lmk);
-
-    FactorBasePtr ctr = prc_apr->createFactor(f1, lmk);
-
-    ASSERT_TRUE(ctr->getType() == "AUTODIFF APRILTAG");
-}
-
-TEST_F(ProcessorTrackerLandmarkApriltag_class, computeInformation)
-{
-    Scalar cx = 320;
-    Scalar cy = 240;
-    Scalar fx = 320;
-    Scalar fy = 320;
-    Eigen::Matrix3s K;
-    K <<  fx,  0, cx,
-          0,  fy, cy,
-          0,    0,   1;
-    Eigen::Vector3s t; t << 0.0, 0.0, 0.4;
-    Eigen::Vector3s v; v << 0.2, 0.0, 0.0;
-    Scalar tag_width = 0.05;
-    Scalar s = tag_width/2;
-    Eigen::Vector3s p1; p1 <<  s,  s, 0; // bottom right
-    Eigen::Vector3s p2; p2 << -s,  s, 0; // bottom left
-
-    // Got from Matlab code:
-    // Top left corner
-    Eigen::Vector3s h1_matlab; h1_matlab <<   137.5894, 105.0325, 0.4050;
-    Eigen::Matrix3s J_h_T1_matlab;
-    J_h_T1_matlab << 320,  0, 320,
-                     0,  320, 240,
-                     0,    0,   1;
-    Eigen::Matrix3s J_h_R1_matlab;
-    J_h_R1_matlab << 7.8405, -7.8405, -6.4106,
-                     4.2910, -4.2910,  9.0325,
-                     0.0245, -0.0245,  0.0050;
-    // Top right corner
-    Eigen::Vector3s h2_matlab; h2_matlab << 121.5894, 105.0325, 0.4050;
-    Eigen::Matrix3s J_h_T2_matlab;
-    J_h_T2_matlab << 320,  0, 320,
-                     0,  320, 240,
-                     0,    0,   1;
-    Eigen::Matrix3s J_h_R2_matlab;
-    J_h_R2_matlab << 7.8405, 7.8405, -9.5894,
-                     4.2910, 4.2910, -9.0325,
-                     0.0245, 0.0245, -0.0050;
-
-    Eigen::Vector3s h1;
-    Eigen::Matrix3s J_h_T1;
-    Eigen::Matrix3s J_h_R1;
-    Eigen::Vector3s h2;
-    Eigen::Matrix3s J_h_T2;
-    Eigen::Matrix3s J_h_R2;
-
-    prc_apr->pinholeHomogeneous(K, t, v2R(v), p1, h1, J_h_T1, J_h_R1);
-    prc_apr->pinholeHomogeneous(K, t, v2R(v), p2, h2, J_h_T2, J_h_R2);
-
-    ASSERT_MATRIX_APPROX(h1, h1_matlab, 1e-3);
-    ASSERT_MATRIX_APPROX(J_h_T1, J_h_T1_matlab, 1e-3);
-    ASSERT_MATRIX_APPROX(J_h_R1, J_h_R1_matlab, 1e-3);
-    ASSERT_MATRIX_APPROX(h2, h2_matlab, 1e-3);
-    ASSERT_MATRIX_APPROX(J_h_T2, J_h_T2_matlab, 1e-3);
-    ASSERT_MATRIX_APPROX(J_h_R2, J_h_R2_matlab, 1e-3);
-
-    Scalar sig_q = 2;
-    Eigen::Matrix6s transformation_info = prc_apr->computeInformation(t, v2R(v), K, tag_width, sig_q);
-
-    // From Matlab
-//    Eigen::Matrix6s transformation_cov_matlab;
-//    transformation_cov_matlab <<
-//    0.0000,    0.0000,   -0.0000,    0.0000,   -0.0002,    0.0000,
-//    0.0000,    0.0000,   -0.0000,    0.0002,    0.0000,    0.0000,
-//   -0.0000,   -0.0000,    0.0004,   -0.0040,   -0.0000,    0.0000,
-//    0.0000,    0.0002,   -0.0040,    0.1027,    0.0000,    0.0000,
-//   -0.0002,    0.0000,   -0.0000,    0.0000,    0.1074,   -0.0106,
-//    0.0000,    0.0000,    0.0000,    0.0000,   -0.0106,    0.0023;
-
-    Eigen::Matrix6s transformation_info_matlab;
-    transformation_info_matlab <<
-    6.402960973553990,                   0,   0.000000000000000,  -0.000000000000000,   0.009809735541319,   0.001986080274985,
-                    0,   6.402960973553990,   0.014610695222409,  -0.008824560412472,   0.000000000000000,   0.000000000000000,
-    0.000000000000000,   0.014610695222409,   0.049088870761338,   0.001889201771982,   0.000000000000000,   0.000000000000000,
-   -0.000000000000000,  -0.008824560412472,   0.001889201771982,   0.000183864607538,  -0.000000000000000,   0.000000000000000,
-    0.009809735541319,   0.000000000000000,   0.000000000000000,  -0.000000000000000,   0.000183864607538,   0.000773944077821,
-    0.001986080274985,   0.000000000000000,   0.000000000000000,  -0.000000000000000,   0.000773944077821,   0.007846814985446;
-
-    transformation_info_matlab = transformation_info_matlab*100000.0;
-
-
-    ASSERT_MATRIX_APPROX(transformation_info, transformation_info_matlab, 1e-3);
-
-
-}
-
-int main(int argc, char **argv)
-{
-    testing::InitGoogleTest(&argc, argv);
-    return RUN_ALL_TESTS();
-}
-