Compare revisions

88e4c32b · 9ca98bdf · 6509f917 · 2ed9fb69 · 66ee16ce · 6e4bc591
--- a/.ci_templates/.install_core.yml
+++ b/.ci_templates/.install_core.yml
 .install_wolf_script:
+  - cd ${CI_PROJECT_DIR}
+  # create 'ci_deps' folder (if not exists)
+  - mkdir -pv ci_deps
+  # clone or pull
  - cd ${CI_PROJECT_DIR}/ci_deps
  - if [ -d wolf ]; then
  -   echo "directory wolf exists"
@@ -10,6 +16,8 @@
  -   git clone -b $WOLF_CORE_BRANCH ssh://git@gitlab.iri.upc.edu:2202/mobile_robotics/wolf_projects/wolf_lib/wolf.git
  -   cd wolf
  - fi
+  # build and install
  - mkdir -pv build
  - cd build
  - cmake -DCMAKE_BUILD_TYPE=release -DBUILD_DEMOS=OFF -DBUILD_TESTS=OFF ..

--- a/.ci_templates/.license_headers.yml
+++ b/.ci_templates/.license_headers.yml
@@ -23,7 +23,7 @@
  -   echo "File license_manager.sh already exists."
  - else
  -   echo "Downloading file license_manager.sh..."
-  -   wget -P /ci_deps  https://gitlab.iri.upc.edu/mobile_robotics/wolf_projects/wolf_lib/wolf/-/raw/$WOLF_CORE_BRANCH/wolf_scripts/license_manager.sh
+  -   wget -P /ci_deps  https://gitlab.iri.upc.edu/mobile_robotics/wolf_projects/wolf_lib/wolf/-/raw/$WOLF_CORE_BRANCH/scripts/license_manager.sh
  - fi
  # license headers
@@ -40,7 +40,7 @@
  -   export PREV_YEAR=$(( CURRENT_YEAR-1 ))
  -   echo "Creating new file license_header_${CURRENT_YEAR}.txt..."
  -   git mv license_header_${PREV_YEAR}.txt license_header_${CURRENT_YEAR}.txt
-  -   sed -i "s/${PREV_YEAR}/${PREV_YEAR},${CURRENT_YEAR}/g" license_header_${CURRENT_YEAR}.txt
+  -   sed -i "s/${PREV_YEAR}/${CURRENT_YEAR}/g" license_header_${CURRENT_YEAR}.txt
      # add new license headers to all files
  -   source /ci_deps/license_manager.sh --add --path=. --config-path=. --exclude=ci_deps
  - fi

--- a/.ci_templates/.yaml_schema_cpp.yml
+++ b/.ci_templates/.yaml_schema_cpp.yml
@@ -12,7 +12,7 @@
  -   git checkout main
  -   git pull 
  - else
-  -   git clone -b main ssh://git@gitlab.iri.upc.edu:2202/labrobotica/algorithms/yaml-schema-cpp.git
+  -   git clone -b main https://github.com/joanvallve/yaml-schema-cpp.git
  -   cd yaml-schema-cpp
  - fi
@@ -62,7 +62,7 @@
  -   tmp=${p%.*}
  -   name=${tmp:$folder_length+1}
  -   mkdir -p ${CI_PROJECT_DIR}/yaml_templates/$folder
-  -   yaml_template_generator $name "[${CI_PROJECT_DIR}/schema ${CI_PROJECT_DIR}/ci_deps]" ${CI_PROJECT_DIR}/yaml_templates/$folder/$name.yaml
+  -   yaml_template_generator $name "[${CI_PROJECT_DIR}/schema /usr/local/include/wolf ${EXTRA_SCHEMA_FOLDERS}]" ${CI_PROJECT_DIR}/yaml_templates/$folder/$name.yaml
  - done <schemas.txt
  # remove list of all schema files

--- a/.clang-format
+++ b/.clang-format
 ---
 Language: Cpp
 BasedOnStyle: Google
-IndentAccessModifiers: false
+#IndentAccessModifiers: false
 AccessModifierOffset: -2
 AlignAfterOpenBracket: Align
 AlignConsecutiveAssignments: true

--- a/.gitlab-ci.yml
+++ b/.gitlab-ci.yml
+include:
+  - '.ci_templates/.license_headers.yml'
+  - '.ci_templates/.yaml_schema_cpp.yml'
+  - '.ci_templates/.clang_format.yml'
+  - '.ci_templates/.preliminaries.yml'
+# RUN the pipeline if it is triggered:
+# - manually from web
+# - scheduled 
+# - by a merge_request event
+# - by a commit (push)
+# DO NOT RUN the pipeline 
+# - after a commit in a MR (avoid duplicated pipelines)
 workflow:
-  rules:
+  rules: 
    - if: '$CI_PIPELINE_SOURCE == "web"'
-    - if: $CI_COMMIT_BRANCH && $CI_OPEN_MERGE_REQUESTS && $CI_PIPELINE_SOURCE == "push"
+    - if: '$CI_PIPELINE_SOURCE == "schedule"'
-      when: never
    - if: '$CI_PIPELINE_SOURCE == "merge_request_event"'
    - if: '$CI_COMMIT_BRANCH && $CI_OPEN_MERGE_REQUESTS'
      when: never
@@ -10,21 +22,20 @@ workflow:
 stages:
  - license
+  - clang
+  - yaml_templates
  - build_and_test
  - deploy_plugins
-  - final_plugins
  - deploy_ros
+  - deploy_ros2
+variables:
+  BUILD_TYPE: debug
 ############ YAML ANCHORS ############
 .print_variables_template: &print_variables_definition
-  ## FIX VARIABLES
-  - if [ "$CI_COMMIT_BRANCH" == "" ]; then
-  -   export CI_COMMIT_BRANCH=$CI_MERGE_REQUEST_SOURCE_BRANCH_NAME
-  - fi
-  - export WOLF_CORE_BRANCH=$CI_COMMIT_BRANCH
-  # Print variables
  - echo $CI_COMMIT_REF_NAME
-  - echo $CI_COMMIT_BRANCH
+  - echo $WOLF_CORE_BRANCH
  - echo $WOLF_IMU_BRANCH
  - echo $WOLF_GNSS_BRANCH
  - echo $WOLF_LASER_BRANCH
@@ -41,69 +52,28 @@ stages:
  - echo $WOLF_ROS_VISION_BRANCH
  - echo $WOLF_ROS_APRILTAG_BRANCH
  - echo $WOLF_ROS_BODYDYNAMICS_BRANCH
+  - echo $WOLF_ROS2_NODE_BRANCH
+  - echo $WOLF_ROS2_IMU_BRANCH
+  - echo $WOLF_ROS2_GNSS_BRANCH
+  - echo $WOLF_ROS2_LASER_BRANCH
+  - echo $WOLF_ROS2_VISION_BRANCH
+  - echo $WOLF_ROS2_APRILTAG_BRANCH
+  - echo $WOLF_ROS2_BODYDYNAMICS_BRANCH
 .preliminaries_template: &preliminaries_definition
-  ## Install ssh-agent if not already installed, it is required by Docker.
+  - export WOLF_CORE_BRANCH=$CI_COMMIT_REF_NAME
-  ## (change apt-get to yum if you use an RPM-based image)
-  - 'which ssh-agent || ( apt-get update -y && apt-get install openssh-client -y )'
-  ## Run ssh-agent (inside the build environment)
-  - eval $(ssh-agent -s)
-  ## Add the SSH key stored in SSH_PRIVATE_KEY variable to the agent store
-  ## We're using tr to fix line endings which makes ed25519 keys work
-  ## without extra base64 encoding.
-  ## https://gitlab.com/gitlab-examples/ssh-private-key/issues/1#note_48526556
-  - mkdir -p ~/.ssh
-  - chmod 700 ~/.ssh  
-  - echo "$SSH_PRIVATE_KEY" | tr -d '\r' | ssh-add - > /dev/null
-  # - echo "$SSH_KNOWN_HOSTS" > $HOME/.ssh/known_hosts
-  - ssh-keyscan -H -p 2202 gitlab.iri.upc.edu >> $HOME/.ssh/known_hosts
-  # update apt
-  - apt-get update
-.license_header_template: &license_header_definition
-  - cd $CI_PROJECT_DIR
-  # configure git
+  ## PRINT VARIABLES
-  - export CI_NEW_BRANCH=ci_processing$RANDOM
+  - *print_variables_definition
-  - echo creating new temporary branch... $CI_NEW_BRANCH
-  - git config --global user.email "${CI_EMAIL}"
+  ## preliminaries script (ssh, apt update and mkdir ci_deps)
-  - git config --global user.name "${CI_USERNAME}"
+  - !reference [.preliminaries_script]
-  - git checkout -b $CI_NEW_BRANCH # temporary branch
-  # license headers
-  - export CURRENT_YEAR=$( date +'%Y' )
-  - echo "current year:" ${CURRENT_YEAR}
-  - cd wolf_scripts
-  - if [ -f license_header_${CURRENT_YEAR}.txt ]; then
-      # add license headers to new files
-  -   echo "File license_header_${CURRENT_YEAR}.txt already exists. License headers are assumed to be updated. Adding headers to new files..."
-  -   ./license_manager.sh --add --path=${CI_PROJECT_DIR} --license-header=license_header_${CURRENT_YEAR}.txt
-  - else
-      # update license headers of all files
-  -   export PREV_YEAR=$(( CURRENT_YEAR-1 ))
-  -   echo "Creating new file license_header_${CURRENT_YEAR}.txt..."
-  -   git mv license_header_${PREV_YEAR}.txt license_header_${CURRENT_YEAR}.txt
-  -   sed -i "s/${PREV_YEAR}/${PREV_YEAR},${CURRENT_YEAR}/g" license_header_${CURRENT_YEAR}.txt
-  -   ./license_manager.sh --update --path=${CI_PROJECT_DIR} --license-header=license_header_${CURRENT_YEAR}.txt
-  - fi
-  - cd ..
-  # push changes (if any)
-  - if git commit -a -m "[skip ci] license headers added or modified" ; then
-  -   git remote set-url --push origin "ssh://git@gitlab.iri.upc.edu:2202/${CI_PROJECT_PATH}.git"
-  -   git push origin $CI_NEW_BRANCH:${CI_COMMIT_REF_NAME}
-  - else
-  -   echo "No changes, nothing to commit!"
-  - fi
 .build_and_test_template: &build_and_test_definition
  - cd $CI_PROJECT_DIR
  - mkdir -pv build
  - cd build
-  - cmake -DCMAKE_BUILD_TYPE=release -DBUILD_DEMOS=ON -DBUILD_TESTS=ON ..
+  - cmake -DCMAKE_BUILD_TYPE=$BUILD_TYPE -DBUILD_DEMOS=ON -DBUILD_TESTS=ON ..
  - make -j$(nproc)
  - ctest -j$(nproc) --output-on-failure
  # run demos
@@ -116,37 +86,89 @@ license_header:
  stage: license
  image: labrobotica/wolf_deps:20.04
  before_script:  
-    - *print_variables_definition
    - *preliminaries_definition
  script: 
-    - *license_header_definition
+    - !reference [.license_header_script]
+############ CLANG FORMAT ############
+clang_format:
+  stage: clang
+  image: labrobotica/wolf_deps:20.04
+  before_script:  
+    - *preliminaries_definition
+  script: 
+    - !reference [.clang_format_script]
+############ YAML TEMPLATES GENERATION ############
+yaml_templates_generation:
+  stage: yaml_templates
+  image: labrobotica/wolf_deps:20.04
+  cache:
+    - key: yamlschemacpp-focal
+      paths:
+      - ci_deps/yaml-schema-cpp/
+  before_script:  
+    - *preliminaries_definition
+    - !reference [.install_yamlschemacpp_script]
+  script:
+    - !reference [.generate_yaml_templates_script]
 ############ UBUNTU 18.04 TESTS ############
 build_and_test:bionic:
  stage: build_and_test
  image: labrobotica/wolf_deps:18.04
+  cache:
+    - key: yamlschemacpp-bionic
+      paths:
+      - ci_deps/yaml-schema-cpp/
+  before_script:  
+    - *preliminaries_definition
+    - !reference [.install_yamlschemacpp_script]
  script:
-    - *print_variables_definition
+    - *build_and_test_definition
+    - export BUILD_TYPE=release
    - *build_and_test_definition
 ############ UBUNTU 20.04 TESTS ############
 build_and_test:focal:
  stage: build_and_test
  image: labrobotica/wolf_deps:20.04
+  cache:
+    - key: yamlschemacpp-focal
+      paths:
+      - ci_deps/yaml-schema-cpp/
+  before_script:  
+    - *preliminaries_definition
+    - !reference [.install_yamlschemacpp_script]
+  script:
+    - *build_and_test_definition
+    - export BUILD_TYPE=release
+    - *build_and_test_definition
+############ UBUNTU 22.04 TESTS ############
+build_and_test:jammy:
+  stage: build_and_test
+  image: labrobotica/wolf_deps:22.04
+  cache:
+    - key: yamlschemacpp-jammy
+      paths:
+      - ci_deps/yaml-schema-cpp/
+  before_script:  
+    - *preliminaries_definition
+    - !reference [.install_yamlschemacpp_script]
  script:
-    - *print_variables_definition
+    - *build_and_test_definition
+    - export BUILD_TYPE=release
    - *build_and_test_definition
 ############ DEPLOY PLUGINS ANY BRANCY EXCEPT FOR main ############
 deploy_imu:
  stage: deploy_plugins
  rules: 
-    - if: $CI_COMMIT_BRANCH != "main"
+    - if: $CI_COMMIT_REF_NAME != "main"
-    - if: $CI_PIPELINE_SOURCE == "merge_request_event"
-      when: never
  variables:
-    WOLF_CORE_BRANCH: $CI_COMMIT_BRANCH
+    WOLF_CORE_BRANCH: $CI_COMMIT_REF_NAME
-    DEPLOY_CI_ROS: "false"
+    DEPLOY_CI_ROS: false
  trigger: 
    project: mobile_robotics/wolf_projects/wolf_lib/plugins/imu
    branch: $WOLF_IMU_BRANCH
@@ -155,13 +177,11 @@ deploy_imu:
 deploy_gnss:
  stage: deploy_plugins
  rules: 
-    - if: $CI_COMMIT_BRANCH != "main"
+    - if: $CI_COMMIT_REF_NAME != "main"
-    - if: $CI_PIPELINE_SOURCE == "merge_request_event"
-      when: never
  variables:
-    WOLF_CORE_BRANCH: $CI_COMMIT_BRANCH
+    WOLF_CORE_BRANCH: $CI_COMMIT_REF_NAME
    GNSSUTILS_BRANCH: $GNSSUTILS_BRANCH
