diff --git a/.ci_templates/.clang_format.yml b/.ci_templates/.clang_format.yml
new file mode 100644
index 0000000000000000000000000000000000000000..dc263197d73bc5bf68967dfd390ae2933bde2a31
--- /dev/null
+++ b/.ci_templates/.clang_format.yml
@@ -0,0 +1,29 @@
+.clang_format_script:
+
+ - apt install -y clang-format-12 # available in ubuntu 20.04 or newer
+ - cd $CI_PROJECT_DIR
+
+ # configure git
+ - git remote set-url origin "ssh://git@gitlab.iri.upc.edu:2202/${CI_PROJECT_PATH}.git"
+ - git pull origin ${CI_COMMIT_REF_NAME}
+ - git config --global user.email "${CI_EMAIL}"
+ - git config --global user.name "${CI_USERNAME}"
+
+ # create temporary branch
+ - if [ `git rev-parse --verify ci_clangformat 2>/dev/null` ]; then
+ - git branch --delete ci_clangformat
+ - fi
+ - export CI_NEW_BRANCH_CLANG=ci_clangformat
+ - echo creating new temporary branch... $CI_NEW_BRANCH_CLANG
+ - git checkout -b $CI_NEW_BRANCH_CLANG
+
+ # apply clang format to all code files
+ - cd ${CI_PROJECT_DIR}
+ - find . -iname *.h -o -iname *.cpp -o -iname *.hpp | xargs clang-format-12 --style=file -i
+
+ # push changes (if any)
+ - if git commit -a -m "[skip ci] applied clang format" ; then
+ - git push origin $CI_NEW_BRANCH_CLANG:${CI_COMMIT_REF_NAME}
+ - else
+ - echo "No changes, nothing to commit!"
+ - fi
\ No newline at end of file
diff --git a/.ci_templates/.install_core.yml b/.ci_templates/.install_core.yml
new file mode 100644
index 0000000000000000000000000000000000000000..39c5b5686638381e2e315bcaf285d9c6cf7f7299
--- /dev/null
+++ b/.ci_templates/.install_core.yml
@@ -0,0 +1,17 @@
+.install_wolf_script:
+ - cd ${CI_PROJECT_DIR}/ci_deps
+ - if [ -d wolf ]; then
+ - echo "directory wolf exists"
+ - cd wolf
+ - git fetch --all
+ - git checkout $WOLF_CORE_BRANCH
+ - git pull
+ - else
+ - git clone -b $WOLF_CORE_BRANCH ssh://git@gitlab.iri.upc.edu:2202/mobile_robotics/wolf_projects/wolf_lib/wolf.git
+ - cd wolf
+ - fi
+ - mkdir -pv build
+ - cd build
+ - cmake -DCMAKE_BUILD_TYPE=release -DBUILD_DEMOS=OFF -DBUILD_TESTS=OFF ..
+ - make -j$(nproc)
+ - make install
\ No newline at end of file
diff --git a/.ci_templates/.license_headers.yml b/.ci_templates/.license_headers.yml
new file mode 100644
index 0000000000000000000000000000000000000000..cc3b675ccd6bf027e21971d951052563cd64d77a
--- /dev/null
+++ b/.ci_templates/.license_headers.yml
@@ -0,0 +1,53 @@
+.license_header_script:
+ - cd $CI_PROJECT_DIR
+
+ # create 'ci_deps' folder (if not exists)
+ - mkdir -pv ci_deps
+
+ # configure git
+ - git remote set-url origin "ssh://git@gitlab.iri.upc.edu:2202/${CI_PROJECT_PATH}.git"
+ - git pull origin ${CI_COMMIT_REF_NAME}
+ - git config --global user.email "${CI_EMAIL}"
+ - git config --global user.name "${CI_USERNAME}"
+
+ # create temporary branch
+ - if [ `git rev-parse --verify ci_license_header 2>/dev/null` ]; then
+ - git branch --delete ci_license_header
+ - fi
+ - export CI_NEW_BRANCH_LICENSE=ci_license_header
+ - echo creating new temporary branch... $CI_NEW_BRANCH_LICENSE
+ - git checkout -b $CI_NEW_BRANCH_LICENSE
+
+ # 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 push origin $CI_NEW_BRANCH_LICENSE:${CI_COMMIT_REF_NAME}
+ - else
+ - echo "No changes, nothing to commit!"
+ - fi
\ No newline at end of file
diff --git a/.ci_templates/.preliminaries.yml b/.ci_templates/.preliminaries.yml
new file mode 100644
index 0000000000000000000000000000000000000000..c0f9a861efa4f7dec508a68e96a4f94ac25e2466
--- /dev/null
+++ b/.ci_templates/.preliminaries.yml
@@ -0,0 +1,25 @@
+.preliminaries_script:
+
+ ## Install ssh-agent if not already installed, it is required by Docker.
+ ## (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
+
+ # create 'ci_deps' folder (if not exists)
+ - cd ${CI_PROJECT_DIR}
+ - mkdir -pv ci_deps
\ No newline at end of file
diff --git a/.ci_templates/.yaml_schema_cpp.yml b/.ci_templates/.yaml_schema_cpp.yml
new file mode 100644
index 0000000000000000000000000000000000000000..fb7056750f841bfdd95de84630bd4f1da6127089
--- /dev/null
+++ b/.ci_templates/.yaml_schema_cpp.yml
@@ -0,0 +1,78 @@
+.install_yamlschemacpp_script:
+ - cd ${CI_PROJECT_DIR}
+
+ # create 'ci_deps' folder (if not exists)
+ - mkdir -pv ci_deps
+ - cd ${CI_PROJECT_DIR}/ci_deps
+
+ # clone or pull
+ - if [ -d yaml-schema-cpp ]; then
+ - echo "directory yaml-schema-cpp exists"
+ - cd yaml-schema-cpp
+ - git checkout main
+ - git pull
+ - else
+ - git clone -b main ssh://git@gitlab.iri.upc.edu:2202/labrobotica/algorithms/yaml-schema-cpp.git
+ - cd yaml-schema-cpp
+ - fi
+
+ # build and install
+ - mkdir -pv build
+ - cd build
+ - cmake -DCMAKE_BUILD_TYPE=release -DBUILD_TESTS=OFF ..
+ - make -j$(nproc)
+ - make install
+ - ldconfig
+
+.generate_yaml_templates_script:
+ - cd ${CI_PROJECT_DIR}
+
+ # create ci_deps if not created yet
+ - mkdir -pv ci_deps
+
+ # configure git
+ - git remote set-url origin "ssh://git@gitlab.iri.upc.edu:2202/${CI_PROJECT_PATH}.git"
+ - git pull origin ${CI_COMMIT_REF_NAME}
+ - git config --global user.email "${CI_EMAIL}"
+ - git config --global user.name "${CI_USERNAME}"
+
+ # create temporary branch
+ - if [ `git rev-parse --verify ci_yamlschemacpp 2>/dev/null` ]; then
+ - git branch --delete ci_yamlschemacpp
+ - fi
+ - export CI_NEW_BRANCH_YAML=ci_yamlschemacpp
+ - echo creating new temporary branch... $CI_NEW_BRANCH_YAML
+ - git checkout -b $CI_NEW_BRANCH_YAML
+
+ # remove all existing yaml templates
+ - cd ${CI_PROJECT_DIR}
+ - rm -rf yaml_templates
+
+ # go to schema folder
+ - cd ${CI_PROJECT_DIR}/schema
+
+ # list all schema files
+ - find . -iname "*.schema" > schemas.txt
+
+ # generate yaml template for each schema file in yaml_templates folder
+ - while read p; do
+ - echo "Generating template for schema $p"
+ - folder=${p%/*}
+ - folder_length=${#folder}
+ - 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
+ - done <schemas.txt
+
+ # remove list of all schema files
+ - rm schemas.txt
+
+ # push changes (if any)
+ - git add ../yaml_templates
+ - if git commit -a -m "[skip ci] yaml templates added or modified" ; then
+ - git push origin $CI_NEW_BRANCH_YAML:${CI_COMMIT_REF_NAME}
+ - else
+ - echo "No changes, nothing to commit!"
+ - fi
+
diff --git a/.gitignore b/.gitignore
index c81d93b330db1ee8455e4c073e979746edbc3728..04d76471866180564bbd8d0827aead7804aee0f9 100644
--- a/.gitignore
+++ b/.gitignore
@@ -37,3 +37,5 @@ src/examples/map_apriltag_save.yaml
.clangd
.ccls*
compile_commands.json
+
+test/yaml/maps/*.yaml
\ No newline at end of file
diff --git a/.gitlab-ci.yml b/.gitlab-ci.yml
index b6b0a3b5c9c447bf5402a403714351831a9cf342..611abc0f187fd5bd6058be070173d08f4611290d 100644
--- a/.gitlab-ci.yml
+++ b/.gitlab-ci.yml
@@ -17,7 +17,13 @@ stages:
############ 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_IMU_BRANCH
- echo $WOLF_GNSS_BRANCH
diff --git a/.license_template_ci.yml b/.license_template_ci.yml
new file mode 100644
index 0000000000000000000000000000000000000000..3d827c7398e9da4850d13245d5810e3c287400e4
--- /dev/null
+++ b/.license_template_ci.yml
@@ -0,0 +1,47 @@
+.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
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 897854374187e8d3fb517b1ec1bf5f849c719f51..6581448051c1e87448f4fd0b70852a51f085e589 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -2,7 +2,7 @@
MESSAGE(STATUS "Starting WOLF CMakeLists ...")
# Pre-requisites about cmake itself
-CMAKE_MINIMUM_REQUIRED(VERSION 3.10)
+CMAKE_MINIMUM_REQUIRED(VERSION 3.16)
# MAC OSX RPATH
SET(CMAKE_MACOSX_RPATH 1)
@@ -152,6 +152,7 @@ SET(HDRS_FACTOR
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
@@ -165,6 +166,7 @@ SET(HDRS_HARDWARE
)
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
@@ -273,6 +275,7 @@ SET(SRCS_FACTOR
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
@@ -285,6 +288,7 @@ SET(SRCS_HARDWARE
)
SET(SRCS_LANDMARK
src/landmark/landmark_base.cpp
+ src/landmark/landmark_external.cpp
)
SET(SRCS_MAP
src/map/map_base.cpp
@@ -360,6 +364,7 @@ IF (Ceres_FOUND)
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
)
diff --git a/include/core/capture/capture_landmarks_external.h b/include/core/capture/capture_landmarks_external.h
index e5ca355db8757c96108dad3382dc2c5756f20edf..0ae7c7d2e1747d4ecc687723f2a1c47733132a89 100644
--- a/include/core/capture/capture_landmarks_external.h
+++ b/include/core/capture/capture_landmarks_external.h
@@ -21,41 +21,50 @@
//--------LICENSE_END--------
#pragma once
-//Wolf includes
+// Wolf includes
#include "core/capture/capture_base.h"
-namespace wolf {
+namespace wolf
+{
struct LandmarkDetection
{
int id; // id of landmark
+ int type; // type of landmark
Eigen::VectorXd measure; // either pose or position
Eigen::MatrixXd covariance; // covariance of the measure
- double quality; // [0, 1] quality of the detection
+ double quality; // [0, 1] quality of the detection
};
WOLF_PTR_TYPEDEFS(CaptureLandmarksExternal);
-//class CaptureLandmarksExternal
+// class CaptureLandmarksExternal
class CaptureLandmarksExternal : public CaptureBase
{
- protected:
- std::vector<LandmarkDetection> detections_;
-
- public:
+ protected:
+ std::vector<LandmarkDetection> detections_;
- CaptureLandmarksExternal(const TimeStamp& _ts,
- SensorBasePtr _sensor_ptr,
- const std::vector<int>& _ids = {},
- const std::vector<Eigen::VectorXd>& _detections = {},
- const std::vector<Eigen::MatrixXd>& _covs = {},
- const std::vector<double>& _qualities = {});
+ public:
+ CaptureLandmarksExternal(const TimeStamp& _ts,
+ SensorBasePtr _sensor_ptr,
+ const std::vector<int>& _ids = {},
+ const std::vector<int>& _types = {},
+ const std::vector<Eigen::VectorXd>& _detections = {},
+ const std::vector<Eigen::MatrixXd>& _covs = {},
+ const std::vector<double>& _qualities = {});
- ~CaptureLandmarksExternal() override;
+ ~CaptureLandmarksExternal() override;
- std::vector<LandmarkDetection> getDetections() const {return detections_;};
+ std::vector<LandmarkDetection> getDetections() const
+ {
+ return detections_;
+ };
- void addDetection(const int& _id, const Eigen::VectorXd& _detection, const Eigen::MatrixXd& _cov, const double& quality);
+ void addDetection(const int& _id,
+ const int& _type,
+ const Eigen::VectorXd& _detection,
+ const Eigen::MatrixXd& _cov,
+ const double& quality);
};
-} //namespace wolf
+} // namespace wolf
diff --git a/include/core/ceres_wrapper/wolf_jet.h b/include/core/ceres_wrapper/wolf_jet.h
new file mode 100644
index 0000000000000000000000000000000000000000..238711f8cf515abfc4b3b2a4492552d5a4f28f8b
--- /dev/null
+++ b/include/core/ceres_wrapper/wolf_jet.h
@@ -0,0 +1,51 @@
+//--------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--------
+
+#pragma once
+
+// CERES JET
+#include "ceres/jet.h"
+
+#include "core/math/rotations.h"
+
+namespace ceres{
+
+// // efficient implementation of pi2pi for jets (without while)
+// template<int N>
+// inline ceres::Jet<double, N> pi2pi(const ceres::Jet<double, N>& _angle)
+// {
+// ceres::Jet<double, N> angle = _angle;
+// angle.a = pi2pi(_angle.a);
+// return angle;
+// }
+// }
+
+using std::remainder;
+
+template<int N>
+inline Jet<double, N> remainder(const Jet<double, N>& _x, long double _y)
+{
+ Jet<double, N> res = _x;
+ res.a = remainder(_x.a, _y);
+ return res;
+}
+}
diff --git a/include/core/factor/factor_autodiff.h b/include/core/factor/factor_autodiff.h
index e3827aad41552e08e5e8a989432fc41d3d76dc24..5b6d0710b715c033f19df5ab6f4a1032065efb27 100644
--- a/include/core/factor/factor_autodiff.h
+++ b/include/core/factor/factor_autodiff.h
@@ -27,8 +27,8 @@
#include "core/factor/factor_base.h"
#include "core/state_block/state_block.h"
-// CERES
-#include "ceres/jet.h"
+// jet
+#include "core/ceres_wrapper/wolf_jet.h"
// GENERAL
#include <array>
@@ -36,37 +36,1043 @@
namespace wolf {
//template class FactorAutodiff
-template <class FacT, unsigned int RES, unsigned int B0, unsigned int B1 = 0, unsigned int B2 = 0, unsigned int B3 = 0, unsigned int B4 = 0, unsigned int B5 = 0, unsigned int B6 = 0, unsigned int B7 = 0, unsigned int B8 = 0, unsigned int B9 = 0, unsigned int B10 = 0, unsigned int B11 = 0>
+template <class FacT, unsigned int RES, unsigned int B0, unsigned int B1 = 0, unsigned int B2 = 0, unsigned int B3 = 0, unsigned int B4 = 0, unsigned int B5 = 0, unsigned int B6 = 0, unsigned int B7 = 0, unsigned int B8 = 0, unsigned int B9 = 0, unsigned int B10 = 0, unsigned int B11 = 0, unsigned int B12 = 0, unsigned int B13 = 0, unsigned int B14 = 0>
class FactorAutodiff : public FactorBase
{
public:
+ static const unsigned int n_blocks = 15;
- static const unsigned int residualSize = RES;
- static const unsigned int block0Size = B0;
- static const unsigned int block1Size = B1;
- static const unsigned int block2Size = B2;
- static const unsigned int block3Size = B3;
- static const unsigned int block4Size = B4;
- static const unsigned int block5Size = B5;
- static const unsigned int block6Size = B6;
- static const unsigned int block7Size = B7;
- static const unsigned int block8Size = B8;
- static const unsigned int block9Size = B9;
- static const unsigned int block10Size = B10;
- static const unsigned int block11Size = B11;
- static const unsigned int n_blocks = 12;
+ static const std::vector<unsigned int> state_block_sizes_;
+
+ typedef ceres::Jet<double, B0 + B1 + B2 + B3 + B4 +
+ B5 + B6 + B7 + B8 + B9 +
+ B10 + B11 + B12 + B13 + B14> WolfJet;
+ protected:
+
+ std::vector<StateBlockPtr> state_ptrs_;
+ std::vector<StateBlockConstPtr> state_ptrs_const_;
+
+ static const std::vector<unsigned int> jacobian_locations_;
+ mutable std::array<WolfJet, RES> residuals_jets_;
+ mutable std::array<WolfJet, B0> jets_0_;
+ mutable std::array<WolfJet, B1> jets_1_;
+ mutable std::array<WolfJet, B2> jets_2_;
+ mutable std::array<WolfJet, B3> jets_3_;
+ mutable std::array<WolfJet, B4> jets_4_;
+ mutable std::array<WolfJet, B5> jets_5_;
+ mutable std::array<WolfJet, B6> jets_6_;
+ mutable std::array<WolfJet, B7> jets_7_;
+ mutable std::array<WolfJet, B8> jets_8_;
+ mutable std::array<WolfJet, B9> jets_9_;
+ mutable std::array<WolfJet, B10> jets_10_;
+ mutable std::array<WolfJet, B11> jets_11_;
+ mutable std::array<WolfJet, B12> jets_12_;
+ mutable std::array<WolfJet, B13> jets_13_;
+ mutable std::array<WolfJet, B14> jets_14_;
+
+ public:
+ /** \brief Constructor valid for all categories (ABSOLUTE, FRAME, FEATURE, LANDMARK)
+ **/
+ FactorAutodiff(const std::string& _tp,
+ const FactorTopology& _top,
+ const FeatureBasePtr& _feature_ptr,
+ const FrameBasePtr& _frame_other_ptr,
+ const CaptureBasePtr& _capture_other_ptr,
+ const FeatureBasePtr& _feature_other_ptr,
+ const LandmarkBasePtr& _landmark_other_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status,
+ StateBlockPtr _state0Ptr,
+ StateBlockPtr _state1Ptr,
+ StateBlockPtr _state2Ptr,
+ StateBlockPtr _state3Ptr,
+ StateBlockPtr _state4Ptr,
+ StateBlockPtr _state5Ptr,
+ StateBlockPtr _state6Ptr,
+ StateBlockPtr _state7Ptr,
+ StateBlockPtr _state8Ptr,
+ StateBlockPtr _state9Ptr,
+ StateBlockPtr _state10Ptr,
+ StateBlockPtr _state11Ptr,
+ StateBlockPtr _state12Ptr,
+ StateBlockPtr _state13Ptr,
+ StateBlockPtr _state14Ptr) :
+ FactorAutodiff(_tp,
+ _top,
+ _feature_ptr,
+ _frame_other_ptr ? FrameBasePtrList({_frame_other_ptr}) : FrameBasePtrList(),
+ _capture_other_ptr ? CaptureBasePtrList({_capture_other_ptr}) : CaptureBasePtrList(),
+ _feature_other_ptr ? FeatureBasePtrList({_feature_other_ptr}) : FeatureBasePtrList(),
+ _landmark_other_ptr ? LandmarkBasePtrList({_landmark_other_ptr}) : LandmarkBasePtrList(),
+ _processor_ptr,
+ _apply_loss_function,
+ _status,
+ _state0Ptr,
+ _state1Ptr,
+ _state2Ptr,
+ _state3Ptr,
+ _state4Ptr,
+ _state5Ptr,
+ _state6Ptr,
+ _state7Ptr,
+ _state8Ptr,
+ _state9Ptr,
+ _state10Ptr,
+ _state11Ptr,
+ _state12Ptr,
+ _state13Ptr,
+ _state14Ptr){}
+
+ FactorAutodiff(const std::string& _tp,
+ const FactorTopology& _top,
+ const FeatureBasePtr& _feature_ptr,
+ const FrameBasePtrList& _frame_other_list,
+ const CaptureBasePtrList& _capture_other_list,
+ const FeatureBasePtrList& _feature_other_list,
+ const LandmarkBasePtrList& _landmark_other_list,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status,
+ StateBlockPtr _state0Ptr,
+ StateBlockPtr _state1Ptr,
+ StateBlockPtr _state2Ptr,
+ StateBlockPtr _state3Ptr,
+ StateBlockPtr _state4Ptr,
+ StateBlockPtr _state5Ptr,
+ StateBlockPtr _state6Ptr,
+ StateBlockPtr _state7Ptr,
+ StateBlockPtr _state8Ptr,
+ StateBlockPtr _state9Ptr,
+ StateBlockPtr _state10Ptr,
+ StateBlockPtr _state11Ptr,
+ StateBlockPtr _state12Ptr,
+ StateBlockPtr _state13Ptr,
+ StateBlockPtr _state14Ptr) :
+ FactorBase(_tp, _top, _feature_ptr, _frame_other_list, _capture_other_list, _feature_other_list, _landmark_other_list, _processor_ptr, _apply_loss_function, _status),
+ state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr,_state5Ptr,_state6Ptr,_state7Ptr,_state8Ptr,_state9Ptr,_state10Ptr,_state11Ptr,_state12Ptr,_state13Ptr,_state14Ptr}),
+ state_ptrs_const_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr,_state5Ptr,_state6Ptr,_state7Ptr,_state8Ptr,_state9Ptr,_state10Ptr,_state11Ptr,_state12Ptr,_state13Ptr,_state14Ptr})
+ {
+ // asserts for null states
+ assert(state_ptrs_.size() == n_blocks && "state_ptrs wrongly initialized");
+ assert(state_ptrs_const_.size() == n_blocks && "state_ptrs wrongly initialized");
+ assert(_state0Ptr != nullptr && "nullptr state block");
+ assert(_state1Ptr != nullptr && "nullptr state block");
+ assert(_state2Ptr != nullptr && "nullptr state block");
+ assert(_state3Ptr != nullptr && "nullptr state block");
+ assert(_state4Ptr != nullptr && "nullptr state block");
+ assert(_state5Ptr != nullptr && "nullptr state block");
+ assert(_state6Ptr != nullptr && "nullptr state block");
+ assert(_state7Ptr != nullptr && "nullptr state block");
+ assert(_state8Ptr != nullptr && "nullptr state block");
+ assert(_state9Ptr != nullptr && "nullptr state block");
+ assert(_state10Ptr != nullptr && "nullptr state block");
+ assert(_state11Ptr != nullptr && "nullptr state block");
+ assert(_state12Ptr != nullptr && "nullptr state block");
+ assert(_state13Ptr != nullptr && "nullptr state block");
+ assert(_state14Ptr != nullptr && "nullptr state block");
+
+ // initialize jets
+ unsigned int last_jet_idx = 0;
+ for (unsigned int i = 0; i < B0; i++)
+ jets_0_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B1; i++)
+ jets_1_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B2; i++)
+ jets_2_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B3; i++)
+ jets_3_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B4; i++)
+ jets_4_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B5; i++)
+ jets_5_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B6; i++)
+ jets_6_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B7; i++)
+ jets_7_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B8; i++)
+ jets_8_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B9; i++)
+ jets_9_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B10; i++)
+ jets_10_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B11; i++)
+ jets_11_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B12; i++)
+ jets_12_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B13; i++)
+ jets_13_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B14; i++)
+ jets_14_[i] = WolfJet(0, last_jet_idx++);
+ }
+
+ ~FactorAutodiff() override = default;
+
+ JacobianMethod getJacobianMethod() const override
+ {
+ return JAC_AUTO;
+ }
+
+ /** \brief Returns the residual and jacobians given the state values
+ *
+ * Returns the residual and jacobians given the state values
+ *
+ **/
+ bool evaluate(double const* const* parameters, double* residuals, double** jacobians) const override
+ {
+ // only residuals
+ if (jacobians == nullptr)
+ {
+ (*static_cast<FacT const*>(this))(parameters[0],
+ parameters[1],
+ parameters[2],
+ parameters[3],
+ parameters[4],
+ parameters[5],
+ parameters[6],
+ parameters[7],
+ parameters[8],
+ parameters[9],
+ parameters[10],
+ parameters[11],
+ parameters[12],
+ parameters[13],
+ parameters[14],
+ residuals);
+ }
+ // also compute jacobians
+ else
+ {
+ // update jets real part
+ std::vector<double const*> param_vec;
+ param_vec.assign(parameters,parameters+n_blocks);
+ updateJetsRealPart(param_vec);
+
+ // call functor
+ (*static_cast<FacT const*>(this))(jets_0_.data(),
+ jets_1_.data(),
+ jets_2_.data(),
+ jets_3_.data(),
+ jets_4_.data(),
+ jets_5_.data(),
+ jets_6_.data(),
+ jets_7_.data(),
+ jets_8_.data(),
+ jets_9_.data(),
+ jets_10_.data(),
+ jets_11_.data(),
+ jets_12_.data(),
+ jets_13_.data(),
+ jets_14_.data(),
+ residuals_jets_.data());
+
+ // fill the residual array
+ for (unsigned int i = 0; i < RES; i++)
+ residuals[i] = residuals_jets_[i].a;
+
+ // fill the jacobian matrices
+ for (unsigned int i = 0; i<n_blocks; i++)
+ if (jacobians[i] != nullptr)
+ for (unsigned int row = 0; row < RES; row++)
+ std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
+ residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
+ jacobians[i] + row * state_block_sizes_.at(i));
+ }
+ return true;
+ }
+
+ /** \brief Updates all jets real part with values of parameters
+ *
+ **/
+ void updateJetsRealPart(const std::vector<double const*>& parameters) const
+ {
+ // update jets real part
+ for (unsigned int i = 0; i < B0; i++)
+ jets_0_[i].a = parameters[0][i];
+ for (unsigned int i = 0; i < B1; i++)
+ jets_1_[i].a = parameters[1][i];
+ for (unsigned int i = 0; i < B2; i++)
+ jets_2_[i].a = parameters[2][i];
+ for (unsigned int i = 0; i < B3; i++)
+ jets_3_[i].a = parameters[3][i];
+ for (unsigned int i = 0; i < B4; i++)
+ jets_4_[i].a = parameters[4][i];
+ for (unsigned int i = 0; i < B5; i++)
+ jets_5_[i].a = parameters[5][i];
+ for (unsigned int i = 0; i < B6; i++)
+ jets_6_[i].a = parameters[6][i];
+ for (unsigned int i = 0; i < B7; i++)
+ jets_7_[i].a = parameters[7][i];
+ for (unsigned int i = 0; i < B8; i++)
+ jets_8_[i].a = parameters[8][i];
+ for (unsigned int i = 0; i < B9; i++)
+ jets_9_[i].a = parameters[9][i];
+ for (unsigned int i = 0; i < B10; i++)
+ jets_10_[i].a = parameters[10][i];
+ for (unsigned int i = 0; i < B11; i++)
+ jets_11_[i].a = parameters[11][i];
+ for (unsigned int i = 0; i < B12; i++)
+ jets_12_[i].a = parameters[12][i];
+ for (unsigned int i = 0; i < B13; i++)
+ jets_13_[i].a = parameters[13][i];
+ for (unsigned int i = 0; i < B14; i++)
+ jets_14_[i].a = parameters[14][i];
+ }
+
+ /** \brief Returns a vector of Jacobian matrix corresponding to each state block evaluated in the point provided in _states_ptr
+ *
+ **/
+ void evaluate(const std::vector<const double*>& _states_ptr, Eigen::VectorXd& residual_, std::vector<Eigen::MatrixXd>& jacobians_) const override
+ {
+ assert(residual_.size() == RES);
+ jacobians_.clear();
+
+ assert(_states_ptr.size() == n_blocks);
+
+ // update jets real part
+ updateJetsRealPart(_states_ptr);
+
+ // call functor
+ (*static_cast<FacT const*>(this))(jets_0_.data(),
+ jets_1_.data(),
+ jets_2_.data(),
+ jets_3_.data(),
+ jets_4_.data(),
+ jets_5_.data(),
+ jets_6_.data(),
+ jets_7_.data(),
+ jets_8_.data(),
+ jets_9_.data(),
+ jets_10_.data(),
+ jets_11_.data(),
+ jets_12_.data(),
+ jets_13_.data(),
+ jets_14_.data(),
+ residuals_jets_.data());
+
+ // fill the residual vector
+ for (unsigned int i = 0; i < RES; i++)
+ residual_(i) = residuals_jets_[i].a;
+
+ // fill the jacobian matrices
+ for (unsigned int i = 0; i<n_blocks; i++)
+ {
+ // Create a row major Jacobian
+ Eigen::MatrixRowXd Ji(Eigen::MatrixRowXd::Zero(RES, state_block_sizes_[i]));
+ // Fill Jacobian rows from jets
+ if (!state_ptrs_[i]->isFixed())
+ for (unsigned int row = 0; row < RES; row++)
+ std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
+ residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
+ Ji.data() + row * state_block_sizes_.at(i));
+ // add to the Jacobians vector
+ jacobians_.push_back(Ji);
+ }
+ }
+
+ /** \brief Returns a vector of pointers to the states
+ *
+ * Returns a vector of pointers to the state in which this factor depends
+ *
+ **/
+ std::vector<StateBlockConstPtr> getStateBlockPtrVector() const override
+ {
+ return state_ptrs_const_;
+ }
+ std::vector<StateBlockPtr> getStateBlockPtrVector() override
+ {
+ return state_ptrs_;
+ }
+
+ /** \brief Returns a vector of the states sizes
+ *
+ **/
+ std::vector<unsigned int> getStateSizes() const override
+ {
+ return state_block_sizes_;
+ }
+
+ /** \brief Returns the residual size
+ *
+ * Returns the residual size
+ *
+ **/
+ unsigned int getSize() const override
+ {
+ return RES;
+ }
+};
+
+////////////////// SPECIALIZATION 14 BLOCKS ////////////////////////////////////////////////////////////////////////
+
+template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6,unsigned int B7,unsigned int B8,unsigned int B9,unsigned int B10,unsigned int B11,unsigned int B12,unsigned int B13>
+class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,B9,B10,B11,B12,B13,0> : public FactorBase
+{
+ public:
+ static const unsigned int n_blocks = 14;
+
+ static const std::vector<unsigned int> state_block_sizes_;
+
+ typedef ceres::Jet<double, B0 + B1 + B2 + B3 + B4 + B5 + B6 +
+ B7 + B8 + B9 + B10 + B11 + B12 + B13> WolfJet;
+
+ protected:
+
+ std::vector<StateBlockPtr> state_ptrs_;
+ std::vector<StateBlockConstPtr> state_ptrs_const_;
+
+ static const std::vector<unsigned int> jacobian_locations_;
+ mutable std::array<WolfJet, RES> residuals_jets_;
+ mutable std::array<WolfJet, B0> jets_0_;
+ mutable std::array<WolfJet, B1> jets_1_;
+ mutable std::array<WolfJet, B2> jets_2_;
+ mutable std::array<WolfJet, B3> jets_3_;
+ mutable std::array<WolfJet, B4> jets_4_;
+ mutable std::array<WolfJet, B5> jets_5_;
+ mutable std::array<WolfJet, B6> jets_6_;
+ mutable std::array<WolfJet, B7> jets_7_;
+ mutable std::array<WolfJet, B8> jets_8_;
+ mutable std::array<WolfJet, B9> jets_9_;
+ mutable std::array<WolfJet, B10> jets_10_;
+ mutable std::array<WolfJet, B11> jets_11_;
+ mutable std::array<WolfJet, B12> jets_12_;
+ mutable std::array<WolfJet, B13> jets_13_;
+
+ public:
+
+ FactorAutodiff(const std::string& _tp,
+ const FactorTopology& _top,
+ const FeatureBasePtr& _feature_ptr,
+ const FrameBasePtr& _frame_other_ptr,
+ const CaptureBasePtr& _capture_other_ptr,
+ const FeatureBasePtr& _feature_other_ptr,
+ const LandmarkBasePtr& _landmark_other_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status,
+ StateBlockPtr _state0Ptr,
+ StateBlockPtr _state1Ptr,
+ StateBlockPtr _state2Ptr,
+ StateBlockPtr _state3Ptr,
+ StateBlockPtr _state4Ptr,
+ StateBlockPtr _state5Ptr,
+ StateBlockPtr _state6Ptr,
+ StateBlockPtr _state7Ptr,
+ StateBlockPtr _state8Ptr,
+ StateBlockPtr _state9Ptr,
+ StateBlockPtr _state10Ptr,
+ StateBlockPtr _state11Ptr,
+ StateBlockPtr _state12Ptr,
+ StateBlockPtr _state13Ptr) :
+ FactorAutodiff(_tp,
+ _top,
+ _feature_ptr,
+ _frame_other_ptr ? FrameBasePtrList({_frame_other_ptr}) : FrameBasePtrList(),
+ _capture_other_ptr ? CaptureBasePtrList({_capture_other_ptr}) : CaptureBasePtrList(),
+ _feature_other_ptr ? FeatureBasePtrList({_feature_other_ptr}) : FeatureBasePtrList(),
+ _landmark_other_ptr ? LandmarkBasePtrList({_landmark_other_ptr}) : LandmarkBasePtrList(),
+ _processor_ptr,
+ _apply_loss_function,
+ _status,
+ _state0Ptr,
+ _state1Ptr,
+ _state2Ptr,
+ _state3Ptr,
+ _state4Ptr,
+ _state5Ptr,
+ _state6Ptr,
+ _state7Ptr,
+ _state8Ptr,
+ _state9Ptr,
+ _state10Ptr,
+ _state11Ptr,
+ _state12Ptr,
+ _state13Ptr){}
+
+ FactorAutodiff(const std::string& _tp,
+ const FactorTopology& _top,
+ const FeatureBasePtr& _feature_ptr,
+ const FrameBasePtrList& _frame_other_list,
+ const CaptureBasePtrList& _capture_other_list,
+ const FeatureBasePtrList& _feature_other_list,
+ const LandmarkBasePtrList& _landmark_other_list,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status,
+ StateBlockPtr _state0Ptr,
+ StateBlockPtr _state1Ptr,
+ StateBlockPtr _state2Ptr,
+ StateBlockPtr _state3Ptr,
+ StateBlockPtr _state4Ptr,
+ StateBlockPtr _state5Ptr,
+ StateBlockPtr _state6Ptr,
+ StateBlockPtr _state7Ptr,
+ StateBlockPtr _state8Ptr,
+ StateBlockPtr _state9Ptr,
+ StateBlockPtr _state10Ptr,
+ StateBlockPtr _state11Ptr,
+ StateBlockPtr _state12Ptr,
+ StateBlockPtr _state13Ptr) :
+ FactorBase(_tp, _top, _feature_ptr, _frame_other_list, _capture_other_list, _feature_other_list, _landmark_other_list, _processor_ptr, _apply_loss_function, _status),
+ state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr,_state5Ptr,_state6Ptr,_state7Ptr,_state8Ptr,_state9Ptr,_state10Ptr,_state11Ptr, _state12Ptr, _state13Ptr}),
+ state_ptrs_const_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr,_state5Ptr,_state6Ptr,_state7Ptr,_state8Ptr,_state9Ptr,_state10Ptr,_state11Ptr,_state12Ptr, _state13Ptr})
+ {
+ // asserts for null states
+ assert(state_ptrs_.size() == n_blocks && "state_ptrs wrongly initialized");
+ assert(state_ptrs_const_.size() == n_blocks && "state_ptrs wrongly initialized");
+ assert(_state0Ptr != nullptr && "nullptr state block");
+ assert(_state1Ptr != nullptr && "nullptr state block");
+ assert(_state2Ptr != nullptr && "nullptr state block");
+ assert(_state3Ptr != nullptr && "nullptr state block");
+ assert(_state4Ptr != nullptr && "nullptr state block");
+ assert(_state5Ptr != nullptr && "nullptr state block");
+ assert(_state6Ptr != nullptr && "nullptr state block");
+ assert(_state7Ptr != nullptr && "nullptr state block");
+ assert(_state8Ptr != nullptr && "nullptr state block");
+ assert(_state9Ptr != nullptr && "nullptr state block");
+ assert(_state10Ptr != nullptr && "nullptr state block");
+ assert(_state11Ptr != nullptr && "nullptr state block");
+ assert(_state12Ptr != nullptr && "nullptr state block");
+ assert(_state13Ptr != nullptr && "nullptr state block");
+
+ // initialize jets
+ unsigned int last_jet_idx = 0;
+ for (unsigned int i = 0; i < B0; i++)
+ jets_0_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B1; i++)
+ jets_1_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B2; i++)
+ jets_2_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B3; i++)
+ jets_3_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B4; i++)
+ jets_4_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B5; i++)
+ jets_5_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B6; i++)
+ jets_6_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B7; i++)
+ jets_7_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B8; i++)
+ jets_8_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B9; i++)
+ jets_9_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B10; i++)
+ jets_10_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B11; i++)
+ jets_11_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B12; i++)
+ jets_12_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B13; i++)
+ jets_13_[i] = WolfJet(0, last_jet_idx++);
+ }
+
+ ~FactorAutodiff() override = default;
+
+ JacobianMethod getJacobianMethod() const override
+ {
+ return JAC_AUTO;
+ }
+
+ bool evaluate(double const* const* parameters, double* residuals, double** jacobians) const override
+ {
+ // only residuals
+ if (jacobians == nullptr)
+ {
+ (*static_cast<FacT const*>(this))(parameters[0],
+ parameters[1],
+ parameters[2],
+ parameters[3],
+ parameters[4],
+ parameters[5],
+ parameters[6],
+ parameters[7],
+ parameters[8],
+ parameters[9],
+ parameters[10],
+ parameters[11],
+ parameters[12],
+ parameters[13],
+ residuals);
+ }
+ // also compute jacobians
+ else
+ {
+ // update jets real part
+ std::vector<double const*> param_vec;
+ param_vec.assign(parameters,parameters+n_blocks);
+ updateJetsRealPart(param_vec);
+
+ // call functor
+ (*static_cast<FacT const*>(this))(jets_0_.data(),
+ jets_1_.data(),
+ jets_2_.data(),
+ jets_3_.data(),
+ jets_4_.data(),
+ jets_5_.data(),
+ jets_6_.data(),
+ jets_7_.data(),
+ jets_8_.data(),
+ jets_9_.data(),
+ jets_10_.data(),
+ jets_11_.data(),
+ jets_12_.data(),
+ jets_13_.data(),
+ residuals_jets_.data());
+
+ // fill the residual array
+ for (unsigned int i = 0; i < RES; i++)
+ residuals[i] = residuals_jets_[i].a;
+
+ // fill the jacobian matrices
+ for (unsigned int i = 0; i<n_blocks; i++)
+ if (jacobians[i] != nullptr)
+ for (unsigned int row = 0; row < RES; row++)
+ std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
+ residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
+ jacobians[i] + row * state_block_sizes_.at(i));
+ }
+ return true;
+ }
+
+ void updateJetsRealPart(const std::vector<double const*>& parameters) const
+ {
+ // update jets real part
+ for (unsigned int i = 0; i < B0; i++)
+ jets_0_[i].a = parameters[0][i];
+ for (unsigned int i = 0; i < B1; i++)
+ jets_1_[i].a = parameters[1][i];
+ for (unsigned int i = 0; i < B2; i++)
+ jets_2_[i].a = parameters[2][i];
+ for (unsigned int i = 0; i < B3; i++)
+ jets_3_[i].a = parameters[3][i];
+ for (unsigned int i = 0; i < B4; i++)
+ jets_4_[i].a = parameters[4][i];
+ for (unsigned int i = 0; i < B5; i++)
+ jets_5_[i].a = parameters[5][i];
+ for (unsigned int i = 0; i < B6; i++)
+ jets_6_[i].a = parameters[6][i];
+ for (unsigned int i = 0; i < B7; i++)
+ jets_7_[i].a = parameters[7][i];
+ for (unsigned int i = 0; i < B8; i++)
+ jets_8_[i].a = parameters[8][i];
+ for (unsigned int i = 0; i < B9; i++)
+ jets_9_[i].a = parameters[9][i];
+ for (unsigned int i = 0; i < B10; i++)
+ jets_10_[i].a = parameters[10][i];
+ for (unsigned int i = 0; i < B11; i++)
+ jets_11_[i].a = parameters[11][i];
+ for (unsigned int i = 0; i < B12; i++)
+ jets_12_[i].a = parameters[12][i];
+ for (unsigned int i = 0; i < B13; i++)
+ jets_13_[i].a = parameters[13][i];
+ }
+
+ void evaluate(const std::vector<const double*>& _states_ptr, Eigen::VectorXd& residual_, std::vector<Eigen::MatrixXd>& jacobians_) const override
+ {
+ assert(residual_.size() == RES);
+ jacobians_.clear();
+
+ assert(_states_ptr.size() == n_blocks);
+
+ // update jets real part
+ updateJetsRealPart(_states_ptr);
+
+ // call functor
+ (*static_cast<FacT const*>(this))(jets_0_.data(),
+ jets_1_.data(),
+ jets_2_.data(),
+ jets_3_.data(),
+ jets_4_.data(),
+ jets_5_.data(),
+ jets_6_.data(),
+ jets_7_.data(),
+ jets_8_.data(),
+ jets_9_.data(),
+ jets_10_.data(),
+ jets_11_.data(),
+ jets_12_.data(),
+ jets_13_.data(),
+ residuals_jets_.data());
+
+ // fill the residual vector
+ for (unsigned int i = 0; i < RES; i++)
+ residual_(i) = residuals_jets_[i].a;
+
+ // fill the jacobian matrices
+ for (unsigned int i = 0; i<n_blocks; i++)
+ {
+ // Create a row major Jacobian
+ Eigen::MatrixRowXd Ji(Eigen::MatrixRowXd::Zero(RES, state_block_sizes_[i]));
+ // Fill Jacobian rows from jets
+ if (!state_ptrs_[i]->isFixed())
+ for (unsigned int row = 0; row < RES; row++)
+ std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
+ residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
+ Ji.data() + row * state_block_sizes_.at(i));
+ // add to the Jacobians vector
+ jacobians_.push_back(Ji);
+ }
+ }
+
+ std::vector<StateBlockConstPtr> getStateBlockPtrVector() const override
+ {
+ return state_ptrs_const_;
+ }
+ std::vector<StateBlockPtr> getStateBlockPtrVector() override
+ {
+ return state_ptrs_;
+ }
+
+ std::vector<unsigned int> getStateSizes() const override
+ {
+ return state_block_sizes_;
+ }
+
+ unsigned int getSize() const override
+ {
+ return RES;
+ }
+};
+
+////////////////// SPECIALIZATION 13 BLOCKS ////////////////////////////////////////////////////////////////////////
+
+template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6,unsigned int B7,unsigned int B8,unsigned int B9,unsigned int B10,unsigned int B11,unsigned int B12>
+class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,B9,B10,B11,B12,0,0> : public FactorBase
+{
+ public:
+ static const unsigned int n_blocks = 13;
+
+ static const std::vector<unsigned int> state_block_sizes_;
+
+ typedef ceres::Jet<double, B0 + B1 + B2 + B3 + B4 + B5 + B6 +
+ B7 + B8 + B9 + B10 + B11 + B12> WolfJet;
+
+ protected:
+
+ std::vector<StateBlockPtr> state_ptrs_;
+ std::vector<StateBlockConstPtr> state_ptrs_const_;
+
+ static const std::vector<unsigned int> jacobian_locations_;
+ mutable std::array<WolfJet, RES> residuals_jets_;
+ mutable std::array<WolfJet, B0> jets_0_;
+ mutable std::array<WolfJet, B1> jets_1_;
+ mutable std::array<WolfJet, B2> jets_2_;
+ mutable std::array<WolfJet, B3> jets_3_;
+ mutable std::array<WolfJet, B4> jets_4_;
+ mutable std::array<WolfJet, B5> jets_5_;
+ mutable std::array<WolfJet, B6> jets_6_;
+ mutable std::array<WolfJet, B7> jets_7_;
+ mutable std::array<WolfJet, B8> jets_8_;
+ mutable std::array<WolfJet, B9> jets_9_;
+ mutable std::array<WolfJet, B10> jets_10_;
+ mutable std::array<WolfJet, B11> jets_11_;
+ mutable std::array<WolfJet, B12> jets_12_;
+
+ public:
+
+ FactorAutodiff(const std::string& _tp,
+ const FactorTopology& _top,
+ const FeatureBasePtr& _feature_ptr,
+ const FrameBasePtr& _frame_other_ptr,
+ const CaptureBasePtr& _capture_other_ptr,
+ const FeatureBasePtr& _feature_other_ptr,
+ const LandmarkBasePtr& _landmark_other_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status,
+ StateBlockPtr _state0Ptr,
+ StateBlockPtr _state1Ptr,
+ StateBlockPtr _state2Ptr,
+ StateBlockPtr _state3Ptr,
+ StateBlockPtr _state4Ptr,
+ StateBlockPtr _state5Ptr,
+ StateBlockPtr _state6Ptr,
+ StateBlockPtr _state7Ptr,
+ StateBlockPtr _state8Ptr,
+ StateBlockPtr _state9Ptr,
+ StateBlockPtr _state10Ptr,
+ StateBlockPtr _state11Ptr,
+ StateBlockPtr _state12Ptr) :
+ FactorAutodiff(_tp,
+ _top,
+ _feature_ptr,
+ _frame_other_ptr ? FrameBasePtrList({_frame_other_ptr}) : FrameBasePtrList(),
+ _capture_other_ptr ? CaptureBasePtrList({_capture_other_ptr}) : CaptureBasePtrList(),
+ _feature_other_ptr ? FeatureBasePtrList({_feature_other_ptr}) : FeatureBasePtrList(),
+ _landmark_other_ptr ? LandmarkBasePtrList({_landmark_other_ptr}) : LandmarkBasePtrList(),
+ _processor_ptr,
+ _apply_loss_function,
+ _status,
+ _state0Ptr,
+ _state1Ptr,
+ _state2Ptr,
+ _state3Ptr,
+ _state4Ptr,
+ _state5Ptr,
+ _state6Ptr,
+ _state7Ptr,
+ _state8Ptr,
+ _state9Ptr,
+ _state10Ptr,
+ _state11Ptr,
+ _state12Ptr){}
+
+ FactorAutodiff(const std::string& _tp,
+ const FactorTopology& _top,
+ const FeatureBasePtr& _feature_ptr,
+ const FrameBasePtrList& _frame_other_list,
+ const CaptureBasePtrList& _capture_other_list,
+ const FeatureBasePtrList& _feature_other_list,
+ const LandmarkBasePtrList& _landmark_other_list,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status,
+ StateBlockPtr _state0Ptr,
+ StateBlockPtr _state1Ptr,
+ StateBlockPtr _state2Ptr,
+ StateBlockPtr _state3Ptr,
+ StateBlockPtr _state4Ptr,
+ StateBlockPtr _state5Ptr,
+ StateBlockPtr _state6Ptr,
+ StateBlockPtr _state7Ptr,
+ StateBlockPtr _state8Ptr,
+ StateBlockPtr _state9Ptr,
+ StateBlockPtr _state10Ptr,
+ StateBlockPtr _state11Ptr,
+ StateBlockPtr _state12Ptr) :
+ FactorBase(_tp, _top, _feature_ptr, _frame_other_list, _capture_other_list, _feature_other_list, _landmark_other_list, _processor_ptr, _apply_loss_function, _status),
+ state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr,_state5Ptr,_state6Ptr,_state7Ptr,_state8Ptr,_state9Ptr,_state10Ptr,_state11Ptr,_state12Ptr}),
+ state_ptrs_const_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr,_state5Ptr,_state6Ptr,_state7Ptr,_state8Ptr,_state9Ptr,_state10Ptr,_state11Ptr,_state12Ptr})
+ {
+ // asserts for null states
+ assert(state_ptrs_.size() == n_blocks && "state_ptrs wrongly initialized");
+ assert(state_ptrs_const_.size() == n_blocks && "state_ptrs wrongly initialized");
+ assert(_state0Ptr != nullptr && "nullptr state block");
+ assert(_state1Ptr != nullptr && "nullptr state block");
+ assert(_state2Ptr != nullptr && "nullptr state block");
+ assert(_state3Ptr != nullptr && "nullptr state block");
+ assert(_state4Ptr != nullptr && "nullptr state block");
+ assert(_state5Ptr != nullptr && "nullptr state block");
+ assert(_state6Ptr != nullptr && "nullptr state block");
+ assert(_state7Ptr != nullptr && "nullptr state block");
+ assert(_state8Ptr != nullptr && "nullptr state block");
+ assert(_state9Ptr != nullptr && "nullptr state block");
+ assert(_state10Ptr != nullptr && "nullptr state block");
+ assert(_state11Ptr != nullptr && "nullptr state block");
+ assert(_state12Ptr != nullptr && "nullptr state block");
+
+ // initialize jets
+ unsigned int last_jet_idx = 0;
+ for (unsigned int i = 0; i < B0; i++)
+ jets_0_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B1; i++)
+ jets_1_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B2; i++)
+ jets_2_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B3; i++)
+ jets_3_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B4; i++)
+ jets_4_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B5; i++)
+ jets_5_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B6; i++)
+ jets_6_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B7; i++)
+ jets_7_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B8; i++)
+ jets_8_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B9; i++)
+ jets_9_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B10; i++)
+ jets_10_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B11; i++)
+ jets_11_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B12; i++)
+ jets_12_[i] = WolfJet(0, last_jet_idx++);
+ }
+
+ ~FactorAutodiff() override = default;
+
+ JacobianMethod getJacobianMethod() const override
+ {
+ return JAC_AUTO;
+ }
+
+ bool evaluate(double const* const* parameters, double* residuals, double** jacobians) const override
+ {
+ // only residuals
+ if (jacobians == nullptr)
+ {
+ (*static_cast<FacT const*>(this))(parameters[0],
+ parameters[1],
+ parameters[2],
+ parameters[3],
+ parameters[4],
+ parameters[5],
+ parameters[6],
+ parameters[7],
+ parameters[8],
+ parameters[9],
+ parameters[10],
+ parameters[11],
+ parameters[12],
+ residuals);
+ }
+ // also compute jacobians
+ else
+ {
+ // update jets real part
+ std::vector<double const*> param_vec;
+ param_vec.assign(parameters,parameters+n_blocks);
+ updateJetsRealPart(param_vec);
+
+ // call functor
+ (*static_cast<FacT const*>(this))(jets_0_.data(),
+ jets_1_.data(),
+ jets_2_.data(),
+ jets_3_.data(),
+ jets_4_.data(),
+ jets_5_.data(),
+ jets_6_.data(),
+ jets_7_.data(),
+ jets_8_.data(),
+ jets_9_.data(),
+ jets_10_.data(),
+ jets_11_.data(),
+ jets_12_.data(),
+ residuals_jets_.data());
+
+ // fill the residual array
+ for (unsigned int i = 0; i < RES; i++)
+ residuals[i] = residuals_jets_[i].a;
+
+ // fill the jacobian matrices
+ for (unsigned int i = 0; i<n_blocks; i++)
+ if (jacobians[i] != nullptr)
+ for (unsigned int row = 0; row < RES; row++)
+ std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
+ residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
+ jacobians[i] + row * state_block_sizes_.at(i));
+ }
+ return true;
+ }
+
+ void updateJetsRealPart(const std::vector<double const*>& parameters) const
+ {
+ // update jets real part
+ for (unsigned int i = 0; i < B0; i++)
+ jets_0_[i].a = parameters[0][i];
+ for (unsigned int i = 0; i < B1; i++)
+ jets_1_[i].a = parameters[1][i];
+ for (unsigned int i = 0; i < B2; i++)
+ jets_2_[i].a = parameters[2][i];
+ for (unsigned int i = 0; i < B3; i++)
+ jets_3_[i].a = parameters[3][i];
+ for (unsigned int i = 0; i < B4; i++)
+ jets_4_[i].a = parameters[4][i];
+ for (unsigned int i = 0; i < B5; i++)
+ jets_5_[i].a = parameters[5][i];
+ for (unsigned int i = 0; i < B6; i++)
+ jets_6_[i].a = parameters[6][i];
+ for (unsigned int i = 0; i < B7; i++)
+ jets_7_[i].a = parameters[7][i];
+ for (unsigned int i = 0; i < B8; i++)
+ jets_8_[i].a = parameters[8][i];
+ for (unsigned int i = 0; i < B9; i++)
+ jets_9_[i].a = parameters[9][i];
+ for (unsigned int i = 0; i < B10; i++)
+ jets_10_[i].a = parameters[10][i];
+ for (unsigned int i = 0; i < B11; i++)
+ jets_11_[i].a = parameters[11][i];
+ for (unsigned int i = 0; i < B12; i++)
+ jets_12_[i].a = parameters[12][i];
+ }
+
+ void evaluate(const std::vector<const double*>& _states_ptr, Eigen::VectorXd& residual_, std::vector<Eigen::MatrixXd>& jacobians_) const override
+ {
+ assert(residual_.size() == RES);
+ jacobians_.clear();
+
+ assert(_states_ptr.size() == n_blocks);
+
+ // update jets real part
+ updateJetsRealPart(_states_ptr);
+
+ // call functor
+ (*static_cast<FacT const*>(this))(jets_0_.data(),
+ jets_1_.data(),
+ jets_2_.data(),
+ jets_3_.data(),
+ jets_4_.data(),
+ jets_5_.data(),
+ jets_6_.data(),
+ jets_7_.data(),
+ jets_8_.data(),
+ jets_9_.data(),
+ jets_10_.data(),
+ jets_11_.data(),
+ jets_12_.data(),
+ residuals_jets_.data());
+
+ // fill the residual vector
+ for (unsigned int i = 0; i < RES; i++)
+ residual_(i) = residuals_jets_[i].a;
+
+ // fill the jacobian matrices
+ for (unsigned int i = 0; i<n_blocks; i++)
+ {
+ // Create a row major Jacobian
+ Eigen::MatrixRowXd Ji(Eigen::MatrixRowXd::Zero(RES, state_block_sizes_[i]));
+ // Fill Jacobian rows from jets
+ if (!state_ptrs_[i]->isFixed())
+ for (unsigned int row = 0; row < RES; row++)
+ std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
+ residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
+ Ji.data() + row * state_block_sizes_.at(i));
+ // add to the Jacobians vector
+ jacobians_.push_back(Ji);
+ }
+ }
- static const std::vector<unsigned int> state_block_sizes_;
+ std::vector<StateBlockConstPtr> getStateBlockPtrVector() const override
+ {
+ return state_ptrs_const_;
+ }
+ std::vector<StateBlockPtr> getStateBlockPtrVector() override
+ {
+ return state_ptrs_;
+ }
+
+ std::vector<unsigned int> getStateSizes() const override
+ {
+ return state_block_sizes_;
+ }
+
+ unsigned int getSize() const override
+ {
+ return RES;
+ }
+};
- typedef ceres::Jet<double, B0 + B1 + B2 + B3 + B4 +
- B5 + B6 + B7 + B8 + B9 +
- B10 + B11> WolfJet;
+////////////////// SPECIALIZATION 12 BLOCKS ////////////////////////////////////////////////////////////////////////
+template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6,unsigned int B7,unsigned int B8,unsigned int B9,unsigned int B10,unsigned int B11>
+class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,B9,B10,B11,0,0,0> : public FactorBase
+{
+ public:
+ static const unsigned int n_blocks = 12;
+
+ static const std::vector<unsigned int> state_block_sizes_;
+
+ typedef ceres::Jet<double, B0 + B1 + B2 + B3 + B4 + B5 +
+ B6 + B7 + B8 + B9 + B10 + B11> WolfJet;
+
protected:
-
+
std::vector<StateBlockPtr> state_ptrs_;
std::vector<StateBlockConstPtr> state_ptrs_const_;
-
+
static const std::vector<unsigned int> jacobian_locations_;
mutable std::array<WolfJet, RES> residuals_jets_;
mutable std::array<WolfJet, B0> jets_0_;
@@ -81,10 +1087,9 @@ class FactorAutodiff : public FactorBase
mutable std::array<WolfJet, B9> jets_9_;
mutable std::array<WolfJet, B10> jets_10_;
mutable std::array<WolfJet, B11> jets_11_;
-
+
public:
- /** \brief Constructor valid for all categories (ABSOLUTE, FRAME, FEATURE, LANDMARK)
- **/
+
FactorAutodiff(const std::string& _tp,
const FactorTopology& _top,
const FeatureBasePtr& _feature_ptr,
@@ -171,7 +1176,7 @@ class FactorAutodiff : public FactorBase
assert(_state9Ptr != nullptr && "nullptr state block");
assert(_state10Ptr != nullptr && "nullptr state block");
assert(_state11Ptr != nullptr && "nullptr state block");
-
+
// initialize jets
unsigned int last_jet_idx = 0;
for (unsigned int i = 0; i < B0; i++)
@@ -198,22 +1203,16 @@ class FactorAutodiff : public FactorBase
jets_10_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B11; i++)
jets_11_[i] = WolfJet(0, last_jet_idx++);
+
}
-
- ~FactorAutodiff() override
- {
- }
-
+
+ ~FactorAutodiff() override = default;
+
JacobianMethod getJacobianMethod() const override
{
return JAC_AUTO;
}
-
- /** \brief Returns the residual and jacobians given the state values
- *
- * Returns the residual and jacobians given the state values
- *
- **/
+
bool evaluate(double const* const* parameters, double* residuals, double** jacobians) const override
{
// only residuals
@@ -240,7 +1239,7 @@ class FactorAutodiff : public FactorBase
std::vector<double const*> param_vec;
param_vec.assign(parameters,parameters+n_blocks);
updateJetsRealPart(param_vec);
-
+
// call functor
(*static_cast<FacT const*>(this))(jets_0_.data(),
jets_1_.data(),
@@ -255,11 +1254,11 @@ class FactorAutodiff : public FactorBase
jets_10_.data(),
jets_11_.data(),
residuals_jets_.data());
-
+
// fill the residual array
for (unsigned int i = 0; i < RES; i++)
residuals[i] = residuals_jets_[i].a;
-
+
// fill the jacobian matrices
for (unsigned int i = 0; i<n_blocks; i++)
if (jacobians[i] != nullptr)
@@ -270,10 +1269,7 @@ class FactorAutodiff : public FactorBase
}
return true;
}
-
- /** \brief Updates all jets real part with values of parameters
- *
- **/
+
void updateJetsRealPart(const std::vector<double const*>& parameters) const
{
// update jets real part
@@ -302,20 +1298,17 @@ class FactorAutodiff : public FactorBase
for (unsigned int i = 0; i < B11; i++)
jets_11_[i].a = parameters[11][i];
}
-
- /** \brief Returns a vector of Jacobian matrix corresponding to each state block evaluated in the point provided in _states_ptr
- *
- **/
+
void evaluate(const std::vector<const double*>& _states_ptr, Eigen::VectorXd& residual_, std::vector<Eigen::MatrixXd>& jacobians_) const override
{
assert(residual_.size() == RES);
jacobians_.clear();
-
+
assert(_states_ptr.size() == n_blocks);
-
+
// update jets real part
updateJetsRealPart(_states_ptr);
-
+
// call functor
(*static_cast<FacT const*>(this))(jets_0_.data(),
jets_1_.data(),
@@ -330,11 +1323,11 @@ class FactorAutodiff : public FactorBase
jets_10_.data(),
jets_11_.data(),
residuals_jets_.data());
-
+
// fill the residual vector
for (unsigned int i = 0; i < RES; i++)
residual_(i) = residuals_jets_[i].a;
-
+
// fill the jacobian matrices
for (unsigned int i = 0; i<n_blocks; i++)
{
@@ -349,17 +1342,8 @@ class FactorAutodiff : public FactorBase
// add to the Jacobians vector
jacobians_.push_back(Ji);
}
-
- // print jacobian matrices
- // for (unsigned int i = 0; i < n_blocks; i++)
- // std::cout << jacobians_[i] << std::endl << std::endl;
}
- /** \brief Returns a vector of pointers to the states
- *
- * Returns a vector of pointers to the state in which this factor depends
- *
- **/
std::vector<StateBlockConstPtr> getStateBlockPtrVector() const override
{
return state_ptrs_const_;
@@ -368,47 +1352,24 @@ class FactorAutodiff : public FactorBase
{
return state_ptrs_;
}
-
- /** \brief Returns a vector of the states sizes
- *
- **/
+
std::vector<unsigned int> getStateSizes() const override
{
return state_block_sizes_;
}
-
- /** \brief Returns the residual size
- *
- * Returns the residual size
- *
- **/
+
unsigned int getSize() const override
{
return RES;
}
};
-
////////////////// SPECIALIZATION 11 BLOCKS ////////////////////////////////////////////////////////////////////////
template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6,unsigned int B7,unsigned int B8,unsigned int B9,unsigned int B10>
-class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,B9,B10,0> : public FactorBase
+class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,B9,B10,0,0,0,0> : public FactorBase
{
public:
-
- static const unsigned int residualSize = RES;
- static const unsigned int block0Size = B0;
- static const unsigned int block1Size = B1;
- static const unsigned int block2Size = B2;
- static const unsigned int block3Size = B3;
- static const unsigned int block4Size = B4;
- static const unsigned int block5Size = B5;
- static const unsigned int block6Size = B6;
- static const unsigned int block7Size = B7;
- static const unsigned int block8Size = B8;
- static const unsigned int block9Size = B9;
- static const unsigned int block10Size = B10;
- static const unsigned int block11Size = 0;
static const unsigned int n_blocks = 11;
static const std::vector<unsigned int> state_block_sizes_;
@@ -547,9 +1508,7 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,B9,B10,0> : public Fact
}
- ~FactorAutodiff() override
- {
- }
+ ~FactorAutodiff() override = default;
JacobianMethod getJacobianMethod() const override
{
@@ -680,10 +1639,6 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,B9,B10,0> : public Fact
// add to the Jacobians vector
jacobians_.push_back(Ji);
}
-
- // print jacobian matrices
- // for (unsigned int i = 0; i < n_blocks; i++)
- // std::cout << jacobians_[i] << std::endl << std::endl;
}
std::vector<StateBlockConstPtr> getStateBlockPtrVector() const override
@@ -709,23 +1664,9 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,B9,B10,0> : public Fact
////////////////// SPECIALIZATION 10 BLOCKS ////////////////////////////////////////////////////////////////////////
template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6,unsigned int B7,unsigned int B8,unsigned int B9>
-class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,B9,0,0> : public FactorBase
+class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,B9,0,0,0,0,0> : public FactorBase
{
public:
-
- static const unsigned int residualSize = RES;
- static const unsigned int block0Size = B0;
- static const unsigned int block1Size = B1;
- static const unsigned int block2Size = B2;
- static const unsigned int block3Size = B3;
- static const unsigned int block4Size = B4;
- static const unsigned int block5Size = B5;
- static const unsigned int block6Size = B6;
- static const unsigned int block7Size = B7;
- static const unsigned int block8Size = B8;
- static const unsigned int block9Size = B9;
- static const unsigned int block10Size = 0;
- static const unsigned int block11Size = 0;
static const unsigned int n_blocks = 10;
static const std::vector<unsigned int> state_block_sizes_;
@@ -857,9 +1798,7 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,B9,0,0> : public Factor
}
- ~FactorAutodiff() override
- {
- }
+ ~FactorAutodiff() override = default;
JacobianMethod getJacobianMethod() const override
{
@@ -985,10 +1924,6 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,B9,0,0> : public Factor
// add to the Jacobians vector
jacobians_.push_back(Ji);
}
-
- // print jacobian matrices
- // for (unsigned int i = 0; i < n_blocks; i++)
- // std::cout << jacobians_[i] << std::endl << std::endl;
}
std::vector<StateBlockConstPtr> getStateBlockPtrVector() const override
@@ -1014,23 +1949,9 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,B9,0,0> : public Factor
////////////////// SPECIALIZATION 9 BLOCKS ////////////////////////////////////////////////////////////////////////
template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6,unsigned int B7,unsigned int B8>
-class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,0,0,0> : public FactorBase
+class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,0,0,0,0,0,0> : public FactorBase
{
public:
-
- static const unsigned int residualSize = RES;
- static const unsigned int block0Size = B0;
- static const unsigned int block1Size = B1;
- static const unsigned int block2Size = B2;
- static const unsigned int block3Size = B3;
- static const unsigned int block4Size = B4;
- static const unsigned int block5Size = B5;
- static const unsigned int block6Size = B6;
- static const unsigned int block7Size = B7;
- static const unsigned int block8Size = B8;
- static const unsigned int block9Size = 0;
- static const unsigned int block10Size = 0;
- static const unsigned int block11Size = 0;
static const unsigned int n_blocks = 9;
static const std::vector<unsigned int> state_block_sizes_;
@@ -1155,10 +2076,8 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,0,0,0> : public FactorB
}
- ~FactorAutodiff() override
- {
- }
-
+ ~FactorAutodiff() override = default;
+
JacobianMethod getJacobianMethod() const override
{
return JAC_AUTO;
@@ -1278,10 +2197,6 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,0,0,0> : public FactorB
// add to the Jacobians vector
jacobians_.push_back(Ji);
}
-
- // print jacobian matrices
- // for (unsigned int i = 0; i < n_blocks; i++)
- // std::cout << jacobians_[i] << std::endl << std::endl;
}
std::vector<StateBlockConstPtr> getStateBlockPtrVector() const override
@@ -1307,23 +2222,9 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,0,0,0> : public FactorB
////////////////// SPECIALIZATION 8 BLOCKS ////////////////////////////////////////////////////////////////////////
template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6,unsigned int B7>
-class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,0,0,0,0> : public FactorBase
+class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,0,0,0,0,0,0,0> : public FactorBase
{
public:
-
- static const unsigned int residualSize = RES;
- static const unsigned int block0Size = B0;
- static const unsigned int block1Size = B1;
- static const unsigned int block2Size = B2;
- static const unsigned int block3Size = B3;
- static const unsigned int block4Size = B4;
- static const unsigned int block5Size = B5;
- static const unsigned int block6Size = B6;
- static const unsigned int block7Size = B7;
- static const unsigned int block8Size = 0;
- static const unsigned int block9Size = 0;
- static const unsigned int block10Size = 0;
- static const unsigned int block11Size = 0;
static const unsigned int n_blocks = 8;
static const std::vector<unsigned int> state_block_sizes_;
@@ -1441,9 +2342,7 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,0,0,0,0> : public FactorBa
}
- ~FactorAutodiff() override
- {
- }
+ ~FactorAutodiff() override = default;
JacobianMethod getJacobianMethod() const override
{
@@ -1559,10 +2458,6 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,0,0,0,0> : public FactorBa
// add to the Jacobians vector
jacobians_.push_back(Ji);
}
-
- // print jacobian matrices
- // for (unsigned int i = 0; i < n_blocks; i++)
- // std::cout << jacobians_[i] << std::endl << std::endl;
}
std::vector<StateBlockConstPtr> getStateBlockPtrVector() const override
@@ -1587,23 +2482,9 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,0,0,0,0> : public FactorBa
////////////////// SPECIALIZATION 7 BLOCKS ////////////////////////////////////////////////////////////////////////
template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6>
-class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,0,0,0,0,0> : public FactorBase
+class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,0,0,0,0,0,0,0,0> : public FactorBase
{
public:
-
- static const unsigned int residualSize = RES;
- static const unsigned int block0Size = B0;
- static const unsigned int block1Size = B1;
- static const unsigned int block2Size = B2;
- static const unsigned int block3Size = B3;
- static const unsigned int block4Size = B4;
- static const unsigned int block5Size = B5;
- static const unsigned int block6Size = B6;
- static const unsigned int block7Size = 0;
- static const unsigned int block8Size = 0;
- static const unsigned int block9Size = 0;
- static const unsigned int block10Size = 0;
- static const unsigned int block11Size = 0;
static const unsigned int n_blocks = 7;
static const std::vector<unsigned int> state_block_sizes_;
@@ -1714,9 +2595,7 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,0,0,0,0,0> : public FactorBas
}
- ~FactorAutodiff() override
- {
- }
+ ~FactorAutodiff() override = default;
JacobianMethod getJacobianMethod() const override
{
@@ -1827,10 +2706,6 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,0,0,0,0,0> : public FactorBas
// add to the Jacobians vector
jacobians_.push_back(Ji);
}
-
- // print jacobian matrices
- //for (unsigned int i = 0; i < n_blocks; i++)
- // std::cout << jacobians_[i] << std::endl << std::endl;
}
std::vector<StateBlockConstPtr> getStateBlockPtrVector() const override
@@ -1855,23 +2730,9 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,0,0,0,0,0> : public FactorBas
////////////////// SPECIALIZATION 6 BLOCKS ////////////////////////////////////////////////////////////////////////
template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5>
-class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,0,0,0,0,0,0> : public FactorBase
+class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,0,0,0,0,0,0,0,0,0> : public FactorBase
{
public:
-
- static const unsigned int residualSize = RES;
- static const unsigned int block0Size = B0;
- static const unsigned int block1Size = B1;
- static const unsigned int block2Size = B2;
- static const unsigned int block3Size = B3;
- static const unsigned int block4Size = B4;
- static const unsigned int block5Size = B5;
- static const unsigned int block6Size = 0;
- static const unsigned int block7Size = 0;
- static const unsigned int block8Size = 0;
- static const unsigned int block9Size = 0;
- static const unsigned int block10Size = 0;
- static const unsigned int block11Size = 0;
static const unsigned int n_blocks = 6;
static const std::vector<unsigned int> state_block_sizes_;
@@ -1975,9 +2836,7 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,0,0,0,0,0,0> : public FactorBase
}
- ~FactorAutodiff() override
- {
- }
+ ~FactorAutodiff() override = default;
JacobianMethod getJacobianMethod() const override
{
@@ -2107,23 +2966,9 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,0,0,0,0,0,0> : public FactorBase
////////////////// SPECIALIZATION 5 BLOCKS ////////////////////////////////////////////////////////////////////////
template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4>
-class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,0,0,0,0,0,0,0> : public FactorBase
+class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,0,0,0,0,0,0,0,0,0,0> : public FactorBase
{
public:
-
- static const unsigned int residualSize = RES;
- static const unsigned int block0Size = B0;
- static const unsigned int block1Size = B1;
- static const unsigned int block2Size = B2;
- static const unsigned int block3Size = B3;
- static const unsigned int block4Size = B4;
- static const unsigned int block5Size = 0;
- static const unsigned int block6Size = 0;
- static const unsigned int block7Size = 0;
- static const unsigned int block8Size = 0;
- static const unsigned int block9Size = 0;
- static const unsigned int block10Size = 0;
- static const unsigned int block11Size = 0;
static const unsigned int n_blocks = 5;
static const std::vector<unsigned int> state_block_sizes_;
@@ -2219,9 +3064,7 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,0,0,0,0,0,0,0> : public FactorBase
}
- ~FactorAutodiff() override
- {
- }
+ ~FactorAutodiff() override = default;
JacobianMethod getJacobianMethod() const override
{
@@ -2346,23 +3189,9 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,0,0,0,0,0,0,0> : public FactorBase
////////////////// SPECIALIZATION 4 BLOCKS ////////////////////////////////////////////////////////////////////////
template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3>
-class FactorAutodiff<FacT,RES,B0,B1,B2,B3,0,0,0,0,0,0,0,0> : public FactorBase
+class FactorAutodiff<FacT,RES,B0,B1,B2,B3,0,0,0,0,0,0,0,0,0,0,0> : public FactorBase
{
public:
-
- static const unsigned int residualSize = RES;
- static const unsigned int block0Size = B0;
- static const unsigned int block1Size = B1;
- static const unsigned int block2Size = B2;
- static const unsigned int block3Size = B3;
- static const unsigned int block4Size = 0;
- static const unsigned int block5Size = 0;
- static const unsigned int block6Size = 0;
- static const unsigned int block7Size = 0;
- static const unsigned int block8Size = 0;
- static const unsigned int block9Size = 0;
- static const unsigned int block10Size = 0;
- static const unsigned int block11Size = 0;
static const unsigned int n_blocks = 4;
static const std::vector<unsigned int> state_block_sizes_;
@@ -2451,9 +3280,7 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,0,0,0,0,0,0,0,0> : public FactorBase
}
- ~FactorAutodiff() override
- {
- }
+ ~FactorAutodiff() override = default;
JacobianMethod getJacobianMethod() const override
{
@@ -2549,10 +3376,6 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,0,0,0,0,0,0,0,0> : public FactorBase
// add to the Jacobians vector
jacobians_.push_back(Ji);
}
-
- // print jacobian matrices
- //for (unsigned int i = 0; i < n_blocks; i++)
- // std::cout << jacobians_[i] << std::endl << std::endl;
}
std::vector<StateBlockConstPtr> getStateBlockPtrVector() const override
@@ -2577,23 +3400,9 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,0,0,0,0,0,0,0,0> : public FactorBase
////////////////// SPECIALIZATION 3 BLOCKS ////////////////////////////////////////////////////////////////////////
template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2>
-class FactorAutodiff<FacT,RES,B0,B1,B2,0,0,0,0,0,0,0,0,0> : public FactorBase
+class FactorAutodiff<FacT,RES,B0,B1,B2,0,0,0,0,0,0,0,0,0,0,0,0> : public FactorBase
{
public:
-
- static const unsigned int residualSize = RES;
- static const unsigned int block0Size = B0;
- static const unsigned int block1Size = B1;
- static const unsigned int block2Size = B2;
- static const unsigned int block3Size = 0;
- static const unsigned int block4Size = 0;
- static const unsigned int block5Size = 0;
- static const unsigned int block6Size = 0;
- static const unsigned int block7Size = 0;
- static const unsigned int block8Size = 0;
- static const unsigned int block9Size = 0;
- static const unsigned int block10Size = 0;
- static const unsigned int block11Size = 0;
static const unsigned int n_blocks = 3;
static const std::vector<unsigned int> state_block_sizes_;
@@ -2675,9 +3484,7 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,0,0,0,0,0,0,0,0,0> : public FactorBase
}
- ~FactorAutodiff() override
- {
- }
+ ~FactorAutodiff() override = default;
JacobianMethod getJacobianMethod() const override
{
@@ -2768,10 +3575,6 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,0,0,0,0,0,0,0,0,0> : public FactorBase
// add to the Jacobians vector
jacobians_.push_back(Ji);
}
-
- // print jacobian matrices
- //for (unsigned int i = 0; i < n_blocks; i++)
- // std::cout << jacobians_[i] << std::endl << std::endl;
}
std::vector<StateBlockConstPtr> getStateBlockPtrVector() const override
@@ -2796,23 +3599,9 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,0,0,0,0,0,0,0,0,0> : public FactorBase
////////////////// SPECIALIZATION 2 BLOCKS ////////////////////////////////////////////////////////////////////////
template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1>
-class FactorAutodiff<FacT,RES,B0,B1,0,0,0,0,0,0,0,0,0,0> : public FactorBase
+class FactorAutodiff<FacT,RES,B0,B1,0,0,0,0,0,0,0,0,0,0,0,0,0> : public FactorBase
{
public:
-
- static const unsigned int residualSize = RES;
- static const unsigned int block0Size = B0;
- static const unsigned int block1Size = B1;
- static const unsigned int block2Size = 0;
- static const unsigned int block3Size = 0;
- static const unsigned int block4Size = 0;
- static const unsigned int block5Size = 0;
- static const unsigned int block6Size = 0;
- static const unsigned int block7Size = 0;
- static const unsigned int block8Size = 0;
- static const unsigned int block9Size = 0;
- static const unsigned int block10Size = 0;
- static const unsigned int block11Size = 0;
static const unsigned int n_blocks = 2;
static const std::vector<unsigned int> state_block_sizes_;
@@ -2887,9 +3676,7 @@ class FactorAutodiff<FacT,RES,B0,B1,0,0,0,0,0,0,0,0,0,0> : public FactorBase
}
- ~FactorAutodiff() override
- {
- }
+ ~FactorAutodiff() override = default;
JacobianMethod getJacobianMethod() const override
{
@@ -2975,10 +3762,6 @@ class FactorAutodiff<FacT,RES,B0,B1,0,0,0,0,0,0,0,0,0,0> : public FactorBase
// add to the Jacobians vector
jacobians_.push_back(Ji);
}
-
- // print jacobian matrices
- //for (unsigned int i = 0; i < n_blocks; i++)
- // std::cout << jacobians_[i] << std::endl << std::endl;
}
std::vector<StateBlockConstPtr> getStateBlockPtrVector() const override
@@ -3002,23 +3785,9 @@ class FactorAutodiff<FacT,RES,B0,B1,0,0,0,0,0,0,0,0,0,0> : public FactorBase
////////////////// SPECIALIZATION 1 BLOCK ////////////////////////////////////////////////////////////////////////
template <class FacT,unsigned int RES,unsigned int B0>
-class FactorAutodiff<FacT,RES,B0,0,0,0,0,0,0,0,0,0,0,0> : public FactorBase
+class FactorAutodiff<FacT,RES,B0,0,0,0,0,0,0,0,0,0,0,0,0,0,0> : public FactorBase
{
public:
-
- static const unsigned int residualSize = RES;
- static const unsigned int block0Size = B0;
- static const unsigned int block1Size = 0;
- static const unsigned int block2Size = 0;
- static const unsigned int block3Size = 0;
- static const unsigned int block4Size = 0;
- static const unsigned int block5Size = 0;
- static const unsigned int block6Size = 0;
- static const unsigned int block7Size = 0;
- static const unsigned int block8Size = 0;
- static const unsigned int block9Size = 0;
- static const unsigned int block10Size = 0;
- static const unsigned int block11Size = 0;
static const unsigned int n_blocks = 1;
static const std::vector<unsigned int> state_block_sizes_;
@@ -3086,9 +3855,7 @@ class FactorAutodiff<FacT,RES,B0,0,0,0,0,0,0,0,0,0,0,0> : public FactorBase
}
- ~FactorAutodiff() override
- {
- }
+ ~FactorAutodiff() override = default;
JacobianMethod getJacobianMethod() const override
{
@@ -3169,10 +3936,6 @@ class FactorAutodiff<FacT,RES,B0,0,0,0,0,0,0,0,0,0,0,0> : public FactorBase
// add to the Jacobians vector
jacobians_.push_back(Ji);
}
-
- // print jacobian matrices
- //for (unsigned int i = 0; i < n_blocks; i++)
- // std::cout << jacobians_[i] << std::endl << std::endl;
}
std::vector<StateBlockConstPtr> getStateBlockPtrVector() const override
@@ -3199,6 +3962,15 @@ class FactorAutodiff<FacT,RES,B0,0,0,0,0,0,0,0,0,0,0,0> : public FactorBase
// STATIC CONST VECTORS INITIALIZATION
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// state_block_sizes_
+// 15 BLOCKS
+template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6,unsigned int B7,unsigned int B8,unsigned int B9,unsigned int B10,unsigned int B11,unsigned int B12,unsigned int B13,unsigned int B14>
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,B9,B10,B11,B12,B13,B14>::state_block_sizes_ = {B0,B1,B2,B3,B4,B5,B6,B7,B8,B9,B10,B11,B12,B13,B14};
+// 14 BLOCKS
+template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6,unsigned int B7,unsigned int B8,unsigned int B9,unsigned int B10,unsigned int B11,unsigned int B12,unsigned int B13>
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,B9,B10,B11,B12,B13>::state_block_sizes_ = {B0,B1,B2,B3,B4,B5,B6,B7,B8,B9,B10,B11,B12,B13};
+// 13 BLOCKS
+template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6,unsigned int B7,unsigned int B8,unsigned int B9,unsigned int B10,unsigned int B11,unsigned int B12>
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,B9,B10,B11,B12>::state_block_sizes_ = {B0,B1,B2,B3,B4,B5,B6,B7,B8,B9,B10,B11,B12};
// 12 BLOCKS
template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6,unsigned int B7,unsigned int B8,unsigned int B9,unsigned int B10,unsigned int B11>
const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,B9,B10,B11>::state_block_sizes_ = {B0,B1,B2,B3,B4,B5,B6,B7,B8,B9,B10,B11};
@@ -3237,108 +4009,156 @@ template <class FacT,unsigned int RES,unsigned int B0>
const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,0,0,0,0,0,0,0,0,0,0,0>::state_block_sizes_ = {B0};
// jacobian_locations_
+// 15 BLOCKS
+template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6,unsigned int B7,unsigned int B8,unsigned int B9,unsigned int B10,unsigned int B11,unsigned int B12,unsigned int B13,unsigned int B14>
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,B9,B10,B11,B12,B13,B14>::jacobian_locations_ = {0,
+ B0,
+ B0+B1,
+ B0+B1+B2,
+ B0+B1+B2+B3,
+ B0+B1+B2+B3+B4,
+ B0+B1+B2+B3+B4+B5,
+ B0+B1+B2+B3+B4+B5+B6,
+ B0+B1+B2+B3+B4+B5+B6+B7,
+ B0+B1+B2+B3+B4+B5+B6+B7+B8,
+ B0+B1+B2+B3+B4+B5+B6+B7+B8+B9,
+ B0+B1+B2+B3+B4+B5+B6+B7+B8+B9+B10,
+ B0+B1+B2+B3+B4+B5+B6+B7+B8+B9+B10+B11,
+ B0+B1+B2+B3+B4+B5+B6+B7+B8+B9+B10+B11+B12,
+ B0+B1+B2+B3+B4+B5+B6+B7+B8+B9+B10+B11+B12+B13};
+// 14 BLOCKS
+template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6,unsigned int B7,unsigned int B8,unsigned int B9,unsigned int B10,unsigned int B11,unsigned int B12,unsigned int B13>
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,B9,B10,B11,B12,B13,0>::jacobian_locations_ = {0,
+ B0,
+ B0+B1,
+ B0+B1+B2,
+ B0+B1+B2+B3,
+ B0+B1+B2+B3+B4,
+ B0+B1+B2+B3+B4+B5,
+ B0+B1+B2+B3+B4+B5+B6,
+ B0+B1+B2+B3+B4+B5+B6+B7,
+ B0+B1+B2+B3+B4+B5+B6+B7+B8,
+ B0+B1+B2+B3+B4+B5+B6+B7+B8+B9,
+ B0+B1+B2+B3+B4+B5+B6+B7+B8+B9+B10,
+ B0+B1+B2+B3+B4+B5+B6+B7+B8+B9+B10+B11,
+ B0+B1+B2+B3+B4+B5+B6+B7+B8+B9+B10+B11+B12};
+// 13 BLOCKS
+template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6,unsigned int B7,unsigned int B8,unsigned int B9,unsigned int B10,unsigned int B11,unsigned int B12>
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,B9,B10,B11,B12,0,0>::jacobian_locations_ = {0,
+ B0,
+ B0+B1,
+ B0+B1+B2,
+ B0+B1+B2+B3,
+ B0+B1+B2+B3+B4,
+ B0+B1+B2+B3+B4+B5,
+ B0+B1+B2+B3+B4+B5+B6,
+ B0+B1+B2+B3+B4+B5+B6+B7,
+ B0+B1+B2+B3+B4+B5+B6+B7+B8,
+ B0+B1+B2+B3+B4+B5+B6+B7+B8+B9,
+ B0+B1+B2+B3+B4+B5+B6+B7+B8+B9+B10,
+ B0+B1+B2+B3+B4+B5+B6+B7+B8+B9+B10+B11};
// 12 BLOCKS
template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6,unsigned int B7,unsigned int B8,unsigned int B9,unsigned int B10,unsigned int B11>
-const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,B9,B10,B11>::jacobian_locations_ = {0,
- B0,
- B0+B1,
- B0+B1+B2,
- B0+B1+B2+B3,
- B0+B1+B2+B3+B4,
- B0+B1+B2+B3+B4+B5,
- B0+B1+B2+B3+B4+B5+B6,
- B0+B1+B2+B3+B4+B5+B6+B7,
- B0+B1+B2+B3+B4+B5+B6+B7+B8,
- B0+B1+B2+B3+B4+B5+B6+B7+B8+B9,
- B0+B1+B2+B3+B4+B5+B6+B7+B8+B9+B10};
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,B9,B10,B11,0,0,0>::jacobian_locations_ = {0,
+ B0,
+ B0+B1,
+ B0+B1+B2,
+ B0+B1+B2+B3,
+ B0+B1+B2+B3+B4,
+ B0+B1+B2+B3+B4+B5,
+ B0+B1+B2+B3+B4+B5+B6,
+ B0+B1+B2+B3+B4+B5+B6+B7,
+ B0+B1+B2+B3+B4+B5+B6+B7+B8,
+ B0+B1+B2+B3+B4+B5+B6+B7+B8+B9,
+ B0+B1+B2+B3+B4+B5+B6+B7+B8+B9+B10};
// 10 BLOCKS
template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6,unsigned int B7,unsigned int B8,unsigned int B9,unsigned int B10>
-const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,B9,B10,0>::jacobian_locations_ = {0,
- B0,
- B0+B1,
- B0+B1+B2,
- B0+B1+B2+B3,
- B0+B1+B2+B3+B4,
- B0+B1+B2+B3+B4+B5,
- B0+B1+B2+B3+B4+B5+B6,
- B0+B1+B2+B3+B4+B5+B6+B7,
- B0+B1+B2+B3+B4+B5+B6+B7+B8,
- B0+B1+B2+B3+B4+B5+B6+B7+B8+B9};
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,B9,B10,0,0,0,0>::jacobian_locations_ = {0,
+ B0,
+ B0+B1,
+ B0+B1+B2,
+ B0+B1+B2+B3,
+ B0+B1+B2+B3+B4,
+ B0+B1+B2+B3+B4+B5,
+ B0+B1+B2+B3+B4+B5+B6,
+ B0+B1+B2+B3+B4+B5+B6+B7,
+ B0+B1+B2+B3+B4+B5+B6+B7+B8,
+ B0+B1+B2+B3+B4+B5+B6+B7+B8+B9};
// 10 BLOCKS
template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6,unsigned int B7,unsigned int B8,unsigned int B9>
-const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,B9,0,0>::jacobian_locations_ = {0,
- B0,
- B0+B1,
- B0+B1+B2,
- B0+B1+B2+B3,
- B0+B1+B2+B3+B4,
- B0+B1+B2+B3+B4+B5,
- B0+B1+B2+B3+B4+B5+B6,
- B0+B1+B2+B3+B4+B5+B6+B7,
- B0+B1+B2+B3+B4+B5+B6+B7+B8};
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,B9,0,0,0,0,0>::jacobian_locations_ = {0,
+ B0,
+ B0+B1,
+ B0+B1+B2,
+ B0+B1+B2+B3,
+ B0+B1+B2+B3+B4,
+ B0+B1+B2+B3+B4+B5,
+ B0+B1+B2+B3+B4+B5+B6,
+ B0+B1+B2+B3+B4+B5+B6+B7,
+ B0+B1+B2+B3+B4+B5+B6+B7+B8};
// 9 BLOCKS
template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6,unsigned int B7,unsigned int B8>
-const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,0,0,0>::jacobian_locations_ = {0,
- B0,
- B0+B1,
- B0+B1+B2,
- B0+B1+B2+B3,
- B0+B1+B2+B3+B4,
- B0+B1+B2+B3+B4+B5,
- B0+B1+B2+B3+B4+B5+B6,
- B0+B1+B2+B3+B4+B5+B6+B7};
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,0,0,0,0,0,0>::jacobian_locations_ = {0,
+ B0,
+ B0+B1,
+ B0+B1+B2,
+ B0+B1+B2+B3,
+ B0+B1+B2+B3+B4,
+ B0+B1+B2+B3+B4+B5,
+ B0+B1+B2+B3+B4+B5+B6,
+ B0+B1+B2+B3+B4+B5+B6+B7};
// 8 BLOCKS
template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6,unsigned int B7>
-const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,0,0,0,0>::jacobian_locations_ = {0,
- B0,
- B0+B1,
- B0+B1+B2,
- B0+B1+B2+B3,
- B0+B1+B2+B3+B4,
- B0+B1+B2+B3+B4+B5,
- B0+B1+B2+B3+B4+B5+B6};
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,0,0,0,0,0,0,0>::jacobian_locations_ = {0,
+ B0,
+ B0+B1,
+ B0+B1+B2,
+ B0+B1+B2+B3,
+ B0+B1+B2+B3+B4,
+ B0+B1+B2+B3+B4+B5,
+ B0+B1+B2+B3+B4+B5+B6};
// 7 BLOCKS
template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6>
-const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,0,0,0,0,0>::jacobian_locations_ = {0,
- B0,
- B0+B1,
- B0+B1+B2,
- B0+B1+B2+B3,
- B0+B1+B2+B3+B4,
- B0+B1+B2+B3+B4+B5};
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,0,0,0,0,0,0,0,0>::jacobian_locations_ = {0,
+ B0,
+ B0+B1,
+ B0+B1+B2,
+ B0+B1+B2+B3,
+ B0+B1+B2+B3+B4,
+ B0+B1+B2+B3+B4+B5};
// 6 BLOCKS
template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5>
-const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,0,0,0,0,0,0>::jacobian_locations_ = {0,
- B0,
- B0+B1,
- B0+B1+B2,
- B0+B1+B2+B3,
- B0+B1+B2+B3+B4};
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,0,0,0,0,0,0,0,0,0>::jacobian_locations_ = {0,
+ B0,
+ B0+B1,
+ B0+B1+B2,
+ B0+B1+B2+B3,
+ B0+B1+B2+B3+B4};
// 5 BLOCKS
template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4>
-const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,0,0,0,0,0,0,0>::jacobian_locations_ = {0,
- B0,
- B0+B1,
- B0+B1+B2,
- B0+B1+B2+B3};
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,0,0,0,0,0,0,0,0,0,0>::jacobian_locations_ = {0,
+ B0,
+ B0+B1,
+ B0+B1+B2,
+ B0+B1+B2+B3};
// 4 BLOCKS
template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3>
-const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,0,0,0,0,0,0,0,0>::jacobian_locations_ = {0,
- B0,
- B0+B1,
- B0+B1+B2};
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,0,0,0,0,0,0,0,0,0,0,0>::jacobian_locations_ = {0,
+ B0,
+ B0+B1,
+ B0+B1+B2};
// 3 BLOCKS
template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2>
-const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,0,0,0,0,0,0,0,0,0>::jacobian_locations_ = {0,
- B0,
- B0+B1};
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,0,0,0,0,0,0,0,0,0,0,0,0>::jacobian_locations_ = {0,
+ B0,
+ B0+B1};
// 2 BLOCKS
template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1>
-const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,0,0,0,0,0,0,0,0,0,0>::jacobian_locations_ = {0,
- B0};
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,0,0,0,0,0,0,0,0,0,0,0,0,0>::jacobian_locations_ = {0,
+ B0};
// 1 BLOCK
template <class FacT,unsigned int RES,unsigned int B0>
-const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,0,0,0,0,0,0,0,0,0,0,0>::jacobian_locations_ = {0};
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,0,0,0,0,0,0,0,0,0,0,0,0,0,0>::jacobian_locations_ = {0};
} // namespace wolf
diff --git a/include/core/feature/feature_landmark_external.h b/include/core/feature/feature_landmark_external.h
new file mode 100644
index 0000000000000000000000000000000000000000..c7a907d1eaebcd2a0abbd0cbb21cd364a97d7d78
--- /dev/null
+++ b/include/core/feature/feature_landmark_external.h
@@ -0,0 +1,63 @@
+//--------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--------
+#pragma once
+
+// Wolf includes
+#include "core/feature/feature_base.h"
+
+// std includes
+
+namespace wolf
+{
+
+WOLF_PTR_TYPEDEFS(FeatureLandmarkExternal);
+
+// class FeatureLandmarkExternal
+class FeatureLandmarkExternal : public FeatureBase
+{
+ protected:
+ int external_id_; ///< The id of the landmark assigned by an external classifier
+ int external_type_; ///< The type of the landmark assigned by an external classifier
+
+ public:
+ /** \brief Constructor from capture pointer and measure
+ *
+ * \param _measurement the measurement
+ * \param _meas_covariance the noise of the measurement
+ * \param _external_id the id assigned by an external tracker (-1 for not tracked)
+ * \param _external_type the type index of the landmark detection (-1 for not classified)
+ *
+ */
+ FeatureLandmarkExternal(const Eigen::VectorXd& _measurement,
+ const Eigen::MatrixXd& _meas_covariance,
+ const int& _external_id = -1,
+ const int& _external_type = -1);
+
+ ~FeatureLandmarkExternal() override;
+
+ int getExternalType() const;
+ void setExternalType(const int& _external_type);
+ int getExternalId() const;
+ void setExternalId(const int& _external_id);
+};
+
+} // namespace wolf
diff --git a/include/core/landmark/landmark_base.h b/include/core/landmark/landmark_base.h
index ff740a6fd1b3371ab99dac5645fc3328064c4c55..49144ad50fb89e207a3880837a34014f162feefb 100644
--- a/include/core/landmark/landmark_base.h
+++ b/include/core/landmark/landmark_base.h
@@ -49,8 +49,6 @@ class LandmarkBase : public NodeBase, public HasStateBlocks, public std::enable_
private:
MapBaseWPtr map_ptr_;
FactorBasePtrList constrained_by_list_;
- //std::vector<StateBlockPtr> state_block_vec_; ///< vector of state blocks, additional to those in the state_block_map from HasStateBlocks.
- //std::vector<StateBlockConstPtr> state_block_const_vec_; ///< vector of state blocks, additional to those in the state_block_map from HasStateBlocks.
static unsigned int landmark_id_count_;
@@ -58,6 +56,7 @@ class LandmarkBase : public NodeBase, public HasStateBlocks, public std::enable_
// Navigate wolf tree
void setProblem(ProblemPtr _problem) override final;
unsigned int landmark_id_; ///< landmark unique id
+ unsigned int track_id_; ///< associated track id
TimeStamp stamp_; ///< stamp of the creation of the landmark
Eigen::VectorXd descriptor_; //TODO: agree? JS: No: It is not general enough as descriptor to be in LmkBase.
@@ -80,6 +79,9 @@ class LandmarkBase : public NodeBase, public HasStateBlocks, public std::enable_
unsigned int id() const;
void setId(unsigned int _id);
+ unsigned int trackId(); // get track ID
+ void setTrackId(unsigned int _track_id); // set track ID
+
// State blocks
//std::vector<StateBlockConstPtr> getUsedStateBlockVec() const;
//std::vector<StateBlockPtr> getUsedStateBlockVec();
@@ -176,6 +178,16 @@ inline void LandmarkBase::setId(unsigned int _id)
landmark_id_count_ = _id;
}
+inline unsigned int LandmarkBase::trackId()
+{
+ return track_id_;
+}
+
+inline void LandmarkBase::setTrackId(unsigned int _track_id)
+{
+ track_id_ = _track_id;
+}
+
inline unsigned int LandmarkBase::getHits() const
{
return constrained_by_list_.size();
diff --git a/include/core/landmark/landmark_external.h b/include/core/landmark/landmark_external.h
new file mode 100644
index 0000000000000000000000000000000000000000..ba363bfa2efec32d57c397a2986080ccc70cf9b0
--- /dev/null
+++ b/include/core/landmark/landmark_external.h
@@ -0,0 +1,89 @@
+//--------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--------
+#ifndef LANDMARK_EXTERNAL_H_
+#define LANDMARK_EXTERNAL_H_
+
+// Wolf includes
+#include "core/landmark/landmark_base.h"
+
+namespace wolf
+{
+WOLF_PTR_TYPEDEFS(LandmarkExternal);
+
+// class LandmarkExternal
+class LandmarkExternal : public LandmarkBase
+{
+ protected:
+ int external_id_; ///< Id provided by an external tracker (-1 untracked)
+ int external_type_; ///< Type provided by an external classifier (-1 unclassified)
+
+ public:
+ /** \brief Constructor with the position state pointer (optional orientation state pointer)
+ *
+ * Constructor with type, and state pointer
+ * \param _external_id ID provided by an external tracker (-1 for untracked)
+ * \param _external_type TYPE provided by an external classifier (-1 for unknown)
+ * \param _p_ptr StateBlock pointer to the position
+ * \param _o_ptr StateBlock pointer to the orientation (default: nullptr)
+ *
+ **/
+ LandmarkExternal(const int& _external_id,
+ const int& _external_type,
+ StateBlockPtr _p_ptr,
+ StateBlockPtr _o_ptr = nullptr);
+ ~LandmarkExternal() = default;
+ YAML::Node saveToYaml() const override;
+
+ /** \brief Creator for Factory<LandmarkExternal, YAML::Node>
+ * Caution: This creator does not set the landmark's anchor frame and sensor.
+ * These need to be set afterwards.
+ */
+ static LandmarkBasePtr create(const YAML::Node& _node);
+
+ void setExternalId(const int& _external_id);
+ int getExternalId() const;
+ void setExternalType(const int& _external_type);
+ int getExternalType() const;
+};
+
+inline void LandmarkExternal::setExternalId(const int& _external_id)
+{
+ external_id_ = _external_id;
+}
+
+inline int LandmarkExternal::getExternalId() const
+{
+ return external_id_;
+}
+
+inline void LandmarkExternal::setExternalType(const int& _external_type)
+{
+ external_type_ = _external_type;
+}
+
+inline int LandmarkExternal::getExternalType() const
+{
+ return external_type_;
+}
+
+} // namespace wolf
+#endif
\ No newline at end of file
diff --git a/include/core/map/map_base.h b/include/core/map/map_base.h
index d4b551a6e6f5f76f02791d23cbec54d0f1aaca9d..ee3988f4c3db13b390e57ddf0940c3a214793879 100644
--- a/include/core/map/map_base.h
+++ b/include/core/map/map_base.h
@@ -116,8 +116,8 @@ class MapBase : public NodeBase, public std::enable_shared_from_this<MapBase>
LandmarkBaseConstPtr getLandmark(const unsigned int& _id) const;
LandmarkBasePtr getLandmark(const unsigned int& _id);
- void load(const std::string& _map_file_yaml);
- void save(const std::string& _map_file_yaml, const std::string& _map_name = "Map automatically saved by Wolf");
+ void load(std::string _map_file_yaml);
+ void save(std::string _map_file_yaml, const std::string& _map_name = "Map automatically saved by Wolf") const;
virtual void printHeader(int depth, //
bool constr_by, //
@@ -136,7 +136,7 @@ class MapBase : public NodeBase, public std::enable_shared_from_this<MapBase>
bool check(CheckLog& _log, NodeBaseConstPtr _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs = "") const;
private:
- std::string dateTimeNow();
+ std::string dateTimeNow() const;
};
inline LandmarkBaseConstPtrList MapBase::getLandmarkList() const
diff --git a/include/core/math/SE2.h b/include/core/math/SE2.h
index 517f253b346e3cd10a6816e02a1510eb78fdf309..cb3f3c686616906ad13f4f5fe14bc17e3d120a96 100644
--- a/include/core/math/SE2.h
+++ b/include/core/math/SE2.h
@@ -113,6 +113,18 @@ inline void inverse(const MatrixBase<D1>& p, const typename D1::Scalar& o,
io = -o;
}
+template<typename D1, typename D2, typename D3, typename D4>
+inline void inverse(const MatrixBase<D1>& p, const MatrixBase<D2>& o,
+ MatrixBase<D3>& ip, MatrixBase<D4>& io)
+{
+ MatrixSizeCheck<2, 1>::check(p);
+ MatrixSizeCheck<1, 1>::check(o);
+ MatrixSizeCheck<2, 1>::check(ip);
+ MatrixSizeCheck<1, 1>::check(io);
+
+ inverse(p, o(0), ip, io(0));
+}
+
template<typename D1, typename D2>
inline void inverse(const MatrixBase<D1>& d,
MatrixBase<D2>& id)
@@ -134,6 +146,18 @@ inline Matrix<typename D::Scalar, 3, 1> inverse(const MatrixBase<D>& d)
return id;
}
+inline void inverse(const VectorComposite& v, VectorComposite& c)
+{
+ inverse(v.at('P'), v.at('O'), c['P'], c['O']);
+}
+
+inline VectorComposite inverse(const VectorComposite& v)
+{
+ VectorComposite c("PO", {2,1});
+ inverse(v, c);
+ return c;
+}
+
template<class D1, class D2>
inline void exp(const Eigen::MatrixBase<D1>& _tau, Eigen::MatrixBase<D2>& _delta)
{
@@ -236,7 +260,7 @@ inline void exp(const VectorComposite& _tau, VectorComposite& _delta, MatrixComp
}
template<class D1, class D2, class D3>
-inline void compose(const MatrixBase<D1> &_delta1, MatrixBase<D2> &_delta2, MatrixBase<D3> &_delta1_compose_delta2)
+inline void compose(const MatrixBase<D1> &_delta1, const MatrixBase<D2> &_delta2, MatrixBase<D3> &_delta1_compose_delta2)
{
MatrixSizeCheck<3, 1>::check(_delta1);
MatrixSizeCheck<3, 1>::check(_delta2);
@@ -247,12 +271,20 @@ inline void compose(const MatrixBase<D1> &_delta1, MatrixBase<D2> &_delta2, Matr
_delta1_compose_delta2(2) = pi2pi(_delta1(2) + _delta2(2));
}
+template<class D1, class D2>
+inline Matrix<typename D1::Scalar,3,1> compose(const MatrixBase<D1> &_delta1, const MatrixBase<D2> &_delta2)
+{
+ Matrix<typename D1::Scalar,3,1> delta1_compose_delta2;
+ compose(_delta1, _delta2, delta1_compose_delta2);
+ return delta1_compose_delta2;
+}
+
template<class D1, class D2, class D3, class D4, class D5>
inline void compose(const MatrixBase<D1>& _delta1,
- const MatrixBase<D2>& _delta2,
- MatrixBase<D3>& _delta1_compose_delta2,
- MatrixBase<D4>& _J_compose_delta1,
- MatrixBase<D5>& _J_compose_delta2)
+ const MatrixBase<D2>& _delta2,
+ MatrixBase<D3>& _delta1_compose_delta2,
+ MatrixBase<D4>& _J_compose_delta1,
+ MatrixBase<D5>& _J_compose_delta2)
{
MatrixSizeCheck<3, 1>::check(_delta1);
MatrixSizeCheck<3, 1>::check(_delta2);
@@ -328,10 +360,10 @@ inline VectorComposite compose(const VectorComposite& x1, const VectorComposite&
}
inline void compose(const VectorComposite& _x1,
- const VectorComposite& _x2,
- VectorComposite& _c,
- MatrixComposite& _J_c_x1,
- MatrixComposite& _J_c_x2)
+ const VectorComposite& _x2,
+ VectorComposite& _c,
+ MatrixComposite& _J_c_x1,
+ MatrixComposite& _J_c_x2)
{
const auto& p1 = _x1.at('P');
const auto& a1 = _x1.at('O')(0); // angle
diff --git a/include/core/math/rotations.h b/include/core/math/rotations.h
index db34dd0802bccbbe5add3b17636b1164d35ecdc3..36856a257b1d7aa115c5c056a788d00d618ed8fb 100644
--- a/include/core/math/rotations.h
+++ b/include/core/math/rotations.h
@@ -37,22 +37,27 @@ namespace wolf
//////////////////////////////////////////////////////////////
/** \brief Return angle between -pi and pi
- *
+ * This implementation uses std::remainder which makes it incompatible for ceres::jet (this implementation was included in ceres_wrapper/wolf_jet.h)
* @param angle
* @return formatted angle
*/
template<typename T>
-inline T
-pi2pi(const T _angle)
-{
- T angle = _angle;
- while (angle > T( M_PI ))
- angle -= T( 2 * M_PI );
- while (angle <= T( -M_PI ))
- angle += T( 2 * M_PI );
-
- return angle;
-}
+inline T pi2pi(const T& _angle)
+{
+ return remainder(_angle,2*M_PI);
+}
+// inline long double pi2pi(const long double& _angle)
+// {
+// return std::remainder(_angle,2*M_PI);
+// }
+// inline double pi2pi(const double& _angle)
+// {
+// return std::remainder(_angle,2*M_PI);
+// }
+// inline float pi2pi(const float& _angle)
+// {
+// return std::remainder(_angle,2*M_PI);
+// }
/** \brief Conversion to radians
*
diff --git a/include/core/processor/processor_base.h b/include/core/processor/processor_base.h
index efb8eb2fa36757f69d1504fc375902943b686199..23744b7f1a70cf0a0feb4269c8bf69742bfe0664 100644
--- a/include/core/processor/processor_base.h
+++ b/include/core/processor/processor_base.h
@@ -352,10 +352,10 @@ class ProcessorBase : public NodeBase, public std::enable_shared_from_this<Proce
void setTimeTolerance(double _time_tolerance);
double getTimeTolerance() const;
- bool checkTimeTolerance (const TimeStamp& _ts1, const TimeStamp& _ts2);
- bool checkTimeTolerance (const CaptureBasePtr _cap, const TimeStamp& _ts);
- bool checkTimeTolerance (const FrameBasePtr _frm, const TimeStamp& _ts);
- bool checkTimeTolerance (const FrameBasePtr _frm, const CaptureBasePtr _cap);
+ bool checkTimeTolerance (const TimeStamp& _ts1, const TimeStamp& _ts2) const;
+ bool checkTimeTolerance (const CaptureBasePtr _cap, const TimeStamp& _ts) const;
+ bool checkTimeTolerance (const FrameBasePtr _frm, const TimeStamp& _ts) const;
+ bool checkTimeTolerance (const FrameBasePtr _frm, const CaptureBasePtr _cap) const;
void link(SensorBasePtr);
diff --git a/include/core/processor/processor_landmark_external.h b/include/core/processor/processor_landmark_external.h
index a91f4660783fa3d1c65d183eba4be7c32f4c518b..9017022ef0e9bf5bf1258ad1b5fbee7850cfa5ec 100644
--- a/include/core/processor/processor_landmark_external.h
+++ b/include/core/processor/processor_landmark_external.h
@@ -20,11 +20,14 @@
//
//--------LICENSE_END--------
-#pragma once
+#ifndef PROCESSOR_LANDMARK_EXTERNAL_H_
+#define PROCESSOR_LANDMARK_EXTERNAL_H_
#include "core/common/wolf.h"
#include "core/processor/processor_tracker.h"
#include "core/processor/track_matrix.h"
+#include "core/landmark/landmark_external.h"
+#include "core/feature/feature_landmark_external.h"
namespace wolf
{
@@ -33,127 +36,145 @@ WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorLandmarkExternal);
struct ParamsProcessorLandmarkExternal : public ParamsProcessorTracker
{
- bool use_orientation; ///< use orientation measure or not when emplacing factors
- double match_dist_th; ///< for considering tracked detection: distance threshold to previous detection
- unsigned int new_features_for_keyframe; ///< for keyframe voting: amount of new features with respect to origin (sufficient condition if more than min_features_for_keyframe)
- double filter_quality_th; ///< min quality to consider the detection
- unsigned int filter_track_length_th; ///< length of the track necessary to consider the detection
- double time_span; ///< for keyframe voting: time span since last frame (sufficient condition if more than min_features_for_keyframe)
+ bool use_orientation; ///< use orientation measure or not when emplacing factors
+
+ unsigned int new_features_for_keyframe; ///< for keyframe voting: amount of new features with respect to origin
+ ///< (sufficient condition if more than min_features_for_keyframe)
+ double time_span; ///< for keyframe voting: time span since last frame (sufficient condition if more than
+ ///< min_features_for_keyframe)
+
+ double quality_th; ///< min quality to consider the detection
+
+ // Matching distance threshold to previous detection considering motion (necessary condition)
+ double match_dist_th_id; ///< Match by ID
+ double match_dist_th_type; ///< Match by TYPE
+ double match_dist_th_unknown; ///< No ID/TYPE information
+
+ unsigned int track_length_th; ///< Track length threshold to emplace factors (necessary condition)
+
+ bool close_loops_by_id; ///< Close loop if ID matches (ID unchanged guaranteed)
+ bool close_loops_by_type; ///< Close loop if TYPE matches (also distance check)
ParamsProcessorLandmarkExternal() = default;
- ParamsProcessorLandmarkExternal(std::string _unique_name,
- const wolf::ParamsServer & _server):
- ParamsProcessorTracker(_unique_name, _server)
+ ParamsProcessorLandmarkExternal(std::string _unique_name, const wolf::ParamsServer& _server)
+ : ParamsProcessorTracker(_unique_name, _server)
{
- use_orientation = _server.getParam<bool> (prefix + _unique_name + "/use_orientation");
- filter_quality_th = _server.getParam<double> (prefix + _unique_name + "/filter/quality_th");
- filter_track_length_th = _server.getParam<unsigned int>(prefix + _unique_name + "/filter/track_length_th");
- match_dist_th = _server.getParam<double> (prefix + _unique_name + "/match_dist_th");
- time_span = _server.getParam<double> (prefix + _unique_name + "/keyframe_vote/time_span");
+ use_orientation = _server.getParam<bool>(prefix + _unique_name + "/use_orientation");
+ new_features_for_keyframe =
+ _server.getParam<unsigned int>(prefix + _unique_name + "/keyframe_vote/new_features_for_keyframe");
+ time_span = _server.getParam<double>(prefix + _unique_name + "/keyframe_vote/time_span");
+ quality_th = _server.getParam<double>(prefix + _unique_name + "/quality_th");
+ match_dist_th_id = _server.getParam<double>(prefix + _unique_name + "/match_dist_th_id");
+ match_dist_th_type = _server.getParam<double>(prefix + _unique_name + "/match_dist_th_type");
+ match_dist_th_unknown = _server.getParam<double>(prefix + _unique_name + "/match_dist_th_unknown");
+ track_length_th = _server.getParam<unsigned int>(prefix + _unique_name + "/track_length_th");
+ close_loops_by_id = _server.getParam<bool>(prefix + _unique_name + "/close_loops_by_id");
+ close_loops_by_type = _server.getParam<bool>(prefix + _unique_name + "/close_loops_by_type");
}
};
WOLF_PTR_TYPEDEFS(ProcessorLandmarkExternal);
-//Class
+// Class
class ProcessorLandmarkExternal : public ProcessorTracker
{
- public:
- ProcessorLandmarkExternal(ParamsProcessorLandmarkExternalPtr _params_tracker_feature);
- ~ProcessorLandmarkExternal() override;
-
- // Factory method for high level API
- WOLF_PROCESSOR_CREATE(ProcessorLandmarkExternal, ParamsProcessorLandmarkExternal);
-
- void configure(SensorBasePtr _sensor) override { };
-
- protected:
-
- ParamsProcessorLandmarkExternalPtr params_tfle_;
- TrackMatrix track_matrix_;
- std::set<SizeStd> lmks_ids_origin_;
-
- /** Pre-process incoming Capture
- *
- * This is called by process() just after assigning incoming_ptr_ to a valid Capture.
- *
- * Extract the detections and fill unmatched_detections_incoming_ (FeaturePtrList)
- */
- void preProcess() override;
-
-
- /** \brief Process known Features
- * \return The number of successful matches.
- *
- * This function tracks features from last to incoming and starts new tracks for new (not tracked) features in incoming
- */
- unsigned int processKnown() override;
-
- /**\brief Process new Features
- *
- */
- unsigned int processNew(const int& _max_features) override { return 0;};
-
- /** \brief Vote for KeyFrame generation
- *
- * If a KeyFrame criterion is validated, this function returns true,
- * meaning that it wants to create a KeyFrame at the \b last Capture.
- *
- * WARNING! This function only votes! It does not create KeyFrames!
- */
- bool voteForKeyFrame() const override;
-
- /** \brief Emplaces a new factor for each known feature in Capture \b last
- */
- void establishFactors() override;
-
- /** \brief Emplaces a new factor
- * \param _feature feature pointer
- * \param _landmark pointer to the landmark
- */
- FactorBasePtr emplaceFactor(FeatureBasePtr _feature, LandmarkBasePtr _landmark);
-
- /** \brief Emplaces a landmark or modifies (if needed) a landmark
- * \param _feature_ptr pointer to the Feature
- */
- LandmarkBasePtr emplaceLandmark(FeatureBasePtr _feature_ptr);
-
- /** \brief Modifies (if needed) a landmark
- * \param _landmark pointer to the landmark
- * \param _feature pointer to the Feature.
- */
- void modifyLandmark(LandmarkBasePtr _landmark,
- FeatureBasePtr _feature);
-
- /** Post-process
- *
- * This is called by process() after finishing the processing algorithm.
- *
- * It does nothing for now
- */
- void postProcess() override {};
-
- void advanceDerived() override;
- void resetDerived() override;
-
- double detectionDistance(FeatureBasePtr _ftr1,
- FeatureBasePtr _ftr2,
- const VectorComposite& _pose1,
- const VectorComposite& _pose2,
- const VectorComposite& _pose_sen) const;
+ public:
+ ProcessorLandmarkExternal(ParamsProcessorLandmarkExternalPtr _params_tracker_feature);
+ ~ProcessorLandmarkExternal() override{};
+
+ // Factory method for high level API
+ WOLF_PROCESSOR_CREATE(ProcessorLandmarkExternal, ParamsProcessorLandmarkExternal);
+
+ void configure(SensorBasePtr _sensor) override{};
+
+ protected:
+ ParamsProcessorLandmarkExternalPtr params_tfle_;
+ TrackMatrix track_matrix_;
+ // std::set<SizeStd> lmks_ids_origin_;
+
+ /** Pre-process incoming Capture
+ *
+ * This is called by process() just after assigning incoming_ptr_ to a valid Capture.
+ *
+ * Extract the detections and fill unmatched_detections_incoming_ (FeaturePtrList)
+ */
+ void preProcess() override;
+
+ /** \brief Process known Features
+ * \return The number of successful matches.
+ *
+ * This function tracks features from last to incoming and starts new tracks for new (not tracked) features in
+ * incoming
+ */
+ unsigned int processKnown() override;
+
+ /**\brief Process new Features
+ *
+ */
+ unsigned int processNew(const int& _max_features) override
+ {
+ return 0;
+ };
+
+ /** \brief Vote for KeyFrame generation
+ *
+ * If a KeyFrame criterion is validated, this function returns true,
+ * meaning that it wants to create a KeyFrame at the \b last Capture.
+ *
+ * WARNING! This function only votes! It does not create KeyFrames!
+ */
+ bool voteForKeyFrame() const override;
+
+ /** \brief Emplaces a new factor for each known feature in Capture \b last
+ */
+ void establishFactors() override;
+
+ /** \brief Emplaces a new factor
+ * \param _feature feature pointer
+ * \param _landmark pointer to the landmark
+ */
+ FactorBasePtr emplaceFactor(FeatureBasePtr _feature, LandmarkBasePtr _landmark);
+
+ /** \brief Emplaces a landmark or modifies (if needed) a landmark
+ * \param _feature_ptr pointer to the Feature
+ */
+ LandmarkExternalPtr emplaceLandmark(FeatureLandmarkExternalPtr _feature_ptr);
+
+ /** \brief Modifies (if needed) a landmark
+ * \param _landmark pointer to the landmark
+ * \param _feature pointer to the Feature.
+ */
+ void modifyLandmark(LandmarkExternalPtr _landmark, FeatureLandmarkExternalPtr _feature);
+
+ /** Post-process
+ *
+ * This is called by process() after finishing the processing algorithm.
+ *
+ * It does nothing for now
+ */
+ void postProcess() override{};
+
+ void advanceDerived() override;
+ void resetDerived() override;
+
+ double detectionDistance(FeatureBasePtr _ftr1,
+ FeatureBasePtr _ftr2,
+ const VectorComposite& _pose1,
+ const VectorComposite& _pose2,
+ const VectorComposite& _pose_sen) const;
+
+ double detectionDistance(FeatureBasePtr _ftr,
+ LandmarkBasePtr _lmk,
+ const VectorComposite& _pose_frm,
+ const VectorComposite& _pose_sen) const;
};
-inline ProcessorLandmarkExternal::ProcessorLandmarkExternal(ParamsProcessorLandmarkExternalPtr _params_tfle) :
- ProcessorTracker("ProcessorLandmarkExternal", "PO", 0, _params_tfle),
- params_tfle_(_params_tfle),
- lmks_ids_origin_()
-{
- //
-}
-
-inline ProcessorLandmarkExternal::~ProcessorLandmarkExternal()
+inline ProcessorLandmarkExternal::ProcessorLandmarkExternal(ParamsProcessorLandmarkExternalPtr _params_tfle)
+ : ProcessorTracker("ProcessorLandmarkExternal", "PO", 0, _params_tfle),
+ params_tfle_(_params_tfle)//,lmks_ids_origin_()
{
//
}
-} // namespace wolf
+} // namespace wolf
+#endif
\ No newline at end of file
diff --git a/include/core/processor/processor_motion.h b/include/core/processor/processor_motion.h
index b20577316ba403c6f63200401d8634d8f2984f36..8ac416337042c72316eda35b134fb70934b13c5c 100644
--- a/include/core/processor/processor_motion.h
+++ b/include/core/processor/processor_motion.h
@@ -313,7 +313,7 @@ class ProcessorMotion : public ProcessorBase, public MotionProvider
FrameBasePtr computeProcessingStep();
-
+ void assertsCaptureMotion(CaptureMotionPtr _capture, std::string error_prefix) const;
// These are the pure virtual functions doing the mathematics
public:
diff --git a/include/core/processor/processor_tracker.h b/include/core/processor/processor_tracker.h
index 53b6013ed1f7719020dfac40afec0c7deef3e0e1..d1ad927c1a49e750e01883858fc0f27927b112a4 100644
--- a/include/core/processor/processor_tracker.h
+++ b/include/core/processor/processor_tracker.h
@@ -142,6 +142,8 @@ class ProcessorTracker : public ProcessorBase
virtual CaptureBasePtr getOrigin();
virtual CaptureBaseConstPtr getLast() const;
virtual CaptureBasePtr getLast();
+ virtual FrameBaseConstPtr getLastFrame() const;
+ virtual FrameBasePtr getLastFrame();
virtual CaptureBaseConstPtr getIncoming() const;
virtual CaptureBasePtr getIncoming();
@@ -343,6 +345,16 @@ inline CaptureBasePtr ProcessorTracker::getLast()
return last_ptr_;
}
+inline FrameBaseConstPtr ProcessorTracker::getLastFrame() const
+{
+ return last_frame_ptr_;
+}
+
+inline FrameBasePtr ProcessorTracker::getLastFrame()
+{
+ return last_frame_ptr_;
+}
+
inline CaptureBaseConstPtr ProcessorTracker::getIncoming() const
{
return incoming_ptr_;
diff --git a/src/capture/capture_landmarks_external.cpp b/src/capture/capture_landmarks_external.cpp
index 471faafacc75c7540f93ecc7c1a22e323dbd7964..c3a14c19331e1c13bcd838761f55ec860f052e52 100644
--- a/src/capture/capture_landmarks_external.cpp
+++ b/src/capture/capture_landmarks_external.cpp
@@ -26,6 +26,7 @@ namespace wolf{
CaptureLandmarksExternal::CaptureLandmarksExternal(const TimeStamp& _ts,
SensorBasePtr _sensor_ptr,
const std::vector<int>& _ids,
+ const std::vector<int>& _types,
const std::vector<Eigen::VectorXd>& _detections,
const std::vector<Eigen::MatrixXd>& _covs,
const std::vector<double>& _qualities) :
@@ -37,7 +38,7 @@ CaptureLandmarksExternal::CaptureLandmarksExternal(const TimeStamp& _ts,
throw std::runtime_error("CaptureLandmarksExternal constructor: '_ids', '_detections', '_covs', '_qualities' should have the same size.");
for (auto i = 0; i < _detections.size(); i++)
- addDetection(_ids.at(i), _detections.at(i), _covs.at(i), _qualities.at(i));
+ addDetection(_ids.at(i), _types.at(i), _detections.at(i), _covs.at(i), _qualities.at(i));
}
CaptureLandmarksExternal::~CaptureLandmarksExternal()
@@ -45,9 +46,9 @@ CaptureLandmarksExternal::~CaptureLandmarksExternal()
//
}
-void CaptureLandmarksExternal::addDetection(const int& _id, const Eigen::VectorXd& _detection, const Eigen::MatrixXd& _cov, const double& _quality)
+void CaptureLandmarksExternal::addDetection(const int& _id, const int& _type, const Eigen::VectorXd& _detection, const Eigen::MatrixXd& _cov, const double& _quality)
{
- detections_.push_back(LandmarkDetection{_id, _detection, _cov, _quality});
+ detections_.push_back(LandmarkDetection{_id, _type, _detection, _cov, _quality});
}
} // namespace wolf
diff --git a/src/feature/feature_landmark_external.cpp b/src/feature/feature_landmark_external.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..686d1261db1c38d3975f19b386b5d4701478769b
--- /dev/null
+++ b/src/feature/feature_landmark_external.cpp
@@ -0,0 +1,62 @@
+//--------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--------
+#include "core/feature/feature_landmark_external.h"
+
+namespace wolf
+{
+
+FeatureLandmarkExternal::FeatureLandmarkExternal(const Eigen::VectorXd& _measurement,
+ const Eigen::MatrixXd& _meas_covariance,
+ const int& _external_id,
+ const int& _external_type)
+ : FeatureBase("FeatureLandmarkExternal", _measurement, _meas_covariance),
+ external_id_(_external_id),
+ external_type_(_external_type)
+{
+}
+
+FeatureLandmarkExternal::~FeatureLandmarkExternal()
+{
+ //
+}
+
+int FeatureLandmarkExternal::getExternalType() const
+{
+ return external_type_;
+}
+
+void FeatureLandmarkExternal::setExternalType(const int& _external_type)
+{
+ external_type_ = _external_type;
+}
+
+int FeatureLandmarkExternal::getExternalId() const
+{
+ return external_id_;
+}
+
+void FeatureLandmarkExternal::setExternalId(const int& _external_id)
+{
+ external_id_ = _external_id;
+}
+
+} // namespace wolf
diff --git a/src/landmark/landmark_base.cpp b/src/landmark/landmark_base.cpp
index 5569280d4fa7d6dde04b26606fda51d67fece67a..7bec4d89f3098b651fd8c74f7ea07f9210f1e814 100644
--- a/src/landmark/landmark_base.cpp
+++ b/src/landmark/landmark_base.cpp
@@ -38,7 +38,6 @@ LandmarkBase::LandmarkBase(const std::string& _type, StateBlockPtr _p_ptr, State
NodeBase("LANDMARK", _type),
HasStateBlocks(""),
map_ptr_(),
- //state_block_vec_(0), // Resize in derived constructors if needed.
landmark_id_(++landmark_id_count_)
{
if (_p_ptr)
@@ -85,46 +84,6 @@ void LandmarkBase::remove(bool viral_remove_empty_parent)
}
}
-// std::vector<StateBlockConstPtr> LandmarkBase::getUsedStateBlockVec() const
-// {
-// std::vector<StateBlockConstPtr> used_state_block_vec(0);
-
-// // normal state blocks in {P,O,V,W}
-// for (auto key : getStructure())
-// {
-// const auto sbp = getStateBlock(key);
-// if (sbp)
-// used_state_block_vec.push_back(sbp);
-// }
-
-// // // other state blocks in a vector
-// // for (auto sbp : state_block_vec_)
-// // if (sbp)
-// // used_state_block_vec.push_back(sbp);
-
-// return used_state_block_vec;
-// }
-
-// std::vector<StateBlockPtr> LandmarkBase::getUsedStateBlockVec()
-// {
-// std::vector<StateBlockPtr> used_state_block_vec(0);
-
-// // normal state blocks in {P,O,V,W}
-// for (auto key : getStructure())
-// {
-// auto sbp = getStateBlock(key);
-// if (sbp)
-// used_state_block_vec.push_back(sbp);
-// }
-
-// // // other state blocks in a vector
-// // for (auto sbp : state_block_vec_)
-// // if (sbp)
-// // used_state_block_vec.push_back(sbp);
-
-// return used_state_block_vec;
-// }
-
bool LandmarkBase::getCovariance(Eigen::MatrixXd& _cov) const
{
return getProblem()->getLandmarkCovariance(shared_from_this(), _cov);
@@ -212,6 +171,7 @@ void LandmarkBase::print(int _depth, bool _constr_by, bool _metric, bool _state_
{
printHeader(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs);
}
+
LandmarkBasePtr LandmarkBase::create(const YAML::Node& _node)
{
unsigned int id = _node["id"] .as< unsigned int >();
@@ -325,6 +285,7 @@ bool LandmarkBase::check(CheckLog& _log, NodeBaseConstPtr _node_ptr, bool _verbo
return _log.is_consistent_;
}
+
// Register landmark creator
namespace
{
diff --git a/src/landmark/landmark_external.cpp b/src/landmark/landmark_external.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..8bbd6eaa6aa75756c45824837a899c89a44ab99a
--- /dev/null
+++ b/src/landmark/landmark_external.cpp
@@ -0,0 +1,82 @@
+//--------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--------
+
+#include "core/landmark/landmark_external.h"
+#include "core/state_block/state_block_derived.h"
+#include "core/state_block/state_angle.h"
+#include "core/state_block/state_quaternion.h"
+#include "core/state_block/factory_state_block.h"
+#include "core/yaml/yaml_conversion.h"
+
+namespace wolf
+{
+
+LandmarkExternal::LandmarkExternal(const int& _external_id,
+ const int& _external_type,
+ StateBlockPtr _p_ptr,
+ StateBlockPtr _o_ptr)
+ : LandmarkBase("LandmarkExternal", _p_ptr, _o_ptr), external_id_(_external_id), external_type_(_external_type)
+{
+ assert(external_id_ >= 0 or external_id_ == -1);
+ assert(external_type_ >= 0 or external_type_ == -1);
+}
+
+YAML::Node LandmarkExternal::saveToYaml() const
+{
+ YAML::Node node = LandmarkBase::saveToYaml();
+ node["external_id"] = external_id_;
+ node["external_type"] = external_type_;
+ return node;
+}
+
+LandmarkBasePtr LandmarkExternal::create(const YAML::Node& _node)
+{
+ unsigned int external_id = _node["external_id"].as<int>();
+ unsigned int external_type = _node["external_type"].as<int>();
+ Eigen::VectorXd pos = _node["position"].as<Eigen::VectorXd>();
+ bool pos_fixed = _node["position fixed"].as<bool>();
+
+ StateBlockPtr pos_sb = FactoryStateBlock::create("P", pos, pos_fixed);
+ StateBlockPtr ori_sb = nullptr;
+
+ if (_node["orientation"])
+ {
+ Eigen::VectorXd ori = _node["orientation"].as<Eigen::VectorXd>();
+ bool ori_fixed = _node["orientation fixed"].as<bool>();
+
+ if (ori.size() == 4)
+ ori_sb = std::make_shared<StateQuaternion>(ori, ori_fixed);
+ else
+ ori_sb = std::make_shared<StateAngle>(ori(0), ori_fixed);
+ }
+
+ return std::make_shared<LandmarkExternal>(external_id, external_type, pos_sb, ori_sb);
+}
+
+// Register landmark creator
+namespace
+{
+const bool WOLF_UNUSED registered_lmk_external =
+ FactoryLandmark::registerCreator("LandmarkExternal", LandmarkExternal::create);
+}
+
+} // namespace wolf
diff --git a/src/map/map_base.cpp b/src/map/map_base.cpp
index 3ad1158ca2f1d3dddda64189e8bdd96413976cdb..e9f37bfb008dbbd9691cdbee3bbf8f42fc7c430d 100644
--- a/src/map/map_base.cpp
+++ b/src/map/map_base.cpp
@@ -95,20 +95,34 @@ LandmarkBasePtr MapBase::getLandmark(const unsigned int& _id)
return (*lmk_it);
}
-void MapBase::load(const std::string& _map_file_dot_yaml)
+void MapBase::load(std::string _map_file_dot_yaml)
{
- YAML::Node map = YAML::LoadFile(_map_file_dot_yaml);
+ // change ~ with HOME using environment variable
+ if (_map_file_dot_yaml.at(0) == '~')
+ _map_file_dot_yaml = std::string(std::getenv("HOME")) + _map_file_dot_yaml.substr(1);
- for (unsigned int i = 0; i < map["landmarks"].size(); i++)
+ try
{
- YAML::Node lmk_node = map["landmarks"][i];
- LandmarkBasePtr lmk_ptr = FactoryLandmark::create(lmk_node["type"].as<std::string>(), lmk_node);
- lmk_ptr->link(shared_from_this());
+ YAML::Node map = YAML::LoadFile(_map_file_dot_yaml);
+ for (unsigned int i = 0; i < map["landmarks"].size(); i++)
+ {
+ YAML::Node lmk_node = map["landmarks"][i];
+ LandmarkBasePtr lmk_ptr = FactoryLandmark::create(lmk_node["type"].as<std::string>(), lmk_node);
+ lmk_ptr->link(shared_from_this());
+ }
}
+ catch(const std::exception& e)
+ {
+ std::cerr << "MapBase::load: " << e.what() << '\n';
+ }
}
-void MapBase::save(const std::string& _map_file_yaml, const std::string& _map_name)
+void MapBase::save(std::string _map_file_yaml, const std::string& _map_name) const
{
+ // change ~ with HOME using environment variable
+ if (_map_file_yaml.at(0) == '~')
+ _map_file_yaml = std::string(std::getenv("HOME")) + _map_file_yaml.substr(1);
+
YAML::Emitter emitter;
emitter << YAML::BeginMap;
@@ -117,7 +131,7 @@ void MapBase::save(const std::string& _map_file_yaml, const std::string& _map_na
emitter << "landmarks" << YAML::BeginSeq;
- for (LandmarkBasePtr lmk_ptr : getLandmarkList())
+ for (LandmarkBaseConstPtr lmk_ptr : getLandmarkList())
{
emitter << YAML::Flow << lmk_ptr->saveToYaml();
}
@@ -128,7 +142,7 @@ void MapBase::save(const std::string& _map_file_yaml, const std::string& _map_na
fout.close();
}
-std::string MapBase::dateTimeNow()
+std::string MapBase::dateTimeNow() const
{
// Get date and time for archiving purposes
std::time_t rawtime;
diff --git a/src/processor/motion_buffer.cpp b/src/processor/motion_buffer.cpp
index 9142edd1ca774437cbbc83f8d04d84e00abc3e6a..4b68c61340c51fa8cdbeefb85129ddd1bc1f11ea 100644
--- a/src/processor/motion_buffer.cpp
+++ b/src/processor/motion_buffer.cpp
@@ -120,8 +120,8 @@ void MotionBuffer::getMotion(const TimeStamp& _ts, Motion& _motion) const
void MotionBuffer::split(const TimeStamp& _ts, MotionBuffer& _buffer_part_before_ts)
{
- assert((this->front().ts_ <= _ts) && "Error: Query time stamp is smaller than the buffer's first tick");
- assert((this->back().ts_ >= _ts) && "Error: Query time stamp is greater than the buffer's last tick");
+ assert((this->front().ts_ <= _ts) && "Error: Query time stamp is smaller or equal than the buffer's first tick");
+ assert((this->back().ts_ >= _ts) && "Error: Query time stamp is greater or equal than the buffer's last tick");
auto previous = std::find_if(this->rbegin(), this->rend(), [&](const Motion& m)
{
diff --git a/src/processor/processor_base.cpp b/src/processor/processor_base.cpp
index aa034dd72eb6c26825a037792400dcef988c312c..5b15b2761d099fe39e61598467b4fa337cac544c 100644
--- a/src/processor/processor_base.cpp
+++ b/src/processor/processor_base.cpp
@@ -217,23 +217,23 @@ void ProcessorBase::printProfiling(std::ostream& _stream) const
}
-bool ProcessorBase::checkTimeTolerance (const TimeStamp& _ts1, const TimeStamp& _ts2)
+bool ProcessorBase::checkTimeTolerance (const TimeStamp& _ts1, const TimeStamp& _ts2) const
{
auto dt = std::fabs(_ts1 - _ts2);
return dt <= params_->time_tolerance;
}
-bool ProcessorBase::checkTimeTolerance (const CaptureBasePtr _cap, const TimeStamp& _ts)
+bool ProcessorBase::checkTimeTolerance (const CaptureBasePtr _cap, const TimeStamp& _ts) const
{
return checkTimeTolerance(_cap->getTimeStamp(), _ts);
}
-bool ProcessorBase::checkTimeTolerance (const FrameBasePtr _frm, const TimeStamp& _ts)
+bool ProcessorBase::checkTimeTolerance (const FrameBasePtr _frm, const TimeStamp& _ts) const
{
return checkTimeTolerance(_frm->getTimeStamp(), _ts);
}
-bool ProcessorBase::checkTimeTolerance (const FrameBasePtr _frm, const CaptureBasePtr _cap)
+bool ProcessorBase::checkTimeTolerance (const FrameBasePtr _frm, const CaptureBasePtr _cap) const
{
return checkTimeTolerance(_frm->getTimeStamp(), _cap->getTimeStamp());
}
diff --git a/src/processor/processor_landmark_external.cpp b/src/processor/processor_landmark_external.cpp
index 5ca9a8c1bcf45f3f8a4c055d478cafc095aa2eae..7a2de8df3753961a8c6a60177445ba9bb355fdc7 100644
--- a/src/processor/processor_landmark_external.cpp
+++ b/src/processor/processor_landmark_external.cpp
@@ -22,12 +22,10 @@
#include "core/processor/processor_landmark_external.h"
#include "core/capture/capture_landmarks_external.h"
-#include "core/feature/feature_base.h"
#include "core/factor/factor_relative_pose_2d_with_extrinsics.h"
#include "core/factor/factor_relative_position_2d_with_extrinsics.h"
#include "core/factor/factor_relative_pose_3d_with_extrinsics.h"
#include "core/factor/factor_relative_position_3d_with_extrinsics.h"
-#include "core/landmark/landmark_base.h"
#include "core/state_block/state_block_derived.h"
#include "core/state_block/state_quaternion.h"
#include "core/state_block/state_angle.h"
@@ -41,23 +39,24 @@ namespace wolf
void ProcessorLandmarkExternal::preProcess()
{
- assert(new_features_incoming_.empty());
+ new_features_incoming_.clear();
- auto dim = getProblem()->getDim();
+ auto dim = getProblem()->getDim();
auto cap_landmarks = std::dynamic_pointer_cast<CaptureLandmarksExternal>(incoming_ptr_);
if (not cap_landmarks)
- throw std::runtime_error("ProcessorLandmarkExternal::preProcess incoming_ptr_ should be of type 'CaptureLandmarksExternal'");
+ throw std::runtime_error(
+ "ProcessorLandmarkExternal::preProcess incoming_ptr_ should be of type 'CaptureLandmarksExternal'");
// Extract features from capture detections
- auto landmark_detections = cap_landmarks->getDetections();
+ auto landmark_detections = cap_landmarks->getDetections();
std::set<int> ids;
for (auto detection : landmark_detections)
{
- WOLF_WARN_COND(ids.count(detection.id), "ProcessorLandmarkExternal::preProcess: detection with repeated id, discarding...");
+ WOLF_WARN_COND(ids.count(detection.id),
+ "ProcessorLandmarkExternal::preProcess: detection with repeated id, discarding...");
// Filter by quality
- if (detection.quality < params_tfle_->filter_quality_th or ids.count(detection.id))
- continue;
+ if (detection.quality < params_tfle_->quality_th or ids.count(detection.id)) continue;
// measure and covariance
VectorXd meas;
@@ -68,33 +67,29 @@ void ProcessorLandmarkExternal::preProcess()
assert(detection.covariance.rows() >= dim and detection.covariance.rows() == detection.covariance.cols());
meas = detection.measure.head(dim);
- cov = detection.covariance.topLeftCorner(dim,dim);
+ cov = detection.covariance.topLeftCorner(dim, dim);
}
else
{
assert(detection.measure.size() == (dim == 2 ? 3 : 7));
- assert(detection.covariance.rows() == (dim == 2 ? 3 : 6) and detection.covariance.rows() == detection.covariance.cols());
+ assert(detection.covariance.rows() == (dim == 2 ? 3 : 6) and
+ detection.covariance.rows() == detection.covariance.cols());
meas = detection.measure;
cov = detection.covariance;
}
// emplace feature
- FeatureBasePtr ftr = FeatureBase::emplace<FeatureBase>(cap_landmarks,
- "FeatureLandmarkExternal",
- meas,
- cov);
- ftr->setLandmarkId(detection.id);
-
- if (detection.id != -1 and detection.id != 0)
- ids.insert(detection.id);
-
- //unmatched_detections_incoming_.push_back(ftr);
+ FeatureBasePtr ftr =
+ FeatureBase::emplace<FeatureLandmarkExternal>(cap_landmarks, meas, cov, detection.id, detection.type);
+ if (detection.id >= 0) ids.insert(detection.id);
// add new feature
new_features_incoming_.push_back(ftr);
}
- WOLF_INFO("ProcessorLandmarkExternal::preprocess: found ", new_features_incoming_.size(), " new features");
+ WOLF_DEBUG("ProcessorLandmarkExternal::preprocess: found ",
+ new_features_incoming_.size(),
+ " features in incoming capture");
}
unsigned int ProcessorLandmarkExternal::processKnown()
@@ -103,40 +98,217 @@ unsigned int ProcessorLandmarkExternal::processKnown()
known_features_incoming_.clear();
// Track features from last_ptr_ to incoming_ptr_
- if (last_ptr_)
+ if (not last_ptr_) return 0;
+
+ WOLF_DEBUG("Searching ", known_features_last_.size(), " tracked features...");
+ auto pose_sen = getSensor()->getState("PO");
+ auto pose_last = getProblem()->getState(last_ptr_->getTimeStamp(), "PO");
+ auto pose_incoming = getProblem()->getState(incoming_ptr_->getTimeStamp(), "PO");
+
+ for (auto feat_last : known_features_last_)
{
- WOLF_INFO("Searching ", known_features_last_.size(), " tracked features...");
- auto pose_sen = getSensor()->getState("PO");
- auto pose_in = getProblem()->getState(last_ptr_->getTimeStamp(), "PO");
- auto pose_out = getProblem()->getState(incoming_ptr_->getTimeStamp(), "PO");
+ auto feat_lmk_last = std::static_pointer_cast<FeatureLandmarkExternal>(feat_last);
+ bool matched = false;
+ WOLF_DEBUG("Tracking feature last: ",
+ feat_lmk_last->id(),
+ " - ID: ",
+ feat_lmk_last->getExternalId(),
+ " - TYPE: ",
+ feat_lmk_last->getExternalType(),
+ " meas: ",
+ feat_lmk_last->getMeasurement().transpose());
+
+ // First we try to match by EXTERNAL_ID
+ if (feat_lmk_last->getExternalId() != -1)
+ {
+ auto feature_incoming_it = new_features_incoming_.begin();
+ while (feature_incoming_it != new_features_incoming_.end())
+ {
+ auto feat_lmk_incoming = std::static_pointer_cast<FeatureLandmarkExternal>(*feature_incoming_it);
+
+ WOLF_DEBUG("Feature incoming candidate (by ID): ",
+ feat_lmk_incoming->id(),
+ " - ID: ",
+ feat_lmk_incoming->getExternalId(),
+ " - TYPE: ",
+ feat_lmk_incoming->getExternalType(),
+ " meas: ",
+ feat_lmk_incoming->getMeasurement().transpose());
+
+ // MATCH NECESSARY CONDITIONS:
+ // 1. Same EXTERNAL_ID
+ // 2. Compatible TYPE (either not defined or same)
+ // 3. Detections close enough
+ if (feat_lmk_incoming->getExternalId() == feat_lmk_last->getExternalId() and // cond 1
+ (feat_lmk_incoming->getExternalType() == -1 or // cond 2
+ feat_lmk_last->getExternalType() == -1 or //
+ feat_lmk_incoming->getExternalType() == feat_lmk_last->getExternalType()) and //
+ detectionDistance( // cond 3
+ feat_lmk_last,
+ feat_lmk_incoming,
+ pose_last,
+ pose_incoming,
+ pose_sen) < params_tfle_->match_dist_th_id)
+ {
+ WOLF_DEBUG("Feature last: ",
+ feat_lmk_last->id(),
+ " matched with feature ",
+ feat_lmk_incoming->id(),
+ " with landmark ID: ",
+ feat_lmk_last->landmarkId());
+ matched = true;
+
+ // set LANDMARK_ID if defined
+ if (feat_lmk_last->landmarkId() != 0)
+ feat_lmk_incoming->setLandmarkId(feat_lmk_last->landmarkId());
+
+ // grow track
+ track_matrix_.add(feat_lmk_last, feat_lmk_incoming);
+
+ // feature is known
+ known_features_incoming_.push_back(feat_lmk_incoming);
+
+ // remove from unmatched
+ feature_incoming_it = new_features_incoming_.erase(feature_incoming_it);
+ break;
+ }
+ else
+ feature_incoming_it++;
+ }
+ }
+ // skip search (already found match)
+ if (matched) continue;
- for (auto feat_last : known_features_last_)
+ // Second we try to match by TYPE (if defined)
+ if (feat_lmk_last->getExternalType() != -1)
{
- auto feat_candidate_it = new_features_incoming_.begin();
- while (feat_candidate_it != new_features_incoming_.end())
+ auto feature_incoming_it = new_features_incoming_.begin();
+ while (feature_incoming_it != new_features_incoming_.end())
{
- if ((*feat_candidate_it)->landmarkId() == feat_last->landmarkId() and
- detectionDistance(feat_last, (*feat_candidate_it), pose_in, pose_out, pose_sen) < params_tfle_->match_dist_th)
+ auto feat_lmk_incoming = std::static_pointer_cast<FeatureLandmarkExternal>(*feature_incoming_it);
+ WOLF_DEBUG("Feature incoming candidate (by TYPE): ",
+ feat_lmk_incoming->id(),
+ " - ID: ",
+ feat_lmk_incoming->getExternalId(),
+ " - TYPE: ",
+ feat_lmk_incoming->getExternalType(),
+ " meas: ",
+ feat_lmk_incoming->getMeasurement().transpose());
+
+ // MATCH NECESSARY CONDITIONS:
+ // 1. Compatible EXTERNAL_ID (either not defined or same)
+ // 2. Same TYPE
+ // 3. Detections close enough
+ if ((feat_lmk_incoming->getExternalId() == -1 or // cond 1
+ feat_lmk_last->getExternalId() == -1 or //
+ feat_lmk_incoming->getExternalId() == feat_lmk_last->getExternalId()) and //
+ feat_lmk_incoming->getExternalType() == feat_lmk_last->getExternalType() and // cond 2
+ detectionDistance( // cond 3
+ feat_lmk_last,
+ feat_lmk_incoming,
+ pose_last,
+ pose_incoming,
+ pose_sen) < params_tfle_->match_dist_th_type)
{
+ WOLF_DEBUG("Feature last: ",
+ feat_lmk_last->id(),
+ " matched with feature ",
+ feat_lmk_incoming->id(),
+ " with landmark ID: ",
+ feat_lmk_last->landmarkId());
+ matched = true;
+
+ // set LANDMARK_ID last -> incoming
+ if (feat_lmk_last->landmarkId() != 0)
+ feat_lmk_incoming->setLandmarkId(feat_lmk_last->landmarkId());
+
+ // set EXTERNAL_ID last -> incoming
+ if (feat_lmk_last->getExternalId() != -1)
+ feat_lmk_incoming->setExternalId(feat_lmk_last->getExternalId());
+
// grow track
- track_matrix_.add(feat_last, *feat_candidate_it);
+ track_matrix_.add(feat_lmk_last, feat_lmk_incoming);
// feature is known
- known_features_incoming_.push_back((*feat_candidate_it));
+ known_features_incoming_.push_back(feat_lmk_incoming);
// remove from unmatched
- feat_candidate_it = new_features_incoming_.erase(feat_candidate_it);
+ feature_incoming_it = new_features_incoming_.erase(feature_incoming_it);
break;
}
else
- feat_candidate_it++;
+ feature_incoming_it++;
}
}
- WOLF_INFO("Tracked ", known_features_incoming_.size(), " features.");
+ // skip search (already found match)
+ if (matched) continue;
+
+ // Finally, we try to match by distance
+ auto feature_incoming_it = new_features_incoming_.begin();
+ while (feature_incoming_it != new_features_incoming_.end())
+ {
+ auto feat_lmk_incoming = std::static_pointer_cast<FeatureLandmarkExternal>(*feature_incoming_it);
+
+ WOLF_DEBUG("Feature incoming candidate (by distance): ",
+ feat_lmk_incoming->id(),
+ " - ID: ",
+ feat_lmk_incoming->getExternalId(),
+ " - TYPE: ",
+ feat_lmk_incoming->getExternalType(),
+ " meas: ",
+ feat_lmk_incoming->getMeasurement().transpose());
+
+ // MATCH NECESSARY CONDITIONS:
+ // 1. Compatible EXTERNAL_ID (either not defined or same)
+ // 2. Compatible TYPE (either not defined or same)
+ // 3. Detections close enough
+ if ((feat_lmk_incoming->getExternalId() == -1 or // cond 1
+ feat_lmk_last->getExternalId() == -1 or //
+ feat_lmk_incoming->getExternalId() == feat_lmk_last->getExternalId()) and //
+ (feat_lmk_incoming->getExternalType() == -1 or // cond 2
+ feat_lmk_last->getExternalType() == -1 or //
+ feat_lmk_incoming->getExternalType() == feat_lmk_last->getExternalType()) and //
+ detectionDistance(feat_lmk_last, feat_lmk_incoming, pose_last, pose_incoming, pose_sen) < // cond 3
+ params_tfle_->match_dist_th_unknown)
+ {
+ WOLF_DEBUG("Feature last: ",
+ feat_lmk_last->id(),
+ " matched with feature ",
+ feat_lmk_incoming->id(),
+ " with landmark ID: ",
+ feat_lmk_last->landmarkId());
+ matched = true;
+
+ // set LANDMARK_ID last -> incoming
+ if (feat_lmk_last->landmarkId() != 0) feat_lmk_incoming->setLandmarkId(feat_lmk_last->landmarkId());
+
+ // set TYPE last -> incoming
+ if (feat_lmk_last->getExternalType() != -1)
+ feat_lmk_incoming->setExternalType(feat_lmk_last->getExternalType());
+
+ // set EXTERNAL_ID last -> incoming
+ if (feat_lmk_last->getExternalId() != -1)
+ feat_lmk_incoming->setExternalId(feat_lmk_last->getExternalId());
+
+ // grow track
+ track_matrix_.add(feat_lmk_last, feat_lmk_incoming);
+
+ // feature is known
+ known_features_incoming_.push_back(feat_lmk_incoming);
+
+ // remove from unmatched
+ feature_incoming_it = new_features_incoming_.erase(feature_incoming_it);
+ break;
+ }
+ else
+ feature_incoming_it++;
+ }
+ WOLF_DEBUG_COND(not matched, "Feature ", feat_lmk_last->id(), " not tracked.");
}
+ WOLF_DEBUG("Tracked ", known_features_incoming_.size(), " features.");
// Add new features (not tracked) as known features
- WOLF_INFO_COND(not new_features_incoming_.empty(), "Adding new features ", new_features_incoming_.size());
+ WOLF_DEBUG_COND(not new_features_incoming_.empty(), "Adding new features ", new_features_incoming_.size());
while (not new_features_incoming_.empty())
{
auto ftr = new_features_incoming_.front();
@@ -152,20 +324,23 @@ unsigned int ProcessorLandmarkExternal::processKnown()
}
// check all features have been emplaced
- assert(std::all_of(known_features_incoming_.begin(), known_features_incoming_.end(), [](FeatureBasePtr f){return f->getCapture() != nullptr;}) &&
+ assert(std::all_of(known_features_incoming_.begin(),
+ known_features_incoming_.end(),
+ [](FeatureBasePtr f) { return f->getCapture() != nullptr; }) &&
"any not linked feature returned by trackFeatures()");
return known_features_incoming_.size();
}
-double ProcessorLandmarkExternal::detectionDistance(FeatureBasePtr _ftr1,
- FeatureBasePtr _ftr2,
- const VectorComposite& _pose1,
- const VectorComposite& _pose2,
- const VectorComposite& _pose_sen) const
+double ProcessorLandmarkExternal::detectionDistance(FeatureBasePtr _ftr1,
+ FeatureBasePtr _ftr2,
+ const VectorComposite& _pose1,
+ const VectorComposite& _pose2,
+ const VectorComposite& _pose_sen) const
{
// Any not available info of poses, assume identity
- if (not _pose1.includesStructure("PO") or not _pose2.includesStructure("PO") or not _pose_sen.includesStructure("PO"))
+ if (not _pose1.includesStructure("PO") or not _pose2.includesStructure("PO") or
+ not _pose_sen.includesStructure("PO"))
{
if (getProblem()->getDim() == 2)
return (_ftr1->getMeasurement().head<2>() - _ftr2->getMeasurement().head<2>()).norm();
@@ -176,163 +351,276 @@ double ProcessorLandmarkExternal::detectionDistance(FeatureBasePtr _ftr1,
{
if (getProblem()->getDim() == 2)
{
- auto pose_s1 = SE2::compose(_pose1, _pose_sen);
- auto pose_s2 = SE2::compose(_pose2, _pose_sen);
- auto p1 = pose_s1.at('P') + Rotation2Dd(pose_s1.at('O')(0)) * _ftr1->getMeasurement().head<2>();
- auto p2 = pose_s2.at('P') + Rotation2Dd(pose_s2.at('O')(0)) * _ftr2->getMeasurement().head<2>();
- return (p1-p2).norm();
+ VectorComposite pose_s1 = SE2::compose(_pose1, _pose_sen);
+ VectorComposite pose_s2 = SE2::compose(_pose2, _pose_sen);
+
+ Eigen::Vector2d p1 = pose_s1.at('P') + Rotation2Dd(pose_s1.at('O')(0)) * _ftr1->getMeasurement().head<2>();
+ Eigen::Vector2d p2 = pose_s2.at('P') + Rotation2Dd(pose_s2.at('O')(0)) * _ftr2->getMeasurement().head<2>();
+
+ return (p1 - p2).norm();
}
else
{
- auto pose_s1 = SE3::compose(_pose1, _pose_sen);
- auto pose_s2 = SE3::compose(_pose2, _pose_sen);
- auto p1 = pose_s1.at('P') + Quaterniond(Vector4d(pose_s1.at('O'))) * _ftr1->getMeasurement().head<3>();
- auto p2 = pose_s2.at('P') + Quaterniond(Vector4d(pose_s2.at('O'))) * _ftr2->getMeasurement().head<3>();
- return (p1-p2).norm();
+ VectorComposite pose_s1 = SE3::compose(_pose1, _pose_sen);
+ VectorComposite pose_s2 = SE3::compose(_pose2, _pose_sen);
+ Eigen::Vector3d p1 =
+ pose_s1.at('P') + Quaterniond(Vector4d(pose_s1.at('O'))) * _ftr1->getMeasurement().head<3>();
+ Eigen::Vector3d p2 =
+ pose_s2.at('P') + Quaterniond(Vector4d(pose_s2.at('O'))) * _ftr2->getMeasurement().head<3>();
+
+ return (p1 - p2).norm();
}
}
}
+double ProcessorLandmarkExternal::detectionDistance(FeatureBasePtr _ftr,
+ LandmarkBasePtr _lmk,
+ const VectorComposite& _pose_frm,
+ const VectorComposite& _pose_sen) const
+{
+ // Needed all poses
+ if (not _pose_frm.includesStructure("PO") or not _pose_sen.includesStructure("PO") or not _lmk->hasStateBlock('P'))
+ {
+ throw std::runtime_error(
+ "ProcessorLandmarkExternal::detectionDistance: Missing any required geometric information");
+ }
+
+ if (getProblem()->getDim() == 2)
+ {
+ auto pose_s = SE2::compose(_pose_frm, _pose_sen);
+ Eigen::Vector2d p = pose_s.at('P') + Rotation2Dd(pose_s.at('O')(0)) * _ftr->getMeasurement().head<2>();
+ return (p - _lmk->getP()->getState().head<2>()).norm();
+ }
+ else
+ {
+ auto pose_s = SE3::compose(_pose_frm, _pose_sen);
+ Eigen::Vector3d p = pose_s.at('P') + Quaterniond(Vector4d(pose_s.at('O'))) * _ftr->getMeasurement().head<3>();
+ return (p - _lmk->getP()->getState().head<3>()).norm();
+ }
+}
+
bool ProcessorLandmarkExternal::voteForKeyFrame() const
{
auto track_ids_last = track_matrix_.trackIds(last_ptr_);
- WOLF_INFO("Active feature tracks: " , track_ids_last.size() );
-
- // no tracks longer than filter_track_length_th
- auto n_tracks = 0;
+ WOLF_DEBUG("Active feature tracks: ", track_ids_last.size());
+
+ // number of tracks longer than track_length_th
+ auto n_tracks = 0;
auto n_new_tracks = 0;
for (auto track_id : track_ids_last)
{
- if (track_matrix_.trackSize(track_id) >= params_tfle_->filter_track_length_th)
+ if (track_matrix_.trackSize(track_id) >= params_tfle_->track_length_th)
{
n_tracks++;
- if (not lmks_ids_origin_.count(track_matrix_.feature(track_id, last_ptr_)->landmarkId()))
- n_new_tracks++;
+ if (track_matrix_.feature(track_id, last_ptr_)->landmarkId() == 0) n_new_tracks++;
}
}
-
+
// Necessary condition: active valid tracks
bool vote_min_features = n_tracks >= params_tfle_->min_features_for_keyframe;
- WOLF_INFO("vote_min_features: ", vote_min_features,
- " - Active feature tracks longer than ", params_tfle_->filter_track_length_th, ": ", n_tracks,
- " (should be equal or bigger than ", params_tfle_->min_features_for_keyframe, ")");
+ WOLF_DEBUG("vote_min_features: ",
+ vote_min_features,
+ " - Active feature tracks longer than ",
+ params_tfle_->track_length_th,
+ ": ",
+ n_tracks,
+ " (should be equal or bigger than ",
+ params_tfle_->min_features_for_keyframe,
+ ")");
bool vote_new_features(true), vote_time_span(true);
if (vote_min_features and origin_ptr_)
{
// Sufficient condition: new valid tracks
vote_new_features = n_new_tracks >= params_tfle_->new_features_for_keyframe;
- WOLF_INFO("vote_new_features: " , vote_new_features,
- " - n_new_tracks = ", n_new_tracks,
- " (should be equal or bigger than ", params_tfle_->new_features_for_keyframe, ")");
+ WOLF_DEBUG("vote_new_features: ",
+ vote_new_features,
+ " - n_new_tracks = ",
+ n_new_tracks,
+ " (should be equal or bigger than ",
+ params_tfle_->new_features_for_keyframe,
+ ")");
// Sufficient condition: time span
vote_time_span = last_ptr_->getTimeStamp() - origin_ptr_->getTimeStamp() > params_tfle_->time_span;
- WOLF_INFO("vote_time_span: " , vote_time_span,
- " - time_span = ", last_ptr_->getTimeStamp() - origin_ptr_->getTimeStamp(),
- " (should be bigger than ", params_tfle_->time_span, ")");
+ WOLF_DEBUG("vote_time_span: ",
+ vote_time_span,
+ " - time_span = ",
+ last_ptr_->getTimeStamp() - origin_ptr_->getTimeStamp(),
+ " (should be bigger than ",
+ params_tfle_->time_span,
+ ")");
}
bool vote = vote_min_features and (vote_new_features or vote_time_span);
- WOLF_INFO( (vote ? "Vote ": "Do NOT vote ") , "for KF" );
+ WOLF_DEBUG((vote ? "Vote " : "Do NOT vote "), "for KF");
return vote;
}
void ProcessorLandmarkExternal::establishFactors()
{
- if (origin_ptr_ == last_ptr_)
- return;
-
- // reset n_tracks_origin_
- lmks_ids_origin_.clear();
-
- // will emplace a factor (and landmark if needed) for each known feature in last with long tracks (params_tfle_->filter_track_length_th)
+ // will emplace a factor (and landmark if needed) for each known feature in last with long tracks
FactorBasePtrList fac_list;
- auto track_ids_last = track_matrix_.trackIds(last_ptr_);
+ auto track_ids_last = track_matrix_.trackIds(last_ptr_);
+ auto pose_sen = getSensor()->getState("PO");
+ auto pose_frm = getProblem()->getState(last_ptr_->getTimeStamp(), "PO");
for (auto track_id : track_ids_last)
{
- // check track length
- if (track_matrix_.trackSize(track_id) < params_tfle_->filter_track_length_th)
- continue;
+ auto feature = std::static_pointer_cast<FeatureLandmarkExternal>(track_matrix_.feature(track_id, last_ptr_));
+
+ WOLF_DEBUG("Feature ",
+ feature->id(),
+ ": ID: ",
+ feature->landmarkId(),
+ " EXTERNAL_ID: ",
+ feature->getExternalId(),
+ " TYPE: ",
+ feature->getExternalType());
+
+ // not enough long track
+ WOLF_DEBUG_COND(track_matrix_.trackSize(track_id) < params_tfle_->track_length_th,
+ "Track NOT long enough ",
+ track_matrix_.trackSize(track_id));
+ if (track_matrix_.trackSize(track_id) < params_tfle_->track_length_th) continue;
+
+ // Landmark match
+ LandmarkExternalPtr lmk;
+ if (feature->landmarkId() == 0) // landmark id 0 never used
+ {
+ // LOOP CLOSURE
+ // By ID
+ WOLF_DEBUG("Searching Loop closure by ID...");
+ if (params_tfle_->close_loops_by_id and feature->getExternalId() != -1)
+ {
+ auto lmk_list = getProblem()->getMap()->getLandmarkList();
- auto feature = track_matrix_.feature(track_id, last_ptr_);
+ for (auto lmk_ptr : lmk_list)
+ {
+ auto lmk_ext = std::dynamic_pointer_cast<LandmarkExternal>(lmk_ptr);
+
+ if (lmk_ext and lmk_ext->getExternalId() == feature->getExternalId() and
+ detectionDistance(feature, lmk_ext, pose_frm, pose_sen) < params_tfle_->match_dist_th_id)
+ {
+ lmk = lmk_ext;
+ WOLF_DEBUG("Found loop closure by EXTERNAL_ID with ", lmk->id());
+ break;
+ }
+ WOLF_DEBUG_COND(lmk_ext and lmk_ext->getExternalId() == feature->getExternalId(),
+ "Landmark with EXTERNAL_ID found but not matched due to distance: ",
+ detectionDistance(feature, lmk_ext, pose_frm, pose_sen))
+ }
+ }
+ // By TYPE
+ WOLF_DEBUG("Searching Loop closure by TYPE...");
+ if (not lmk and params_tfle_->close_loops_by_type and feature->getExternalType() != -1)
+ {
+ auto lmk_list = getProblem()->getMap()->getLandmarkList();
- // get landmark
- LandmarkBasePtr lmk = getProblem()->getMap()->getLandmark(feature->landmarkId());
-
- // emplace landmark
- if (not lmk)
- lmk = emplaceLandmark(feature);
+ for (auto lmk_ptr : lmk_list)
+ {
+ auto lmk_ext = std::dynamic_pointer_cast<LandmarkExternal>(lmk_ptr);
+
+ if (lmk_ext and lmk_ext->getExternalType() == feature->getExternalType() and
+ detectionDistance(feature, lmk_ext, pose_frm, pose_sen) < params_tfle_->match_dist_th_type)
+ {
+ lmk = lmk_ext;
+ WOLF_DEBUG("Found loop closure by TYPE with ", lmk->id());
+ break;
+ }
+ WOLF_DEBUG_COND(lmk_ext and lmk_ext->getExternalType() == feature->getExternalType(),
+ "Landmark with TYPE found but not matched due to distance: ",
+ detectionDistance(feature, lmk_ext, pose_frm, pose_sen))
+ }
- // modify landmark
- modifyLandmark(lmk, feature);
+ // update Landmark EXTERNAL_ID (if available)
+ if (feature->getExternalId() != -1) lmk->setExternalId(feature->getExternalId());
+ }
- // emplace factor
- auto fac = emplaceFactor(feature, lmk);
+ // Emplace new landmark (loop closure not found or not enabled)
+ if (not lmk)
+ {
+ lmk = emplaceLandmark(feature);
+ WOLF_DEBUG("Emplaced new landmark ", lmk->id());
+ }
- lmks_ids_origin_.insert(lmk->id());
+ // set LANDMARK_ID in all features of the track
+ for (auto feat_pair : track_matrix_.track(track_id))
+ {
+ feat_pair.second->setLandmarkId(lmk->id());
+ WOLF_DEBUG("Setting landmark id in feature ",
+ feat_pair.second->id(),
+ " landmark_id: ",
+ feat_pair.second->landmarkId());
+ }
+ }
+ // landmarkId already set
+ else
+ {
+ lmk = std::dynamic_pointer_cast<LandmarkExternal>(
+ getProblem()->getMap()->getLandmark(feature->landmarkId()));
+ if (not lmk)
+ throw std::runtime_error(
+ "ProcessorLandmarkExternal::establishFactors: Feature has LANDMARK_ID but there is no "
+ "landmark of "
+ "type LandmarkExternal with this id.");
+ }
- if (fac)
- fac_list.push_back(fac);
+ // modify landmark (add missing state blocks if needed)
+ modifyLandmark(lmk, feature);
+
+ // Emplace factors for all tracked features in keyframes
+ for (auto feat_pair : track_matrix_.trackAtKeyframes(track_id))
+ if (feat_pair.second->getFactorList().empty())
+ {
+ auto fac = emplaceFactor(feature, lmk);
+ if (fac) fac_list.push_back(fac);
+ }
}
- WOLF_INFO("ProcessorLandmarkExternal::establishFactors: emplaced ", fac_list.size(), " factors!");
+ WOLF_DEBUG("ProcessorLandmarkExternal::establishFactors: emplaced ", fac_list.size(), " factors!");
}
FactorBasePtr ProcessorLandmarkExternal::emplaceFactor(FeatureBasePtr _feature, LandmarkBasePtr _landmark)
{
assert(_feature);
assert(_landmark);
-
+
// 2D - Relative Position
if (getProblem()->getDim() == 2 and (_feature->getMeasurement().size() == 2 or not params_tfle_->use_orientation))
{
- return FactorBase::emplace<FactorRelativePosition2dWithExtrinsics>(_feature,
- _feature,
- _landmark,
- shared_from_this(),
- params_tfle_->apply_loss_function);
+ return FactorBase::emplace<FactorRelativePosition2dWithExtrinsics>(
+ _feature, _feature, _landmark, shared_from_this(), params_tfle_->apply_loss_function);
}
// 2D - Relative Pose
else if (getProblem()->getDim() == 2 and _feature->getMeasurement().size() == 3 and params_tfle_->use_orientation)
{
- return FactorBase::emplace<FactorRelativePose2dWithExtrinsics>(_feature,
- _feature,
- _landmark,
- shared_from_this(),
- params_tfle_->apply_loss_function);
+ return FactorBase::emplace<FactorRelativePose2dWithExtrinsics>(
+ _feature, _feature, _landmark, shared_from_this(), params_tfle_->apply_loss_function);
}
// 3D - Relative Position
- else if (getProblem()->getDim() == 3 and (_feature->getMeasurement().size() == 3 or not params_tfle_->use_orientation))
+ else if (getProblem()->getDim() == 3 and
+ (_feature->getMeasurement().size() == 3 or not params_tfle_->use_orientation))
{
- return FactorBase::emplace<FactorRelativePosition3dWithExtrinsics>(_feature,
- _feature,
- _landmark,
- shared_from_this(),
- params_tfle_->apply_loss_function);
+ return FactorBase::emplace<FactorRelativePosition3dWithExtrinsics>(
+ _feature, _feature, _landmark, shared_from_this(), params_tfle_->apply_loss_function);
}
// 3D - Relative Pose
else if (getProblem()->getDim() == 3 and _feature->getMeasurement().size() == 7 and params_tfle_->use_orientation)
{
- return FactorBase::emplace<FactorRelativePose3dWithExtrinsics>(_feature,
- _feature,
- _landmark,
- shared_from_this(),
- params_tfle_->apply_loss_function);
+ return FactorBase::emplace<FactorRelativePose3dWithExtrinsics>(
+ _feature, _feature, _landmark, shared_from_this(), params_tfle_->apply_loss_function);
}
- else
+ else
throw std::runtime_error("ProcessorLandmarkExternal::emplaceFactor unknown case");
- // not reachable
- return nullptr;
-
+ return nullptr; // not going to happen
}
-
-LandmarkBasePtr ProcessorLandmarkExternal::emplaceLandmark(FeatureBasePtr _feature)
+
+LandmarkExternalPtr ProcessorLandmarkExternal::emplaceLandmark(FeatureLandmarkExternalPtr _feature)
{
assert(_feature);
assert(_feature->getCapture());
@@ -342,85 +630,89 @@ LandmarkBasePtr ProcessorLandmarkExternal::emplaceLandmark(FeatureBasePtr _featu
assert(getProblem());
assert(getProblem()->getMap());
- if (getProblem()->getMap()->getLandmark(_feature->landmarkId()) != nullptr)
- throw std::runtime_error("ProcessorLandmarkExternal::emplaceLandmark: attempting to create an existing landmark");
+ if (_feature->landmarkId() != 0)
+ throw std::runtime_error(
+ "ProcessorLandmarkExternal::emplaceLandmark: attempting to emplace a landmark for a feature that has "
+ "been "
+ "already matched with an existing one");
- LandmarkBasePtr lmk;
+ LandmarkExternalPtr lmk;
// 2D - Relative Position
if (getProblem()->getDim() == 2 and (_feature->getMeasurement().size() == 2 or not params_tfle_->use_orientation))
{
Vector2d frm_p = _feature->getCapture()->getFrame()->getP()->getState();
Vector2d sen_p = _feature->getCapture()->getSensorP()->getState();
- double frm_o = _feature->getCapture()->getFrame()->getO()->getState()(0);
- double sen_o = _feature->getCapture()->getSensorO()->getState()(0);
-
+ double frm_o = _feature->getCapture()->getFrame()->getO()->getState()(0);
+ double sen_o = _feature->getCapture()->getSensorO()->getState()(0);
+
Vector2d lmk_p = frm_p + Rotation2Dd(frm_o) * (sen_p + Rotation2Dd(sen_o) * _feature->getMeasurement());
- lmk = LandmarkBase::emplace<LandmarkBase>(getProblem()->getMap(),
- "LandmarkRelativePosition2d",
- std::make_shared<StatePoint2d>(lmk_p),
- nullptr);
- lmk->setId(_feature->landmarkId());
+ lmk = LandmarkBase::emplace<LandmarkExternal>(getProblem()->getMap(),
+ _feature->getExternalId(),
+ _feature->getExternalType(),
+ std::make_shared<StatePoint2d>(lmk_p),
+ nullptr);
}
// 2D - Relative Pose
else if (getProblem()->getDim() == 2 and _feature->getMeasurement().size() == 3 and params_tfle_->use_orientation)
{
Vector2d frm_p = _feature->getCapture()->getFrame()->getP()->getState();
Vector2d sen_p = _feature->getCapture()->getSensorP()->getState();
- double frm_o = _feature->getCapture()->getFrame()->getO()->getState()(0);
- double sen_o = _feature->getCapture()->getSensorO()->getState()(0);
-
- Vector2d lmk_p = frm_p + Rotation2Dd(frm_o) * (sen_p + Rotation2Dd(sen_o) * _feature->getMeasurement().head<2>());
- double lmk_o = frm_o + sen_o + _feature->getMeasurement()(2);
-
- lmk = LandmarkBase::emplace<LandmarkBase>(getProblem()->getMap(),
- "LandmarkRelativePose2d",
- std::make_shared<StatePoint2d>(lmk_p),
- std::make_shared<StateAngle>(lmk_o));
- lmk->setId(_feature->landmarkId());
+ double frm_o = _feature->getCapture()->getFrame()->getO()->getState()(0);
+ double sen_o = _feature->getCapture()->getSensorO()->getState()(0);
+
+ Vector2d lmk_p =
+ frm_p + Rotation2Dd(frm_o) * (sen_p + Rotation2Dd(sen_o) * _feature->getMeasurement().head<2>());
+ double lmk_o = frm_o + sen_o + _feature->getMeasurement()(2);
+
+ lmk = LandmarkBase::emplace<LandmarkExternal>(getProblem()->getMap(),
+ _feature->getExternalId(),
+ _feature->getExternalType(),
+ std::make_shared<StatePoint2d>(lmk_p),
+ std::make_shared<StateAngle>(lmk_o));
}
// 3D - Relative Position
- else if (getProblem()->getDim() == 3 and (_feature->getMeasurement().size() == 3 or not params_tfle_->use_orientation))
+ else if (getProblem()->getDim() == 3 and
+ (_feature->getMeasurement().size() == 3 or not params_tfle_->use_orientation))
{
- Vector3d frm_p = _feature->getCapture()->getFrame()->getP()->getState();
- Vector3d sen_p = _feature->getCapture()->getSensorP()->getState();
+ Vector3d frm_p = _feature->getCapture()->getFrame()->getP()->getState();
+ Vector3d sen_p = _feature->getCapture()->getSensorP()->getState();
Quaterniond frm_o = Quaterniond(Vector4d(_feature->getCapture()->getFrame()->getO()->getState()));
Quaterniond sen_o = Quaterniond(Vector4d(_feature->getCapture()->getSensorO()->getState()));
Vector3d lmk_p = frm_p + frm_o * (sen_p + sen_o * _feature->getMeasurement());
- lmk = LandmarkBase::emplace<LandmarkBase>(getProblem()->getMap(),
- "LandmarkRelativePosition3d",
- std::make_shared<StatePoint3d>(lmk_p),
- nullptr);
- lmk->setId(_feature->landmarkId());
+ lmk = LandmarkBase::emplace<LandmarkExternal>(getProblem()->getMap(),
+ _feature->getExternalId(),
+ _feature->getExternalType(),
+ std::make_shared<StatePoint3d>(lmk_p),
+ nullptr);
}
// 3D - Relative Pose
else if (getProblem()->getDim() == 3 and _feature->getMeasurement().size() == 7 and params_tfle_->use_orientation)
{
- Vector3d frm_p = _feature->getCapture()->getFrame()->getP()->getState();
- Vector3d sen_p = _feature->getCapture()->getSensorP()->getState();
+ Vector3d frm_p = _feature->getCapture()->getFrame()->getP()->getState();
+ Vector3d sen_p = _feature->getCapture()->getSensorP()->getState();
Quaterniond frm_o = Quaterniond(Vector4d(_feature->getCapture()->getFrame()->getO()->getState()));
Quaterniond sen_o = Quaterniond(Vector4d(_feature->getCapture()->getSensorO()->getState()));
- Vector3d lmk_p = frm_p + frm_o * (sen_p + sen_o * _feature->getMeasurement().head<3>());
+ Vector3d lmk_p = frm_p + frm_o * (sen_p + sen_o * _feature->getMeasurement().head<3>());
Quaterniond lmk_o = frm_o * sen_o * Quaterniond(_feature->getMeasurement().tail<4>());
- lmk = LandmarkBase::emplace<LandmarkBase>(getProblem()->getMap(),
- "LandmarkRelativePose3d",
- std::make_shared<StatePoint3d>(lmk_p),
- std::make_shared<StateQuaternion>(lmk_o));
- lmk->setId(_feature->landmarkId());
+ lmk = LandmarkBase::emplace<LandmarkExternal>(getProblem()->getMap(),
+ _feature->getExternalId(),
+ _feature->getExternalType(),
+ std::make_shared<StatePoint3d>(lmk_p),
+ std::make_shared<StateQuaternion>(lmk_o));
}
- else
+ else
throw std::runtime_error("ProcessorLandmarkExternal::emplaceLandmark unknown case");
return lmk;
}
-void ProcessorLandmarkExternal::modifyLandmark(LandmarkBasePtr _landmark,
- FeatureBasePtr _feature)
+void ProcessorLandmarkExternal::modifyLandmark(LandmarkExternalPtr _landmark, FeatureLandmarkExternalPtr _feature)
{
assert(_feature);
assert(_feature->getCapture());
@@ -428,9 +720,8 @@ void ProcessorLandmarkExternal::modifyLandmark(LandmarkBasePtr _landmark,
assert(_feature->getCapture()->getSensorP());
assert(_feature->getCapture()->getSensorO());
assert(getProblem());
-
- if (not _landmark)
- throw std::runtime_error("ProcessorLandmarkExternal::modifyLandmark: null landmark");
+
+ if (not _landmark) throw std::runtime_error("ProcessorLandmarkExternal::modifyLandmark: null landmark");
// 2D - Relative Position
if (getProblem()->getDim() == 2 and (_feature->getMeasurement().size() == 2 or not params_tfle_->use_orientation))
@@ -453,7 +744,8 @@ void ProcessorLandmarkExternal::modifyLandmark(LandmarkBasePtr _landmark,
}
}
// 3D - Relative Position
- else if (getProblem()->getDim() == 3 and (_feature->getMeasurement().size() == 3 or not params_tfle_->use_orientation))
+ else if (getProblem()->getDim() == 3 and
+ (_feature->getMeasurement().size() == 3 or not params_tfle_->use_orientation))
{
// nothing to do (we assume P already in landmark)
return;
@@ -472,9 +764,9 @@ void ProcessorLandmarkExternal::modifyLandmark(LandmarkBasePtr _landmark,
_landmark->addStateBlock('O', std::make_shared<StateQuaternion>(lmk_o), getProblem());
}
}
- else
+ else
throw std::runtime_error("ProcessorLandmarkExternal::modifyLandmark unknown case");
-}
+}
void ProcessorLandmarkExternal::advanceDerived()
{
@@ -487,11 +779,12 @@ void ProcessorLandmarkExternal::resetDerived()
known_features_last_ = std::move(known_features_incoming_);
}
-} // namespace wolf
+} // namespace wolf
// Register in the FactoryProcessor
#include "core/processor/factory_processor.h"
-namespace wolf {
+namespace wolf
+{
WOLF_REGISTER_PROCESSOR(ProcessorLandmarkExternal)
WOLF_REGISTER_PROCESSOR_AUTO(ProcessorLandmarkExternal)
-} // namespace wolf
+} // namespace wolf
diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp
index 83844d6baf0c1d34a3f1169453383442e3e2ecde..8aa29f9a38f489c7fc82ddd4e922e58e28042529 100644
--- a/src/processor/processor_motion.cpp
+++ b/src/processor/processor_motion.cpp
@@ -84,13 +84,13 @@ ProcessorMotion::~ProcessorMotion()
// std::cout << "destructed -p-Mot" << id() << std::endl;
}
-
void ProcessorMotion::mergeCaptures(CaptureMotionPtr cap_prev,
CaptureMotionPtr cap_target)
{
- assert(cap_prev != nullptr);
- assert(cap_target != nullptr);
- assert(cap_prev == cap_target->getOriginCapture() && "merging not consecutive capture motions");
+ assertsCaptureMotion(cap_prev, "ProcessorMotion::mergeCaptures: cap_prev");
+ assertsCaptureMotion(cap_target, "ProcessorMotion::mergeCaptures: cap_target");
+
+ assert(cap_prev == cap_target->getOriginCapture() && "ProcessorMotion::mergeCaptures: merging not consecutive capture motions");
// add prev buffer (discarding the first zero motion)
cap_target->getBuffer().pop_front();
@@ -144,6 +144,33 @@ void ProcessorMotion::splitBuffer(const CaptureMotionPtr& _capture_source,
* origin target source
*
*/
+ // CHECKS
+ assertsCaptureMotion(_capture_source, "ProcessorMotion::splitBuffer _capture_source (before split)");
+ if (checkTimeTolerance(_capture_source, _ts_split))
+ {
+ WOLF_ERROR("ProcessorMotion::splitBuffer: _capture_source.ts == ts_split within tolerance, shouldn't split!");
+ }
+ if (_capture_source->getOriginCapture() and checkTimeTolerance(_capture_source->getOriginCapture(), _ts_split))
+ {
+ WOLF_ERROR("ProcessorMotion::splitBuffer: _capture_source->getOriginCapture().ts == ts_split within tolerance, shouldn't split!");
+ }
+ if (_capture_source->getBuffer().front().ts_ >= _ts_split)
+ {
+ WOLF_ERROR("ProcessorMotion::splitBuffer: Query time stamp is smaller or equal than the buffer's first tick");
+ getProblem()->print();
+ throw std::runtime_error("");
+ }
+ if (_capture_source->getBuffer().back().ts_ <= _ts_split)
+ {
+ WOLF_ERROR("ProcessorMotion::splitBuffer: Query time stamp is greater or equal than the buffer's last tick");
+ getProblem()->print();
+ throw std::runtime_error("");
+ }
+
+ // guarantee that capture_target is empty
+ WOLF_WARN_COND(not _capture_target->getBuffer().empty(), "ProcessorMotion::splitBuffer: capture_target buffer not empty! size: ", _capture_target->getBuffer().size());
+ if (not _capture_target->getBuffer().empty())
+ _capture_target->getBuffer().clear();
// split the buffer
// and give the part of the buffer before the new keyframe to the capture for the KF callback
@@ -167,6 +194,9 @@ void ProcessorMotion::splitBuffer(const CaptureMotionPtr& _capture_source,
// re-integrate source capture's buffer -- note: the result of re-integration is stored in the same buffer!
reintegrateBuffer(_capture_source);
+
+ assertsCaptureMotion(_capture_source, "ProcessorMotion::splitBuffer _capture_source (after split)");
+ assertsCaptureMotion(_capture_target, "ProcessorMotion::splitBuffer _capture_target (after split)");
}
void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
@@ -274,7 +304,7 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
// find the capture whose buffer is affected by the new keyframe
auto capture_existing = findCaptureContainingTimeStamp(timestamp_from_callback);
- if (!capture_existing)
+ if (not capture_existing)
{
WOLF_WARN(getName(), ": Cannot join KF. The received KF (TS = ", timestamp_from_callback, ") is older than the first motion capture.");
break;
@@ -293,6 +323,11 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
// Find the capture acting as the buffer's origin
auto capture_origin = capture_existing->getOriginCapture();
+ if (not capture_origin)
+ {
+ WOLF_WARN(getName(), ": (should be unreachable) Cannot join KF. Capture containing (TS = ", timestamp_from_callback, ") does not have origin capture.");
+ break;
+ }
// Get calibration params for preintegration from origin, since it has chances to have optimized values
VectorXd calib_origin = getCalibration(capture_origin);
@@ -731,6 +766,9 @@ void ProcessorMotion::setOrigin(FrameBasePtr _origin_frame)
getCalibration(),
nullptr);
+ // add zero motion (just for consistency)
+ origin_ptr_->getBuffer().push_back(motionZero(_origin_frame->getTimeStamp()));
+
// ---------- LAST ----------
// Make non-key-frame for last Capture
last_frame_ptr_ = std::make_shared<FrameBase>(origin_ts,
@@ -761,6 +799,9 @@ void ProcessorMotion::integrateOneStep()
// Set dt
dt_ = updateDt();
+ WOLF_WARN_COND(dt_ < 0, "ProcessorMotion::integrateOneStep: dt < 0, skipping integration");
+ if (dt_ < 0)
+ return;
assert(dt_ >= 0 && "Time interval _dt is negative!");
// get vector of parameters to calibrate
@@ -816,6 +857,9 @@ void ProcessorMotion::integrateOneStep()
void ProcessorMotion::reintegrateBuffer(CaptureMotionPtr _capture_ptr) const
{
+ // check capture (not null, not empty buffer, first buffer's motion zero)
+ assertsCaptureMotion(_capture_ptr, "ProcessorMotion::reintegrateBuffer");
+
VectorXd calib = _capture_ptr->getCalibrationPreint();
// some temporaries for integration
@@ -823,11 +867,6 @@ void ProcessorMotion::reintegrateBuffer(CaptureMotionPtr _capture_ptr) const
delta_integrated_cov_.setZero();
jacobian_calib_ .setZero();
- // check that first motion in buffer is zero!
- assert(_capture_ptr->getBuffer().front().delta_integr_ == delta_integrated_ && "Buffer's first motion is not zero!");
- assert(_capture_ptr->getBuffer().front().delta_integr_cov_ == delta_integrated_cov_ && "Buffer's first motion is not zero!");
- assert(_capture_ptr->getBuffer().front().jacobian_calib_ == jacobian_calib_ && "Buffer's first motion is not zero!");
-
// Iterate all the buffer
auto motion_it = _capture_ptr->getBuffer().begin();
auto prev_motion_it = motion_it;
@@ -928,9 +967,7 @@ CaptureMotionConstPtr ProcessorMotion::findCaptureContainingTimeStamp(const Time
CaptureMotionPtr ProcessorMotion::findCaptureContainingTimeStamp(const TimeStamp& _ts)
{
assert(_ts.ok());
- // assert(last_ptr_ != nullptr);
- //Need to uncomment this line so that wolf_ros_apriltag doesn't crash
if(last_ptr_ == nullptr) return nullptr;
// First check if last_ptr is the one we are looking for
@@ -1044,6 +1081,15 @@ FrameBasePtr ProcessorMotion::computeProcessingStep()
return nullptr;
}
+void ProcessorMotion::assertsCaptureMotion(CaptureMotionPtr _capture, std::string error_prefix) const
+{
+ assert(_capture != nullptr && (error_prefix + ": null capture").c_str());
+ assert(not _capture->getBuffer().empty() && (error_prefix + ": empty buffer").c_str());
+ assert((_capture->getBuffer().front().delta_integr_ - deltaZero()).isZero(Constants::EPS_SMALL) && (error_prefix + ": buffer's first motion is not zero!").c_str());
+ assert(_capture->getBuffer().front().delta_integr_cov_.isZero(Constants::EPS_SMALL) && (error_prefix + ": Buffer's first motion covariance is not zero!").c_str());
+ assert(_capture->getBuffer().front().jacobian_calib_.isZero(Constants::EPS_SMALL) && (error_prefix + ": Buffer's first motion jacobian is not zero!").c_str());
+}
+
TimeStamp ProcessorMotion::getTimeStamp ( ) const
{
if (not origin_ptr_ or
diff --git a/src/processor/processor_tracker.cpp b/src/processor/processor_tracker.cpp
index dc2043c7624b67f853f6cb5fe6097df93b934479..32eb7fdd653dd2d04651eda77d5e3c3d9c97ea7e 100644
--- a/src/processor/processor_tracker.cpp
+++ b/src/processor/processor_tracker.cpp
@@ -82,10 +82,25 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
WOLF_DEBUG( "PT ", getName(), " FIRST_TIME_WITH_KEYFRAME: KF" , keyframe_from_callback->id() , " callback unpacked with ts= " , keyframe_from_callback->getTimeStamp() );
- // Append incoming to KF
- incoming_ptr_->link(keyframe_from_callback);
+ // check if the callback keyframe has a capture of this sensor
+ auto capture_from_callback = keyframe_from_callback->getCaptureOf(this->getSensor());
+
+ if (incoming_ptr_ == capture_from_callback)
+ {
+ // If captures match, then frames must match too
+ assert(incoming_ptr_->getFrame() != nullptr
+ and incoming_ptr_->getFrame() == keyframe_from_callback
+ and "The keyframe has a Capture from this sensor, but this capture is not incoming!");
+ WOLF_DEBUG("PT ", getName(), " Incoming capture has been processed previously by another processor!")
+ }
+ else
+ {
+ WOLF_DEBUG("PT ", getName(), " Incoming capture had not been processed by any other processor!")
+
+ // Join KF
+ incoming_ptr_->link(keyframe_from_callback);
+ }
- // Process info
// TrackerFeature: We only process new features in Last, here last = nullptr, so we do not have anything to do.
// TrackerLandmark: If we have been given a map, all landmarks in the map are known. Process them.
processKnown();
@@ -95,6 +110,7 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
// Update pointers
origin_ptr_ = incoming_ptr_;
last_ptr_ = incoming_ptr_;
+ last_frame_ptr_ = keyframe_from_callback;
incoming_ptr_ = nullptr;
break;
@@ -103,20 +119,24 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
{
WOLF_DEBUG( "PT ", getName(), " FIRST_TIME_WITHOUT_KEYFRAME" );
+ // Check if incoming has already a Frame
+ auto frame_incoming = incoming_ptr_->getFrame();
+ assert(frame_incoming == nullptr and " Incoming capture has been processed and linked by another processor, but no keyframe callback was received!");
+
+ WOLF_DEBUG("PT ", getName(), " Incoming capture has not been processed by another processor!")
+
// make a new KF at incoming
FrameBasePtr keyframe = FrameBase::emplace<FrameBase>(getProblem()->getTrajectory(),
- incoming_ptr_->getTimeStamp(),
- getProblem()->getFrameStructure(),
- getProblem()->getState());
-
- // link incoming to the new KF
+ incoming_ptr_->getTimeStamp(),
+ getProblem()->getFrameStructure(),
+ getProblem()->getState(incoming_ptr_->getTimeStamp()));
+ // Append incoming to KF
incoming_ptr_->link(keyframe);
// Process info
// TrackerFeature: We only process new features in Last, here last = nullptr, so we do not have anything to do.
// TrackerLandmark: If we have been given a map, all landmarks in the map are known. Process them.
- if (not getProblem()->getMap()->getLandmarkList().empty())
- processKnown();
+ processKnown();
// Issue KF callback with new KF
getProblem()->keyFrameCallback(keyframe, shared_from_this());
@@ -126,6 +146,7 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
// Update pointers
origin_ptr_ = incoming_ptr_;
last_ptr_ = incoming_ptr_;
+ last_frame_ptr_ = keyframe;
incoming_ptr_ = nullptr;
break;
@@ -154,13 +175,10 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
// Make a NON KEY Frame to hold incoming capture
FrameBasePtr frame = std::make_shared<FrameBase>(incoming_ptr_->getTimeStamp(),
getProblem()->getFrameStructure(),
- getProblem()->getState());
- incoming_ptr_->link(frame);
+ getProblem()->getState(incoming_ptr_->getTimeStamp()));
- // Process info
- // If we have known landmarks or features. Process them.
- if (not getProblem()->getMap()->getLandmarkList().empty() or not known_features_last_.empty())
- processKnown();
+ // Process known information
+ processKnown();
// Both Trackers: We have a last_ Capture with not enough features, so populate it.
processNew(params_tracker_->max_new_features);
@@ -188,16 +206,29 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
processKnown();
- // Capture last_ is added to the new keyframe
- FrameBasePtr last_old_frame = last_ptr_->getFrame();
- last_ptr_->move(keyframe_from_callback);
- last_old_frame->remove();
+ // chack if the received KF has a capture of this sensor, and if it matches with last_ptr
+ if (last_ptr_ == keyframe_from_callback->getCaptureOf(this->getSensor()))
+ {
+ WOLF_DEBUG("PT ", getName(), " Last capture has been processed previously by another processor!")
+
+ // If captures match, then frames must match too
+ assert( last_ptr_->getFrame() == keyframe_from_callback
+ and "The keyframe has a Capture from this sensor, but this capture is not last!");
+
+ // No need to join KF since it is the same capture, and it was already joined by the other processor
+ }
+ else
+ {
+ WOLF_DEBUG("PT ", getName(), " Last capture had not been processed previously!")
+
+ // join KF
+ last_ptr_->link(keyframe_from_callback);
+ }
// Create new NON KEY frame for incoming
FrameBasePtr frame = std::make_shared<FrameBase>(incoming_ptr_->getTimeStamp(),
getProblem()->getFrameStructure(),
getProblem()->getState());
- incoming_ptr_->link(frame);
// Detect new Features, initialize Landmarks, ...
processNew(params_tracker_->max_new_features);
@@ -228,21 +259,21 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
// We create a KF
// set KF on last
- last_ptr_->getFrame()->setState(getProblem()->getState(last_ptr_->getTimeStamp()));
- last_ptr_->getFrame()->link(getProblem());
+ last_frame_ptr_->setState(getProblem()->getState(last_ptr_->getTimeStamp()));
+ last_ptr_->link(last_frame_ptr_);
+ last_frame_ptr_->link(getProblem());
// Establish factors
establishFactors();
- // Call the new keyframe callback in order to let the other processors to establish their factors
- getProblem()->keyFrameCallback(last_ptr_->getFrame(), shared_from_this());
+ // Call the new keyframe callback in order to let the other processors to join
+ getProblem()->keyFrameCallback(last_frame_ptr_, shared_from_this());
// make NON KEY frame; append incoming to new frame
FrameBasePtr frame = std::make_shared<FrameBase>(incoming_ptr_->getTimeStamp(),
getProblem()->getFrameStructure(),
getProblem()->getState(incoming_ptr_->getTimeStamp()));
- incoming_ptr_ ->link(frame);
// Reset this
resetDerived();
@@ -261,8 +292,6 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
FrameBasePtr frame = std::make_shared<FrameBase>(incoming_ptr_->getTimeStamp(),
getProblem()->getFrameStructure(),
getProblem()->getState(incoming_ptr_->getTimeStamp()));
- incoming_ptr_->link(frame);
- last_ptr_->unlink(); // unlink last (destroying the frame) instead of frame destruction that would implicitly destroy last
// Advance this
advanceDerived();
@@ -315,7 +344,7 @@ void ProcessorTracker::computeProcessingStep()
if (buffer_frame_.select(last_ptr_->getTimeStamp(), params_tracker_->time_tolerance))
{
- if (last_ptr_->getFrame()->getProblem())
+ if (last_frame_ptr_->getProblem())
{
WOLF_WARN("||*||");
WOLF_INFO(" ... It seems you missed something!");
@@ -341,7 +370,7 @@ void ProcessorTracker::printHeader(int _depth, bool _constr_by, bool _metric, bo
<< getOrigin()->getFrame()->id() << std::endl;
if (getLast())
_stream << _tabs << " " << "l: Cap" << getLast()->id() << " - " << " Frm"
- << getLast()->getFrame()->id() << std::endl;
+ << getLastFrame()->id() << std::endl;
if (getIncoming())
_stream << _tabs << " " << "i: Cap" << getIncoming()->id() << std::endl;
}
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index 299594805cb46a551145613baf30481f4e7aed22..1709df19170f9110e8e2d180c7d27597a6becc64 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -225,6 +225,10 @@ wolf_add_gtest(gtest_processor_and_factor_pose_3d_with_extrinsics gtest_processo
wolf_add_gtest(gtest_processor_tracker_feature_dummy gtest_processor_tracker_feature_dummy.cpp)
target_link_libraries(gtest_processor_tracker_feature_dummy PUBLIC dummy)
+# ProcessorTrackerFeatureDummy 2 processors in one sensor test
+wolf_add_gtest(gtest_processor_tracker_two_processors_one_sensor gtest_processor_tracker_two_processors_one_sensor.cpp)
+target_link_libraries(gtest_processor_tracker_two_processors_one_sensor PUBLIC dummy)
+
# ProcessorTrackerLandmarkDummy class test
wolf_add_gtest(gtest_processor_tracker_landmark_dummy gtest_processor_tracker_landmark_dummy.cpp)
target_link_libraries(gtest_processor_tracker_landmark_dummy PUBLIC dummy)
diff --git a/test/dummy/factor_dummy_zero_12.h b/test/dummy/factor_dummy_zero_15.h
similarity index 79%
rename from test/dummy/factor_dummy_zero_12.h
rename to test/dummy/factor_dummy_zero_15.h
index 168692c45c6fcc86ca5a9285eb7190c9694b8e62..3599664e14c3df5b683f60834ec463cbff17cb2c 100644
--- a/test/dummy/factor_dummy_zero_12.h
+++ b/test/dummy/factor_dummy_zero_15.h
@@ -19,24 +19,21 @@
// along with this program. If not, see <http://www.gnu.org/licenses/>.
//
//--------LICENSE_END--------
-#ifndef FACTOR_DUMMY_ZERO_12_H_
-#define FACTOR_DUMMY_ZERO_12_H_
+#pragma once
//Wolf includes
#include "core/factor/factor_autodiff.h"
-//#include "ceres/jet.h"
-
namespace wolf {
//class
template <unsigned int ID>
-class FactorDummyZero12 : public FactorAutodiff<FactorDummyZero12<ID>, ID, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12>
+class FactorDummyZero15 : public FactorAutodiff<FactorDummyZero15<ID>, ID, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15>
{
- using Base = FactorAutodiff<FactorDummyZero12<ID>, ID, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12>;
+ using Base = FactorAutodiff<FactorDummyZero15<ID>, ID, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15>;
static const unsigned int id = ID;
public:
- FactorDummyZero12(const FeatureBasePtr& _ftr_ptr,
+ FactorDummyZero15(const FeatureBasePtr& _ftr_ptr,
const StateBlockPtr& _sb1_ptr,
const StateBlockPtr& _sb2_ptr,
const StateBlockPtr& _sb3_ptr,
@@ -48,7 +45,10 @@ class FactorDummyZero12 : public FactorAutodiff<FactorDummyZero12<ID>, ID, 1, 2,
const StateBlockPtr& _sb9_ptr,
const StateBlockPtr& _sb10_ptr,
const StateBlockPtr& _sb11_ptr,
- const StateBlockPtr& _sb12_ptr) :
+ const StateBlockPtr& _sb12_ptr,
+ const StateBlockPtr& _sb13_ptr,
+ const StateBlockPtr& _sb14_ptr,
+ const StateBlockPtr& _sb15_ptr) :
Base("FactorDummy12",
TOP_OTHER,
_ftr_ptr,
@@ -67,12 +67,15 @@ class FactorDummyZero12 : public FactorAutodiff<FactorDummyZero12<ID>, ID, 1, 2,
_sb9_ptr,
_sb10_ptr,
_sb11_ptr,
- _sb12_ptr)
+ _sb12_ptr,
+ _sb13_ptr,
+ _sb14_ptr,
+ _sb15_ptr)
{
- assert(id > 0 && id <= 12);
+ assert(id > 0 && id <= 15);
}
- ~FactorDummyZero12() override = default;
+ ~FactorDummyZero15() override = default;
template<typename T>
bool operator ()(const T* const _st1,
@@ -87,6 +90,9 @@ class FactorDummyZero12 : public FactorAutodiff<FactorDummyZero12<ID>, ID, 1, 2,
const T* const _st10,
const T* const _st11,
const T* const _st12,
+ const T* const _st13,
+ const T* const _st14,
+ const T* const _st15,
T* _residuals) const
{
Eigen::Map<Eigen::Matrix<T,ID,1>> residuals(_residuals);
@@ -164,6 +170,24 @@ class FactorDummyZero12 : public FactorAutodiff<FactorDummyZero12<ID>, ID, 1, 2,
residuals = st12;
break;
}
+ case 13:
+ {
+ Eigen::Map<const Eigen::Matrix<T,ID,1>> st13(_st13);
+ residuals = st13;
+ break;
+ }
+ case 14:
+ {
+ Eigen::Map<const Eigen::Matrix<T,ID,1>> st14(_st14);
+ residuals = st14;
+ break;
+ }
+ case 15:
+ {
+ Eigen::Map<const Eigen::Matrix<T,ID,1>> st15(_st15);
+ residuals = st15;
+ break;
+ }
default:
throw std::runtime_error("ID not found");
}
@@ -172,5 +196,3 @@ class FactorDummyZero12 : public FactorAutodiff<FactorDummyZero12<ID>, ID, 1, 2,
};
} // namespace wolf
-
-#endif
diff --git a/test/gtest/CMakeLists.txt b/test/gtest/CMakeLists.txt
index 2294c819aec95c1f2c6bed5c0e7b1d6cc857c614..f15e97fc69307cd2b3c610f08b4f18c1373b5d85 100644
--- a/test/gtest/CMakeLists.txt
+++ b/test/gtest/CMakeLists.txt
@@ -1,73 +1,15 @@
-if(${CMAKE_VERSION} VERSION_LESS "3.11.0")
- message("CMake version less than 3.11.0")
+include(FetchContent)
- # Enable ExternalProject CMake module
- include(ExternalProject)
+FetchContent_Declare(
+ googletest
+ GIT_REPOSITORY https://github.com/google/googletest.git
+ GIT_TAG main)
- set(GTEST_FORCE_SHARED_CRT ON)
- set(GTEST_DISABLE_PTHREADS ON) # without this in ubuntu 18.04 we get linking errors
-
- # Download GoogleTest
- ExternalProject_Add(googletest
- GIT_REPOSITORY https://github.com/google/googletest.git
- GIT_TAG v1.8.x
- # TIMEOUT 1 # We'll try this
- CMAKE_ARGS -DCMAKE_ARCHIVE_OUTPUT_DIRECTORY_DEBUG:PATH=DebugLibs
- -DCMAKE_ARCHIVE_OUTPUT_DIRECTORY_RELEASE:PATH=ReleaseLibs
- -DCMAKE_CXX_FLAGS=${MSVC_COMPILER_DEFS}
- -Dgtest_force_shared_crt=${GTEST_FORCE_SHARED_CRT}
- -Dgtest_disable_pthreads=${GTEST_DISABLE_PTHREADS}
- -DBUILD_GTEST=ON
- PREFIX "${CMAKE_CURRENT_BINARY_DIR}"
- # Disable install step
- INSTALL_COMMAND ""
- UPDATE_DISCONNECTED 1 # 1: do not update googletest; 0: update googletest via github
- )
-
- # Get GTest source and binary directories from CMake project
-
- # Specify include dir
- ExternalProject_Get_Property(googletest source_dir)
- set(GTEST_INCLUDE_DIRS ${source_dir}/googletest/include PARENT_SCOPE)
-
- # Specify MainTest's link libraries
- ExternalProject_Get_Property(googletest binary_dir)
- set(GTEST_LIBS_DIR ${binary_dir}/googlemock/gtest PARENT_SCOPE)
-
- # Create a libgtest target to be used as a dependency by test programs
- add_library(libgtest IMPORTED STATIC GLOBAL)
- add_dependencies(libgtest googletest)
-
- # Set libgtest properties
- set_target_properties(libgtest PROPERTIES
- "IMPORTED_LOCATION" "${binary_dir}/googlemock/gtest/libgtest.a"
- "IMPORTED_LINK_INTERFACE_LIBRARIES" "${CMAKE_THREAD_LIBS_INIT}"
- )
-
-else()
-
- message("CMake version equal or greater than 3.11.0")
-
- include(FetchContent)
-
- FetchContent_Declare(
- googletest
- GIT_REPOSITORY https://github.com/google/googletest.git
- GIT_TAG main)
-
- SET(INSTALL_GTEST OFF) # Disable installation of googletest
- FetchContent_MakeAvailable(googletest)
-
-endif()
-
+SET(INSTALL_GTEST OFF) # Disable installation of googletest
+FetchContent_MakeAvailable(googletest)
+
function(wolf_add_gtest target)
add_executable(${target} ${ARGN})
- if(${CMAKE_VERSION} VERSION_LESS "3.11.0")
- add_dependencies(${target} libgtest)
- target_link_libraries(${target} PUBLIC libgtest ${PLUGIN_NAME})
- target_include_directories(${target} PUBLIC ${GTEST_INCLUDE_DIRS})
- else()
- target_link_libraries(${target} PUBLIC gtest_main ${PLUGIN_NAME})
- endif()
+ target_link_libraries(${target} PUBLIC gtest_main ${PLUGIN_NAME})
add_test(NAME ${target} COMMAND ${target})
endfunction()
diff --git a/test/gtest_factor_autodiff.cpp b/test/gtest_factor_autodiff.cpp
index e677d7b384d80d1ff55d56fca4ccadb1701cf8a9..b724256e49c1894901bd9ad7efd86d932c7e5168 100644
--- a/test/gtest_factor_autodiff.cpp
+++ b/test/gtest_factor_autodiff.cpp
@@ -28,7 +28,8 @@
#include "dummy/factor_odom_2d_autodiff.h"
#include "dummy/factor_dummy_zero_1.h"
-#include "dummy/factor_dummy_zero_12.h"
+// #include "dummy/factor_dummy_zero_12.h"
+#include "dummy/factor_dummy_zero_15.h"
#include "core/state_block/state_block_derived.h"
#include "core/factor/factor_relative_pose_2d.h"
@@ -42,10 +43,35 @@
using namespace wolf;
using namespace Eigen;
+void testFactorAutodiff15(FactorBasePtr _fac, StateBlockPtr _sb, const std::vector<const double*>& _states_ptr)
+{
+ unsigned int i = _fac->getSize();
+ std::vector<MatrixXd> J;
+ VectorXd residuals(i);
+
+ // Jacobian
+ _fac->evaluate(_states_ptr, residuals, J);
+ ASSERT_MATRIX_APPROX(residuals, _sb->getState(), wolf::Constants::EPS);
+
+ // std::cout << "i = " << i << std::endl;
+ for (unsigned int j = 0; j < 15; j++)
+ {
+ // std::cout << "j = " << j << std::endl;
+ // std::cout << "Jacobian size: " << J[j].rows() << " " << J[j].cols() << std::endl;
+ if (j == i-1)
+ {
+ ASSERT_MATRIX_APPROX(J[j], MatrixXd::Identity(i,i), wolf::Constants::EPS);
+ }
+ else
+ {
+ ASSERT_MATRIX_APPROX(J[j], MatrixXd::Zero(i,j+1), wolf::Constants::EPS);
+ }
+ }
+}
TEST(FactorAutodiff, AutodiffDummy1)
{
- StateBlockPtr sb = std::make_shared<StateParams1>(Eigen::Vector1d::Random());
+ StateBlockPtr sb = std::make_shared<StateParams1>(Vector1d::Random());
FeatureBasePtr ftr = std::make_shared<FeatureBase>("Feature", Vector2d(1,2), Matrix2d::Identity());
auto fac = std::make_shared<FactorDummyZero1>(ftr, sb);
@@ -53,322 +79,108 @@ TEST(FactorAutodiff, AutodiffDummy1)
// COMPUTE JACOBIANS
std::vector<const double*> states_ptr({sb->getStateData()});
- std::vector<Eigen::MatrixXd> J;
- Eigen::VectorXd residuals(fac->getSize());
+ std::vector<MatrixXd> J;
+ VectorXd residuals(fac->getSize());
fac->evaluate(states_ptr, residuals, J);
- ASSERT_MATRIX_APPROX(J[0], Eigen::Matrix1d::Ones(), wolf::Constants::EPS);
+ ASSERT_MATRIX_APPROX(J[0], Matrix1d::Ones(), wolf::Constants::EPS);
}
-TEST(FactorAutodiff, AutodiffDummy12)
+TEST(FactorAutodiff, AutodiffDummy15)
{
- StateBlockPtr sb1 = std::make_shared<StateParams1>(Eigen::Vector1d::Random());
- StateBlockPtr sb2 = std::make_shared<StateParams2>(Eigen::Vector2d::Random());
- StateBlockPtr sb3 = std::make_shared<StateParams3>(Eigen::Vector3d::Random());
- StateBlockPtr sb4 = std::make_shared<StateParams4>(Eigen::Vector4d::Random());
- StateBlockPtr sb5 = std::make_shared<StateParams5>(Eigen::Vector5d::Random());
- StateBlockPtr sb6 = std::make_shared<StateParams6>(Eigen::Vector6d::Random());
- StateBlockPtr sb7 = std::make_shared<StateParams7>(Eigen::Vector7d::Random());
- StateBlockPtr sb8 = std::make_shared<StateParams8>(Eigen::Vector8d::Random());
- StateBlockPtr sb9 = std::make_shared<StateParams9>(Eigen::Vector9d::Random());
- StateBlockPtr sb10 = std::make_shared<StateParams10>(Eigen::Vector10d::Random());
- StateBlockPtr sb11 = std::make_shared<StateParams<11>>(Eigen::VectorXd::Random(11));
- StateBlockPtr sb12 = std::make_shared<StateParams<12>>(Eigen::VectorXd::Random(12));
-
- std::vector<const double*> states_ptr({sb1->getStateData(),sb2->getStateData(),sb3->getStateData(),sb4->getStateData(),sb5->getStateData(),sb6->getStateData(),sb7->getStateData(),sb8->getStateData(),sb9->getStateData(),sb10->getStateData(),sb11->getStateData(),sb12->getStateData()});
- std::vector<Eigen::MatrixXd> J;
- Eigen::VectorXd residuals;
- unsigned int i;
+ StateBlockPtr sb1 = std::make_shared<StateParams1> (Vector1d::Random());
+ StateBlockPtr sb2 = std::make_shared<StateParams2> (Vector2d::Random());
+ StateBlockPtr sb3 = std::make_shared<StateParams3> (Vector3d::Random());
+ StateBlockPtr sb4 = std::make_shared<StateParams4> (Vector4d::Random());
+ StateBlockPtr sb5 = std::make_shared<StateParams5> (Vector5d::Random());
+ StateBlockPtr sb6 = std::make_shared<StateParams6> (Vector6d::Random());
+ StateBlockPtr sb7 = std::make_shared<StateParams7> (Vector7d::Random());
+ StateBlockPtr sb8 = std::make_shared<StateParams8> (Vector8d::Random());
+ StateBlockPtr sb9 = std::make_shared<StateParams9> (Vector9d::Random());
+ StateBlockPtr sb10 = std::make_shared<StateParams10> (Vector10d::Random());
+ StateBlockPtr sb11 = std::make_shared<StateParams<11>>(VectorXd::Random(11));
+ StateBlockPtr sb12 = std::make_shared<StateParams<12>>(VectorXd::Random(12));
+ StateBlockPtr sb13 = std::make_shared<StateParams<13>>(VectorXd::Random(13));
+ StateBlockPtr sb14 = std::make_shared<StateParams<14>>(VectorXd::Random(14));
+ StateBlockPtr sb15 = std::make_shared<StateParams<15>>(VectorXd::Random(15));
+
+ std::vector<const double*> states_ptr({sb1->getStateData(),
+ sb2->getStateData(),
+ sb3->getStateData(),
+ sb4->getStateData(),
+ sb5->getStateData(),
+ sb6->getStateData(),
+ sb7->getStateData(),
+ sb8->getStateData(),
+ sb9->getStateData(),
+ sb10->getStateData(),
+ sb11->getStateData(),
+ sb12->getStateData(),
+ sb13->getStateData(),
+ sb14->getStateData(),
+ sb15->getStateData()});
FactorBasePtr fac = nullptr;
FeatureBasePtr ftr = std::make_shared<FeatureBase>("Feature", Vector2d(1,2), Matrix2d::Identity());
// 1 ------------------------------------------------------------------------
- fac = std::make_shared<FactorDummyZero12<1>>(ftr,sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12);
- i = fac->getSize();
-
- // Jacobian
- J.clear();
- residuals.resize(i);
- fac->evaluate(states_ptr, residuals, J);
-
- std::cout << "i = " << i << std::endl;
- for (unsigned int j = 0; j < 12; j++)
- {
- std::cout << "j = " << j << std::endl;
- std::cout << "Jacobian size: " << J[j].rows() << " " << J[j].cols() << std::endl;
- if (j == i-1)
- {
- ASSERT_MATRIX_APPROX(J[j], Eigen::MatrixXd::Identity(i,i), wolf::Constants::EPS);
- }
- else
- {
- ASSERT_MATRIX_APPROX(J[j], Eigen::MatrixXd::Zero(i,j+1), wolf::Constants::EPS);
- }
- }
+ fac = std::make_shared<FactorDummyZero15<1>>(ftr,sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12,sb13,sb14,sb15);
+ testFactorAutodiff15(fac, sb1, states_ptr);
// 2 ------------------------------------------------------------------------
- fac = std::make_shared<FactorDummyZero12<2>>(ftr,sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12);
- i = fac->getSize();
-
- // Jacobian
- J.clear();
- residuals.resize(i);
- fac->evaluate(states_ptr, residuals, J);
-
- std::cout << "i = " << i << std::endl;
- for (unsigned int j = 0; j < 12; j++)
- {
- std::cout << "j = " << j << std::endl;
- std::cout << "Jacobian size: " << J[j].rows() << " " << J[j].cols() << std::endl;
- if (j == i-1)
- {
- ASSERT_MATRIX_APPROX(J[j], Eigen::MatrixXd::Identity(i,i), wolf::Constants::EPS);
- }
- else
- {
- ASSERT_MATRIX_APPROX(J[j], Eigen::MatrixXd::Zero(i,j+1), wolf::Constants::EPS);
- }
- }
+ fac = std::make_shared<FactorDummyZero15<2>>(ftr,sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12,sb13,sb14,sb15);
+ testFactorAutodiff15(fac, sb2, states_ptr);
// 3 ------------------------------------------------------------------------
- fac = std::make_shared<FactorDummyZero12<3>>(ftr,sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12);
- i = fac->getSize();
-
- // Jacobian
- J.clear();
- residuals.resize(i);
- fac->evaluate(states_ptr, residuals, J);
-
- std::cout << "i = " << i << std::endl;
- for (unsigned int j = 0; j < 12; j++)
- {
- std::cout << "j = " << j << std::endl;
- std::cout << "Jacobian size: " << J[j].rows() << " " << J[j].cols() << std::endl;
- if (j == i-1)
- {
- ASSERT_MATRIX_APPROX(J[j], Eigen::MatrixXd::Identity(i,i), wolf::Constants::EPS);
- }
- else
- {
- ASSERT_MATRIX_APPROX(J[j], Eigen::MatrixXd::Zero(i,j+1), wolf::Constants::EPS);
- }
- }
+ fac = std::make_shared<FactorDummyZero15<3>>(ftr,sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12,sb13,sb14,sb15);
+ testFactorAutodiff15(fac, sb3, states_ptr);
// 4 ------------------------------------------------------------------------
- fac = std::make_shared<FactorDummyZero12<4>>(ftr,sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12);
- i = fac->getSize();
-
- // Jacobian
- J.clear();
- residuals.resize(i);
- fac->evaluate(states_ptr, residuals, J);
-
- std::cout << "i = " << i << std::endl;
- for (unsigned int j = 0; j < 12; j++)
- {
- std::cout << "j = " << j << std::endl;
- std::cout << "Jacobian size: " << J[j].rows() << " " << J[j].cols() << std::endl;
- if (j == i-1)
- {
- ASSERT_MATRIX_APPROX(J[j], Eigen::MatrixXd::Identity(i,i), wolf::Constants::EPS);
- }
- else
- {
- ASSERT_MATRIX_APPROX(J[j], Eigen::MatrixXd::Zero(i,j+1), wolf::Constants::EPS);
- }
- }
+ fac = std::make_shared<FactorDummyZero15<4>>(ftr,sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12,sb13,sb14,sb15);
+ testFactorAutodiff15(fac, sb4, states_ptr);
// 5 ------------------------------------------------------------------------
- fac = std::make_shared<FactorDummyZero12<5>>(ftr,sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12);
- i = fac->getSize();
-
- // Jacobian
- J.clear();
- residuals.resize(i);
- fac->evaluate(states_ptr, residuals, J);
-
- std::cout << "i = " << i << std::endl;
- for (unsigned int j = 0; j < 12; j++)
- {
- std::cout << "j = " << j << std::endl;
- std::cout << "Jacobian size: " << J[j].rows() << " " << J[j].cols() << std::endl;
- if (j == i-1)
- {
- ASSERT_MATRIX_APPROX(J[j], Eigen::MatrixXd::Identity(i,i), wolf::Constants::EPS);
- }
- else
- {
- ASSERT_MATRIX_APPROX(J[j], Eigen::MatrixXd::Zero(i,j+1), wolf::Constants::EPS);
- }
- }
+ fac = std::make_shared<FactorDummyZero15<5>>(ftr,sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12,sb13,sb14,sb15);
+ testFactorAutodiff15(fac, sb5, states_ptr);
// 6 ------------------------------------------------------------------------
- fac = std::make_shared<FactorDummyZero12<6>>(ftr,sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12);
- i = fac->getSize();
-
- // Jacobian
- J.clear();
- residuals.resize(i);
- fac->evaluate(states_ptr, residuals, J);
-
- std::cout << "i = " << i << std::endl;
- for (unsigned int j = 0; j < 12; j++)
- {
- std::cout << "j = " << j << std::endl;
- std::cout << "Jacobian size: " << J[j].rows() << " " << J[j].cols() << std::endl;
- if (j == i-1)
- {
- ASSERT_MATRIX_APPROX(J[j], Eigen::MatrixXd::Identity(i,i), wolf::Constants::EPS);
- }
- else
- {
- ASSERT_MATRIX_APPROX(J[j], Eigen::MatrixXd::Zero(i,j+1), wolf::Constants::EPS);
- }
- }
+ fac = std::make_shared<FactorDummyZero15<6>>(ftr,sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12,sb13,sb14,sb15);
+ testFactorAutodiff15(fac, sb6, states_ptr);
// 7 ------------------------------------------------------------------------
- fac = std::make_shared<FactorDummyZero12<7>>(ftr,sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12);
- i = fac->getSize();
-
- // Jacobian
- J.clear();
- residuals.resize(i);
- fac->evaluate(states_ptr, residuals, J);
-
- std::cout << "i = " << i << std::endl;
- for (unsigned int j = 0; j < 12; j++)
- {
- std::cout << "j = " << j << std::endl;
- std::cout << "Jacobian size: " << J[j].rows() << " " << J[j].cols() << std::endl;
- if (j == i-1)
- {
- ASSERT_MATRIX_APPROX(J[j], Eigen::MatrixXd::Identity(i,i), wolf::Constants::EPS);
- }
- else
- {
- ASSERT_MATRIX_APPROX(J[j], Eigen::MatrixXd::Zero(i,j+1), wolf::Constants::EPS);
- }
- }
+ fac = std::make_shared<FactorDummyZero15<7>>(ftr,sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12,sb13,sb14,sb15);
+ testFactorAutodiff15(fac, sb7, states_ptr);
// 8 ------------------------------------------------------------------------
- fac = std::make_shared<FactorDummyZero12<8>>(ftr,sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12);
- i = fac->getSize();
-
- // Jacobian
- J.clear();
- residuals.resize(i);
- fac->evaluate(states_ptr, residuals, J);
-
- std::cout << "i = " << i << std::endl;
- for (unsigned int j = 0; j < 12; j++)
- {
- std::cout << "j = " << j << std::endl;
- std::cout << "Jacobian size: " << J[j].rows() << " " << J[j].cols() << std::endl;
- if (j == i-1)
- {
- ASSERT_MATRIX_APPROX(J[j], Eigen::MatrixXd::Identity(i,i), wolf::Constants::EPS);
- }
- else
- {
- ASSERT_MATRIX_APPROX(J[j], Eigen::MatrixXd::Zero(i,j+1), wolf::Constants::EPS);
- }
- }
+ fac = std::make_shared<FactorDummyZero15<8>>(ftr,sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12,sb13,sb14,sb15);
+ testFactorAutodiff15(fac, sb8, states_ptr);
// 9 ------------------------------------------------------------------------
- fac = std::make_shared<FactorDummyZero12<9>>(ftr,sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12);
- i = fac->getSize();
-
- // Jacobian
- J.clear();
- residuals.resize(i);
- fac->evaluate(states_ptr, residuals, J);
-
- std::cout << "i = " << i << std::endl;
- for (unsigned int j = 0; j < 12; j++)
- {
- std::cout << "j = " << j << std::endl;
- std::cout << "Jacobian size: " << J[j].rows() << " " << J[j].cols() << std::endl;
- if (j == i-1)
- {
- ASSERT_MATRIX_APPROX(J[j], Eigen::MatrixXd::Identity(i,i), wolf::Constants::EPS);
- }
- else
- {
- ASSERT_MATRIX_APPROX(J[j], Eigen::MatrixXd::Zero(i,j+1), wolf::Constants::EPS);
- }
- }
+ fac = std::make_shared<FactorDummyZero15<9>>(ftr,sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12,sb13,sb14,sb15);
+ testFactorAutodiff15(fac, sb9, states_ptr);
// 10 ------------------------------------------------------------------------
- fac = std::make_shared<FactorDummyZero12<10>>(ftr,sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12);
- i = fac->getSize();
-
- // Jacobian
- J.clear();
- residuals.resize(i);
- fac->evaluate(states_ptr, residuals, J);
-
- std::cout << "i = " << i << std::endl;
- for (unsigned int j = 0; j < 12; j++)
- {
- std::cout << "j = " << j << std::endl;
- std::cout << "Jacobian size: " << J[j].rows() << " " << J[j].cols() << std::endl;
- if (j == i-1)
- {
- ASSERT_MATRIX_APPROX(J[j], Eigen::MatrixXd::Identity(i,i), wolf::Constants::EPS);
- }
- else
- {
- ASSERT_MATRIX_APPROX(J[j], Eigen::MatrixXd::Zero(i,j+1), wolf::Constants::EPS);
- }
- }
+ fac = std::make_shared<FactorDummyZero15<10>>(ftr,sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12,sb13,sb14,sb15);
+ testFactorAutodiff15(fac, sb10, states_ptr);
// 11 ------------------------------------------------------------------------
- fac = std::make_shared<FactorDummyZero12<11>>(ftr,sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12);
- i = fac->getSize();
-
- // Jacobian
- J.clear();
- residuals.resize(i);
- fac->evaluate(states_ptr, residuals, J);
-
- std::cout << "i = " << i << std::endl;
- for (unsigned int j = 0; j < 12; j++)
- {
- std::cout << "j = " << j << std::endl;
- std::cout << "Jacobian size: " << J[j].rows() << " " << J[j].cols() << std::endl;
- if (j == i-1)
- {
- ASSERT_MATRIX_APPROX(J[j], Eigen::MatrixXd::Identity(i,i), wolf::Constants::EPS);
- }
- else
- {
- ASSERT_MATRIX_APPROX(J[j], Eigen::MatrixXd::Zero(i,j+1), wolf::Constants::EPS);
- }
- }
+ fac = std::make_shared<FactorDummyZero15<11>>(ftr,sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12,sb13,sb14,sb15);
+ testFactorAutodiff15(fac, sb11, states_ptr);
// 12 ------------------------------------------------------------------------
- fac = std::make_shared<FactorDummyZero12<12>>(ftr,sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12);
- i = fac->getSize();
+ fac = std::make_shared<FactorDummyZero15<12>>(ftr,sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12,sb13,sb14,sb15);
+ testFactorAutodiff15(fac, sb12, states_ptr);
- // Jacobian
- J.clear();
- residuals.resize(i);
- fac->evaluate(states_ptr, residuals, J);
+ // 13 ------------------------------------------------------------------------
+ fac = std::make_shared<FactorDummyZero15<13>>(ftr,sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12,sb13,sb14,sb15);
+ testFactorAutodiff15(fac, sb13, states_ptr);
- std::cout << "i = " << i << std::endl;
- for (unsigned int j = 0; j < 12; j++)
- {
- std::cout << "j = " << j << std::endl;
- std::cout << "Jacobian size: " << J[j].rows() << " " << J[j].cols() << std::endl;
- if (j == i-1)
- {
- ASSERT_MATRIX_APPROX(J[j], Eigen::MatrixXd::Identity(i,i), wolf::Constants::EPS);
- }
- else
- {
- ASSERT_MATRIX_APPROX(J[j], Eigen::MatrixXd::Zero(i,j+1), wolf::Constants::EPS);
- }
- }
+ // 14 ------------------------------------------------------------------------
+ fac = std::make_shared<FactorDummyZero15<14>>(ftr,sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12,sb13,sb14,sb15);
+ testFactorAutodiff15(fac, sb14, states_ptr);
+
+ // 15 ------------------------------------------------------------------------
+ fac = std::make_shared<FactorDummyZero15<15>>(ftr,sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12,sb13,sb14,sb15);
+ testFactorAutodiff15(fac, sb15, states_ptr);
}
TEST(FactorAutodiff, EmplaceOdom2d)
@@ -389,7 +201,7 @@ TEST(FactorAutodiff, EmplaceOdom2d)
auto capture_ptr = CaptureBase::emplace<CaptureVoid>(fr2_ptr, TimeStamp(0), sensor_ptr);
// FEATURE
- auto feature_ptr = FeatureBase::emplace<FeatureOdom2d>(capture_ptr, Eigen::Vector3d::Zero(), Eigen::Matrix3d::Identity());
+ auto feature_ptr = FeatureBase::emplace<FeatureOdom2d>(capture_ptr, Vector3d::Zero(), Matrix3d::Identity());
// FACTOR
auto factor_ptr = FactorBase::emplace<FactorOdom2dAutodiff>(feature_ptr, feature_ptr, fr1_ptr, nullptr, false);
@@ -405,7 +217,7 @@ TEST(FactorAutodiff, ResidualOdom2d)
{
ProblemPtr problem = Problem::create("PO", 2);
- Eigen::Vector3d f1_pose, f2_pose;
+ Vector3d f1_pose, f2_pose;
f1_pose << 2,1,M_PI;
f2_pose << 3,5,3*M_PI/2;
@@ -423,24 +235,24 @@ TEST(FactorAutodiff, ResidualOdom2d)
// FEATURE
- Eigen::Vector3d d;
+ Vector3d d;
d << -1, -4, M_PI/2;
- auto feature_ptr = FeatureBase::emplace<FeatureOdom2d>(capture_ptr, d, Eigen::Matrix3d::Identity());
+ auto feature_ptr = FeatureBase::emplace<FeatureOdom2d>(capture_ptr, d, Matrix3d::Identity());
// FACTOR
auto factor_ptr = FactorBase::emplace<FactorOdom2dAutodiff>(feature_ptr, feature_ptr, fr1_ptr, nullptr, false);
// EVALUATE
- Eigen::VectorXd fr1_pstate = fr1_ptr->getP()->getState();
- Eigen::VectorXd fr1_ostate = fr1_ptr->getO()->getState();
- Eigen::VectorXd fr2_pstate = fr2_ptr->getP()->getState();
- Eigen::VectorXd fr2_ostate = fr2_ptr->getO()->getState();
+ VectorXd fr1_pstate = fr1_ptr->getP()->getState();
+ VectorXd fr1_ostate = fr1_ptr->getO()->getState();
+ VectorXd fr2_pstate = fr2_ptr->getP()->getState();
+ VectorXd fr2_ostate = fr2_ptr->getO()->getState();
std::vector<double*> states_ptr({fr1_pstate.data(), fr1_ostate.data(), fr2_pstate.data(), fr2_ostate.data()});
double const* const* parameters = states_ptr.data();
- Eigen::VectorXd residuals(factor_ptr->getSize());
+ VectorXd residuals(factor_ptr->getSize());
factor_ptr->evaluate(parameters, residuals.data(), nullptr);
ASSERT_TRUE(residuals.maxCoeff() < 1e-9);
@@ -451,7 +263,7 @@ TEST(FactorAutodiff, JacobianOdom2d)
{
ProblemPtr problem = Problem::create("PO", 2);
- Eigen::Vector3d f1_pose, f2_pose;
+ Vector3d f1_pose, f2_pose;
f1_pose << 2,1,M_PI;
f2_pose << 3,5,3*M_PI/2;
@@ -468,51 +280,51 @@ TEST(FactorAutodiff, JacobianOdom2d)
auto capture_ptr = CaptureBase::emplace<CaptureVoid>(fr2_ptr, TimeStamp(0), sensor_ptr);
// FEATURE
- Eigen::Vector3d d;
+ Vector3d d;
d << -1, -4, M_PI/2;
- auto feature_ptr = FeatureBase::emplace<FeatureOdom2d>(capture_ptr, d, Eigen::Matrix3d::Identity());
+ auto feature_ptr = FeatureBase::emplace<FeatureOdom2d>(capture_ptr, d, Matrix3d::Identity());
// FACTOR
auto factor_ptr = FactorBase::emplace<FactorOdom2dAutodiff>(feature_ptr, feature_ptr, fr1_ptr, nullptr, false);
// COMPUTE JACOBIANS
- const Eigen::VectorXd fr1_pstate = fr1_ptr->getP()->getState();
- const Eigen::VectorXd fr1_ostate = fr1_ptr->getO()->getState();
- const Eigen::VectorXd fr2_pstate = fr2_ptr->getP()->getState();
- const Eigen::VectorXd fr2_ostate = fr2_ptr->getO()->getState();
+ const VectorXd fr1_pstate = fr1_ptr->getP()->getState();
+ const VectorXd fr1_ostate = fr1_ptr->getO()->getState();
+ const VectorXd fr2_pstate = fr2_ptr->getP()->getState();
+ const VectorXd fr2_ostate = fr2_ptr->getO()->getState();
std::vector<const double*> states_ptr({fr1_pstate.data(), fr1_ostate.data(), fr2_pstate.data(), fr2_ostate.data()});
- std::vector<Eigen::MatrixXd> Jauto;
- Eigen::VectorXd residuals(factor_ptr->getSize());
+ std::vector<MatrixXd> Jauto;
+ VectorXd residuals(factor_ptr->getSize());
factor_ptr->evaluate(states_ptr, residuals, Jauto);
ASSERT_EQ(Jauto.size(),4);
// ANALYTIC JACOBIANS
- Eigen::MatrixXd J0(3,2);
+ MatrixXd J0(3,2);
J0 << -cos(f1_pose(2)), -sin(f1_pose(2)),
sin(f1_pose(2)), -cos(f1_pose(2)),
0, 0;
ASSERT_MATRIX_APPROX(J0, Jauto[0], wolf::Constants::EPS);
//ASSERT_TRUE((J0-Jauto[0]).maxCoeff() < 1e-9 && (J0-Jauto[0]).minCoeff() > -1e-9);
- Eigen::MatrixXd J1(3,1);
+ MatrixXd J1(3,1);
J1 << -(f2_pose(0) - f1_pose(0)) * sin(f1_pose(2)) + (f2_pose(1) - f1_pose(1)) * cos(f1_pose(2)),
-(f2_pose(0) - f1_pose(0)) * cos(f1_pose(2)) - (f2_pose(1) - f1_pose(1)) * sin(f1_pose(2)),
-1;
ASSERT_MATRIX_APPROX(J1, Jauto[1], wolf::Constants::EPS);
//ASSERT_TRUE((J1-Jauto[1]).maxCoeff() < 1e-9 && (J1-Jauto[1]).minCoeff() > -1e-9);
- Eigen::MatrixXd J2(3,2);
+ MatrixXd J2(3,2);
J2 << cos(f1_pose(2)), sin(f1_pose(2)),
-sin(f1_pose(2)), cos(f1_pose(2)),
0, 0;
ASSERT_MATRIX_APPROX(J2, Jauto[2], wolf::Constants::EPS);
//ASSERT_TRUE((J2-Jauto[2]).maxCoeff() < 1e-9 && (J2-Jauto[2]).minCoeff() > -1e-9);
- Eigen::MatrixXd J3(3,1);
+ MatrixXd J3(3,1);
J3 << 0, 0, 1;
ASSERT_MATRIX_APPROX(J3, Jauto[3], wolf::Constants::EPS);
//ASSERT_TRUE((J3-Jauto[3]).maxCoeff() < 1e-9 && (J3-Jauto[3]).minCoeff() > -1e-9);
@@ -531,7 +343,7 @@ TEST(FactorAutodiff, AutodiffVsAnalytic)
{
ProblemPtr problem = Problem::create("PO", 2);
- Eigen::Vector3d f1_pose, f2_pose;
+ Vector3d f1_pose, f2_pose;
f1_pose << 2,1,M_PI;
f2_pose << 3,5,3*M_PI/2;
@@ -548,24 +360,24 @@ TEST(FactorAutodiff, AutodiffVsAnalytic)
auto capture_ptr = CaptureBase::emplace<CaptureVoid>(fr2_ptr, TimeStamp(0), sensor_ptr);
// FEATURE
- Eigen::Vector3d d;
+ Vector3d d;
d << -1, -4, M_PI/2;
- auto feature_ptr = FeatureBase::emplace<FeatureOdom2d>(capture_ptr, d, Eigen::Matrix3d::Identity());
+ auto feature_ptr = FeatureBase::emplace<FeatureOdom2d>(capture_ptr, d, Matrix3d::Identity());
// FACTOR
auto fac_autodiff_ptr = FactorBase::emplace<FactorOdom2dAutodiff>(feature_ptr, feature_ptr, fr1_ptr, nullptr, false);
auto fac_analytic_ptr = FactorBase::emplace<FactorRelativePose2d>(feature_ptr, feature_ptr, fr1_ptr, nullptr, false, TOP_MOTION);
// COMPUTE JACOBIANS
- const Eigen::VectorXd fr1_pstate = fr1_ptr->getP()->getState();
- const Eigen::VectorXd fr1_ostate = fr1_ptr->getO()->getState();
- const Eigen::VectorXd fr2_pstate = fr2_ptr->getP()->getState();
- const Eigen::VectorXd fr2_ostate = fr2_ptr->getO()->getState();
+ const VectorXd fr1_pstate = fr1_ptr->getP()->getState();
+ const VectorXd fr1_ostate = fr1_ptr->getO()->getState();
+ const VectorXd fr2_pstate = fr2_ptr->getP()->getState();
+ const VectorXd fr2_ostate = fr2_ptr->getO()->getState();
std::vector<const double*> states_ptr({fr1_pstate.data(), fr1_ostate.data(), fr2_pstate.data(), fr2_ostate.data()});
- std::vector<Eigen::MatrixXd> Jautodiff, Janalytic;
- Eigen::VectorXd residuals(fac_autodiff_ptr->getSize());
+ std::vector<MatrixXd> Jautodiff, Janalytic;
+ VectorXd residuals(fac_autodiff_ptr->getSize());
clock_t t = clock();
fac_autodiff_ptr->evaluate(states_ptr, residuals, Jautodiff);
std::cout << "autodiff evaluate: " << ((double) clock() - t) / CLOCKS_PER_SEC << "s" << std::endl;
diff --git a/test/gtest_processor_landmark_external.cpp b/test/gtest_processor_landmark_external.cpp
index cb0b336bca4e64b31afbf11e242251144f737a93..8336e31a65f58be3c5a832c0f31b63f12ca03ff9 100644
--- a/test/gtest_processor_landmark_external.cpp
+++ b/test/gtest_processor_landmark_external.cpp
@@ -23,7 +23,7 @@
#include "core/utils/utils_gtest.h"
#include "core/problem/problem.h"
#include "core/capture/capture_landmarks_external.h"
-#include "core/landmark/landmark_base.h"
+#include "core/landmark/landmark_external.h"
#include "core/sensor/sensor_odom_2d.h"
#include "core/sensor/sensor_odom_3d.h"
#include "core/processor/processor_odom_2d.h"
@@ -44,58 +44,69 @@ using namespace Eigen;
class ProcessorLandmarkExternalTest : public testing::Test
{
- protected:
- int dim;
- bool orientation;
- TimeStamp t;
- double dt;
-
- ProblemPtr problem;
- SolverManagerPtr solver;
- SensorBasePtr sensor, sensor_odom;
- ProcessorLandmarkExternalPtr processor;
- ProcessorMotionPtr processor_motion;
- std::vector<LandmarkBasePtr> landmarks;
-
- // Groundtruth states
- VectorComposite state_robot, state_sensor;
- std::vector<VectorComposite> state_landmarks;
-
- virtual void SetUp() {};
- void initProblem(int _dim,
- bool _orientation,
- double _quality_th,
- double _dist_th,
- unsigned int _track_length_th,
- double _time_span,
- bool _add_landmarks);
- void randomStep();
- CaptureLandmarksExternalPtr computeCaptureLandmarks() const;
- void testConfiguration(int dim,
- bool orientation,
- double quality_th,
- double dist_th,
- int track_length,
- double time_span,
- bool add_landmarks);
- void assertVectorComposite(const VectorComposite& v1, const VectorComposite& v2) const;
+ protected:
+ int dim;
+ bool orientation;
+ int mode; // 0: external ID, 1: external TYPE, 2: nothing, 3: all mixed
+ TimeStamp t;
+ double dt;
+
+ ProblemPtr problem;
+ SolverManagerPtr solver;
+ SensorBasePtr sensor, sensor_odom;
+ ProcessorLandmarkExternalPtr processor;
+ ProcessorMotionPtr processor_motion;
+ std::vector<LandmarkExternalPtr> landmarks;
+
+ // Groundtruth states
+ VectorComposite state_robot, state_sensor;
+ std::vector<VectorComposite> state_landmarks;
+
+ virtual void SetUp(){};
+ void initProblem(int _dim,
+ bool _orientation,
+ int _mode,
+ double _quality_th,
+ double _dist_th,
+ unsigned int _track_length_th,
+ double _time_span,
+ bool _init_landmarks);
+ void randomStep();
+ CaptureLandmarksExternalPtr computeCaptureLandmarks() const;
+ void testConfiguration(int _dim,
+ bool _orientation,
+ int _mode,
+ double _quality_th,
+ double _dist_th,
+ int _track_length,
+ double _time_span,
+ bool _init_landmarks);
+ void assertVectorComposite(const VectorComposite& v1, const VectorComposite& v2) const;
};
-void ProcessorLandmarkExternalTest::initProblem(int _dim,
- bool _orientation,
- double _quality_th,
- double _dist_th,
+void ProcessorLandmarkExternalTest::initProblem(int _dim,
+ bool _orientation,
+ int _mode,
+ double _quality_th,
+ double _dist_th,
unsigned int _track_length_th,
- double _time_span,
- bool _add_landmarks)
+ double _time_span,
+ bool _init_landmarks)
{
- dim = _dim;
+ // INCOMPATIBLE OPTIONS
+ if (_init_landmarks and _mode == 2)
+ throw std::runtime_error("Landmarks initialized with mode 2 (no id no type), impossible to close loops");
+ if (_init_landmarks and _mode == 4)
+ throw std::runtime_error("Landmarks initialized with mode 4 (changing), impossible to close loops");
+
+ dim = _dim;
orientation = _orientation;
- t = TimeStamp(0);
- dt = 0.1;
+ mode = _mode;
+ t = TimeStamp(0);
+ dt = 0.1;
problem = Problem::create("PO", dim);
- solver = std::make_shared<SolverCeres>(problem);
+ solver = std::make_shared<SolverCeres>(problem);
// Sensors
if (dim == 2)
@@ -106,58 +117,60 @@ void ProcessorLandmarkExternalTest::initProblem(int _dim,
std::make_shared<StateAngle>(Vector1d::Random() * M_PI),
nullptr,
2);
- sensor_odom = SensorBase::emplace<SensorOdom2d>(problem->getHardware(),
- Vector3d::Zero(),
- ParamsSensorOdom2d());
-
+ sensor_odom =
+ SensorBase::emplace<SensorOdom2d>(problem->getHardware(), Vector3d::Zero(), ParamsSensorOdom2d());
}
else
{
- sensor = SensorBase::emplace<SensorBase>(problem->getHardware(),
+ sensor = SensorBase::emplace<SensorBase>(problem->getHardware(),
"SensorBase",
std::make_shared<StatePoint3d>(Vector3d::Random()),
std::make_shared<StateQuaternion>(Vector4d::Random().normalized()),
nullptr,
3);
- sensor_odom = SensorBase::emplace<SensorOdom3d>(problem->getHardware(),
- (Vector7d() << 0,0,0,0,0,0,1).finished(),
- ParamsSensorOdom3d());
+ sensor_odom = SensorBase::emplace<SensorOdom3d>(
+ problem->getHardware(), (Vector7d() << 0, 0, 0, 0, 0, 0, 1).finished(), ParamsSensorOdom3d());
}
// Processors
ParamsProcessorLandmarkExternalPtr params = std::make_shared<ParamsProcessorLandmarkExternal>();
- params->time_tolerance = dt/2;
- params->max_new_features = -1;
- params->voting_active = true;
- params->apply_loss_function = false;
- params->use_orientation = orientation;
- params->filter_quality_th = _quality_th;
- params->match_dist_th = _dist_th;
- params->min_features_for_keyframe = 1;
- params->new_features_for_keyframe = 1000;
- params->filter_track_length_th = _track_length_th;
- params->time_span = _time_span;
- processor = ProcessorBase::emplace<ProcessorLandmarkExternal>(sensor, params);
+ params->time_tolerance = dt / 2;
+ params->apply_loss_function = false;
+ params->voting_active = true;
+ params->max_new_features = -1;
+ params->min_features_for_keyframe = 1;
+ params->new_features_for_keyframe = 1000;
+ params->time_span = _time_span;
+ params->use_orientation = orientation;
+ params->quality_th = _quality_th;
+ params->track_length_th = _track_length_th;
+ params->match_dist_th_id = _dist_th;
+ params->match_dist_th_type = _dist_th;
+ params->match_dist_th_unknown = _dist_th;
+ params->close_loops_by_id = _init_landmarks;
+ params->close_loops_by_type = _init_landmarks;
+ processor = ProcessorBase::emplace<ProcessorLandmarkExternal>(sensor, params);
if (dim == 2)
{
- auto params_odom = std::make_shared<ParamsProcessorOdom2d>();
- params_odom->state_getter = true;
+ auto params_odom = std::make_shared<ParamsProcessorOdom2d>();
+ params_odom->state_getter = true;
params_odom->voting_active = false;
- processor_motion = std::static_pointer_cast<ProcessorMotion>(ProcessorBase::emplace<ProcessorOdom2d>(sensor_odom,
- params_odom));
+ processor_motion = std::static_pointer_cast<ProcessorMotion>(
+ ProcessorBase::emplace<ProcessorOdom2d>(sensor_odom, params_odom));
}
else
{
- auto params_odom = std::make_shared<ParamsProcessorOdom3d>();
- params_odom->state_getter = true;
+ auto params_odom = std::make_shared<ParamsProcessorOdom3d>();
+ params_odom->state_getter = true;
params_odom->voting_active = false;
- processor_motion = std::static_pointer_cast<ProcessorMotion>(ProcessorBase::emplace<ProcessorOdom3d>(sensor_odom,
- params_odom));
+ processor_motion = std::static_pointer_cast<ProcessorMotion>(
+ ProcessorBase::emplace<ProcessorOdom3d>(sensor_odom, params_odom));
}
// Emplace frame
- auto F0 = problem->emplaceFrame(t, (dim == 2 ? VectorXd::Zero(3) : (VectorXd(7) << 0,0,0,0,0,0,1).finished()));
+ auto F0 =
+ problem->emplaceFrame(t, (dim == 2 ? VectorXd::Zero(3) : (VectorXd(7) << 0, 0, 0, 0, 0, 0, 1).finished()));
F0->fix();
processor->keyFrameCallback(F0);
processor_motion->setOrigin(F0);
@@ -165,23 +178,23 @@ void ProcessorLandmarkExternalTest::initProblem(int _dim,
// Emplace 3 random landmarks
for (auto i = 0; i < 3; i++)
{
- LandmarkBasePtr lmk;
+ bool init_landmark = _init_landmarks and not (mode == 3 and i % 3 == 2);
+ LandmarkExternalPtr lmk;
if (dim == 2)
- lmk = LandmarkBase::emplace<LandmarkBase>(_add_landmarks ? problem->getMap() : nullptr,
- "LandmarkExternal",
- std::make_shared<StatePoint2d>(Vector2d::Random() * 10),
- (orientation ?
- std::make_shared<StateAngle>(Vector1d::Random() * M_PI) :
- nullptr));
-
- else
- lmk = LandmarkBase::emplace<LandmarkBase>(_add_landmarks ? problem->getMap() : nullptr,
- "LandmarkExternal",
- std::make_shared<StatePoint3d>(Vector3d::Random() * 10),
- (orientation ?
- std::make_shared<StateQuaternion>(Vector4d::Random().normalized()) :
- nullptr));
- lmk->setId(i);
+ lmk = LandmarkBase::emplace<LandmarkExternal>(
+ init_landmark ? problem->getMap() : nullptr,
+ i + 1,
+ 3 * i + 10,
+ std::make_shared<StatePoint2d>(Vector2d::Random() * 10),
+ (orientation ? std::make_shared<StateAngle>(Vector1d::Random() * M_PI) : nullptr));
+
+ else
+ lmk = LandmarkBase::emplace<LandmarkExternal>(
+ init_landmark ? problem->getMap() : nullptr,
+ i + 1,
+ 3 * i + 10,
+ std::make_shared<StatePoint3d>(Vector3d::Random() * 10),
+ (orientation ? std::make_shared<StateQuaternion>(Vector4d::Random().normalized()) : nullptr));
landmarks.push_back(lmk);
state_landmarks.push_back(lmk->getState());
}
@@ -198,7 +211,7 @@ void ProcessorLandmarkExternalTest::randomStep()
MatrixXd delta_cov;
if (dim == 2)
{
- delta = Vector2d::Random() * 0.1;
+ delta = Vector2d::Random() * 0.1;
delta_cov = Matrix2d::Identity() * 0.1;
}
else
@@ -223,10 +236,13 @@ CaptureLandmarksExternalPtr ProcessorLandmarkExternalTest::computeCaptureLandmar
for (auto i = 0; i < landmarks.size(); i++)
{
// compute detection
- VectorXd meas(orientation?(dim==2?3:7):(dim==2?2:3));
+ VectorXd meas(orientation ? (dim == 2 ? 3 : 7) : (dim == 2 ? 2 : 3));
if (dim == 2)
{
- meas.head(2) = Rotation2Dd(-state_sensor.at('O')(0))*(Rotation2Dd(-state_robot.at('O')(0))*(state_landmarks.at(i).at('P') - state_robot.at('P')) - state_sensor.at('P'));
+ meas.head(2) =
+ Rotation2Dd(-state_sensor.at('O')(0)) *
+ (Rotation2Dd(-state_robot.at('O')(0)) * (state_landmarks.at(i).at('P') - state_robot.at('P')) -
+ state_sensor.at('P'));
if (orientation)
meas(2) = state_landmarks.at(i).at('O')(0) - state_robot.at('O')(0) - state_sensor.at('O')(0);
}
@@ -235,40 +251,89 @@ CaptureLandmarksExternalPtr ProcessorLandmarkExternalTest::computeCaptureLandmar
auto q_sensor = Quaterniond(Vector4d(state_sensor.at('O')));
auto q_robot = Quaterniond(Vector4d(state_robot.at('O')));
- meas.head(3) = q_sensor.conjugate() * (q_robot.conjugate() * (state_landmarks.at(i).at('P') - state_robot.at('P')) - state_sensor.at('P'));
+ meas.head(3) =
+ q_sensor.conjugate() *
+ (q_robot.conjugate() * (state_landmarks.at(i).at('P') - state_robot.at('P')) - state_sensor.at('P'));
if (orientation)
- meas.tail(4) = (q_sensor.conjugate() * q_robot.conjugate() * Quaterniond(Vector4d(state_landmarks.at(i).at('O')))).coeffs();
+ meas.tail(4) =
+ (q_sensor.conjugate() * q_robot.conjugate() * Quaterniond(Vector4d(state_landmarks.at(i).at('O'))))
+ .coeffs();
}
// cov
- MatrixXd cov = MatrixXd::Identity(meas.size(), meas.size());
- if (orientation and dim != 2)
- cov = MatrixXd::Identity(6, 6);
+ MatrixXd cov = 0.01 * MatrixXd::Identity(meas.size(), meas.size());
+ if (orientation and dim != 2) cov = 0.01 * MatrixXd::Identity(6, 6);
// quality
- double quality = (double) i / (double) (landmarks.size()-1); // increasing from 0 to 1
+ double quality = (double)i / (double)(landmarks.size() - 1); // increasing from 0 to 1
// add detection
- cap->addDetection(landmarks.at(i)->id(), meas, cov, quality );
+ auto step = (int)round(t.get() / dt);
+ // with ID
+ if (mode == 0 or (mode == 3 and i % 3 == 0) or (mode == 4 and step % 3 == 0))
+ cap->addDetection(landmarks.at(i)->getExternalId(), -1, meas, cov, quality);
+ // with TYPE
+ else if (mode == 1 or (mode == 3 and i % 3 == 1) or (mode == 4 and step % 3 == 1))
+ cap->addDetection(-1, landmarks.at(i)->getExternalType(), meas, cov, quality);
+ // with nothing
+ else if (mode == 2 or (mode == 3 and i % 3 == 2) or (mode == 4 and step % 3 == 2))
+ cap->addDetection(-1, -1, meas, cov, quality);
}
return cap;
}
-void ProcessorLandmarkExternalTest::testConfiguration(int dim,
- bool orientation,
- double quality_th,
- double dist_th,
- int track_length,
- double time_span,
- bool add_landmarks)
+void ProcessorLandmarkExternalTest::testConfiguration(int _dim,
+ bool _orientation,
+ int _mode,
+ double _quality_th,
+ double _dist_th,
+ int _track_length,
+ double _time_span,
+ bool _init_landmarks)
{
- initProblem(dim, orientation, quality_th, dist_th, track_length, time_span, add_landmarks);
+ initProblem(_dim, _orientation, _mode, _quality_th, _dist_th, _track_length, _time_span, _init_landmarks);
+
+ FactorBasePtrList fac_list;
ASSERT_TRUE(problem->check());
- for (auto i = 0; i<10; i++)
+ for (auto i = 0; i < 10; i++)
{
WOLF_INFO("\n================= STEP ", i, " t = ", t, " =================");
+ WOLF_INFO("robot state: ", state_robot);
+ WOLF_INFO("sensor state: ", state_sensor);
+
+ // store previous number of kf, lmks, and factors
+ auto n_prev_kf = problem->getTrajectory()->size();
+ auto n_prev_lmk = problem->getMap()->getLandmarkList().size();
+ fac_list.clear();
+ problem->getTrajectory()->getFactorList(fac_list);
+ auto n_prev_fac = fac_list.size();
+
+ // Things to check
+ bool any_active_track = _quality_th <= 1 and i >= _track_length;
+ bool should_emplace_KF =
+ (t - problem->getTrajectory()->getLastFrame()->getTimeStamp().get() - dt > _time_span and
+ any_active_track);
+ bool should_emplace_factors = (i == 1 or should_emplace_KF) and any_active_track;
+
+ WOLF_INFO("should emplace factors: ",
+ should_emplace_factors,
+ " _quality_th = ",
+ _quality_th,
+ " track_length-1 = ",
+ _track_length - 1);
+
+ WOLF_INFO("should emplace KF: ",
+ should_emplace_KF,
+ " i = ",
+ i,
+ " t-last_frame_stamp-dt = ",
+ t - problem->getTrajectory()->getLastFrame()->getTimeStamp().get() - dt,
+ " time_span = ",
+ _time_span,
+ " track_length-1 = ",
+ _track_length - 1);
// detection of landmarks
auto cap = computeCaptureLandmarks();
@@ -277,73 +342,72 @@ void ProcessorLandmarkExternalTest::testConfiguration(int dim,
// process detections
cap->process();
ASSERT_TRUE(problem->check());
- problem->print(4,1,1,1);
+ problem->print(4, 1, 1, 1);
// CHECKS
- FactorBasePtrList fac_list;
+ fac_list.clear();
problem->getTrajectory()->getFactorList(fac_list);
- // should emplace KF in last?
- bool any_active_track = quality_th <=1 and i >= track_length-1;
- bool should_emplace_KF = t-dt > time_span and any_active_track;
- WOLF_INFO("should emplace KF: ", should_emplace_KF, " t-dt = ", t-dt, " time_span = ", time_span, " track_length-1 = ", track_length-1);
-
+ // Check voted&emplaced a keyframe
if (should_emplace_KF)
- {
- // voted for keyframe
- ASSERT_TRUE(problem->getTrajectory()->size() > 1);
-
- // emplaced factors
- ASSERT_FALSE(fac_list.empty());
-
+ {
+ ASSERT_EQ(problem->getTrajectory()->size(), n_prev_kf + 1);
+ }
+ else
+ {
+ ASSERT_EQ(problem->getTrajectory()->size(), n_prev_kf);
+ }
+
+ // Check emplaced factors and landmarks
+ if (should_emplace_factors)
+ {
+ ASSERT_GT(fac_list.size(), n_prev_fac);
+
// factors' type
- if (should_emplace_KF)
- for (auto fac : fac_list)
+ for (auto fac : fac_list)
+ {
+ if (fac->getProcessor() == processor)
{
- if (fac->getProcessor() == processor)
- {
- ASSERT_EQ(fac->getType(), std::string("FactorRelative") +
- (orientation ? "Pose" : "Position") +
- (dim == 2 ? "2d" : "3d") +
- "WithExtrinsics");
- }
+ ASSERT_EQ(fac->getType(),
+ std::string("FactorRelative") + (orientation ? "Pose" : "Position") +
+ (dim == 2 ? "2d" : "3d") + "WithExtrinsics");
}
+ }
+
// landmarks created by processor
- if (not add_landmarks)
+ if (not _init_landmarks)
{
auto landmarks_map = problem->getMap()->getLandmarkList();
- // amount of landmarks due to quality filtering of detections
- auto n_landmarks = (quality_th <= 0 ? 3 : (quality_th <= 0.5 ? 2 : (quality_th <= 1 ? 1 : 0)));
+ // amount of landmarks according to quality filtering of detections
+ auto n_landmarks = (_quality_th <= 0 ? 3 : (_quality_th <= 0.5 ? 2 : (_quality_th <= 1 ? 1 : 0)));
ASSERT_EQ(landmarks_map.size(), n_landmarks);
// check state correctly initialized
for (auto lmk_map : landmarks_map)
{
- ASSERT_TRUE(lmk_map->id() < landmarks.size());
- ASSERT_EQ(lmk_map->id(), landmarks.at(lmk_map->id())->id());
- assertVectorComposite(lmk_map->getState(), state_landmarks.at(lmk_map->id()));
+ auto lmk_ext = std::static_pointer_cast<LandmarkExternal>(lmk_map);
+ if (lmk_ext->getExternalId() != -1)
+ {
+ ASSERT_EQ(lmk_ext->getExternalId(),
+ landmarks.at(lmk_ext->getExternalId() - 1)->getExternalId());
+ assertVectorComposite(lmk_map->getState(), state_landmarks.at(lmk_ext->getExternalId() - 1));
+ }
}
}
}
else
{
- // didn't vote for keyframe
- ASSERT_FALSE(problem->getTrajectory()->size() > 1);
// no factors emplaced
- ASSERT_TRUE(fac_list.empty());
+ ASSERT_EQ(fac_list.size(), n_prev_fac);
+
// no landmarks created yet (if not added by the test)
- ASSERT_EQ(problem->getMap()->getLandmarkList().empty(), not add_landmarks);
+ ASSERT_EQ(problem->getMap()->getLandmarkList().size(), n_prev_lmk);
}
- // step with random movement
- t+=dt;
- randomStep();
-
// solve
- if (should_emplace_KF)
+ if (should_emplace_factors)
{
// perturb landmarks
- for (auto lmk : problem->getMap()->getLandmarkList())
- lmk->perturb();
+ for (auto lmk : problem->getMap()->getLandmarkList()) lmk->perturb();
// fix frame and extrinsics
sensor->fix();
@@ -353,14 +417,18 @@ void ProcessorLandmarkExternalTest::testConfiguration(int dim,
auto report = solver->solve(SolverManager::ReportVerbosity::FULL);
WOLF_INFO(report);
- // check values
- //assertVectorComposite(sensor->getState("PO"), state_sensor);
- //assertVectorComposite(problem->getState("PO"), state_robot);
+ // check values
for (auto lmk_map : problem->getMap()->getLandmarkList())
{
- assertVectorComposite(lmk_map->getState(), state_landmarks.at(lmk_map->id()));
+ auto lmk_ext = std::static_pointer_cast<LandmarkExternal>(lmk_map);
+ if (lmk_ext->getExternalId() != -1)
+ assertVectorComposite(lmk_map->getState(), state_landmarks.at(lmk_ext->getExternalId() - 1));
}
}
+
+ // step with random movement
+ t += dt;
+ randomStep();
}
}
@@ -385,142 +453,638 @@ void ProcessorLandmarkExternalTest::assertVectorComposite(const VectorComposite&
throw std::runtime_error("wrong vector composite");
}
-// / TESTS //////////////////////////////////////////
-TEST_F(ProcessorLandmarkExternalTest, P_2d_existing_lmks)
+/// TESTS //////////////////////////////////////////
+
+//-------------------- Position in 2D --------------------
+TEST_F(ProcessorLandmarkExternalTest, P_2d_loop_closure_id)
{
- testConfiguration(2, //int dim
- false, //bool orientation
- 0, //double quality_th
- 1e6, //double dist_th
- 6, //int track_length
- 4.5*dt, //double time_span
- true); //bool add_landmarks
+ testConfiguration(2, // int dim
+ false, // bool orientation
+ 0, // int mode
+ 0, // double quality_th
+ 1e-2, // double dist_th
+ 6, // int track_length
+ 4.5 * dt, // double time_span
+ true); // bool init_landmarks & loop closure
}
-TEST_F(ProcessorLandmarkExternalTest, P_2d_no_lmks)
+TEST_F(ProcessorLandmarkExternalTest, P_2d_loop_closure_type)
{
- testConfiguration(2, //int dim
- false, //bool orientation
- 0, //double quality_th
- 1e6, //double dist_th
- 2, //int track_length
- 4.5*dt, //double time_span
- false); //bool add_landmarks
+ testConfiguration(2, // int dim
+ false, // bool orientation
+ 1, // int mode
+ 0, // double quality_th
+ 1e-2, // double dist_th
+ 6, // int track_length
+ 4.5 * dt, // double time_span
+ true); // bool init_landmarks & loop closure
}
-TEST_F(ProcessorLandmarkExternalTest, P_2d_no_lmks_quality)
+TEST_F(ProcessorLandmarkExternalTest, P_2d_loop_closure_mixed)
{
- testConfiguration(2, //int dim
- false, //bool orientation
- 0.3, //double quality_th
- 1e6, //double dist_th
- 6, //int track_length
- 4.5*dt, //double time_span
- false); //bool add_landmarks
+ testConfiguration(2, // int dim
+ false, // bool orientation
+ 3, // int mode
+ 0, // double quality_th
+ 1e-2, // double dist_th
+ 6, // int track_length
+ 4.5 * dt, // double time_span
+ true); // bool init_landmarks & loop closure
}
-TEST_F(ProcessorLandmarkExternalTest, PO_2d_existing_lmks)
+TEST_F(ProcessorLandmarkExternalTest, P_2d_id)
{
- testConfiguration(2, //int dim
- true, //bool orientation
- 0, //double quality_th
- 1e6, //double dist_th
- 3, //int track_length
- 4.5*dt, //double time_span
- true); //bool add_landmarks
+ testConfiguration(2, // int dim
+ false, // bool orientation
+ 0, // int mode
+ 0, // double quality_th
+ 1e-2, // double dist_th
+ 2, // int track_length
+ 4.5 * dt, // double time_span
+ false); // bool init_landmarks & loop closure
}
-TEST_F(ProcessorLandmarkExternalTest, PO_2d_no_lmks)
+TEST_F(ProcessorLandmarkExternalTest, P_2d_type)
{
- testConfiguration(2, //int dim
- true, //bool orientation
- 0, //double quality_th
- 1e6, //double dist_th
- 1, //int track_length
- 4.5*dt, //double time_span
- false); //bool add_landmarks
+ testConfiguration(2, // int dim
+ false, // bool orientation
+ 1, // int mode
+ 0, // double quality_th
+ 1e-2, // double dist_th
+ 2, // int track_length
+ 4.5 * dt, // double time_span
+ false); // bool init_landmarks & loop closure
}
-TEST_F(ProcessorLandmarkExternalTest, PO_2d_no_lmks_quality)
+TEST_F(ProcessorLandmarkExternalTest, P_2d_nothing)
{
- testConfiguration(2, //int dim
- true, //bool orientation
- 0.3, //double quality_th
- 1e6, //double dist_th
- 0, //int track_length
- 4.5*dt, //double time_span
- false); //bool add_landmarks
+ testConfiguration(2, // int dim
+ false, // bool orientation
+ 2, // int mode
+ 0, // double quality_th
+ 1e-2, // double dist_th
+ 2, // int track_length
+ 4.5 * dt, // double time_span
+ false); // bool init_landmarks & loop closure
}
-TEST_F(ProcessorLandmarkExternalTest, P_3d_existing_lmks)
+TEST_F(ProcessorLandmarkExternalTest, P_2d_mixed)
{
- testConfiguration(3, //int dim
- false, //bool orientation
- 0, //double quality_th
- 1e6, //double dist_th
- 7, //int track_length
- 4.5*dt, //double time_span
- true); //bool add_landmarks
+ testConfiguration(2, // int dim
+ false, // bool orientation
+ 3, // int mode
+ 0, // double quality_th
+ 1e-2, // double dist_th
+ 2, // int track_length
+ 4.5 * dt, // double time_span
+ false); // bool init_landmarks & loop closure
}
-TEST_F(ProcessorLandmarkExternalTest, P_3d_no_lmks)
+TEST_F(ProcessorLandmarkExternalTest, P_2d_changing)
{
- testConfiguration(3, //int dim
- false, //bool orientation
- 0, //double quality_th
- 1e6, //double dist_th
- 53, //int track_length
- 4.5*dt, //double time_span
- false); //bool add_landmarks
+ testConfiguration(2, // int dim
+ false, // bool orientation
+ 4, // int mode
+ 0, // double quality_th
+ 1e-2, // double dist_th
+ 2, // int track_length
+ 4.5 * dt, // double time_span
+ false); // bool init_landmarks & loop closure
}
-TEST_F(ProcessorLandmarkExternalTest, P_3d_no_lmks_quality)
+TEST_F(ProcessorLandmarkExternalTest, P_2d_quality_id)
{
- testConfiguration(3, //int dim
- false, //bool orientation
- 0.3, //double quality_th
- 1e6, //double dist_th
- 2, //int track_length
- 4.5*dt, //double time_span
- false); //bool add_landmarks
+ testConfiguration(2, // int dim
+ false, // bool orientation
+ 0, // int mode
+ 0.3, // double quality_th
+ 1e-2, // double dist_th
+ 6, // int track_length
+ 4.5 * dt, // double time_span
+ false); // bool init_landmarks & loop closure
}
-TEST_F(ProcessorLandmarkExternalTest, PO_3d_existing_lmks)
+TEST_F(ProcessorLandmarkExternalTest, P_2d_quality_type)
{
- testConfiguration(3, //int dim
- true, //bool orientation
- 0, //double quality_th
- 1e6, //double dist_th
- 1, //int track_length
- 4.5*dt, //double time_span
- true); //bool add_landmarks
+ testConfiguration(2, // int dim
+ false, // bool orientation
+ 1, // int mode
+ 0.3, // double quality_th
+ 1e-2, // double dist_th
+ 6, // int track_length
+ 4.5 * dt, // double time_span
+ false); // bool init_landmarks & loop closure
}
-TEST_F(ProcessorLandmarkExternalTest, PO_3d_no_lmks)
+TEST_F(ProcessorLandmarkExternalTest, P_2d_quality_nothing)
{
- testConfiguration(3, //int dim
- true, //bool orientation
- 0, //double quality_th
- 1e6, //double dist_th
- 4, //int track_length
- 4.5*dt, //double time_span
- false); //bool add_landmarks
+ testConfiguration(2, // int dim
+ false, // bool orientation
+ 2, // int mode
+ 0.3, // double quality_th
+ 1e-2, // double dist_th
+ 6, // int track_length
+ 4.5 * dt, // double time_span
+ false); // bool init_landmarks & loop closure
}
-TEST_F(ProcessorLandmarkExternalTest, PO_3d_no_lmks_quality)
+TEST_F(ProcessorLandmarkExternalTest, P_2d_quality_mixed)
{
- testConfiguration(3, //int dim
- true, //bool orientation
- 0.2, //double quality_th
- 1e6, //double dist_th
- 5, //int track_length
- 4.5*dt, //double time_span
- false); //bool add_landmarks
+ testConfiguration(2, // int dim
+ false, // bool orientation
+ 3, // int mode
+ 0.3, // double quality_th
+ 1e-2, // double dist_th
+ 6, // int track_length
+ 4.5 * dt, // double time_span
+ false); // bool init_landmarks & loop closure
}
+TEST_F(ProcessorLandmarkExternalTest, P_2d_quality_changing)
+{
+ testConfiguration(2, // int dim
+ false, // bool orientation
+ 4, // int mode
+ 0.3, // double quality_th
+ 1e-2, // double dist_th
+ 6, // int track_length
+ 4.5 * dt, // double time_span
+ false); // bool init_landmarks & loop closure
+}
+
+//-------------------- Position & Orientation in 2D --------------------
+TEST_F(ProcessorLandmarkExternalTest, PO_2d_loop_closure_id)
+{
+ testConfiguration(2, // int dim
+ true, // bool orientation
+ 0, // int mode
+ 0, // double quality_th
+ 1e-2, // double dist_th
+ 6, // int track_length
+ 4.5 * dt, // double time_span
+ true); // bool init_landmarks & loop closure
+}
+
+TEST_F(ProcessorLandmarkExternalTest, PO_2d_loop_closure_type)
+{
+ testConfiguration(2, // int dim
+ true, // bool orientation
+ 1, // int mode
+ 0, // double quality_th
+ 1e-2, // double dist_th
+ 6, // int track_length
+ 4.5 * dt, // double time_span
+ true); // bool init_landmarks & loop closure
+}
+
+TEST_F(ProcessorLandmarkExternalTest, PO_2d_loop_closure_mixed)
+{
+ testConfiguration(2, // int dim
+ true, // bool orientation
+ 3, // int mode
+ 0, // double quality_th
+ 1e-2, // double dist_th
+ 6, // int track_length
+ 4.5 * dt, // double time_span
+ true); // bool init_landmarks & loop closure
+}
+
+TEST_F(ProcessorLandmarkExternalTest, PO_2d_id)
+{
+ testConfiguration(2, // int dim
+ true, // bool orientation
+ 0, // int mode
+ 0, // double quality_th
+ 1e-2, // double dist_th
+ 2, // int track_length
+ 4.5 * dt, // double time_span
+ false); // bool init_landmarks & loop closure
+}
+
+TEST_F(ProcessorLandmarkExternalTest, PO_2d_type)
+{
+ testConfiguration(2, // int dim
+ true, // bool orientation
+ 1, // int mode
+ 0, // double quality_th
+ 1e-2, // double dist_th
+ 2, // int track_length
+ 4.5 * dt, // double time_span
+ false); // bool init_landmarks & loop closure
+}
+
+TEST_F(ProcessorLandmarkExternalTest, PO_2d_nothing)
+{
+ testConfiguration(2, // int dim
+ true, // bool orientation
+ 2, // int mode
+ 0, // double quality_th
+ 1e-2, // double dist_th
+ 2, // int track_length
+ 4.5 * dt, // double time_span
+ false); // bool init_landmarks & loop closure
+}
+
+TEST_F(ProcessorLandmarkExternalTest, PO_2d_mixed)
+{
+ testConfiguration(2, // int dim
+ true, // bool orientation
+ 3, // int mode
+ 0, // double quality_th
+ 1e-2, // double dist_th
+ 2, // int track_length
+ 4.5 * dt, // double time_span
+ false); // bool init_landmarks & loop closure
+}
+
+TEST_F(ProcessorLandmarkExternalTest, PO_2d_changing)
+{
+ testConfiguration(2, // int dim
+ true, // bool orientation
+ 4, // int mode
+ 0, // double quality_th
+ 1e-2, // double dist_th
+ 2, // int track_length
+ 4.5 * dt, // double time_span
+ false); // bool init_landmarks & loop closure
+}
+
+TEST_F(ProcessorLandmarkExternalTest, PO_2d_quality_id)
+{
+ testConfiguration(2, // int dim
+ true, // bool orientation
+ 0, // int mode
+ 0.3, // double quality_th
+ 1e-2, // double dist_th
+ 6, // int track_length
+ 4.5 * dt, // double time_span
+ false); // bool init_landmarks & loop closure
+}
+
+TEST_F(ProcessorLandmarkExternalTest, PO_2d_quality_type)
+{
+ testConfiguration(2, // int dim
+ true, // bool orientation
+ 1, // int mode
+ 0.3, // double quality_th
+ 1e-2, // double dist_th
+ 6, // int track_length
+ 4.5 * dt, // double time_span
+ false); // bool init_landmarks & loop closure
+}
+
+TEST_F(ProcessorLandmarkExternalTest, PO_2d_quality_nothing)
+{
+ testConfiguration(2, // int dim
+ true, // bool orientation
+ 2, // int mode
+ 0.3, // double quality_th
+ 1e-2, // double dist_th
+ 6, // int track_length
+ 4.5 * dt, // double time_span
+ false); // bool init_landmarks & loop closure
+}
+
+TEST_F(ProcessorLandmarkExternalTest, PO_2d_quality_mixed)
+{
+ testConfiguration(2, // int dim
+ true, // bool orientation
+ 3, // int mode
+ 0.3, // double quality_th
+ 1e-2, // double dist_th
+ 6, // int track_length
+ 4.5 * dt, // double time_span
+ false); // bool init_landmarks & loop closure
+}
+
+TEST_F(ProcessorLandmarkExternalTest, PO_2d_quality_changing)
+{
+ testConfiguration(2, // int dim
+ true, // bool orientation
+ 4, // int mode
+ 0.3, // double quality_th
+ 1e-2, // double dist_th
+ 6, // int track_length
+ 4.5 * dt, // double time_span
+ false); // bool init_landmarks & loop closure
+}
+
+//-------------------- Position in 3D --------------------
+TEST_F(ProcessorLandmarkExternalTest, P_3d_loop_closure_id)
+{
+ testConfiguration(3, // int dim
+ false, // bool orientation
+ 0, // int mode
+ 0, // double quality_th
+ 1e-2, // double dist_th
+ 6, // int track_length
+ 4.5 * dt, // double time_span
+ true); // bool init_landmarks & loop closure
+}
+
+TEST_F(ProcessorLandmarkExternalTest, P_3d_loop_closure_type)
+{
+ testConfiguration(3, // int dim
+ false, // bool orientation
+ 1, // int mode
+ 0, // double quality_th
+ 1e-2, // double dist_th
+ 6, // int track_length
+ 4.5 * dt, // double time_span
+ true); // bool init_landmarks & loop closure
+}
+
+TEST_F(ProcessorLandmarkExternalTest, P_3d_loop_closure_mixed)
+{
+ testConfiguration(3, // int dim
+ false, // bool orientation
+ 3, // int mode
+ 0, // double quality_th
+ 1e-2, // double dist_th
+ 6, // int track_length
+ 4.5 * dt, // double time_span
+ true); // bool init_landmarks & loop closure
+}
+
+TEST_F(ProcessorLandmarkExternalTest, P_3d_id)
+{
+ testConfiguration(3, // int dim
+ false, // bool orientation
+ 0, // int mode
+ 0, // double quality_th
+ 1e-2, // double dist_th
+ 2, // int track_length
+ 4.5 * dt, // double time_span
+ false); // bool init_landmarks & loop closure
+}
+
+TEST_F(ProcessorLandmarkExternalTest, P_3d_type)
+{
+ testConfiguration(3, // int dim
+ false, // bool orientation
+ 1, // int mode
+ 0, // double quality_th
+ 1e-2, // double dist_th
+ 2, // int track_length
+ 4.5 * dt, // double time_span
+ false); // bool init_landmarks & loop closure
+}
+
+TEST_F(ProcessorLandmarkExternalTest, P_3d_nothing)
+{
+ testConfiguration(3, // int dim
+ false, // bool orientation
+ 2, // int mode
+ 0, // double quality_th
+ 1e-2, // double dist_th
+ 2, // int track_length
+ 4.5 * dt, // double time_span
+ false); // bool init_landmarks & loop closure
+}
+
+TEST_F(ProcessorLandmarkExternalTest, P_3d_mixed)
+{
+ testConfiguration(3, // int dim
+ false, // bool orientation
+ 3, // int mode
+ 0, // double quality_th
+ 1e-2, // double dist_th
+ 2, // int track_length
+ 4.5 * dt, // double time_span
+ false); // bool init_landmarks & loop closure
+}
+
+TEST_F(ProcessorLandmarkExternalTest, P_3d_changing)
+{
+ testConfiguration(3, // int dim
+ false, // bool orientation
+ 4, // int mode
+ 0, // double quality_th
+ 1e-2, // double dist_th
+ 2, // int track_length
+ 4.5 * dt, // double time_span
+ false); // bool init_landmarks & loop closure
+}
+
+TEST_F(ProcessorLandmarkExternalTest, P_3d_quality_id)
+{
+ testConfiguration(3, // int dim
+ false, // bool orientation
+ 0, // int mode
+ 0.3, // double quality_th
+ 1e-2, // double dist_th
+ 6, // int track_length
+ 4.5 * dt, // double time_span
+ false); // bool init_landmarks & loop closure
+}
+
+TEST_F(ProcessorLandmarkExternalTest, P_3d_quality_type)
+{
+ testConfiguration(3, // int dim
+ false, // bool orientation
+ 1, // int mode
+ 0.3, // double quality_th
+ 1e-2, // double dist_th
+ 6, // int track_length
+ 4.5 * dt, // double time_span
+ false); // bool init_landmarks & loop closure
+}
+
+TEST_F(ProcessorLandmarkExternalTest, P_3d_quality_nothing)
+{
+ testConfiguration(3, // int dim
+ false, // bool orientation
+ 2, // int mode
+ 0.3, // double quality_th
+ 1e-2, // double dist_th
+ 6, // int track_length
+ 4.5 * dt, // double time_span
+ false); // bool init_landmarks & loop closure
+}
+
+TEST_F(ProcessorLandmarkExternalTest, P_3d_quality_mixed)
+{
+ testConfiguration(3, // int dim
+ false, // bool orientation
+ 3, // int mode
+ 0.3, // double quality_th
+ 1e-2, // double dist_th
+ 6, // int track_length
+ 4.5 * dt, // double time_span
+ false); // bool init_landmarks & loop closure
+}
+
+TEST_F(ProcessorLandmarkExternalTest, P_3d_quality_changing)
+{
+ testConfiguration(3, // int dim
+ false, // bool orientation
+ 4, // int mode
+ 0.3, // double quality_th
+ 1e-2, // double dist_th
+ 6, // int track_length
+ 4.5 * dt, // double time_span
+ false); // bool init_landmarks & loop closure
+}
+
+//-------------------- Position & Orientation in 3D --------------------
+TEST_F(ProcessorLandmarkExternalTest, PO_3d_loop_closure_id)
+{
+ testConfiguration(3, // int dim
+ true, // bool orientation
+ 0, // int mode
+ 0, // double quality_th
+ 1e-2, // double dist_th
+ 6, // int track_length
+ 4.5 * dt, // double time_span
+ true); // bool init_landmarks & loop closure
+}
+
+TEST_F(ProcessorLandmarkExternalTest, PO_3d_loop_closure_type)
+{
+ testConfiguration(3, // int dim
+ true, // bool orientation
+ 1, // int mode
+ 0, // double quality_th
+ 1e-2, // double dist_th
+ 6, // int track_length
+ 4.5 * dt, // double time_span
+ true); // bool init_landmarks & loop closure
+}
+
+TEST_F(ProcessorLandmarkExternalTest, PO_3d_loop_closure_mixed)
+{
+ testConfiguration(3, // int dim
+ true, // bool orientation
+ 3, // int mode
+ 0, // double quality_th
+ 1e-2, // double dist_th
+ 6, // int track_length
+ 4.5 * dt, // double time_span
+ true); // bool init_landmarks & loop closure
+}
+
+TEST_F(ProcessorLandmarkExternalTest, PO_3d_id)
+{
+ testConfiguration(3, // int dim
+ true, // bool orientation
+ 0, // int mode
+ 0, // double quality_th
+ 1e-2, // double dist_th
+ 2, // int track_length
+ 4.5 * dt, // double time_span
+ false); // bool init_landmarks & loop closure
+}
+
+TEST_F(ProcessorLandmarkExternalTest, PO_3d_type)
+{
+ testConfiguration(3, // int dim
+ true, // bool orientation
+ 1, // int mode
+ 0, // double quality_th
+ 1e-2, // double dist_th
+ 2, // int track_length
+ 4.5 * dt, // double time_span
+ false); // bool init_landmarks & loop closure
+}
+
+TEST_F(ProcessorLandmarkExternalTest, PO_3d_nothing)
+{
+ testConfiguration(3, // int dim
+ true, // bool orientation
+ 2, // int mode
+ 0, // double quality_th
+ 1e-2, // double dist_th
+ 2, // int track_length
+ 4.5 * dt, // double time_span
+ false); // bool init_landmarks & loop closure
+}
+
+TEST_F(ProcessorLandmarkExternalTest, PO_3d_mixed)
+{
+ testConfiguration(3, // int dim
+ true, // bool orientation
+ 3, // int mode
+ 0, // double quality_th
+ 1e-2, // double dist_th
+ 2, // int track_length
+ 4.5 * dt, // double time_span
+ false); // bool init_landmarks & loop closure
+}
+
+TEST_F(ProcessorLandmarkExternalTest, PO_3d_changing)
+{
+ testConfiguration(3, // int dim
+ true, // bool orientation
+ 4, // int mode
+ 0, // double quality_th
+ 1e-2, // double dist_th
+ 2, // int track_length
+ 4.5 * dt, // double time_span
+ false); // bool init_landmarks & loop closure
+}
+
+TEST_F(ProcessorLandmarkExternalTest, PO_3d_quality_id)
+{
+ testConfiguration(3, // int dim
+ true, // bool orientation
+ 0, // int mode
+ 0.3, // double quality_th
+ 1e-2, // double dist_th
+ 6, // int track_length
+ 4.5 * dt, // double time_span
+ false); // bool init_landmarks & loop closure
+}
+
+TEST_F(ProcessorLandmarkExternalTest, PO_3d_quality_type)
+{
+ testConfiguration(3, // int dim
+ true, // bool orientation
+ 1, // int mode
+ 0.3, // double quality_th
+ 1e-2, // double dist_th
+ 6, // int track_length
+ 4.5 * dt, // double time_span
+ false); // bool init_landmarks & loop closure
+}
+
+TEST_F(ProcessorLandmarkExternalTest, PO_3d_quality_nothing)
+{
+ testConfiguration(3, // int dim
+ true, // bool orientation
+ 2, // int mode
+ 0.3, // double quality_th
+ 1e-2, // double dist_th
+ 6, // int track_length
+ 4.5 * dt, // double time_span
+ false); // bool init_landmarks & loop closure
+}
+
+TEST_F(ProcessorLandmarkExternalTest, PO_3d_quality_mixed)
+{
+ testConfiguration(3, // int dim
+ true, // bool orientation
+ 3, // int mode
+ 0.3, // double quality_th
+ 1e-2, // double dist_th
+ 6, // int track_length
+ 4.5 * dt, // double time_span
+ false); // bool init_landmarks & loop closure
+}
+
+TEST_F(ProcessorLandmarkExternalTest, PO_3d_quality_changing)
+{
+ testConfiguration(3, // int dim
+ true, // bool orientation
+ 4, // int mode
+ 0.3, // double quality_th
+ 1e-2, // double dist_th
+ 6, // int track_length
+ 4.5 * dt, // double time_span
+ false); // bool init_landmarks & loop closure
+}
-int main(int argc, char **argv)
+int main(int argc, char** argv)
{
- testing::InitGoogleTest(&argc, argv);
- return RUN_ALL_TESTS();
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
}
diff --git a/test/gtest_processor_tracker_two_processors_one_sensor.cpp b/test/gtest_processor_tracker_two_processors_one_sensor.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..7dc88ebcc2a9b379c867ad9d8c1f1a825f0e1145
--- /dev/null
+++ b/test/gtest_processor_tracker_two_processors_one_sensor.cpp
@@ -0,0 +1,237 @@
+//--------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--------
+// wolf includes
+#include "core/utils/utils_gtest.h"
+#include "core/sensor/factory_sensor.h"
+#include "dummy/processor_tracker_feature_dummy.h"
+#include "core/capture/capture_void.h"
+
+using namespace wolf;
+
+std::string wolf_root = _WOLF_ROOT_DIR;
+
+WOLF_PTR_TYPEDEFS(ProcessorTrackerFeatureDummyDummy);
+
+class ProcessorTrackerFeatureDummyDummy : public ProcessorTrackerFeatureDummy
+{
+ public:
+ ProcessorTrackerFeatureDummyDummy(ParamsProcessorTrackerFeatureDummyPtr& _params)
+ : ProcessorTrackerFeatureDummy(_params)
+ {
+ }
+
+ void setLast(CaptureBasePtr _last_ptr)
+ {
+ last_ptr_ = _last_ptr;
+ }
+ void setInc(CaptureBasePtr _incoming_ptr)
+ {
+ incoming_ptr_ = _incoming_ptr;
+ }
+
+ unsigned int callProcessKnown()
+ {
+ return this->processKnown();
+ }
+
+ unsigned int callProcessNew(const int& _max_new_features)
+ {
+ return this->processNew(_max_new_features);
+ }
+
+ unsigned int callDetectNewFeatures(const int& _max_features,
+ const CaptureBasePtr& _capture,
+ FeatureBasePtrList& _features_out)
+ {
+ return this->detectNewFeatures(_max_features, _capture, _features_out);
+ }
+
+ unsigned int callTrackFeatures(const FeatureBasePtrList& _features_in,
+ const CaptureBasePtr& _capture,
+ FeatureBasePtrList& _features_out,
+ FeatureMatchMap& _feature_correspondences)
+ {
+ return this->trackFeatures(_features_in, _capture, _features_out, _feature_correspondences);
+ }
+
+ FactorBasePtr callEmplaceFactor(FeatureBasePtr _feature_ptr, FeatureBasePtr _feature_other_ptr)
+ {
+ return this->emplaceFactor(_feature_ptr, _feature_other_ptr);
+ }
+
+ void callEstablishFactors()
+ {
+ this->establishFactors();
+ }
+
+ TrackMatrix getTrackMatrix()
+ {
+ return track_matrix_;
+ }
+
+ FeatureMatchMap getMatchesLastFromIncoming()
+ {
+ return matches_last_from_incoming_;
+ }
+
+ void callReset()
+ {
+ this->resetDerived();
+ origin_ptr_ = last_ptr_;
+ last_ptr_ = incoming_ptr_;
+ incoming_ptr_ = nullptr;
+ };
+};
+
+// Use the following in case you want to initialize tests with predefines variables or methods.
+class ProcessorTrackerFeatureDummyDoubledTest : public testing::Test
+{
+ public:
+ ProblemPtr problem;
+ SensorBasePtr sensor;
+ ParamsProcessorTrackerFeatureDummyPtr params;
+ ProcessorTrackerFeatureDummyDummyPtr processor_1, processor_2;
+
+ // ~ProcessorTrackerFeatureDummyDoubledTest() override{}
+
+ void SetUp() override
+ {
+ // Wolf problem
+ problem = Problem::create("PO", 2);
+
+ // Install camera
+ sensor = problem->installSensor("SensorOdom2d",
+ "dummy sensor",
+ (Eigen::Vector3d() << 0, 0, 0).finished(),
+ std::make_shared<ParamsSensorBase>());
+
+ // Install processor
+ params = std::make_shared<ParamsProcessorTrackerFeatureDummy>();
+ params->max_new_features = 10;
+ params->min_features_for_keyframe = 7;
+ params->time_tolerance = 0.5;
+ params->voting_active = true;
+ params->n_tracks_lost = 2; // 1 (the first) track is lost each time trackFeatures is called
+ processor_1 = std::make_shared<ProcessorTrackerFeatureDummyDummy>(params);
+ processor_1->setName("Processor 1");
+ processor_1->link(sensor);
+ processor_2 = std::make_shared<ProcessorTrackerFeatureDummyDummy>(params);
+ processor_2->setName("Processor 2");
+ processor_2->link(sensor);
+
+ problem->print(4, 1, 1, 1);
+ }
+};
+
+// TEST_F(ProcessorTrackerFeatureDummyDoubledTest, installProcessor)
+// {
+// ASSERT_EQ(processor_1->getProblem(), problem);
+// ASSERT_EQ(processor_2->getProblem(), problem);
+// ASSERT_TRUE(problem->check(0));
+// }
+
+// TEST_F(ProcessorTrackerFeatureDummyDoubledTest, process_one_prc)
+// {
+// // Create a capture
+// CaptureBasePtr capture = std::make_shared<CaptureVoid>(0, sensor);
+
+// // Process by one processor
+// processor_1->captureCallback(capture);
+
+// problem->print(4,1,1,1);
+// }
+
+TEST_F(ProcessorTrackerFeatureDummyDoubledTest, process_once_one_prc_then_other)
+{
+ // Create a capture
+ CaptureBasePtr capture = std::make_shared<CaptureVoid>(0, sensor);
+
+ // Process by one processor
+ processor_1->captureCallback(capture);
+ auto frame_1 = capture->getFrame();
+
+ // Process by the other processor
+ processor_2->captureCallback(capture);
+ auto frame_2 = capture->getFrame();
+
+ problem->print(4, 1, 1, 1);
+
+ ASSERT_EQ(problem->getTrajectory()->getFrameMap().size(), 1);
+
+ ASSERT_EQ(frame_1, frame_2);
+}
+
+TEST_F(ProcessorTrackerFeatureDummyDoubledTest, process_once)
+{
+ // Process by both processors automatically in sequence
+ CaptureBasePtr capture = std::make_shared<CaptureVoid>(0, sensor);
+ capture->process();
+ problem->print(4, 1, 1, 1);
+
+ ASSERT_EQ(problem->getTrajectory()->getFrameMap().size(), 1);
+}
+
+TEST_F(ProcessorTrackerFeatureDummyDoubledTest, process_twice)
+{
+ // Process by both processors automatically in sequence
+
+ CaptureBasePtr capture = std::make_shared<CaptureVoid>(0, sensor);
+ capture->process();
+
+ capture = std::make_shared<CaptureVoid>(1, sensor);
+ capture->process();
+
+ problem->print(4, 1, 1, 1);
+}
+
+TEST_F(ProcessorTrackerFeatureDummyDoubledTest, process_thrice)
+{
+ // Create a capture
+ CaptureBasePtr capture = std::make_shared<CaptureVoid>(0, sensor);
+
+ // Process by both processors automatically in sequence
+ capture->process();
+
+ capture = std::make_shared<CaptureVoid>(1, sensor);
+ capture->process();
+
+ capture = std::make_shared<CaptureVoid>(2, sensor);
+ capture->process();
+
+ problem->print(2, 1, 1, 1);
+}
+
+TEST_F(ProcessorTrackerFeatureDummyDoubledTest, process_loop)
+{
+ for (TimeStamp t(0); t <= 6; t = t + 1)
+ {
+ auto capture = std::make_shared<CaptureVoid>(t, sensor);
+ capture->process();
+ }
+ problem->print(2, 1, 1, 1);
+}
+
+int main(int argc, char** argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/wolf_scripts/license_manager.sh b/wolf_scripts/license_manager.sh
index 2e90fb0ad205c6a94908aecb873fd83ebe8804c6..78adf75422f0e5a70b4513a94f2dd06abdd2c6fc 100755
--- a/wolf_scripts/license_manager.sh
+++ b/wolf_scripts/license_manager.sh
@@ -4,6 +4,7 @@
#
# This script is used for managing the license headers of code files (.h, .c, .cpp, .hpp)
# This file is automatically called by the CI in gitlab.
+echo "==== WOLF license_manager script ===="
line_start_mark="//--------LICENSE_START--------"
line_end_mark="//--------LICENSE_END--------"
@@ -142,4 +143,4 @@ do
( echo ${line_start_mark}$'\n//'; cat ${license}; echo $'//\n'${line_end_mark}; cat $i ) > temp_file
mv temp_file $i
fi
-done
\ No newline at end of file
+done