-    DEPLOY_CI_ROS: "false"
+    DEPLOY_CI_ROS: false
  trigger: 
    project: mobile_robotics/wolf_projects/wolf_lib/plugins/gnss
    branch: $WOLF_GNSS_BRANCH
@@ -170,12 +190,10 @@ deploy_gnss:
 deploy_vision:
  stage: deploy_plugins
  rules: 
-    - if: $CI_COMMIT_BRANCH != "main"
+    - if: $CI_COMMIT_REF_NAME != "main"
-    - if: $CI_PIPELINE_SOURCE == "merge_request_event"
-      when: never
  variables:
-    WOLF_CORE_BRANCH: $CI_COMMIT_BRANCH
+    WOLF_CORE_BRANCH: $CI_COMMIT_REF_NAME
-    DEPLOY_CI_ROS: "false"
+    DEPLOY_CI_ROS: false
  trigger: 
    project: mobile_robotics/wolf_projects/wolf_lib/plugins/vision
    branch: $WOLF_VISION_BRANCH
@@ -184,13 +202,11 @@ deploy_vision:
 deploy_laser:
  stage: deploy_plugins
  rules: 
-    - if: $CI_COMMIT_BRANCH != "main"
+    - if: $CI_COMMIT_REF_NAME != "main"
-    - if: $CI_PIPELINE_SOURCE == "merge_request_event"
-      when: never
  variables:
-    WOLF_CORE_BRANCH: $CI_COMMIT_BRANCH
+    WOLF_CORE_BRANCH: $CI_COMMIT_REF_NAME
    LASERSCANUTILS_BRANCH: $LASERSCANUTILS_BRANCH
-    DEPLOY_CI_ROS: "false"
+    DEPLOY_CI_ROS: false
  trigger: 
    project: mobile_robotics/wolf_projects/wolf_lib/plugins/laser
    branch: $WOLF_LASER_BRANCH
@@ -199,13 +215,11 @@ deploy_laser:
 deploy_apriltag:
  stage: deploy_plugins
  rules: 
-    - if: $CI_COMMIT_BRANCH != "main"
+    - if: $CI_COMMIT_REF_NAME != "main"
-    - if: $CI_PIPELINE_SOURCE == "merge_request_event"
-      when: never
  variables:
-    WOLF_CORE_BRANCH: $CI_COMMIT_BRANCH
+    WOLF_CORE_BRANCH: $CI_COMMIT_REF_NAME
    WOLF_VISION_BRANCH: $WOLF_VISION_BRANCH
-    DEPLOY_CI_ROS: "false"
+    DEPLOY_CI_ROS: false
  trigger: 
    project: mobile_robotics/wolf_projects/wolf_lib/plugins/apriltag
    branch: $WOLF_APRILTAG_BRANCH
@@ -214,13 +228,11 @@ deploy_apriltag:
 deploy_bodydynamics:
  stage: deploy_plugins
  rules: 
-    - if: $CI_COMMIT_BRANCH != "main"
+    - if: $CI_COMMIT_REF_NAME != "main"
-    - if: $CI_PIPELINE_SOURCE == "merge_request_event"
-      when: never
  variables:
-    WOLF_CORE_BRANCH: $CI_COMMIT_BRANCH
+    WOLF_CORE_BRANCH: $CI_COMMIT_REF_NAME
    WOLF_IMU_BRANCH: $WOLF_IMU_BRANCH
-    DEPLOY_CI_ROS: "false"
+    DEPLOY_CI_ROS: false
  trigger: 
    project: mobile_robotics/wolf_projects/wolf_lib/plugins/bodydynamics
    branch: $WOLF_BODYDYNAMICS_BRANCH
@@ -230,12 +242,10 @@ deploy_bodydynamics:
 deploy_imu_main:
  stage: deploy_plugins
  rules: 
-    - if: $CI_COMMIT_BRANCH == "main"
+    - if: $CI_COMMIT_REF_NAME == "main"
-    - if: $CI_PIPELINE_SOURCE == "merge_request_event"
-      when: never
  variables:
    WOLF_CORE_BRANCH: main
-    DEPLOY_CI_ROS: "false"
+    DEPLOY_CI_ROS: false
  trigger: 
    project: mobile_robotics/wolf_projects/wolf_lib/plugins/imu
    branch: main
@@ -244,13 +254,11 @@ deploy_imu_main:
 deploy_gnss_main:
  stage: deploy_plugins
  rules: 
-    - if: $CI_COMMIT_BRANCH == "main"
+    - if: $CI_COMMIT_REF_NAME == "main"
-    - if: $CI_PIPELINE_SOURCE == "merge_request_event"
-      when: never
  variables:
    WOLF_CORE_BRANCH: main
    GNSSUTILS_BRANCH: $GNSSUTILS_BRANCH
-    DEPLOY_CI_ROS: "false"
+    DEPLOY_CI_ROS: false
  trigger: 
    project: mobile_robotics/wolf_projects/wolf_lib/plugins/gnss
    branch: main
@@ -259,12 +267,10 @@ deploy_gnss_main:
 deploy_vision_main:
  stage: deploy_plugins
  rules: 
-    - if: $CI_COMMIT_BRANCH == "main"
+    - if: $CI_COMMIT_REF_NAME == "main"
-    - if: $CI_PIPELINE_SOURCE == "merge_request_event"
-      when: never
  variables:
    WOLF_CORE_BRANCH: main
-    DEPLOY_CI_ROS: "false"
+    DEPLOY_CI_ROS: false
  trigger: 
    project: mobile_robotics/wolf_projects/wolf_lib/plugins/vision
    branch: main
@@ -273,13 +279,11 @@ deploy_vision_main:
 deploy_laser_main:
  stage: deploy_plugins
  rules: 
-    - if: $CI_COMMIT_BRANCH == "main"
+    - if: $CI_COMMIT_REF_NAME == "main"
-    - if: $CI_PIPELINE_SOURCE == "merge_request_event"
-      when: never
  variables:
    WOLF_CORE_BRANCH: main
    LASERSCANUTILS_BRANCH: $LASERSCANUTILS_BRANCH
-    DEPLOY_CI_ROS: "false"
+    DEPLOY_CI_ROS: false
  trigger: 
    project: mobile_robotics/wolf_projects/wolf_lib/plugins/laser
    branch: main
@@ -288,13 +292,11 @@ deploy_laser_main:
 deploy_apriltag_main:
  stage: deploy_plugins
  rules: 
-    - if: $CI_COMMIT_BRANCH == "main"
+    - if: $CI_COMMIT_REF_NAME == "main"
-    - if: $CI_PIPELINE_SOURCE == "merge_request_event"
-      when: never
  variables:
    WOLF_CORE_BRANCH: main
    WOLF_VISION_BRANCH: main
-    DEPLOY_CI_ROS: "false"
+    DEPLOY_CI_ROS: false
  trigger: 
    project: mobile_robotics/wolf_projects/wolf_lib/plugins/apriltag
    branch: main
@@ -303,33 +305,23 @@ deploy_apriltag_main:
 deploy_bodydynamics_main:
  stage: deploy_plugins
  rules: 
-    - if: $CI_COMMIT_BRANCH == "main"
+    - if: $CI_COMMIT_REF_NAME == "main"
-    - if: $CI_PIPELINE_SOURCE == "merge_request_event"
-      when: never
  variables:
    WOLF_CORE_BRANCH: main
    WOLF_IMU_BRANCH: main
-    DEPLOY_CI_ROS: "false"
+    DEPLOY_CI_ROS: false
  trigger: 
    project: mobile_robotics/wolf_projects/wolf_lib/plugins/bodydynamics
    branch: main
    strategy: depend
-############ WAIT FOR PLUGINS ############
+############ DEPLOY wolf_ros_node EXCEPT FOR main ############
-final_all_plugins:
-  stage: final_plugins
-  script:
-    - echo "ALL PLUGINS PIPELINES SUCCEED!!!"
-############ DEPLOY WOLF_ROS_NODE EXCEPT FOR main ############
 deploy_wolf_ros_node:
  stage: deploy_ros
  rules: 
-    - if: $CI_COMMIT_BRANCH != "main" && $DEPLOY_CI_ROS == "true"
+    - if: $CI_COMMIT_REF_NAME != "main" && $DEPLOY_CI_ROS == "true"
-    - if: $CI_PIPELINE_SOURCE == "merge_request_event"
-      when: never
  variables:
-    WOLF_CORE_BRANCH: $CI_COMMIT_BRANCH
+    WOLF_CORE_BRANCH: $CI_COMMIT_REF_NAME
    WOLF_IMU_BRANCH: $WOLF_IMU_BRANCH
    WOLF_GNSS_BRANCH: $WOLF_GNSS_BRANCH
    WOLF_LASER_BRANCH: $WOLF_LASER_BRANCH
@@ -348,15 +340,13 @@ deploy_wolf_ros_node:
    project: mobile_robotics/wolf_projects/wolf_ros/wolf_ros_node
    branch: $WOLF_ROS_NODE_BRANCH
-############ DEPLOY WOLF_ROS_NODE FOR main ############
+############ DEPLOY wolf_ros_node FOR main ############
 deploy_wolf_ros_node_main:
  stage: deploy_ros
  rules: 
-    - if: $CI_COMMIT_BRANCH == "main" && $DEPLOY_CI_ROS == "true"
+    - if: $CI_COMMIT_REF_NAME == "main" && $DEPLOY_CI_ROS == "true"
-    - if: $CI_PIPELINE_SOURCE == "merge_request_event"
-      when: never
  variables:
-    WOLF_CORE_BRANCH: $CI_COMMIT_BRANCH
+    WOLF_CORE_BRANCH: $CI_COMMIT_REF_NAME
    WOLF_IMU_BRANCH: main
    WOLF_GNSS_BRANCH: main
    WOLF_LASER_BRANCH: main
@@ -369,15 +359,74 @@ deploy_wolf_ros_node_main:
    WOLF_ROS_VISION_BRANCH: main
    WOLF_ROS_APRILTAG_BRANCH: main
    WOLF_ROS_BODYDYNAMICS_BRANCH: main
-    GNSSUTILS_BRANCH: main
+    GNSSUTILS_BRANCH: $GNSSUTILS_BRANCH
-    LASERSCANUTILS_BRANCH: main
+    LASERSCANUTILS_BRANCH: $LASERSCANUTILS_BRANCH
  trigger: 
    project: mobile_robotics/wolf_projects/wolf_ros/wolf_ros_node
    branch: main
+############ NO DEPLOY wolf_ros_node ############
 no_deploy_wolf_ros_node:
  stage: deploy_ros
  script: 
    - echo "NOT deploying CI for wolf_ros_node, since DEPLOY_CI_ROS is $DEPLOY_CI_ROS (not true)"
  rules: 
    - if: $DEPLOY_CI_ROS != "true"
+############ DEPLOY wolf_ros2_node EXCEPT FOR main ############
+deploy_wolf_ros2_node:
+  stage: deploy_ros2
+  rules: 
+    - if: $CI_COMMIT_REF_NAME != "main" && $DEPLOY_CI_ROS == "true"
+  variables:
+    WOLF_CORE_BRANCH: $CI_COMMIT_REF_NAME
+    WOLF_IMU_BRANCH: $WOLF_IMU_BRANCH
+    WOLF_GNSS_BRANCH: $WOLF_GNSS_BRANCH
+    WOLF_LASER_BRANCH: $WOLF_LASER_BRANCH
+    WOLF_VISION_BRANCH: $WOLF_VISION_BRANCH
+    WOLF_APRILTAG_BRANCH: $WOLF_APRILTAG_BRANCH
+    WOLF_BODYDYNAMICS_BRANCH: $WOLF_BODYDYNAMICS_BRANCH
+    WOLF_ROS2_IMU_BRANCH: $WOLF_ROS2_IMU_BRANCH
+    WOLF_ROS2_GNSS_BRANCH: $WOLF_ROS2_GNSS_BRANCH
+    WOLF_ROS2_LASER_BRANCH: $WOLF_ROS2_LASER_BRANCH
+    WOLF_ROS2_VISION_BRANCH: $WOLF_ROS2_VISION_BRANCH
+    WOLF_ROS2_APRILTAG_BRANCH: $WOLF_ROS2_APRILTAG_BRANCH
+    WOLF_ROS2_BODYDYNAMICS_BRANCH: $WOLF_ROS2_BODYDYNAMICS_BRANCH
+    GNSSUTILS_BRANCH: $GNSSUTILS_BRANCH
+    LASERSCANUTILS_BRANCH: $LASERSCANUTILS_BRANCH
+  trigger: 
+    project: mobile_robotics/wolf_projects/wolf_ros2/wolf_ros2_node
+    branch: $WOLF_ROS2_NODE_BRANCH
+############ DEPLOY wolf_ros2_node FOR main ############
+deploy_wolf_ros2_node_main:
+  stage: deploy_ros2
+  rules: 
+    - if: $CI_COMMIT_REF_NAME == "main" && $DEPLOY_CI_ROS == "true"
+  variables:
+    WOLF_CORE_BRANCH: $CI_COMMIT_REF_NAME
+    WOLF_IMU_BRANCH: main
+    WOLF_GNSS_BRANCH: main
+    WOLF_LASER_BRANCH: main
+    WOLF_VISION_BRANCH: main
+    WOLF_APRILTAG_BRANCH: main
+    WOLF_BODYDYNAMICS_BRANCH: main
+    WOLF_ROS2_IMU_BRANCH: main
+    WOLF_ROS2_GNSS_BRANCH: main
+    WOLF_ROS2_LASER_BRANCH: main
+    WOLF_ROS2_VISION_BRANCH: main
+    WOLF_ROS2_APRILTAG_BRANCH: main
+    WOLF_ROS2_BODYDYNAMICS_BRANCH: main
+    GNSSUTILS_BRANCH: $GNSSUTILS_BRANCH
+    LASERSCANUTILS_BRANCH: $LASERSCANUTILS_BRANCH
+  trigger: 
+    project: mobile_robotics/wolf_projects/wolf_ros2/wolf_ros2_node
+    branch: main
+############ NO DEPLOY wolf_ros2_node ############
+no_deploy_wolf_ros2_node:
+  stage: deploy_ros2
+  script: 
+    - echo "NOT deploying CI for wolf_ros2_node, since DEPLOY_CI_ROS is $DEPLOY_CI_ROS (not true)"
+  rules: 
+    - if: $DEPLOY_CI_ROS != "true"
--- a/.license_template_ci.yml
+++ b/.license_template_ci.yml
-.license_header_script:
-  - cd $CI_PROJECT_DIR
-  # create 'ci_deps' folder (if not exists)
-  - mkdir -pv ci_deps
-  # configure git
-  - export CI_NEW_BRANCH=ci_processing$RANDOM
-  - echo creating new temporary branch... $CI_NEW_BRANCH
-  - git config --global user.email "${CI_EMAIL}"
-  - git config --global user.name "${CI_USERNAME}"
-  - git checkout -b $CI_NEW_BRANCH # temporary branch
-  # download license script
-  - if [ -f /ci_deps/license_manager.sh ]; then
-  -   echo "File license_manager.sh already exists."
-  - else
-  -   echo "Downloading file license_manager.sh..."
-  -   wget -P /ci_deps  https://gitlab.iri.upc.edu/mobile_robotics/wolf_projects/wolf_lib/wolf/-/raw/$WOLF_CORE_BRANCH/wolf_scripts/license_manager.sh
-  - fi
-  # license headers
-  - export CURRENT_YEAR=$( date +'%Y' )
-  - echo "current year:" ${CURRENT_YEAR}
-  - if [ -f license_header_${CURRENT_YEAR}.txt ]; then
-      # add license headers to new files
-  -   echo "File license_header_${CURRENT_YEAR}.txt already exists. License headers are assumed to be updated. Adding headers to new files..."
-  -   source /ci_deps/license_manager.sh --add --path=. --config-path=. --exclude=ci_deps
-  - else
-      # remove license headers of all files
-  -   source /ci_deps/license_manager.sh --remove --path=. --config-path=. --exclude=ci_deps
-      # update license header
-  -   export PREV_YEAR=$(( CURRENT_YEAR-1 ))
-  -   echo "Creating new file license_header_${CURRENT_YEAR}.txt..."
-  -   git mv license_header_${PREV_YEAR}.txt license_header_${CURRENT_YEAR}.txt
-  -   sed -i "s/${PREV_YEAR}/${PREV_YEAR},${CURRENT_YEAR}/g" license_header_${CURRENT_YEAR}.txt
-      # add new license headers to all files
-  -   source /ci_deps/license_manager.sh --add --path=. --config-path=. --exclude=ci_deps
-  - fi
-  # push changes (if any)
-  - if git commit -a -m "[skip ci] license headers added or modified" ; then
-  -   git remote set-url --push origin "ssh://git@gitlab.iri.upc.edu:2202/${CI_PROJECT_PATH}.git"
-  -   git push origin $CI_NEW_BRANCH:${CI_COMMIT_REF_NAME}
-  - else
-  -   echo "No changes, nothing to commit!"
-  - fi
\ No newline at end of file
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -41,7 +41,7 @@ endif()
 if(UNIX)
  # GCC is not strict enough by default, so enable most of the warnings.
  set(CMAKE_CXX_FLAGS
-    "${CMAKE_CXX_FLAGS} -Werror=all -Werror=extra -Wno-unknown-pragmas -Wno-sign-compare -Wno-unused-parameter -Wno-missing-field-initializers")
+    "${CMAKE_CXX_FLAGS} -Werror=all -Werror=extra -Wno-unknown-pragmas -Wno-sign-compare -Wno-unused-parameter -Wno-unused-function -Wno-missing-field-initializers")
 endif(UNIX)
 # Options
@@ -49,10 +49,6 @@ IF(NOT BUILD_TESTS)
  OPTION(BUILD_TESTS "Build Unit tests" ON)
 ENDIF(NOT BUILD_TESTS)
-IF(NOT BUILD_GMOCK)
-  OPTION(BUILD_GMOCK "Build GMock" OFF)
-ENDIF(NOT BUILD_GMOCK)
 IF(NOT BUILD_DEMOS)
  OPTION(BUILD_DEMOS "Build Demos" ON)
 ENDIF(NOT BUILD_DEMOS)
@@ -89,9 +85,10 @@ if(${EIGEN3_VERSION_STRING} VERSION_LESS 3.3)
  message(FATAL_ERROR "Wolf requires Eigen >= 3.3. Found Eigen ${EIGEN3_VERSION_STRING}")
 endif()
 FIND_PACKAGE(yaml-cpp REQUIRED CONFIG)
+FIND_PACKAGE(yaml-schema-cpp REQUIRED CONFIG)
 # ============ config.h ============ 
-set(_WOLF_ROOT_DIR ${CMAKE_SOURCE_DIR})
+set(_WOLF_CODE_DIR ${CMAKE_SOURCE_DIR})
 set(_WOLF_LIB_DIR ${CMAKE_INSTALL_PREFIX}/${LIB_INSTALL_DIR})
 # Define the directory where will be the configured config.h
 SET(WOLF_CONFIG_DIR ${PROJECT_BINARY_DIR}/conf/${PROJECT_NAME}/internal)
@@ -109,152 +106,9 @@ ENDIF()
 # Configure config.h
 configure_file(${CMAKE_CURRENT_SOURCE_DIR}/internal/config.h.in "${WOLF_CONFIG_DIR}/config.h")
-# ============ HEADERS ============ 
-SET(HDRS_CAPTURE
-  include/${PROJECT_NAME}/capture/capture_base.h
-  include/${PROJECT_NAME}/capture/capture_diff_drive.h
-  include/${PROJECT_NAME}/capture/capture_landmarks_external.h
-  include/${PROJECT_NAME}/capture/capture_motion.h
-  include/${PROJECT_NAME}/capture/capture_odom_2d.h
-  include/${PROJECT_NAME}/capture/capture_odom_3d.h
-  include/${PROJECT_NAME}/capture/capture_pose.h
-  include/${PROJECT_NAME}/capture/capture_void.h
-  )
-SET(HDRS_COMMON
-  include/${PROJECT_NAME}/common/factory.h
-  include/${PROJECT_NAME}/common/node_base.h
-  include/${PROJECT_NAME}/common/time_stamp.h
-  include/${PROJECT_NAME}/common/wolf.h
-  include/${PROJECT_NAME}/common/params_base.h
-  )
-SET(HDRS_FACTOR
-  include/${PROJECT_NAME}/factor/factor_analytic.h
-  include/${PROJECT_NAME}/factor/factor_autodiff.h
-  include/${PROJECT_NAME}/factor/factor_base.h
-  include/${PROJECT_NAME}/factor/factor_block_absolute.h
-  include/${PROJECT_NAME}/factor/factor_block_difference.h
-  include/${PROJECT_NAME}/factor/factor_diff_drive.h
-  include/${PROJECT_NAME}/factor/factor_distance_3d.h
-  include/${PROJECT_NAME}/factor/factor_pose_2d.h
-  include/${PROJECT_NAME}/factor/factor_pose_3d.h
-  include/${PROJECT_NAME}/factor/factor_quaternion_absolute.h
-  include/${PROJECT_NAME}/factor/factor_pose_3d_with_extrinsics.h
-  include/${PROJECT_NAME}/factor/factor_relative_pose_2d.h
-  include/${PROJECT_NAME}/factor/factor_relative_pose_2d_with_extrinsics.h
-  include/${PROJECT_NAME}/factor/factor_relative_pose_3d.h
-  include/${PROJECT_NAME}/factor/factor_relative_pose_3d_with_extrinsics.h
-  include/${PROJECT_NAME}/factor/factor_relative_position_2d.h
-  include/${PROJECT_NAME}/factor/factor_relative_position_2d_with_extrinsics.h
-  include/${PROJECT_NAME}/factor/factor_relative_position_3d.h
-  include/${PROJECT_NAME}/factor/factor_relative_position_3d_with_extrinsics.h
-  include/${PROJECT_NAME}/factor/factor_velocity_local_direction_3d.h
-  )
-SET(HDRS_FEATURE
-  include/${PROJECT_NAME}/feature/feature_base.h
-  include/${PROJECT_NAME}/feature/feature_diff_drive.h
-  include/${PROJECT_NAME}/feature/feature_landmark_external.h
-  include/${PROJECT_NAME}/feature/feature_match.h
-  include/${PROJECT_NAME}/feature/feature_motion.h
-  include/${PROJECT_NAME}/feature/feature_odom_2d.h
-  include/${PROJECT_NAME}/feature/feature_pose.h
-  )
-SET(HDRS_FRAME
-  include/${PROJECT_NAME}/frame/frame_base.h
-  )
-SET(HDRS_HARDWARE
-  include/${PROJECT_NAME}/hardware/hardware_base.h
-  )
-SET(HDRS_LANDMARK
-  include/${PROJECT_NAME}/landmark/landmark_base.h
-  include/${PROJECT_NAME}/landmark/landmark_external.h
-  include/${PROJECT_NAME}/landmark/landmark_match.h
-  )
-SET(HDRS_MATH
-  include/${PROJECT_NAME}/math/SE2.h
-  include/${PROJECT_NAME}/math/SE3.h
-  include/${PROJECT_NAME}/math/rotations.h
-  include/${PROJECT_NAME}/math/covariance.h
-  )
-SET(HDRS_MAP
-  include/${PROJECT_NAME}/map/factory_map.h
-  include/${PROJECT_NAME}/map/map_base.h
-  )
-SET(HDRS_PROBLEM
-  include/${PROJECT_NAME}/problem/problem.h
-  )
-SET(HDRS_PROCESSOR
-  include/${PROJECT_NAME}/processor/motion_buffer.h
-  include/${PROJECT_NAME}/processor/motion_provider.h
-  include/${PROJECT_NAME}/processor/processor_base.h
-  include/${PROJECT_NAME}/processor/processor_diff_drive.h
-  include/${PROJECT_NAME}/processor/processor_fixed_wing_model.h
-  include/${PROJECT_NAME}/processor/factory_processor.h
-  include/${PROJECT_NAME}/processor/processor_landmark_external.h
-  include/${PROJECT_NAME}/processor/processor_loop_closure.h
-  include/${PROJECT_NAME}/processor/processor_motion.h
-  include/${PROJECT_NAME}/processor/processor_odom_2d.h
-  include/${PROJECT_NAME}/processor/processor_odom_3d.h
-  include/${PROJECT_NAME}/processor/processor_pose.h
-  include/${PROJECT_NAME}/processor/processor_tracker.h
-  include/${PROJECT_NAME}/processor/processor_tracker_feature.h
-  include/${PROJECT_NAME}/processor/processor_tracker_landmark.h
-  include/${PROJECT_NAME}/processor/track_matrix.h
-  )
-SET(HDRS_SENSOR
-  include/${PROJECT_NAME}/sensor/sensor_base.h
-  include/${PROJECT_NAME}/sensor/sensor_diff_drive.h
-  include/${PROJECT_NAME}/sensor/factory_sensor.h
-  include/${PROJECT_NAME}/sensor/sensor_motion_model.h
-  include/${PROJECT_NAME}/sensor/sensor_odom_2d.h
-  include/${PROJECT_NAME}/sensor/sensor_odom_3d.h
-  include/${PROJECT_NAME}/sensor/sensor_pose.h
-  )
-SET(HDRS_SOLVER
-  include/${PROJECT_NAME}/solver/solver_manager.h
-  include/${PROJECT_NAME}/solver/factory_solver.h
-  )
-SET(HDRS_STATE_BLOCK
-  include/${PROJECT_NAME}/state_block/factory_state_block.h
-  include/${PROJECT_NAME}/state_block/has_state_blocks.h
-  include/${PROJECT_NAME}/state_block/local_parametrization_angle.h
-  include/${PROJECT_NAME}/state_block/local_parametrization_base.h
-  include/${PROJECT_NAME}/state_block/local_parametrization_homogeneous.h
-  include/${PROJECT_NAME}/state_block/local_parametrization_quaternion.h
-  include/${PROJECT_NAME}/state_block/state_angle.h
-  include/${PROJECT_NAME}/state_block/state_block.h
-  include/${PROJECT_NAME}/state_block/state_block_derived.h
-  include/${PROJECT_NAME}/state_block/state_composite.h
-  include/${PROJECT_NAME}/state_block/state_homogeneous_3d.h
-  include/${PROJECT_NAME}/state_block/state_quaternion.h
-  )
-SET(HDRS_TRAJECTORY
-  include/${PROJECT_NAME}/trajectory/trajectory_base.h
-  )
-SET(HDRS_TREE_MANAGER
-  include/${PROJECT_NAME}/tree_manager/factory_tree_manager.h
-  include/${PROJECT_NAME}/tree_manager/tree_manager_base.h
-  include/${PROJECT_NAME}/tree_manager/tree_manager_sliding_window.h
-  include/${PROJECT_NAME}/tree_manager/tree_manager_sliding_window_dual_rate.h
-  )
-SET(HDRS_UTILS
-  include/${PROJECT_NAME}/utils/check_log.h
-  include/${PROJECT_NAME}/utils/converter.h
-  include/${PROJECT_NAME}/utils/eigen_assert.h
-  include/${PROJECT_NAME}/utils/graph_search.h
-  include/${PROJECT_NAME}/utils/loader.h
-  include/${PROJECT_NAME}/utils/logging.h
-  include/${PROJECT_NAME}/utils/params_server.h
-  include/${PROJECT_NAME}/utils/singleton.h
-  include/${PROJECT_NAME}/utils/utils_gtest.h
-  include/${PROJECT_NAME}/utils/converter_utils.h
-  )
-SET(HDRS_YAML
-  include/${PROJECT_NAME}/yaml/parser_yaml.h
-  include/${PROJECT_NAME}/yaml/yaml_conversion.h
-  )
 # ============ SOURCES ============ 
-SET(SRCS_CAPTURE
+SET(SRCS
+  # capture
  src/capture/capture_base.cpp
  src/capture/capture_diff_drive.cpp
  src/capture/capture_landmarks_external.cpp
@@ -263,40 +117,37 @@ SET(SRCS_CAPTURE
  src/capture/capture_odom_3d.cpp
  src/capture/capture_pose.cpp
  src/capture/capture_void.cpp
-  )
+  # common
-SET(SRCS_COMMON
  src/common/node_base.cpp
+  src/common/node_state_blocks.cpp
+  src/common/profiling_unit.cpp
  src/common/time_stamp.cpp
-  )
+  # composite
-SET(SRCS_FACTOR
+  src/composite/prior_composite.cpp
+  src/composite/vector_composite.cpp
+  # factor
  src/factor/factor_analytic.cpp
  src/factor/factor_base.cpp
-  )
+  # feature
-SET(SRCS_FEATURE
  src/feature/feature_base.cpp
  src/feature/feature_diff_drive.cpp
  src/feature/feature_landmark_external.cpp
  src/feature/feature_motion.cpp
  src/feature/feature_odom_2d.cpp
  src/feature/feature_pose.cpp
-  )
+  # frame
-SET(SRCS_FRAME
  src/frame/frame_base.cpp
-  )
+  # hardware
-SET(SRCS_HARDWARE
  src/hardware/hardware_base.cpp
-  )
+  # landmark
-SET(SRCS_LANDMARK
  src/landmark/landmark_base.cpp
-  src/landmark/landmark_external.cpp
+  src/landmark/landmark.cpp
-  )
+  # map
-SET(SRCS_MAP
  src/map/map_base.cpp
-  )
+  # problem
-SET(SRCS_PROBLEM
  src/problem/problem.cpp
-  )
+  # processor
-SET(SRCS_PROCESSOR
+  src/processor/buffer.cpp
  src/processor/motion_buffer.cpp
  src/processor/motion_provider.cpp
  src/processor/processor_base.cpp
@@ -312,96 +163,45 @@ SET(SRCS_PROCESSOR
  src/processor/processor_tracker_feature.cpp
  src/processor/processor_tracker_landmark.cpp
  src/processor/track_matrix.cpp
-  )
+  # sensor
-SET(SRCS_SENSOR
  src/sensor/sensor_base.cpp
  src/sensor/sensor_diff_drive.cpp
  src/sensor/sensor_motion_model.cpp
-  src/sensor/sensor_odom_2d.cpp
+  src/sensor/sensor_odom.cpp
-  src/sensor/sensor_odom_3d.cpp
  src/sensor/sensor_pose.cpp
-  )
+  # solver
-SET(SRCS_SOLVER
  src/solver/solver_manager.cpp
-  )
+  # state_block
-SET(SRCS_STATE_BLOCK
-  src/state_block/has_state_blocks.cpp
  src/state_block/local_parametrization_base.cpp
  src/state_block/local_parametrization_homogeneous.cpp
  src/state_block/local_parametrization_quaternion.cpp
  src/state_block/state_block.cpp
  src/state_block/state_block_derived.cpp
-  src/state_block/state_composite.cpp
+  # trajectory
-  )
-SET(SRCS_TRAJECTORY
  src/trajectory/trajectory_base.cpp
-  )
+  # tree_manager
-SET(SRCS_TREE_MANAGER
  src/tree_manager/tree_manager_sliding_window.cpp
  src/tree_manager/tree_manager_sliding_window_dual_rate.cpp
-  )
+  # utils
-SET(SRCS_UTILS
  src/utils/check_log.cpp
-  src/utils/converter_utils.cpp
  src/utils/graph_search.cpp
  src/utils/loader.cpp
-  src/utils/params_server.cpp
+  src/utils/loader_utils.cpp
-  )
+  src/utils/load_core.cpp
-SET(SRCS_YAML
-  src/yaml/parser_yaml.cpp
-  src/yaml/processor_odom_3d_yaml.cpp
-  src/yaml/sensor_odom_2d_yaml.cpp
-  src/yaml/sensor_odom_3d_yaml.cpp
-  src/yaml/sensor_pose_yaml.cpp
  )
 # ============ OPTIONALS ============ 
 IF (Ceres_FOUND)
-    SET(HDRS_CERES_WRAPPER
+  SET(SRCS ${SRCS}
-      #ceres_wrapper/qr_manager.h
+    src/ceres_wrapper/solver_ceres.cpp
-      include/${PROJECT_NAME}/ceres_wrapper/cost_function_wrapper.h
+    src/ceres_wrapper/local_parametrization_wrapper.cpp
-      include/${PROJECT_NAME}/ceres_wrapper/create_numeric_diff_cost_function.h
+    src/solver/solver_manager.cpp
-      include/${PROJECT_NAME}/ceres_wrapper/local_parametrization_wrapper.h
+    )
-      include/${PROJECT_NAME}/ceres_wrapper/iteration_update_callback.h
-      include/${PROJECT_NAME}/ceres_wrapper/solver_ceres.h
-      include/${PROJECT_NAME}/ceres_wrapper/wolf_jet.h
-      include/${PROJECT_NAME}/solver/solver_manager.h
-      include/${PROJECT_NAME}/solver_suitesparse/sparse_utils.h
-      )
-    SET(SRCS_CERES_WRAPPER
-      #ceres_wrapper/qr_manager.cpp
-      src/ceres_wrapper/solver_ceres.cpp
-      src/ceres_wrapper/local_parametrization_wrapper.cpp
-      src/solver/solver_manager.cpp
-      )
-ELSE(Ceres_FOUND)
-	SET(HDRS_CERES_WRAPPER)
-	SET(SRCS_CERES_WRAPPER)
 ENDIF(Ceres_FOUND)
 # create the shared library
-ADD_LIBRARY(${PLUGIN_NAME}
+ADD_LIBRARY(${PLUGIN_NAME} SHARED ${SRCS})
-  SHARED
-  ${SRCS_CAPTURE}
-  ${SRCS_CERES_WRAPPER}
-  ${SRCS_COMMON}
-  ${SRCS_FACTOR}
-  ${SRCS_FEATURE}
-  ${SRCS_FRAME}
-  ${SRCS_HARDWARE}
-  ${SRCS_LANDMARK}
-  ${SRCS_MAP}
-  ${SRCS_PROBLEM}
-  ${SRCS_PROCESSOR}
-  ${SRCS_SENSOR}
-  ${SRCS_SOLVER}
-  ${SRCS_STATE_BLOCK}
-  ${SRCS_TRAJECTORY}
-  ${SRCS_TREE_MANAGER}
-  ${SRCS_UTILS}
-  ${SRCS_YAML}
-  )
 # Set compiler options
 # ====================  
 if (CMAKE_CXX_COMPILER_ID STREQUAL "Clang")
@@ -418,6 +218,7 @@ endif()
 #=============================================================
 TARGET_LINK_LIBRARIES(${PLUGIN_NAME} PUBLIC ${CMAKE_THREAD_LIBS_INIT} dl)
 TARGET_LINK_LIBRARIES(${PLUGIN_NAME} PUBLIC yaml-cpp)
+TARGET_LINK_LIBRARIES(${PLUGIN_NAME} PUBLIC yaml-schema-cpp::yaml-schema-cpp)
 TARGET_LINK_LIBRARIES(${PLUGIN_NAME} PUBLIC Eigen3::Eigen)
 TARGET_LINK_LIBRARIES(${PLUGIN_NAME} PUBLIC spdlog::spdlog)
 IF (Ceres_FOUND)
@@ -445,7 +246,7 @@ INSTALL(TARGETS ${PLUGIN_NAME} EXPORT ${PLUGIN_NAME}Targets
  LIBRARY DESTINATION ${LIB_INSTALL_DIR}
  ARCHIVE DESTINATION ${LIB_INSTALL_DIR}
 )
-install(EXPORT ${PLUGIN_NAME}Targets DESTINATION lib/${PLUGIN_NAME}/cmake)
+INSTALL(EXPORT ${PLUGIN_NAME}Targets DESTINATION lib/${PLUGIN_NAME}/cmake)
 # Configure the package installation
@@ -471,50 +272,17 @@ target_include_directories(${PLUGIN_NAME} PUBLIC
 )
 #install headers
-INSTALL(FILES ${HDRS_CAPTURE}
+INSTALL(DIRECTORY include/${PROJECT_NAME}
-  DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/capture)
+  DESTINATION ${INCLUDE_INSTALL_DIR})
-INSTALL(FILES ${HDRS_CERES_WRAPPER}
-  DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/ceres_wrapper)
-INSTALL(FILES ${HDRS_COMMON}
-  DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/common)
-INSTALL(FILES ${HDRS_FACTOR}
-  DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/factor)
-INSTALL(FILES ${HDRS_FEATURE}
-  DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/feature)
-INSTALL(FILES ${HDRS_FRAME}
-  DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/frame)
-INSTALL(FILES ${HDRS_HARDWARE}
-  DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/hardware)
-INSTALL(FILES ${HDRS_LANDMARK}
-  DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/landmark)
-INSTALL(FILES ${HDRS_MAP}
-  DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/map)
-INSTALL(FILES ${HDRS_MATH}
-  DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/math)
-INSTALL(FILES ${HDRS_PROBLEM}
-  DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/problem)
-INSTALL(FILES ${HDRS_PROCESSOR}
-  DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/processor)
-INSTALL(FILES ${HDRS_SENSOR}
-  DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/sensor)
-INSTALL(FILES ${HDRS_SOLVER}
-  DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/solver)
-#INSTALL(FILES ${HDRS_SOLVER_SUITESPARSE}
-#  DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/solver_suitesparse)
-INSTALL(FILES ${HDRS_STATE_BLOCK}
-  DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/state_block)
-INSTALL(FILES ${HDRS_TRAJECTORY}
-  DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/trajectory)
-INSTALL(FILES ${HDRS_TREE_MANAGER}
-  DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/tree_manager)
-INSTALL(FILES ${HDRS_UTILS}
-  DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/utils)
-INSTALL(FILES ${HDRS_YAML}
-  DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/yaml)
+#install config.h
 INSTALL(FILES ${WOLF_CONFIG_DIR}/config.h
  DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/internal)
+#install schemas
+INSTALL(DIRECTORY schema 
+  DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME})
 export(PACKAGE ${PLUGIN_NAME})
 FIND_PACKAGE(Doxygen MODULE)

--- a/PluginsInfo.md
+++ b/PluginsInfo.md
-# Overview
-# Installing
-## Installing wolf(core)
-```
-git clone https://gitlab.iri.upc.edu/mobile_robotics/wolf_projects/wolf_lib/wolf.git
-cd wolf 
-mkdir build & cd build
-cmake ..
-make
-make install
-```
-## Installing plugins
-```
-git clone https://gitlab.iri.upc.edu/mobile_robotics/wolf_projects/wolf_lib/plugins/<plugin name>.git
-cd <plugin name>
-mkdir build & cd build
-cmake ..
-make
-make install
-```
-# Using wolf in your applications
-## Using wolf core
-If you want to use the core, you just need to have it installed and in your CMakeLists.txt put the following line
-`find_package(wolf REQUIRED)`.
-If wolf is indeed installed, this will define two variables
-`${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
-If you also want to use some wolf plugin, you just follow the same procedure, changing the name
-`find_package(wolf<plugin name> REQUIRED)`.
-If the pluging is indeed installed, this will define two variables
-`${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 _laser_ plugin. First, we will clone and install it
-```
-git clone https://gitlab.iri.upc.edu/mobile_robotics/wolf_projects/wolf_lib/plugins/laser.git
-cd laser 
-mkdir build && cd build
-cmake ..
-make
-sudo make install
-```
-Then, in the CMakeLists.txt of the application we are developing we will put the following line
-```
-find_package(wolflaser REQUIRED)
-```
-if the plugin has been correctly installed, and thus find_package succeeds, then it will define two variables
- ${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
-We provide a template to create your own plugin.
-You can either clone it and restart the git history
-```
-git clone https://gitlab.iri.upc.edu/mobile_robotics/wolf_projects/wolf_lib/plugins/Template.git
-cd Template
-rm -rf .git
-git init
-```
-or directly fork*(?)* the repository.
-# Contributing
-## Contributing your plugin to wolf
-Will we have some kind of repository keeping track of wolf plugins?
-## Contributing to existing plugins
-???
-## Contributing to wolf core
-Steps:
-* Let us know what is it that you want to contribute. It is very important that you communicate with us. Obviously noone is stopping you 
-from directly forking the repository, but you run the risk of having your pull request declined if we haven't discussed your contribution beforehand.
-* If it is something we are already working on we can maybe integrate you into the working group.
-* Otherwise, fork the repository and pull request when your contribution is ready.
\ No newline at end of file
--- a/ROS-based code style eclipse indented 4sp.xml
+++ b/ROS-based code style eclipse indented 4sp.xml
-<?xml version="1.0" encoding="UTF-8" standalone="no"?>
-<profiles version="1">
-<profile kind="CodeFormatterProfile" name="ROS Formatting indented private" version="1">
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_opening_paren_in_method_declaration" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_after_opening_paren_in_for" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_new_line_in_empty_block" value="insert"/>
-<setting id="org.eclipse.cdt.core.formatter.lineSplit" value="120"/>
-<setting id="org.eclipse.cdt.core.formatter.alignment_for_member_access" value="0"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_comma_in_base_types" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.keep_else_statement_on_same_line" value="false"/>
-<setting id="org.eclipse.cdt.core.formatter.indent_switchstatements_compare_to_switch" value="true"/>
-<setting id="org.eclipse.cdt.core.formatter.alignment_for_constructor_initializer_list" value="2"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_after_opening_brace_in_array_initializer" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_comma_in_method_declaration_parameters" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_after_opening_paren_in_if" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_closing_paren_in_parenthesized_expression" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_after_opening_paren_in_exception_specification" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_after_comma_in_base_types" value="insert"/>
-<setting id="org.eclipse.cdt.core.formatter.indent_body_declarations_compare_to_access_specifier" value="true"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_closing_paren_in_exception_specification" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_after_comma_in_template_arguments" value="insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_opening_brace_in_block" value="insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_closing_paren_in_method_declaration" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.use_tabs_only_for_leading_indentations" value="false"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_colon_in_labeled_statement" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_after_colon_in_case" value="insert"/>
-<setting id="org.eclipse.cdt.core.formatter.comment.min_distance_between_code_and_line_comment" value="1"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_after_comma_in_array_initializer" value="insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_after_comma_in_enum_declarations" value="insert"/>
-<setting id="org.eclipse.cdt.core.formatter.alignment_for_expressions_in_array_initializer" value="18"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_after_comma_in_declarator_list" value="insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_opening_bracket" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_closing_paren_in_for" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_prefix_operator" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.tabulation.size" value="4"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_new_line_before_else_in_if_statement" value="insert"/>
-<setting id="org.eclipse.cdt.core.formatter.alignment_for_enumerator_list" value="16"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_after_opening_paren_in_parenthesized_expression" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_between_empty_parens_in_method_declaration" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_closing_paren_in_switch" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.alignment_for_declarator_list" value="16"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_opening_paren_in_parenthesized_expression" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.indent_empty_lines" value="false"/>
-<setting id="org.eclipse.cdt.core.formatter.indent_switchstatements_compare_to_cases" value="true"/>
-<setting id="org.eclipse.cdt.core.formatter.keep_empty_array_initializer_on_one_line" value="false"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_opening_brace_in_method_declaration" value="insert"/>
-<setting id="org.eclipse.cdt.core.formatter.put_empty_statement_on_new_line" value="true"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_opening_brace_in_switch" value="insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_closing_paren_in_cast" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_between_empty_braces_in_array_initializer" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.brace_position_for_method_declaration" value="next_line"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_closing_paren_in_while" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_after_question_in_conditional" value="insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_semicolon" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_after_closing_angle_bracket_in_template_arguments" value="insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_colon_in_base_clause" value="insert"/>
-<setting id="org.eclipse.cdt.core.formatter.indent_breaks_compare_to_cases" value="true"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_unary_operator" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.join_wrapped_lines" value="true"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_comma_in_declarator_list" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.alignment_for_arguments_in_method_invocation" value="18"/>
-<setting id="org.eclipse.cdt.core.formatter.comment.never_indent_line_comments_on_first_column" value="true"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_opening_paren_in_while" value="insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_between_empty_brackets" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_after_opening_bracket" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.alignment_for_parameters_in_method_declaration" value="18"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_new_line_before_closing_brace_in_array_initializer" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.number_of_empty_lines_to_preserve" value="1"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_after_opening_paren_in_method_invocation" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_closing_brace_in_array_initializer" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_semicolon_in_for" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_colon_in_conditional" value="insert"/>
-<setting id="org.eclipse.cdt.core.formatter.comment.preserve_white_space_between_code_and_line_comments" value="true"/>
-<setting id="org.eclipse.cdt.core.formatter.brace_position_for_block" value="next_line"/>
-<setting id="org.eclipse.cdt.core.formatter.brace_position_for_type_declaration" value="next_line"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_assignment_operator" value="insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_opening_angle_bracket_in_template_arguments" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_comma_in_expression_list" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_after_opening_angle_bracket_in_template_parameters" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.continuation_indentation" value="2"/>
-<setting id="org.eclipse.cdt.core.formatter.alignment_for_expression_list" value="0"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_after_opening_paren_in_method_declaration" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_comma_in_template_parameters" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_colon_in_default" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_after_binary_operator" value="insert"/>
-<setting id="org.eclipse.cdt.core.formatter.alignment_for_conditional_expression" value="16"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_between_empty_parens_in_method_invocation" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_comma_in_array_initializer" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_closing_paren_in_if" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.format_guardian_clause_on_one_line" value="false"/>
-<setting id="org.eclipse.cdt.core.formatter.indent_access_specifier_extra_spaces" value="0"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_after_opening_paren_in_cast" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.indent_access_specifier_compare_to_type_header" value="true"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_opening_brace_in_type_declaration" value="insert"/>
-<setting id="org.eclipse.cdt.core.formatter.continuation_indentation_for_array_initializer" value="2"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_after_colon_in_labeled_statement" value="insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_after_comma_in_method_declaration_parameters" value="insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_after_semicolon_in_for" value="insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_closing_paren_in_method_invocation" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.indent_body_declarations_compare_to_namespace_header" value="false"/>
-<setting id="org.eclipse.cdt.core.formatter.alignment_for_compact_if" value="16"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_after_assignment_operator" value="insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_after_closing_brace_in_block" value="insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_opening_brace_in_array_initializer" value="insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_new_line_at_end_of_file_if_missing" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.alignment_for_assignment" value="16"/>
-<setting id="org.eclipse.cdt.core.formatter.alignment_for_conditional_expression_chain" value="18"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_after_comma_in_template_parameters" value="insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_after_comma_in_expression_list" value="insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_question_in_conditional" value="insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_opening_paren_in_exception_specification" value="insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_binary_operator" value="insert"/>
-<setting id="org.eclipse.cdt.core.formatter.alignment_for_base_clause_in_type_declaration" value="18"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_new_line_before_identifier_in_function_declaration" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_comma_in_method_declaration_throws" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_between_empty_parens_in_exception_specification" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_comma_in_method_invocation_arguments" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.indent_declaration_compare_to_template_header" value="false"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_after_unary_operator" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_opening_paren_in_switch" value="insert"/>
-<setting id="org.eclipse.cdt.core.formatter.indent_statements_compare_to_body" value="true"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_after_comma_in_method_declaration_throws" value="insert"/>
-<setting id="org.eclipse.cdt.core.formatter.alignment_for_binary_expression" value="16"/>
-<setting id="org.eclipse.cdt.core.formatter.indent_statements_compare_to_block" value="true"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_comma_in_template_arguments" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_new_line_before_catch_in_try_statement" value="insert"/>
-<setting id="org.eclipse.cdt.core.formatter.alignment_for_throws_clause_in_method_declaration" value="18"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_opening_paren_in_method_invocation" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_closing_paren_in_catch" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_after_closing_paren_in_cast" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_opening_angle_bracket_in_template_parameters" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.tabulation.char" value="space"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_closing_angle_bracket_in_template_parameters" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_new_line_before_colon_in_constructor_initializer_list" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_after_opening_paren_in_while" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_after_comma_in_method_invocation_arguments" value="insert"/>
-<setting id="org.eclipse.cdt.core.formatter.brace_position_for_block_in_case" value="next_line"/>
-<setting id="org.eclipse.cdt.core.formatter.compact_else_if" value="true"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_after_postfix_operator" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_new_line_after_template_declaration" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_after_colon_in_base_clause" value="insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_after_opening_paren_in_catch" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.keep_then_statement_on_same_line" value="false"/>
-<setting id="org.eclipse.cdt.core.formatter.brace_position_for_switch" value="next_line"/>
-<setting id="org.eclipse.cdt.core.formatter.alignment_for_overloaded_left_shift_chain" value="16"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_opening_paren_in_if" value="insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_after_opening_paren_in_switch" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.keep_imple_if_on_one_line" value="false"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_new_line_after_opening_brace_in_array_initializer" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.indentation.size" value="4"/>
-<setting id="org.eclipse.cdt.core.formatter.brace_position_for_namespace_declaration" value="next_line"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_after_colon_in_conditional" value="insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_comma_in_enum_declarations" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_after_prefix_operator" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_closing_angle_bracket_in_template_arguments" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.brace_position_for_array_initializer" value="end_of_line"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_colon_in_case" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_opening_paren_in_catch" value="insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_opening_brace_in_namespace_declaration" value="insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_postfix_operator" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_closing_bracket" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_new_line_before_while_in_do_statement" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_opening_paren_in_for" value="insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_after_closing_angle_bracket_in_template_parameters" value="insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_after_opening_angle_bracket_in_template_arguments" value="do not insert"/>
-</profile>
-</profiles>
--- a/ROS-based wolf code style QT indented 4sp.xml
+++ b/ROS-based wolf code style QT indented 4sp.xml
-<?xml version="1.0" encoding="UTF-8"?>
-<!DOCTYPE QtCreatorCodeStyle>
-<!-- Written by QtCreator 3.0.1, 2016-02-22T12:02:02. -->
-<qtcreator>
- <data>
-  <variable>CodeStyleData</variable>
-  <valuemap type="QVariantMap">
-   <value type="bool" key="AlignAssignments">false</value>
-   <value type="bool" key="AutoSpacesForTabs">false</value>
-   <value type="bool" key="BindStarToIdentifier">false</value>
-   <value type="bool" key="BindStarToLeftSpecifier">false</value>
-   <value type="bool" key="BindStarToRightSpecifier">false</value>
-   <value type="bool" key="BindStarToTypeName">true</value>
-   <value type="bool" key="ExtraPaddingForConditionsIfConfusingAlign">true</value>
-   <value type="bool" key="IndentAccessSpecifiers">true</value>
-   <value type="bool" key="IndentBlockBody">true</value>
-   <value type="bool" key="IndentBlockBraces">false</value>
-   <value type="bool" key="IndentBlocksRelativeToSwitchLabels">true</value>
-   <value type="bool" key="IndentClassBraces">false</value>
-   <value type="bool" key="IndentControlFlowRelativeToSwitchLabels">true</value>
-   <value type="bool" key="IndentDeclarationsRelativeToAccessSpecifiers">true</value>
-   <value type="bool" key="IndentEnumBraces">false</value>
-   <value type="bool" key="IndentFunctionBody">true</value>
-   <value type="bool" key="IndentFunctionBraces">false</value>
-   <value type="bool" key="IndentNamespaceBody">false</value>
-   <value type="bool" key="IndentNamespaceBraces">false</value>
-   <value type="int" key="IndentSize">4</value>
-   <value type="bool" key="IndentStatementsRelativeToSwitchLabels">true</value>
-   <value type="bool" key="IndentSwitchLabels">true</value>
-   <value type="int" key="PaddingMode">2</value>
-   <value type="bool" key="SpacesForTabs">true</value>
-   <value type="int" key="TabSize">4</value>
-  </valuemap>
- </data>
- <data>
-  <variable>DisplayName</variable>
-  <value type="QString">ROS wolf</value>
- </data>
-</qtcreator>
--- a/cmake_modules/wolfcoreConfig.cmake.in
+++ b/cmake_modules/wolfcoreConfig.cmake.in
@@ -8,6 +8,7 @@ FIND_DEPENDENCY(Threads REQUIRED)
 FIND_DEPENDENCY(Ceres REQUIRED)
 FIND_DEPENDENCY(Eigen3 3.3 REQUIRED)
 FIND_DEPENDENCY(yaml-cpp REQUIRED)
+FIND_DEPENDENCY(yaml-schema-cpp REQUIRED)
 FIND_DEPENDENCY(spdlog REQUIRED)
 include("${CMAKE_CURRENT_LIST_DIR}/@PLUGIN_NAME@Targets.cmake")

--- a/codetemplates eclipse.xml
+++ b/codetemplates eclipse.xml
-<?xml version="1.0" encoding="UTF-8" standalone="no"?><templates><template autoinsert="true" context="org.eclipse.cdt.ui.text.codetemplates.constructorcomment_context" deleted="false" description="Comment for created constructors" enabled="true" id="org.eclipse.cdt.ui.text.codetemplates.constructorcomment" name="constructorcomment">/*
- *
- */</template><template autoinsert="true" context="org.eclipse.cdt.ui.text.codetemplates.destructorcomment_context" deleted="false" description="Comment for created destructors" enabled="true" id="org.eclipse.cdt.ui.text.codetemplates.destructorcomment" name="destructorcomment">/*
- *
- */</template><template autoinsert="false" context="org.eclipse.cdt.ui.text.codetemplates.filecomment_context" deleted="false" description="Comment for created C/C++ files" enabled="true" id="org.eclipse.cdt.ui.text.codetemplates.filecomment" name="filecomment">/**
- * \file ${file_name}
- *
- *  Created on: ${date}
- *      \author: ${user}
- */
-</template><template autoinsert="true" context="org.eclipse.cdt.ui.text.codetemplates.typecomment_context" deleted="false" description="Comment for created classes" enabled="true" id="org.eclipse.cdt.ui.text.codetemplates.typecomment" name="typecomment">/*
- *
- */</template><template autoinsert="true" context="org.eclipse.cdt.ui.text.codetemplates.fieldcomment_context" deleted="false" description="Comment for fields" enabled="true" id="org.eclipse.cdt.ui.text.codetemplates.fieldcomment" name="fieldcomment">/*
- *
- */</template><template autoinsert="true" context="org.eclipse.cdt.ui.text.codetemplates.methodcomment_context" deleted="false" description="Comment for methods" enabled="true" id="org.eclipse.cdt.ui.text.codetemplates.methodcomment" name="methodcomment">/*
- *
- */</template><template autoinsert="false" context="org.eclipse.cdt.core.cxxSource.contenttype_context" deleted="false" description="Default template for newly created C++ source files" enabled="true" id="org.eclipse.cdt.ui.text.codetemplates.cppsourcefile" name="Default C++ source template">${filecomment}
-${includes}
-${namespace_begin}
-${declarations}
-${namespace_end}</template><template autoinsert="false" context="org.eclipse.cdt.core.cxxSource.contenttype_context" deleted="false" description="Default template for newly created C++ test files" enabled="true" id="org.eclipse.cdt.ui.text.codetemplates.cpptestfile" name="Default C++ test template">${filecomment}
-#include "core/utils/utils_gtest.h"
-${includes}
-${namespace_begin}
-${declarations}
-TEST(TestGroup, DummyTestExample)
-{
-    // ${todo}: Automatically generated TEST stub 
-}
-int main(int argc, char **argv)
-{
-  testing::InitGoogleTest(&amp;argc, argv);
-  return RUN_ALL_TESTS();
-}
-${namespace_end}</template><template autoinsert="true" context="org.eclipse.cdt.core.cxxHeader.contenttype_context" deleted="false" description="Default template for newly created C++ header files" enabled="true" id="org.eclipse.cdt.ui.text.codetemplates.cppheaderfile" name="Default C++ header template">${filecomment}
-#ifndef ${include_guard_symbol}
-#define ${include_guard_symbol}
-${includes}
-${namespace_begin}
-${declarations}
-${namespace_end}
-#endif /* ${include_guard_symbol} */</template><template autoinsert="true" context="org.eclipse.cdt.core.cSource.contenttype_context" deleted="false" description="Default template for newly created C source files" enabled="true" id="org.eclipse.cdt.ui.text.codetemplates.csourcefile" name="Default C source template">${filecomment}
-${includes}
-${declarations}</template><template autoinsert="true" context="org.eclipse.cdt.core.cHeader.contenttype_context" deleted="false" description="Default template for newly created C header files" enabled="true" id="org.eclipse.cdt.ui.text.codetemplates.cheaderfile" name="Default C header template">${filecomment}
-#ifndef ${include_guard_symbol}
-#define ${include_guard_symbol}
-${includes}
-${declarations}
-#endif /* ${include_guard_symbol} */</template><template autoinsert="true" context="org.eclipse.cdt.ui.text.codetemplates.namespace_context" deleted="false" description="Beginning of namespace declaration" enabled="true" id="org.eclipse.cdt.ui.text.codetemplates.namespace_begin" name="namespace_begin">namespace ${namespace_name} {</template><template autoinsert="true" context="org.eclipse.cdt.ui.text.codetemplates.namespace_context" deleted="false" description="End of namespace declaration" enabled="true" id="org.eclipse.cdt.ui.text.codetemplates.namespace_end" name="namespace_end">} /* namespace ${namespace_name} */</template><template autoinsert="true" context="org.eclipse.cdt.ui.text.codetemplates.class_context" deleted="false" description="Code in created class definitions" enabled="true" id="org.eclipse.cdt.ui.text.codetemplates.class_body" name="class_body">${declarations}</template><template autoinsert="true" context="org.eclipse.cdt.ui.text.codetemplates.methodbody_context" deleted="false" description="Code in created method stubs" enabled="true" id="org.eclipse.cdt.ui.text.codetemplates.methodbody" name="methodbody">	// ${todo} Auto-generated method stub
-	${body_statement}</template><template autoinsert="true" context="org.eclipse.cdt.ui.text.codetemplates.constructorbody_context" deleted="false" description="Code in created constructor stubs" enabled="true" id="org.eclipse.cdt.ui.text.codetemplates.constructorbody" name="constructorbody">	// ${todo} Auto-generated constructor stub
-	${body_statement}</template><template autoinsert="true" context="org.eclipse.cdt.ui.text.codetemplates.destructorbody_context" deleted="false" description="Code in created destructor stubs" enabled="true" id="org.eclipse.cdt.ui.text.codetemplates.destructorbody" name="destructorbody">	${body_statement}
-	// ${todo} Auto-generated destructor stub</template><template autoinsert="true" context="org.eclipse.cdt.core.asmSource.contenttype_context" deleted="false" description="Default template for newly created assembly files" enabled="true" id="org.eclipse.cdt.ui.text.codetemplates.asmsourcefile" name="Default assembly template">${filecomment}
-</template><template autoinsert="true" context="org.eclipse.core.runtime.text.contenttype_context" deleted="false" description="Default template for newly created text files" enabled="true" id="org.eclipse.cdt.ui.text.codetemplates.textfile" name="Default text file template">${file_name}
- Created on: ${date}
-     Author: ${user}
-</template></templates>
\ No newline at end of file
--- a/demos/CMakeLists.txt
+++ b/demos/CMakeLists.txt
 #Add hello_wolf demo
 add_subdirectory(hello_wolf)
+add_subdirectory(solver)
--- a/demos/demo_analytic_autodiff_factor.cpp
+++ b/demos/demo_analytic_autodiff_factor.cpp
-//--------LICENSE_START--------
-//
-// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
-// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
-// All rights reserved.
-//
-// This file is part of WOLF
-// WOLF is free software: you can redistribute it and/or modify
-// it under the terms of the GNU Lesser General Public License as published by
-// the Free Software Foundation, either version 3 of the License, or
-// at your option) any later version.
-//
-// This program is distributed in the hope that it will be useful,
-// but WITHOUT ANY WARRANTY; without even the implied warranty of
-// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
-// GNU Lesser General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public License
-// along with this program.  If not, see <http://www.gnu.org/licenses/>.
-//
-//--------LICENSE_END--------
-// Testing creating wolf tree from imported .graph file
-//C includes for sleep, time and main args
-#include "unistd.h"
-//std includes
-#include <iostream>
-#include <memory>
-#include <random>
-#include <cmath>
-#include <queue>
-#include "../include/core/ceres_wrapper/solver_ceres.h"
-//Wolf includes
-#include "wolf_manager.h"
-#include "core/capture/capture_void.h"
-// EIGEN
-//#include <Eigen/CholmodSupport>
-namespace wolf {
-// inserts the sparse matrix 'ins' into the sparse matrix 'original' in the place given by 'row' and 'col' integers
-void insertSparseBlock(const Eigen::SparseMatrix<double>& ins, Eigen::SparseMatrix<double>& original, const unsigned int& row, const unsigned int& col)
-{
-  for (int k=0; k<ins.outerSize(); ++k)
-    for (Eigen::SparseMatrix<double>::InnerIterator iti(ins,k); iti; ++iti)
-      original.coeffRef(iti.row() + row, iti.col() + col) = iti.value();
-  original.makeCompressed();
-}
-int main(int argc, char** argv) 
-{
-    using namespace wolf;
-    //Welcome message
-    std::cout << std::endl << " ========= WOLF IMPORTED .graph TEST ===========" << std::endl << std::endl;
-    if (argc != 3 || atoi(argv[2]) < 0 )
-    {
-        std::cout << "Please call me with: [./test_wolf_imported_graph FILE_PATH MAX_VERTICES], where:" << std::endl;
-        std::cout << "    FILE_PATH is the .graph file path" << std::endl;
-        std::cout << "    MAX_VERTICES max edges to be loaded (0: ALL)" << std::endl;
-        std::cout << "EXIT due to bad user input" << std::endl << std::endl;
-        return -1;
-    }
-    // auxiliar variables
-    std::string file_path_ = argv[1];
-    unsigned int MAX_VERTEX = atoi(argv[2]);
-    if (MAX_VERTEX == 0) MAX_VERTEX = 1e6;
-    std::ifstream offLineFile_;
-    ceres::Solver::Summary summary_autodiff, summary_analytic;
-    // loading variables
-    std::map<unsigned int, FrameBasePtr> index_2_frame_ptr_autodiff;
-    std::map<unsigned int, FrameBasePtr> index_2_frame_ptr_analytic;
-    // Wolf problem
-    ProblemPtr wolf_problem_autodiff = new Problem(FRM_PO_2d);
-    ProblemPtr wolf_problem_analytic = new Problem(FRM_PO_2d);
-    SensorBasePtr sensor = new SensorBase("ODOM 2d", std::make_shared<StatePoint2d>(Eigen::VectorXd::Zero(2)), std::make_shared<StateAngle>(0), std::make_shared<StateParams2>(Eigen::VectorXd::Zero(2)), 2);
-    // Ceres wrapper
-    SolverCeres* ceres_manager_autodiff = new SolverCeres(wolf_problem_autodiff, ceres_options);
-    SolverCeres* ceres_manager_analytic = new SolverCeres(wolf_problem_analytic, ceres_options);
-    ceres_manager_autodiff.getSolverOptions().minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;LINE_SEARCH
-    ceres_manager_autodiff.getSolverOptions().max_line_search_step_contraction = 1e-3;
-    ceres_manager_autodiff.getSolverOptions().max_num_iterations = 1e4;
-    ceres_manager_analytic.getSolverOptions().minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;LINE_SEARCH
-    ceres_manager_analytic.getSolverOptions().max_line_search_step_contraction = 1e-3;
-    ceres_manager_analytic.getSolverOptions().max_num_iterations = 1e4;
-    // load graph from .txt
-    offLineFile_.open(file_path_.c_str(), std::ifstream::in);
-    if (offLineFile_.is_open())
-    {
-        std::string buffer;
-        unsigned int j = 0;
-        // Line by line
-        while (std::getline(offLineFile_, buffer))
-        {
-            //std::cout << "new line:" << buffer << std::endl;
-            std::string bNum;
-            unsigned int i = 0;
-            // VERTEX
-            if (buffer.at(0) == 'V')
-            {
-                //skip rest of VERTEX word
-                while (buffer.at(i) != ' ') i++;
-                //skip white spaces
-                while (buffer.at(i) == ' ') i++;
-                //vertex index
-                while (buffer.at(i) != ' ')
-                    bNum.push_back(buffer.at(i++));
-                unsigned int vertex_index = atoi(bNum.c_str());
-                bNum.clear();
-                if (vertex_index <= MAX_VERTEX+1)
-                {
-                    //skip white spaces
-                    while (buffer.at(i) == ' ') i++;
-                    // vertex pose
-                    Eigen::Vector3d vertex_pose;
-                    // x
-                    while (buffer.at(i) != ' ')
-                        bNum.push_back(buffer.at(i++));
-                    vertex_pose(0) = atof(bNum.c_str());
-                    bNum.clear();
-                    //skip white spaces
-                    while (buffer.at(i) == ' ') i++;
-                    // y
-                    while (buffer.at(i) != ' ')
-                        bNum.push_back(buffer.at(i++));
-                    vertex_pose(1) = atof(bNum.c_str());
-                    bNum.clear();
-                    //skip white spaces
-                    while (buffer.at(i) == ' ') i++;
-                    // theta
-                    while (i < buffer.size() && buffer.at(i) != ' ')
-                        bNum.push_back(buffer.at(i++));
-                    vertex_pose(2) = atof(bNum.c_str());
-                    bNum.clear();
-                    // add frame to problem
-                    FrameBasePtr vertex_frame_ptr_autodiff = new FrameBase(TimeStamp(0), std::make_shared<StatePoint2d>(vertex_pose.head(2)), std::make_shared<StateAngle>(vertex_pose.tail(1)));
-                    FrameBasePtr vertex_frame_ptr_analytic = new FrameBase(TimeStamp(0), std::make_shared<StatePoint2d>(vertex_pose.head(2)), std::make_shared<StateAngle>(vertex_pose.tail(1)));
-                    wolf_problem_autodiff->getTrajectory()->addFrame(vertex_frame_ptr_autodiff);
-                    wolf_problem_analytic->getTrajectory()->addFrame(vertex_frame_ptr_analytic);
-                    // store
-                    index_2_frame_ptr_autodiff[vertex_index] = vertex_frame_ptr_autodiff;
-                    index_2_frame_ptr_analytic[vertex_index] = vertex_frame_ptr_analytic;
-                    //std::cout << "Added vertex! index: " << vertex_index << " id: " << vertex_frame_ptr_analytic->id() << std::endl << "pose: " << vertex_pose.transpose() << std::endl;
-                }
-            }
-            // EDGE
-            else if (buffer.at(0) == 'E')
-            {
-                j++;
-                //skip rest of EDGE word
-                while (buffer.at(i) != ' ') i++;
-                //skip white spaces
-                while (buffer.at(i) == ' ') i++;
-                //from
-                while (buffer.at(i) != ' ')
-                    bNum.push_back(buffer.at(i++));
-                unsigned int edge_old = atoi(bNum.c_str());
-                bNum.clear();
-                //skip white spaces
-                while (buffer.at(i) == ' ') i++;
-                //to index
-                while (buffer.at(i) != ' ')
-                    bNum.push_back(buffer.at(i++));
-                unsigned int edge_new = atoi(bNum.c_str());
-                bNum.clear();
-                if (edge_new <= MAX_VERTEX+1 && edge_old <= MAX_VERTEX+1 )
-                {
-                    //skip white spaces
-                    while (buffer.at(i) == ' ') i++;
-                    // edge vector
-                    Eigen::Vector3d edge_vector;
-                    // x
-                    while (buffer.at(i) != ' ')
-                        bNum.push_back(buffer.at(i++));
-                    edge_vector(0) = atof(bNum.c_str());
-                    bNum.clear();
-                    //skip white spaces
-                    while (buffer.at(i) == ' ') i++;
-                    // y
-                    while (buffer.at(i) != ' ')
-                        bNum.push_back(buffer.at(i++));
-                    edge_vector(1) = atof(bNum.c_str());
-                    bNum.clear();
-                    //skip white spaces
-                    while (buffer.at(i) == ' ') i++;
-                    // theta
-                    while (buffer.at(i) != ' ')
-                        bNum.push_back(buffer.at(i++));
-                    edge_vector(2) = atof(bNum.c_str());
-                    bNum.clear();
-                    //skip white spaces
-                    while (buffer.at(i) == ' ') i++;
-                    // edge covariance
-                    Eigen::Matrix3d edge_information;
-                    // xx
-                    while (buffer.at(i) != ' ')
-                        bNum.push_back(buffer.at(i++));
-                    edge_information(0,0) = atof(bNum.c_str());
-                    bNum.clear();
-                    //skip white spaces
-                    while (buffer.at(i) == ' ') i++;
-                    // xy
-                    while (buffer.at(i) != ' ')
-                        bNum.push_back(buffer.at(i++));
-                    edge_information(0,1) = atof(bNum.c_str());
-                    edge_information(1,0) = atof(bNum.c_str());
-                    bNum.clear();
-                    //skip white spaces
-                    while (buffer.at(i) == ' ') i++;
-                    // yy
-                    while (buffer.at(i) != ' ')
-                        bNum.push_back(buffer.at(i++));
-                    edge_information(1,1) = atof(bNum.c_str());
-                    bNum.clear();
-                    //skip white spaces
-                    while (buffer.at(i) == ' ') i++;
-                    // thetatheta
-                    while (buffer.at(i) != ' ')
-                        bNum.push_back(buffer.at(i++));
-                    edge_information(2,2) = atof(bNum.c_str());
-                    bNum.clear();
-                    //skip white spaces
-                    while (buffer.at(i) == ' ') i++;
-                    // xtheta
-                    while (buffer.at(i) != ' ')
-                        bNum.push_back(buffer.at(i++));
-                    edge_information(0,2) = atof(bNum.c_str());
-                    edge_information(2,0) = atof(bNum.c_str());
-                    bNum.clear();
-                    //skip white spaces
-                    while (buffer.at(i) == ' ') i++;
-                    // ytheta
-                    while (i < buffer.size() && buffer.at(i) != ' ')
-                        bNum.push_back(buffer.at(i++));
-                    edge_information(1,2) = atof(bNum.c_str());
-                    edge_information(2,1) = atof(bNum.c_str());
-                    bNum.clear();
-                    // add capture, feature and factor to problem
-                    FeatureBasePtr feature_ptr_autodiff = new FeatureBase("POSE", edge_vector, edge_information.inverse());
-                    CaptureVoid* capture_ptr_autodiff = new CaptureVoid(TimeStamp(0), sensor);
-                    assert(index_2_frame_ptr_autodiff.find(edge_old) != index_2_frame_ptr_autodiff.end() && "edge from vertex not added!");
-                    FrameBasePtr frame_old_ptr_autodiff = index_2_frame_ptr_autodiff[edge_old];
-                    assert(index_2_frame_ptr_autodiff.find(edge_new) != index_2_frame_ptr_autodiff.end() && "edge to vertex not added!");
-                    FrameBasePtr frame_new_ptr_autodiff = index_2_frame_ptr_autodiff[edge_new];
-                    frame_new_ptr_autodiff->addCapture(capture_ptr_autodiff);
-                    capture_ptr_autodiff->addFeature(feature_ptr_autodiff);
-                    FactorOdom2d* factor_ptr_autodiff = new FactorOdom2d(feature_ptr_autodiff, frame_old_ptr_autodiff);
-                    feature_ptr_autodiff->addFactor(factor_ptr_autodiff);
-                    //std::cout << "Added autodiff edge! " << factor_ptr_autodiff->id() << " from vertex " << factor_ptr_autodiff->getCapture()->getFrame()->id() << " to " << factor_ptr_autodiff->getFrameOther()->id() << std::endl;
-                    // add capture, feature and factor to problem
-                    FeatureBasePtr feature_ptr_analytic = new FeatureBase("POSE", edge_vector, edge_information.inverse());
-                    CaptureVoid* capture_ptr_analytic = new CaptureVoid(TimeStamp(0), sensor);
-                    assert(index_2_frame_ptr_analytic.find(edge_old) != index_2_frame_ptr_analytic.end() && "edge from vertex not added!");
-                    FrameBasePtr frame_old_ptr_analytic = index_2_frame_ptr_analytic[edge_old];
-                    assert(index_2_frame_ptr_analytic.find(edge_new) != index_2_frame_ptr_analytic.end() && "edge to vertex not added!");
-                    FrameBasePtr frame_new_ptr_analytic = index_2_frame_ptr_analytic[edge_new];
-                    frame_new_ptr_analytic->addCapture(capture_ptr_analytic);
-                    capture_ptr_analytic->addFeature(feature_ptr_analytic);
-                    FactorOdom2dAnalytic* factor_ptr_analytic = new FactorOdom2dAnalytic(feature_ptr_analytic, frame_old_ptr_analytic);
-                    feature_ptr_analytic->addFactor(factor_ptr_analytic);
-                    //std::cout << "Added analytic edge! " << factor_ptr_analytic->id() << " from vertex " << factor_ptr_analytic->getCapture()->getFrame()->id() << " to " << factor_ptr_analytic->getFrameOther()->id() << std::endl;
-                    //std::cout << "vector " << factor_ptr_analytic->getMeasurement().transpose() << std::endl;
-                    //std::cout << "information " << std::endl << edge_information << std::endl;
-                    //std::cout << "covariance " << std::endl << factor_ptr_analytic->getMeasurementCovariance() << std::endl;
-                }
-            }
-            else
-                assert("unknown line");
-        }
-        printf("\nGraph loaded!\n");
-    }
-    else
-        printf("\nError opening file\n");
-    // PRIOR
-    FrameBasePtr first_frame_autodiff = wolf_problem_autodiff->getTrajectory()->getFirstFrame();
-    FrameBasePtr first_frame_analytic = wolf_problem_analytic->getTrajectory()->getFirstFrame();
-    CaptureFix* initial_covariance_autodiff = new CaptureFix(TimeStamp(0), new SensorBase("ABSOLUTE POSE", nullptr, nullptr, nullptr, 0), first_frame_autodiff->getState(), Eigen::Matrix3d::Identity() * 0.01);
-    CaptureFix* initial_covariance_analytic = new CaptureFix(TimeStamp(0), new SensorBase("ABSOLUTE POSE", nullptr, nullptr, nullptr, 0), first_frame_analytic->getState(), Eigen::Matrix3d::Identity() * 0.01);
-    first_frame_autodiff->addCapture(initial_covariance_autodiff);
-    first_frame_analytic->addCapture(initial_covariance_analytic);
-    initial_covariance_autodiff->emplaceFeatureAndFactor();
-    initial_covariance_analytic->emplaceFeatureAndFactor();
-    // SOLVING PROBLEMS
-    std::cout << "solving..." << std::endl;
-    std::cout << "ANALYTIC -----------------------------------" << std::endl;
-    summary_analytic = ceres_manager_analytic->solve();
-    std::cout << summary_analytic.FullReport() << std::endl;
-    std::cout << "AUTODIFF -----------------------------------" << std::endl;
-    summary_autodiff = ceres_manager_autodiff->solve();
-    std::cout << summary_autodiff.FullReport() << std::endl;
-    // COMPUTE COVARIANCES
-    std::cout << "computing covariances..." << std::endl;
-    std::cout << "ANALYTIC -----------------------------------" << std::endl;
-    clock_t t1 = clock();
-    ceres_manager_analytic->computeCovariances(ALL);//ALL_MARGINALS
-    std::cout << "Time: " << ((double) clock() - t1) / CLOCKS_PER_SEC << "s" << std::endl;
-    std::cout << "AUTODIFF -----------------------------------" << std::endl;
-    t1 = clock();
-    ceres_manager_autodiff->computeCovariances(ALL);//ALL_MARGINALS
-    std::cout << "Time: " << ((double) clock() - t1) / CLOCKS_PER_SEC << "s" << std::endl;
-    delete wolf_problem_autodiff; //not necessary to delete anything more, wolf will do it!
-    std::cout << "wolf_problem_ deleted!" << std::endl;
-    delete ceres_manager_autodiff;
-    delete ceres_manager_analytic;
-    std::cout << "ceres_manager deleted!" << std::endl;
-    //End message
-    std::cout << " =========================== END ===============================" << std::endl << std::endl;
-    //exit
-    return 0;
-}
--- a/demos/demo_wolf_imported_graph.cpp
+++ b/demos/demo_wolf_imported_graph.cpp
--- a/demos/hello_wolf/CMakeLists.txt
+++ b/demos/hello_wolf/CMakeLists.txt
@@ -7,15 +7,13 @@ SET(HDRS_PROCESSOR ${HDRS_PROCESSOR}
    ${CMAKE_CURRENT_SOURCE_DIR}/factor_bearing.h
    ${CMAKE_CURRENT_SOURCE_DIR}/factor_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_HELLOWOLF
+SET(SRCS_HELLOWOLF
    ${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
    )

--- a/demos/hello_wolf/capture_range_bearing.cpp
+++ b/demos/hello_wolf/capture_range_bearing.cpp
-//--------LICENSE_START--------
+// WOLF - Copyright (C) 2020-2025
-//
+// Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
-// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
-// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// Joan Vallvé Navarro (jvallve@iri.upc.edu)
 // All rights reserved.
 //
-// This file is part of WOLF
+// This file is part of WOLF: http://www.iri.upc.edu/wolf
 // WOLF is free software: you can redistribute it and/or modify
-// it under the terms of the GNU Lesser General Public License as published by
+// it under the terms of the GNU General Public License version 3
-// the Free Software Foundation, either version 3 of the License, or
+// as published by the Free Software Foundation.
-// at your option) any later version.
 //
 // This program is distributed in the hope that it will be useful,
 // but WITHOUT ANY WARRANTY; without even the implied warranty of
-// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-// GNU Lesser General Public License for more details.
+// GNU General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public License
-// along with this program.  If not, see <http://www.gnu.org/licenses/>.
 //
-//--------LICENSE_END--------
+// You should have received a copy of the GNU General Public License
-/*
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
- * CaptureRangeBearing2d.cpp
- *
- *  Created on: Nov 30, 2017
- *      Author: jsola
- */
 #include "capture_range_bearing.h"
 namespace wolf
 {
+CaptureRangeBearing::CaptureRangeBearing(const TimeStamp&       _ts,
-CaptureRangeBearing::CaptureRangeBearing(const TimeStamp& _ts, const SensorBasePtr& _scanner, const Eigen::VectorXi& _ids, const Eigen::VectorXd& _ranges, const Eigen::VectorXd& _bearings) :
+                                         const SensorBasePtr&   _scanner,
-        CaptureBase("CaptureRangeBearing", _ts, _scanner),
+                                         const Eigen::VectorXi& _ids,
-        ids_(_ids),
+                                         const Eigen::VectorXd& _ranges,
-        ranges_(_ranges),
+                                         const Eigen::VectorXd& _bearings)
-        bearings_(_bearings)
+    : CaptureBase("CaptureRangeBearing", _ts, _scanner), ids_(_ids), ranges_(_ranges), bearings_(_bearings)
 {
    //
 }

--- a/demos/hello_wolf/capture_range_bearing.h
+++ b/demos/hello_wolf/capture_range_bearing.h
-//--------LICENSE_START--------
+// WOLF - Copyright (C) 2020-2025
-//
+// Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
-// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
-// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// Joan Vallvé Navarro (jvallve@iri.upc.edu)
 // All rights reserved.
 //
-// This file is part of WOLF
+// This file is part of WOLF: http://www.iri.upc.edu/wolf
 // WOLF is free software: you can redistribute it and/or modify
-// it under the terms of the GNU Lesser General Public License as published by
+// it under the terms of the GNU General Public License version 3
-// the Free Software Foundation, either version 3 of the License, or
+// as published by the Free Software Foundation.
-// at your option) any later version.
 //
 // This program is distributed in the hope that it will be useful,
 // but WITHOUT ANY WARRANTY; without even the implied warranty of
-// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-// GNU Lesser General Public License for more details.
+// GNU General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public License
-// along with this program.  If not, see <http://www.gnu.org/licenses/>.
 //
-//--------LICENSE_END--------
+// You should have received a copy of the GNU General Public License
-/*
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
- * capture_range_bearing.h
+#pragma once
- *
- *  Created on: Nov 30, 2017
- *      Author: jsola
- */
-#ifndef HELLO_WOLF_CAPTURE_RANGE_BEARING_H_
-#define HELLO_WOLF_CAPTURE_RANGE_BEARING_H_
 #include "core/capture/capture_base.h"
 namespace wolf
 {
 WOLF_PTR_TYPEDEFS(CaptureRangeBearing)
 using namespace Eigen;
 class CaptureRangeBearing : public CaptureBase
 {
-    public:
+  public:
-        CaptureRangeBearing(const TimeStamp& _ts, const SensorBasePtr& _scanner, const Eigen::VectorXi& _ids, const Eigen::VectorXd& _ranges, const Eigen::VectorXd& _bearings);
+    CaptureRangeBearing(const TimeStamp&       _ts,
-        ~CaptureRangeBearing() override;
+                        const SensorBasePtr&   _scanner,
+                        const Eigen::VectorXi& _ids,
-        const VectorXi&         getIds          ()          const;
+                        const Eigen::VectorXd& _ranges,
-        const int&              getId           (int _i)    const;
+                        const Eigen::VectorXd& _bearings);
-        const Eigen::VectorXd&  getRanges       ()          const;
+    ~CaptureRangeBearing() override;
-        const Eigen::VectorXd&  getBearings     ()          const;
-        const double&           getRange        (int _i)    const;
+    const VectorXi&                   getIds() const;
-        const double&           getBearing      (int _i)    const;
+    const int&                        getId(int _i) const;
-        Eigen::Vector2d         getRangeBearing (int _i)    const;
+    const Eigen::VectorXd&            getRanges() const;
-        Eigen::Matrix<double, Dynamic, 2> getRangeBearing() const;
+    const Eigen::VectorXd&            getBearings() const;
+    const double&                     getRange(int _i) const;
-    private:
+    const double&                     getBearing(int _i) const;
-        VectorXi ids_;          // identifiers
+    Eigen::Vector2d                   getRangeBearing(int _i) const;
-        VectorXd ranges_;       // ranges
+    Eigen::Matrix<double, Dynamic, 2> getRangeBearing() const;
-        VectorXd bearings_;     // bearings
+  private:
+    VectorXi ids_;       // identifiers
+    VectorXd ranges_;    // ranges
+    VectorXd bearings_;  // bearings
 };
 inline const Eigen::VectorXi& CaptureRangeBearing::getIds() const
@@ -89,9 +81,9 @@ inline const double& CaptureRangeBearing::getBearing(int _i) const
    return bearings_(_i);
 }
-inline Eigen::Matrix<double,Dynamic,2> CaptureRangeBearing::getRangeBearing() const
+inline Eigen::Matrix<double, Dynamic, 2> CaptureRangeBearing::getRangeBearing() const
 {
-    return (Matrix<double,Dynamic,2>() << ranges_, bearings_).finished();
+    return (Matrix<double, Dynamic, 2>() << ranges_, bearings_).finished();
 }
 inline Eigen::Vector2d CaptureRangeBearing::getRangeBearing(int _i) const
@@ -100,5 +92,3 @@ inline Eigen::Vector2d CaptureRangeBearing::getRangeBearing(int _i) const
 }
 } /* namespace wolf */
-#endif /* HELLO_WOLF_CAPTURE_RANGE_BEARING_H_ */
--- a/demos/hello_wolf/factor_bearing.h
+++ b/demos/hello_wolf/factor_bearing.h
-//--------LICENSE_START--------
+// WOLF - Copyright (C) 2020-2025
-//
+// Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
-// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
-// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// Joan Vallvé Navarro (jvallve@iri.upc.edu)
 // All rights reserved.
 //
-// This file is part of WOLF
+// This file is part of WOLF: http://www.iri.upc.edu/wolf
 // WOLF is free software: you can redistribute it and/or modify
-// it under the terms of the GNU Lesser General Public License as published by
+// it under the terms of the GNU General Public License version 3
-// the Free Software Foundation, either version 3 of the License, or
+// as published by the Free Software Foundation.
-// at your option) any later version.
 //
 // This program is distributed in the hope that it will be useful,
 // but WITHOUT ANY WARRANTY; without even the implied warranty of
-// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-// GNU Lesser General Public License for more details.
+// GNU General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public License
-// along with this program.  If not, see <http://www.gnu.org/licenses/>.
 //
-//--------LICENSE_END--------
+// You should have received a copy of the GNU General Public License
-/*
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
- * FactorBearing.h
+#pragma once
- *
- *  Created on: Nov 30, 2017
- *      Author: jsola
- */
-#ifndef HELLO_WOLF_FACTOR_BEARING_H_
-#define HELLO_WOLF_FACTOR_BEARING_H_
 #include "core/factor/factor_autodiff.h"
 namespace wolf
 {
 using namespace Eigen;
 class FactorBearing : public FactorAutodiff<FactorBearing, 1, 2, 1, 2, 1, 2>
 {
-    public:
+  public:
-        FactorBearing(const FeatureBasePtr& _feature_ptr,
+    FactorBearing(const FeatureBasePtr&   _feature_ptr,
-                      const LandmarkBasePtr& _landmark_other_ptr,
+                  const LandmarkBasePtr&  _landmark_other_ptr,
-                      const ProcessorBasePtr& _processor_ptr,
+                  const ProcessorBasePtr& _processor_ptr,
-                      bool _apply_loss_function, FactorStatus _status) :
+                  bool                    _apply_loss_function,
-                FactorAutodiff<FactorBearing, 1, 2, 1, 2, 1, 2>("BEARING", 
+                  FactorStatus            _status)
-                                                        TOP_LMK, 
+        : FactorAutodiff<FactorBearing, 1, 2, 1, 2, 1, 2>("BEARING",
-                                                        _feature_ptr,
+                                                          TOP_LMK,
-                                                        nullptr, 
+                                                          _feature_ptr,
-                                                        nullptr, 
+                                                          nullptr,
-                                                        nullptr,
+                                                          nullptr,
-                                                        _landmark_other_ptr, 
+                                                          nullptr,
-                                                        _processor_ptr,
+                                                          _landmark_other_ptr,
-                                                        _apply_loss_function, 
+                                                          _processor_ptr,
-                                                        _status,
+                                                          _apply_loss_function,
-                                                        _feature_ptr->getCapture()->getFrame()->getP(),
+                                                          _status,
-                                                        _feature_ptr->getCapture()->getFrame()->getO(),
+                                                          _feature_ptr->getCapture()->getFrame()->getP(),
-                                                        _feature_ptr->getCapture()->getSensor()->getP(),
+                                                          _feature_ptr->getCapture()->getFrame()->getO(),
-                                                        _feature_ptr->getCapture()->getSensor()->getO(),
+                                                          _feature_ptr->getCapture()->getSensor()->getP(),
-                                                        _landmark_other_ptr->getP())
+                                                          _feature_ptr->getCapture()->getSensor()->getO(),
-        {
+                                                          _landmark_other_ptr->getP())
-            //
+    {
-        }
+        //
+    }
-        virtual ~FactorBearing()
-        {
+    virtual ~FactorBearing()
-            //
+    {
-        }
+        //
+    }
-        template<typename T>
-        bool operator ()(const T* const _p_w_r,
+    template <typename T>
-                         const T* const _o_w_r,
+    bool operator()(const T* const _p_w_r,
-                         const T* const _p_r_s, // sensor position
+                    const T* const _o_w_r,
-                         const T* const _o_r_s, // sensor orientation
+                    const T* const _p_r_s,  // sensor position
-                         const T* const _lmk,
+                    const T* const _o_r_s,  // sensor orientation
-                         T* _res) const;
+                    const T* const _lmk,
+                    T*             _res) const;
 };
 } /* namespace wolf */
@@ -83,46 +71,44 @@ class FactorBearing : public FactorAutodiff<FactorBearing, 1, 2, 1, 2, 1, 2>
 namespace wolf
 {
+template <typename T>
-template<typename T>
+inline bool FactorBearing::operator()(const T* const _p_w_r,
-inline bool FactorBearing::operator ()( const T* const _p_w_r, 
+                                      const T* const _o_w_r,
-                                        const T* const _o_w_r,
+                                      const T* const _p_r_s,  // sensor position
-                                        const T* const _p_r_s, // sensor position
+                                      const T* const _o_r_s,  // sensor orientation
-                                        const T* const _o_r_s, // sensor orientation
+                                      const T* const _lmk,
-                                        const T* const _lmk, 
+                                      T*             _res) const
-                                        T* _res) const
 {
    // 1. produce a transformation matrix to transform from robot frame to world frame
-    Transform<T, 2, Isometry> H_w_r = Translation<T,2>(_p_w_r[0], _p_w_r[1]) * Rotation2D<T>(*_o_w_r) ; // Robot frame = robot-to-world transform
+    Transform<T, 2, Isometry> H_w_r =
-    Transform<T, 2, Isometry> H_r_s = Translation<T,2>(_p_r_s[0], _p_r_s[1]) * Rotation2D<T>(*_o_r_s) ; // Robot frame = robot-to-world transform
+        Translation<T, 2>(_p_w_r[0], _p_w_r[1]) * Rotation2D<T>(*_o_w_r);  // Robot frame = robot-to-world transform
+    Transform<T, 2, Isometry> H_r_s =
+        Translation<T, 2>(_p_r_s[0], _p_r_s[1]) * Rotation2D<T>(*_o_r_s);  // Robot frame = robot-to-world transform
    // Map input pointers into meaningful Eigen elements
-    Map<const Matrix<T, 2, 1>>      point_w(_lmk);
+    Map<const Matrix<T, 2, 1>> point_w(_lmk);
-    Map<const Matrix<T, 1, 1>>      res(_res);
+    Map<const Matrix<T, 1, 1>> res(_res);
    // 2. Transform world point to sensor-referenced point
    Matrix<T, 2, 1> point_s = (H_w_r * H_r_s).inverse() * point_w;
    // 3. Get the expected bearing of the point
-    T bearing   = atan2(point_s(1), point_s(0));
+    T bearing = atan2(point_s(1), point_s(0));
    // 4. Get the measured range-and-bearing to the point
    Matrix<T, 2, 1> range_bearing = getMeasurement().cast<T>();
    // 5. Get the bearing error by comparing against the bearing measurement
-    T er   = range_bearing(1) - bearing;
+    T er = range_bearing(1) - bearing;
    if (er < T(-M_PI))
-        er += T(2*M_PI);
+        er += T(2 * M_PI);
-    else if  (er > T(-M_PI))
+    else if (er > T(-M_PI))
-        er -= T(2*M_PI);
+        er -= T(2 * M_PI);
    // 6. Compute the residual by scaling according to the standard deviation of the bearing part
-    *_res   = er * getMeasurementSquareRootInformationUpper()(1,1);
+    *_res = er * getMeasurementSquareRootInformationUpper()(1, 1);
    return true;
 }
-} // namespace wolf
+}  // namespace wolf
-#endif /* HELLO_WOLF_FACTOR_BEARING_H_ */
--- a/demos/hello_wolf/factor_range_bearing.h
+++ b/demos/hello_wolf/factor_range_bearing.h
-//--------LICENSE_START--------
+// WOLF - Copyright (C) 2020-2025
-//
+// Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
-// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
-// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// Joan Vallvé Navarro (jvallve@iri.upc.edu)
 // All rights reserved.
 //
-// This file is part of WOLF
+// This file is part of WOLF: http://www.iri.upc.edu/wolf
 // WOLF is free software: you can redistribute it and/or modify
-// it under the terms of the GNU Lesser General Public License as published by
+// it under the terms of the GNU General Public License version 3
-// the Free Software Foundation, either version 3 of the License, or
+// as published by the Free Software Foundation.
-// at your option) any later version.
 //
 // This program is distributed in the hope that it will be useful,
 // but WITHOUT ANY WARRANTY; without even the implied warranty of
-// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-// GNU Lesser General Public License for more details.
+// GNU General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public License
-// along with this program.  If not, see <http://www.gnu.org/licenses/>.
 //
-//--------LICENSE_END--------
+// You should have received a copy of the GNU General Public License
-/**
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
- * \file factor_range_bearing.h
+#pragma once
- *
- *  Created on: Dec 1, 2017
- *      \author: jsola
- */
-#ifndef HELLO_WOLF_FACTOR_RANGE_BEARING_H_
-#define HELLO_WOLF_FACTOR_RANGE_BEARING_H_
 #include "core/factor/factor_autodiff.h"
 namespace wolf
 {
 WOLF_PTR_TYPEDEFS(FactorRangeBearing);
 using namespace Eigen;
-class FactorRangeBearing : public FactorAutodiff<FactorRangeBearing, 2, 2, 1, 2, 1, 2>
+class FactorRangeBearing
+    : public FactorAutodiff<FactorRangeBearing, 2, 2, 1, 2, 1, 2>  // sizes of: residual, rob pos, rob ori, sen pos,
+                                                                   // sen ori, lmk pos
 {
-    public:
+  public:
-        FactorRangeBearing(const CaptureBasePtr& _capture_own,      // own capture's pointer
+    FactorRangeBearing(const CaptureBasePtr&   _capture_own,  // own capture's pointer
-                           const FeatureBasePtr& _feature_ptr,
+                       const FeatureBasePtr&   _feature_ptr,
-                           const LandmarkBasePtr& _landmark_other_ptr, // other landmark's pointer
+                       const LandmarkBasePtr&  _landmark_other_ptr,   // other landmark's pointer
-                           const ProcessorBasePtr& _processor_ptr,  // processor having created this
+                       const ProcessorBasePtr& _processor_ptr,        // processor having created this
-                           bool _apply_loss_function,               // apply loss function to residual?
+                       bool                    _apply_loss_function,  // apply loss function to residual?
-                           FactorStatus _status) :              // active factor?
+                       FactorStatus            _status)                          // active factor?
-                    FactorAutodiff<FactorRangeBearing, 2, 2, 1, 2, 1, 2>( // sizes of: residual, rob pos, rob ori, sen pos, sen ori, lmk pos
+        : FactorAutodiff("RANGE BEARING",                             // factor type enum (see wolf.h)
-                            "RANGE BEARING",                             // factor type enum (see wolf.h)
+                         TOP_LMK,                                     // factor topology (see factor_base.h)
-                            TOP_LMK,                                    // factor topology (see factor_base.h)
+                         _feature_ptr->getMeasurement(),
-                            _feature_ptr,
+                         _feature_ptr->getMeasurementSquareRootInformationUpper(),
-                            nullptr,                                    // other frame's pointer
+                         {_capture_own->getFrame(), _capture_own->getSensor(), _landmark_other_ptr},  // nodes factored
-                            nullptr,                                    // other capture's pointer
+                         _processor_ptr,                      // processor having created this
-                            nullptr,                                    // other feature's pointer
+                         {_capture_own->getFrame()->getP(),   // robot position
-                            _landmark_other_ptr,                        // other landmark's pointer
+                          _capture_own->getFrame()->getO(),   // robot orientation state block
-                            _processor_ptr,                             // processor having created this
+                          _capture_own->getSensor()->getP(),  // sensor position state block
-                            _apply_loss_function,                       // apply loss function to residual?
+                          _capture_own->getSensor()->getO(),  // sensor orientation state block
-                            _status,                                    // active factor?
+                          _landmark_other_ptr->getP()},       // landmark position state block
-                            _capture_own->getFrame()->getP(),     // robot position
+                         _apply_loss_function,                // apply loss function to residual?
-                            _capture_own->getFrame()->getO(),     // robot orientation state block
+                         _status)                             // active factor?
-                            _capture_own->getSensor()->getP(),    // sensor position state block
+    {
-                            _capture_own->getSensor()->getO(),    // sensor orientation state block
+        //
-                            _landmark_other_ptr->getP())             // landmark position state block
+    }
-        {
-            //
+    ~FactorRangeBearing() override
-        }
+    {
+        //
-        ~FactorRangeBearing() override
+    }
-        {
-            //
+    template <typename T>
-        }
+    bool operator()(const T* const _p_w_r,  // robot position
+                    const T* const _o_w_r,  // robot orientation
-        template<typename T>
+                    const T* const _p_r_s,  // sensor position
-        bool operator ()(const T* const _p_w_r, // robot position
+                    const T* const _o_r_s,  // sensor orientation
-                         const T* const _o_w_r, // robot orientation
+                    const T* const _lmk,    // landmark position
-                         const T* const _p_r_s, // sensor position
+                    T*             _res) const;         // residuals
-                         const T* const _o_r_s, // sensor orientation
-                         const T* const _lmk,   // landmark position
-                         T* _res) const;        // residuals
 };
 } /* namespace wolf */
@@ -88,14 +74,13 @@ class FactorRangeBearing : public FactorAutodiff<FactorRangeBearing, 2, 2, 1, 2,
 namespace wolf
 {
+template <typename T>
-template<typename T>
+inline bool FactorRangeBearing::operator()(const T* const _p_w_r,  // robot position
-inline bool FactorRangeBearing::operator ()(const T* const _p_w_r, // robot position
+                                           const T* const _o_w_r,  // robot orientation
-                                                const T* const _o_w_r, // robot orientation
+                                           const T* const _p_r_s,  // sensor position
-                                                const T* const _p_r_s, // sensor position
+                                           const T* const _o_r_s,  // sensor orientation
-                                                const T* const _o_r_s, // sensor orientation
+                                           const T* const _lmk,    // landmark position
-                                                const T* const _lmk,   // landmark position
+                                           T*             _res) const          // residuals
-                                                T* _res) const         // residuals
 {
    // NOTE: The scalar type template 'T' can be of two types:
    //       - double     --> this allows direct computation of the residual
@@ -117,43 +102,45 @@ inline bool FactorRangeBearing::operator ()(const T* const _p_w_r, // robot posi
    // 6. Weight the error with the covariance, and compute the residual
    // 0. Map input pointers into meaningful Eigen elements
-    Map<const Matrix<T, 2, 1>>      lmk(_lmk);      // point in world frame
+    Map<const Matrix<T, 2, 1>> lmk(_lmk);  // point in world frame
-    Map<Matrix<T, 2, 1>>            res(_res);      // residual
+    Map<Matrix<T, 2, 1>>       res(_res);  // residual
    // 1. produce transformation matrices to transform from sensor frame --> to robot frame --> to world frame
-    Transform<T, 2, Isometry> H_w_r = Translation<T,2>(_p_w_r[0], _p_w_r[1]) * Rotation2D<T>(*_o_w_r) ; // Robot  frame = robot-to-world transform
+    Transform<T, 2, Isometry> H_w_r =
-    Transform<T, 2, Isometry> H_r_s = Translation<T,2>(_p_r_s[0], _p_r_s[1]) * Rotation2D<T>(*_o_r_s) ; // Sensor frame = sensor-to-robot transform
+        Translation<T, 2>(_p_w_r[0], _p_w_r[1]) * Rotation2D<T>(*_o_w_r);  // Robot  frame = robot-to-world transform
+    Transform<T, 2, Isometry> H_r_s =
+        Translation<T, 2>(_p_r_s[0], _p_r_s[1]) * Rotation2D<T>(*_o_r_s);  // Sensor frame = sensor-to-robot transform
    // 2. Transform world-referenced landmark point to sensor-referenced point
-    Transform<T, 2, Isometry> H_w_s = H_w_r * H_r_s;  // world-to-sensor transform
+    Transform<T, 2, Isometry> H_w_s = H_w_r * H_r_s;          // world-to-sensor transform
-    Matrix<T, 2, 1> lmk_s = H_w_s.inverse() * lmk;  // point in sensor frame
+    Matrix<T, 2, 1>           lmk_s = H_w_s.inverse() * lmk;  // point in sensor frame
    // 3. Get the expected range-and-bearing of the point
    Matrix<T, 2, 1> exp_rb;
-    exp_rb(0)      = sqrt(lmk_s.squaredNorm());        // range is norm. This code workaround is because Eigen::v.norm() is problematic with scalar type ceres::Jet
+    exp_rb(0) = sqrt(lmk_s.squaredNorm());  // range is norm. This code workaround is because Eigen::v.norm() is
-    exp_rb(1)      = atan2(lmk_s(1), lmk_s(0));        // bearing
+                                            // problematic with scalar type ceres::Jet
+    exp_rb(1) = atan2(lmk_s(1), lmk_s(0));  // bearing
    // 4. Get the measured range-and-bearing to the point
    auto& meas_rb = getMeasurement();
    // 5. Get the error by comparing the expected against the measurement
-    Matrix<T, 2, 1> err_rb        = meas_rb - exp_rb;
+    Matrix<T, 2, 1> err_rb = meas_rb - exp_rb;
-    while (err_rb(1) < T(-M_PI))                        // bring angle between  -pi and pi
+    while (err_rb(1) < T(-M_PI))  // bring angle between  -pi and pi
-        err_rb(1) += T(2*M_PI);
+        err_rb(1) += T(2 * M_PI);
-    while (err_rb(1) > T(M_PI))
+    while (err_rb(1) > T(M_PI)) err_rb(1) -= T(2 * M_PI);
-        err_rb(1) -= T(2*M_PI);
    // 6. Compute the residual by weighting the error according to the standard deviation of the bearing part
-    // NOTE: the weight R is the upper square root of the information matrix Omega, which is in turn the inverse of the covariance Cov:
+    // NOTE: the weight R is the upper square root of the information matrix Omega, which is in turn the inverse of the
+    // covariance Cov:
    //    R = Omega^(T/2) = Omega^(1/2)^T = Cov(-T/2)
    // where R is called the upper square root of Omega, and is such that
    //    R^T * R = Omega
    // in other words, R is the Cholesky decomposition of Omega.
    // NOTE: you get R directly from the Feature with getMeasurementSquareRootInformationUpper()
-    res     = getMeasurementSquareRootInformationUpper() * err_rb;
+    res = getMeasurementSquareRootInformationUpper() * err_rb;
    return true;
 }
-} // namespace wolf
+}  // namespace wolf
-#endif /* HELLO_WOLF_FACTOR_RANGE_BEARING_H_ */
No results found