diff --git a/.clang_ci.yml b/.clang_ci.yml
new file mode 100644
index 0000000000000000000000000000000000000000..3fb42c0165c9863e31486f9e034d875b1311c60b
--- /dev/null
+++ b/.clang_ci.yml
@@ -0,0 +1,23 @@
+.clang_format_script:
+
+ - apt install -y clang-format-12 # available in ubuntu 20.04 or newer
+ - cd $CI_PROJECT_DIR
+
+ # configure git
+ - export CI_NEW_BRANCH=ci_clangformat$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
+
+ # 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 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/.gitlab-ci.yml b/.gitlab-ci.yml
index 6f6c7ad50c46b7362b55828736191d0450e02478..cae9e927e1226d79211ba51127ca2fd5198a3afa 100644
--- a/.gitlab-ci.yml
+++ b/.gitlab-ci.yml
@@ -1,5 +1,7 @@
include:
- '.license_template_ci.yml'
+ - '.yamlschemacpp_ci.yml'
+ - '.clang_ci.yml'
workflow:
rules:
@@ -14,6 +16,8 @@ workflow:
stages:
- license
+ - clang
+ - yaml_templates
- build_and_test
- deploy_plugins
- deploy_ros
@@ -75,24 +79,6 @@ stages:
# create 'ci_deps' folder (if not exists)
- mkdir -pv ci_deps
-.install_yamlschemacpp_template: &install_yamlschemacpp_definition
- - cd ${CI_PROJECT_DIR}/ci_deps
- - 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
- - mkdir -pv build
- - cd build
- - cmake -DCMAKE_BUILD_TYPE=release -DBUILD_TESTS=OFF ..
- - make -j$(nproc)
- - make install
- - ldconfig
-
.build_and_test_template: &build_and_test_definition
- cd $CI_PROJECT_DIR
- mkdir -pv build
@@ -114,6 +100,26 @@ license_header:
script:
- !reference [.license_header_script]
+############ CLANG FORMAT ############
+clang_format:
+ stage: clang
+ image: labrobotica/wolf_deps:20.04
+ before_script:
+ - *preliminaries_definition
+ script:
+ - !reference [.clang_format_script]
+
+############ YAML TEMPLATES GENERATION ############
+yaml_templates_generation:
+ stage: yaml_templates
+ image: labrobotica/wolf_deps:20.04
+ before_script:
+ - *preliminaries_definition
+ script:
+ - !reference [.license_header_script]
+ - !reference [.install_yamlschemacpp_script]
+ - !reference [.generate_yaml_templates_script]
+
############ UBUNTU 18.04 TESTS ############
build_and_test:bionic:
stage: build_and_test
@@ -122,9 +128,10 @@ build_and_test:bionic:
- key: yamlschemacpp-bionic
paths:
- ci_deps/yaml-schema-cpp/
- script:
+ before_script:
- *preliminaries_definition
- - *install_yamlschemacpp_definition
+ script:
+ - !reference [.install_yamlschemacpp_script]
- *build_and_test_definition
############ UBUNTU 20.04 TESTS ############
@@ -135,9 +142,10 @@ build_and_test:focal:
- key: yamlschemacpp-focal
paths:
- ci_deps/yaml-schema-cpp/
- script:
+ before_script:
- *preliminaries_definition
- - *install_yamlschemacpp_definition
+ script:
+ - !reference [.install_yamlschemacpp_script]
- *build_and_test_definition
############ DEPLOY PLUGINS ANY BRANCY EXCEPT FOR main ############
diff --git a/.license_template_ci.yml b/.license_template_ci.yml
index 3d827c7398e9da4850d13245d5810e3c287400e4..7207373b1f48872e6d48cc7b163e3ef0809a6db8 100644
--- a/.license_template_ci.yml
+++ b/.license_template_ci.yml
@@ -5,7 +5,7 @@
- mkdir -pv ci_deps
# configure git
- - export CI_NEW_BRANCH=ci_processing$RANDOM
+ - export CI_NEW_BRANCH=ci_license_header$RANDOM
- echo creating new temporary branch... $CI_NEW_BRANCH
- git config --global user.email "${CI_EMAIL}"
- git config --global user.name "${CI_USERNAME}"
diff --git a/.yamlschemacpp_ci.yml b/.yamlschemacpp_ci.yml
new file mode 100644
index 0000000000000000000000000000000000000000..dd284a5c48c82c5e76d725c3f1e5265e33cf3a1d
--- /dev/null
+++ b/.yamlschemacpp_ci.yml
@@ -0,0 +1,66 @@
+.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:
+
+ # configure git
+ - export CI_NEW_BRANCH=ci_yamlschemacpp$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
+
+ # remove all existint 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}/yaml_templates/$folder/$name.yaml
+ - done <schemas.txt
+
+ # remove list of all schema files
+ - rm schemas.txt
+
+ # push changes (if any)
+ - if git commit -a -m "[skip ci] yaml templates 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 a1e21cf8550613d5e2cac4373ca84abce85fc533..3cd3359159288c7e55baa80a9d601f2f2155989c 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -127,14 +127,12 @@ SET(HDRS_COMMON
include/${PROJECT_NAME}/common/node_state_blocks.h
include/${PROJECT_NAME}/common/time_stamp.h
include/${PROJECT_NAME}/common/wolf.h
- include/${PROJECT_NAME}/common/params_base.h
)
SET(HDRS_COMPOSITE
include/${PROJECT_NAME}/composite/composite.h
include/${PROJECT_NAME}/composite/matrix_composite.h
include/${PROJECT_NAME}/composite/spec_state_composite.h
include/${PROJECT_NAME}/composite/spec_state_sensor_composite.h
- # include/${PROJECT_NAME}/composite/state_composite.h DEPRECATED CLASS
include/${PROJECT_NAME}/composite/type_composite.h
include/${PROJECT_NAME}/composite/vector_composite.h
)
@@ -253,6 +251,7 @@ SET(HDRS_UTILS
include/${PROJECT_NAME}/utils/loader.h
include/${PROJECT_NAME}/utils/logging.h
include/${PROJECT_NAME}/utils/singleton.h
+ include/${PROJECT_NAME}/utils/string_utils.h
include/${PROJECT_NAME}/utils/utils_gtest.h
)
@@ -276,7 +275,6 @@ SET(SRCS_COMPOSITE
src/composite/matrix_composite.cpp
src/composite/spec_state_composite.cpp
src/composite/spec_state_sensor_composite.cpp
- # src/composite/state_composite.cpp DEPRECATED CLASS
src/composite/vector_composite.cpp
)
SET(SRCS_FACTOR
diff --git a/demos/demo_analytic_autodiff_factor.cpp b/demos/demo_analytic_autodiff_factor.cpp
index 8e43ff8fc25922d78db414b5f24107ddc6811df6..48d6c903ee2a609b485c14219e93335cf65cae4f 100644
--- a/demos/demo_analytic_autodiff_factor.cpp
+++ b/demos/demo_analytic_autodiff_factor.cpp
@@ -20,17 +20,17 @@
// Testing creating wolf tree from imported .graph file
-//C includes for sleep, time and main args
+// C includes for sleep, time and main args
#include "unistd.h"
-//std includes
+// std includes
#include <iostream>
#include <memory>
#include <random>
#include <cmath>
#include <queue>
-//Wolf includes
+// Wolf includes
#include "core/ceres_wrapper/solver_ceres.h"
#include "wolf_manager.h"
#include "core/capture/capture_void.h"
@@ -38,25 +38,29 @@
// EIGEN
//#include <Eigen/CholmodSupport>
-namespace wolf {
+namespace wolf
+{
// inserts the sparse matrix 'ins' into the sparse matrix 'original' in the place given by 'row' and 'col' integers
-void insertSparseBlock(const Eigen::SparseMatrix<double>& ins, Eigen::SparseMatrix<double>& original, const unsigned int& row, const unsigned int& col)
+void insertSparseBlock(const Eigen::SparseMatrix<double>& ins,
+ Eigen::SparseMatrix<double>& original,
+ const unsigned int& row,
+ const unsigned int& col)
{
- for (int k=0; k<ins.outerSize(); ++k)
- for (Eigen::SparseMatrix<double>::InnerIterator iti(ins,k); iti; ++iti)
- original.coeffRef(iti.row() + row, iti.col() + col) = iti.value();
+ for (int k = 0; k < ins.outerSize(); ++k)
+ for (Eigen::SparseMatrix<double>::InnerIterator iti(ins, k); iti; ++iti)
+ original.coeffRef(iti.row() + row, iti.col() + col) = iti.value();
- original.makeCompressed();
+ original.makeCompressed();
}
-int main(int argc, char** argv)
+int main(int argc, char** argv)
{
using namespace wolf;
- //Welcome message
+ // Welcome message
std::cout << std::endl << " ========= WOLF IMPORTED .graph TEST ===========" << std::endl << std::endl;
- if (argc != 3 || atoi(argv[2]) < 0 )
+ if (argc != 3 || atoi(argv[2]) < 0)
{
std::cout << "Please call me with: [./test_wolf_imported_graph FILE_PATH MAX_VERTICES], where:" << std::endl;
std::cout << " FILE_PATH is the .graph file path" << std::endl;
@@ -66,10 +70,10 @@ int main(int argc, char** argv)
}
// auxiliar variables
- std::string file_path_ = argv[1];
+ std::string file_path_ = argv[1];
unsigned int MAX_VERTEX = atoi(argv[2]);
if (MAX_VERTEX == 0) MAX_VERTEX = 1e6;
- std::ifstream offLineFile_;
+ std::ifstream offLineFile_;
ceres::Solver::Summary summary_autodiff, summary_analytic;
// loading variables
@@ -77,212 +81,220 @@ int main(int argc, char** argv)
std::map<unsigned int, FrameBasePtr> index_2_frame_ptr_analytic;
// Wolf problem
- ProblemPtr wolf_problem_autodiff = new Problem(FRM_PO_2d);
- ProblemPtr wolf_problem_analytic = new Problem(FRM_PO_2d);
- SensorBasePtr sensor = new SensorBase("ODOM 2d", std::make_shared<StatePoint2d>(Eigen::VectorXd::Zero(2)), std::make_shared<StateAngle>(0), std::make_shared<StateParams2>(Eigen::VectorXd::Zero(2)), 2);
+ ProblemPtr wolf_problem_autodiff = new Problem(FRM_PO_2d);
+ ProblemPtr wolf_problem_analytic = new Problem(FRM_PO_2d);
+ SensorBasePtr sensor = new SensorBase("ODOM 2d",
+ std::make_shared<StatePoint2d>(Eigen::VectorXd::Zero(2)),
+ std::make_shared<StateAngle>(0),
+ std::make_shared<StateParams2>(Eigen::VectorXd::Zero(2)),
+ 2);
// Ceres wrapper
- SolverCeres* ceres_manager_autodiff = new SolverCeres(wolf_problem_autodiff, ceres_options);
- SolverCeres* ceres_manager_analytic = new SolverCeres(wolf_problem_analytic, ceres_options);
- ceres_manager_autodiff.getSolverOptions().minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;LINE_SEARCH
+ SolverCeres* ceres_manager_autodiff = new SolverCeres(wolf_problem_autodiff, ceres_options);
+ SolverCeres* ceres_manager_analytic = new SolverCeres(wolf_problem_analytic, ceres_options);
+ ceres_manager_autodiff.getSolverOptions().minimizer_type = ceres::TRUST_REGION; // ceres::TRUST_REGION;LINE_SEARCH
ceres_manager_autodiff.getSolverOptions().max_line_search_step_contraction = 1e-3;
- ceres_manager_autodiff.getSolverOptions().max_num_iterations = 1e4;
- ceres_manager_analytic.getSolverOptions().minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;LINE_SEARCH
+ ceres_manager_autodiff.getSolverOptions().max_num_iterations = 1e4;
+ ceres_manager_analytic.getSolverOptions().minimizer_type = ceres::TRUST_REGION; // ceres::TRUST_REGION;LINE_SEARCH
ceres_manager_analytic.getSolverOptions().max_line_search_step_contraction = 1e-3;
- ceres_manager_analytic.getSolverOptions().max_num_iterations = 1e4;
+ ceres_manager_analytic.getSolverOptions().max_num_iterations = 1e4;
// load graph from .txt
offLineFile_.open(file_path_.c_str(), std::ifstream::in);
if (offLineFile_.is_open())
{
- std::string buffer;
+ std::string buffer;
unsigned int j = 0;
// Line by line
while (std::getline(offLineFile_, buffer))
{
- //std::cout << "new line:" << buffer << std::endl;
- std::string bNum;
+ // std::cout << "new line:" << buffer << std::endl;
+ std::string bNum;
unsigned int i = 0;
// VERTEX
if (buffer.at(0) == 'V')
{
- //skip rest of VERTEX word
+ // skip rest of VERTEX word
while (buffer.at(i) != ' ') i++;
- //skip white spaces
+ // skip white spaces
while (buffer.at(i) == ' ') i++;
- //vertex index
- while (buffer.at(i) != ' ')
- bNum.push_back(buffer.at(i++));
+ // vertex index
+ while (buffer.at(i) != ' ') bNum.push_back(buffer.at(i++));
unsigned int vertex_index = atoi(bNum.c_str());
bNum.clear();
- if (vertex_index <= MAX_VERTEX+1)
+ if (vertex_index <= MAX_VERTEX + 1)
{
- //skip white spaces
+ // skip white spaces
while (buffer.at(i) == ' ') i++;
// vertex pose
Eigen::Vector3d vertex_pose;
// x
- while (buffer.at(i) != ' ')
- bNum.push_back(buffer.at(i++));
+ while (buffer.at(i) != ' ') bNum.push_back(buffer.at(i++));
vertex_pose(0) = atof(bNum.c_str());
bNum.clear();
- //skip white spaces
+ // skip white spaces
while (buffer.at(i) == ' ') i++;
// y
- while (buffer.at(i) != ' ')
- bNum.push_back(buffer.at(i++));
+ while (buffer.at(i) != ' ') bNum.push_back(buffer.at(i++));
vertex_pose(1) = atof(bNum.c_str());
bNum.clear();
- //skip white spaces
+ // skip white spaces
while (buffer.at(i) == ' ') i++;
// theta
- while (i < buffer.size() && buffer.at(i) != ' ')
- bNum.push_back(buffer.at(i++));
+ while (i < buffer.size() && buffer.at(i) != ' ') bNum.push_back(buffer.at(i++));
vertex_pose(2) = atof(bNum.c_str());
bNum.clear();
// add frame to problem
- FrameBasePtr vertex_frame_ptr_autodiff = new FrameBase(TimeStamp(0), std::make_shared<StatePoint2d>(vertex_pose.head(2)), std::make_shared<StateAngle>(vertex_pose.tail(1)));
- FrameBasePtr vertex_frame_ptr_analytic = new FrameBase(TimeStamp(0), std::make_shared<StatePoint2d>(vertex_pose.head(2)), std::make_shared<StateAngle>(vertex_pose.tail(1)));
+ FrameBasePtr vertex_frame_ptr_autodiff =
+ new FrameBase(TimeStamp(0),
+ std::make_shared<StatePoint2d>(vertex_pose.head(2)),
+ std::make_shared<StateAngle>(vertex_pose.tail(1)));
+ FrameBasePtr vertex_frame_ptr_analytic =
+ new FrameBase(TimeStamp(0),
+ std::make_shared<StatePoint2d>(vertex_pose.head(2)),
+ std::make_shared<StateAngle>(vertex_pose.tail(1)));
wolf_problem_autodiff->getTrajectory()->addFrame(vertex_frame_ptr_autodiff);
wolf_problem_analytic->getTrajectory()->addFrame(vertex_frame_ptr_analytic);
// store
index_2_frame_ptr_autodiff[vertex_index] = vertex_frame_ptr_autodiff;
index_2_frame_ptr_analytic[vertex_index] = vertex_frame_ptr_analytic;
- //std::cout << "Added vertex! index: " << vertex_index << " id: " << vertex_frame_ptr_analytic->id() << std::endl << "pose: " << vertex_pose.transpose() << std::endl;
+ // std::cout << "Added vertex! index: " << vertex_index << " id: " <<
+ // vertex_frame_ptr_analytic->id() << std::endl << "pose: " << vertex_pose.transpose() <<
+ // std::endl;
}
}
// EDGE
else if (buffer.at(0) == 'E')
{
j++;
- //skip rest of EDGE word
+ // skip rest of EDGE word
while (buffer.at(i) != ' ') i++;
- //skip white spaces
+ // skip white spaces
while (buffer.at(i) == ' ') i++;
- //from
- while (buffer.at(i) != ' ')
- bNum.push_back(buffer.at(i++));
+ // from
+ while (buffer.at(i) != ' ') bNum.push_back(buffer.at(i++));
unsigned int edge_old = atoi(bNum.c_str());
bNum.clear();
- //skip white spaces
+ // skip white spaces
while (buffer.at(i) == ' ') i++;
- //to index
- while (buffer.at(i) != ' ')
- bNum.push_back(buffer.at(i++));
+ // to index
+ while (buffer.at(i) != ' ') bNum.push_back(buffer.at(i++));
unsigned int edge_new = atoi(bNum.c_str());
bNum.clear();
- if (edge_new <= MAX_VERTEX+1 && edge_old <= MAX_VERTEX+1 )
+ if (edge_new <= MAX_VERTEX + 1 && edge_old <= MAX_VERTEX + 1)
{
- //skip white spaces
+ // skip white spaces
while (buffer.at(i) == ' ') i++;
// edge vector
Eigen::Vector3d edge_vector;
// x
- while (buffer.at(i) != ' ')
- bNum.push_back(buffer.at(i++));
+ while (buffer.at(i) != ' ') bNum.push_back(buffer.at(i++));
edge_vector(0) = atof(bNum.c_str());
bNum.clear();
- //skip white spaces
+ // skip white spaces
while (buffer.at(i) == ' ') i++;
// y
- while (buffer.at(i) != ' ')
- bNum.push_back(buffer.at(i++));
+ while (buffer.at(i) != ' ') bNum.push_back(buffer.at(i++));
edge_vector(1) = atof(bNum.c_str());
bNum.clear();
- //skip white spaces
+ // skip white spaces
while (buffer.at(i) == ' ') i++;
// theta
- while (buffer.at(i) != ' ')
- bNum.push_back(buffer.at(i++));
+ while (buffer.at(i) != ' ') bNum.push_back(buffer.at(i++));
edge_vector(2) = atof(bNum.c_str());
bNum.clear();
- //skip white spaces
+ // skip white spaces
while (buffer.at(i) == ' ') i++;
// edge covariance
Eigen::Matrix3d edge_information;
// xx
- while (buffer.at(i) != ' ')
- bNum.push_back(buffer.at(i++));
- edge_information(0,0) = atof(bNum.c_str());
+ while (buffer.at(i) != ' ') bNum.push_back(buffer.at(i++));
+ edge_information(0, 0) = atof(bNum.c_str());
bNum.clear();
- //skip white spaces
+ // skip white spaces
while (buffer.at(i) == ' ') i++;
// xy
- while (buffer.at(i) != ' ')
- bNum.push_back(buffer.at(i++));
- edge_information(0,1) = atof(bNum.c_str());
- edge_information(1,0) = atof(bNum.c_str());
+ while (buffer.at(i) != ' ') bNum.push_back(buffer.at(i++));
+ edge_information(0, 1) = atof(bNum.c_str());
+ edge_information(1, 0) = atof(bNum.c_str());
bNum.clear();
- //skip white spaces
+ // skip white spaces
while (buffer.at(i) == ' ') i++;
// yy
- while (buffer.at(i) != ' ')
- bNum.push_back(buffer.at(i++));
- edge_information(1,1) = atof(bNum.c_str());
+ while (buffer.at(i) != ' ') bNum.push_back(buffer.at(i++));
+ edge_information(1, 1) = atof(bNum.c_str());
bNum.clear();
- //skip white spaces
+ // skip white spaces
while (buffer.at(i) == ' ') i++;
// thetatheta
- while (buffer.at(i) != ' ')
- bNum.push_back(buffer.at(i++));
- edge_information(2,2) = atof(bNum.c_str());
+ while (buffer.at(i) != ' ') bNum.push_back(buffer.at(i++));
+ edge_information(2, 2) = atof(bNum.c_str());
bNum.clear();
- //skip white spaces
+ // skip white spaces
while (buffer.at(i) == ' ') i++;
// xtheta
- while (buffer.at(i) != ' ')
- bNum.push_back(buffer.at(i++));
- edge_information(0,2) = atof(bNum.c_str());
- edge_information(2,0) = atof(bNum.c_str());
+ while (buffer.at(i) != ' ') bNum.push_back(buffer.at(i++));
+ edge_information(0, 2) = atof(bNum.c_str());
+ edge_information(2, 0) = atof(bNum.c_str());
bNum.clear();
- //skip white spaces
+ // skip white spaces
while (buffer.at(i) == ' ') i++;
// ytheta
- while (i < buffer.size() && buffer.at(i) != ' ')
- bNum.push_back(buffer.at(i++));
- edge_information(1,2) = atof(bNum.c_str());
- edge_information(2,1) = atof(bNum.c_str());
+ while (i < buffer.size() && buffer.at(i) != ' ') bNum.push_back(buffer.at(i++));
+ edge_information(1, 2) = atof(bNum.c_str());
+ edge_information(2, 1) = atof(bNum.c_str());
bNum.clear();
// add capture, feature and factor to problem
- FeatureBasePtr feature_ptr_autodiff = new FeatureBase("POSE", edge_vector, edge_information.inverse());
+ FeatureBasePtr feature_ptr_autodiff =
+ new FeatureBase("POSE", edge_vector, edge_information.inverse());
CaptureVoid* capture_ptr_autodiff = new CaptureVoid(TimeStamp(0), sensor);
- assert(index_2_frame_ptr_autodiff.find(edge_old) != index_2_frame_ptr_autodiff.end() && "edge from vertex not added!");
+ assert(index_2_frame_ptr_autodiff.find(edge_old) != index_2_frame_ptr_autodiff.end() &&
+ "edge from vertex not added!");
FrameBasePtr frame_old_ptr_autodiff = index_2_frame_ptr_autodiff[edge_old];
- assert(index_2_frame_ptr_autodiff.find(edge_new) != index_2_frame_ptr_autodiff.end() && "edge to vertex not added!");
+ assert(index_2_frame_ptr_autodiff.find(edge_new) != index_2_frame_ptr_autodiff.end() &&
+ "edge to vertex not added!");
FrameBasePtr frame_new_ptr_autodiff = index_2_frame_ptr_autodiff[edge_new];
frame_new_ptr_autodiff->addCapture(capture_ptr_autodiff);
capture_ptr_autodiff->addFeature(feature_ptr_autodiff);
FactorOdom2d* factor_ptr_autodiff = new FactorOdom2d(feature_ptr_autodiff, frame_old_ptr_autodiff);
feature_ptr_autodiff->addFactor(factor_ptr_autodiff);
- //std::cout << "Added autodiff edge! " << factor_ptr_autodiff->id() << " from vertex " << factor_ptr_autodiff->getCapture()->getFrame()->id() << " to " << factor_ptr_autodiff->getFrameOther()->id() << std::endl;
+ // std::cout << "Added autodiff edge! " << factor_ptr_autodiff->id() << " from vertex " <<
+ // factor_ptr_autodiff->getCapture()->getFrame()->id() << " to " <<
+ // factor_ptr_autodiff->getFrameOther()->id() << std::endl;
// add capture, feature and factor to problem
- FeatureBasePtr feature_ptr_analytic = new FeatureBase("POSE", edge_vector, edge_information.inverse());
+ FeatureBasePtr feature_ptr_analytic =
+ new FeatureBase("POSE", edge_vector, edge_information.inverse());
CaptureVoid* capture_ptr_analytic = new CaptureVoid(TimeStamp(0), sensor);
- assert(index_2_frame_ptr_analytic.find(edge_old) != index_2_frame_ptr_analytic.end() && "edge from vertex not added!");
+ assert(index_2_frame_ptr_analytic.find(edge_old) != index_2_frame_ptr_analytic.end() &&
+ "edge from vertex not added!");
FrameBasePtr frame_old_ptr_analytic = index_2_frame_ptr_analytic[edge_old];
- assert(index_2_frame_ptr_analytic.find(edge_new) != index_2_frame_ptr_analytic.end() && "edge to vertex not added!");
+ assert(index_2_frame_ptr_analytic.find(edge_new) != index_2_frame_ptr_analytic.end() &&
+ "edge to vertex not added!");
FrameBasePtr frame_new_ptr_analytic = index_2_frame_ptr_analytic[edge_new];
frame_new_ptr_analytic->addCapture(capture_ptr_analytic);
capture_ptr_analytic->addFeature(feature_ptr_analytic);
- FactorOdom2dAnalytic* factor_ptr_analytic = new FactorOdom2dAnalytic(feature_ptr_analytic, frame_old_ptr_analytic);
+ FactorOdom2dAnalytic* factor_ptr_analytic =
+ new FactorOdom2dAnalytic(feature_ptr_analytic, frame_old_ptr_analytic);
feature_ptr_analytic->addFactor(factor_ptr_analytic);
- //std::cout << "Added analytic edge! " << factor_ptr_analytic->id() << " from vertex " << factor_ptr_analytic->getCapture()->getFrame()->id() << " to " << factor_ptr_analytic->getFrameOther()->id() << std::endl;
- //std::cout << "vector " << factor_ptr_analytic->getMeasurement().transpose() << std::endl;
- //std::cout << "information " << std::endl << edge_information << std::endl;
- //std::cout << "covariance " << std::endl << factor_ptr_analytic->getMeasurementCovariance() << std::endl;
+ // std::cout << "Added analytic edge! " << factor_ptr_analytic->id() << " from vertex " <<
+ // factor_ptr_analytic->getCapture()->getFrame()->id() << " to " <<
+ // factor_ptr_analytic->getFrameOther()->id() << std::endl; std::cout << "vector " <<
+ // factor_ptr_analytic->getMeasurement().transpose() << std::endl; std::cout << "information " <<
+ // std::endl << edge_information << std::endl; std::cout << "covariance " << std::endl <<
+ // factor_ptr_analytic->getMeasurementCovariance() << std::endl;
}
}
else
@@ -296,8 +308,16 @@ int main(int argc, char** argv)
// PRIOR
FrameBasePtr first_frame_autodiff = wolf_problem_autodiff->getTrajectory()->getFirstFrame();
FrameBasePtr first_frame_analytic = wolf_problem_analytic->getTrajectory()->getFirstFrame();
- CaptureFix* initial_covariance_autodiff = new CaptureFix(TimeStamp(0), new SensorBase("ABSOLUTE POSE", nullptr, nullptr, nullptr, 0), first_frame_autodiff->getState(), Eigen::Matrix3d::Identity() * 0.01);
- CaptureFix* initial_covariance_analytic = new CaptureFix(TimeStamp(0), new SensorBase("ABSOLUTE POSE", nullptr, nullptr, nullptr, 0), first_frame_analytic->getState(), Eigen::Matrix3d::Identity() * 0.01);
+ CaptureFix* initial_covariance_autodiff =
+ new CaptureFix(TimeStamp(0),
+ new SensorBase("ABSOLUTE POSE", nullptr, nullptr, nullptr, 0),
+ first_frame_autodiff->getState(),
+ Eigen::Matrix3d::Identity() * 0.01);
+ CaptureFix* initial_covariance_analytic =
+ new CaptureFix(TimeStamp(0),
+ new SensorBase("ABSOLUTE POSE", nullptr, nullptr, nullptr, 0),
+ first_frame_analytic->getState(),
+ Eigen::Matrix3d::Identity() * 0.01);
first_frame_autodiff->addCapture(initial_covariance_autodiff);
first_frame_analytic->addCapture(initial_covariance_analytic);
initial_covariance_autodiff->emplaceFeatureAndFactor();
@@ -316,21 +336,21 @@ int main(int argc, char** argv)
std::cout << "computing covariances..." << std::endl;
std::cout << "ANALYTIC -----------------------------------" << std::endl;
clock_t t1 = clock();
- ceres_manager_analytic->computeCovariances(ALL);//ALL_MARGINALS
- std::cout << "Time: " << ((double) clock() - t1) / CLOCKS_PER_SEC << "s" << std::endl;
+ ceres_manager_analytic->computeCovariances(ALL); // ALL_MARGINALS
+ std::cout << "Time: " << ((double)clock() - t1) / CLOCKS_PER_SEC << "s" << std::endl;
std::cout << "AUTODIFF -----------------------------------" << std::endl;
t1 = clock();
- ceres_manager_autodiff->computeCovariances(ALL);//ALL_MARGINALS
- std::cout << "Time: " << ((double) clock() - t1) / CLOCKS_PER_SEC << "s" << std::endl;
+ ceres_manager_autodiff->computeCovariances(ALL); // ALL_MARGINALS
+ std::cout << "Time: " << ((double)clock() - t1) / CLOCKS_PER_SEC << "s" << std::endl;
- delete wolf_problem_autodiff; //not necessary to delete anything more, wolf will do it!
+ delete wolf_problem_autodiff; // not necessary to delete anything more, wolf will do it!
std::cout << "wolf_problem_ deleted!" << std::endl;
delete ceres_manager_autodiff;
delete ceres_manager_analytic;
std::cout << "ceres_manager deleted!" << std::endl;
- //End message
+ // End message
std::cout << " =========================== END ===============================" << std::endl << std::endl;
-
- //exit
+
+ // exit
return 0;
}
diff --git a/demos/demo_wolf_imported_graph.cpp b/demos/demo_wolf_imported_graph.cpp
index 82e3dc3e34d7f2d43858d97e16991ce7b4a0c53a..ae87c1af8f39089fa055086ae031ebfb8facfadd 100644
--- a/demos/demo_wolf_imported_graph.cpp
+++ b/demos/demo_wolf_imported_graph.cpp
@@ -20,17 +20,17 @@
// Testing creating wolf tree from imported .graph file
-//C includes for sleep, time and main args
+// C includes for sleep, time and main args
#include "unistd.h"
-//std includes
+// std includes
#include <iostream>
#include <memory>
#include <random>
#include <cmath>
#include <queue>
-//Wolf includes
+// Wolf includes
#include "ceres_wrapper/solver_ceres.h"
#include "wolf_manager.h"
#include "core/capture/capture_void.h"
@@ -38,25 +38,29 @@
// EIGEN
//#include <Eigen/CholmodSupport>
-namespace wolf{
+namespace wolf
+{
// inserts the sparse matrix 'ins' into the sparse matrix 'original' in the place given by 'row' and 'col' integers
-void insertSparseBlock(const Eigen::SparseMatrix<double>& ins, Eigen::SparseMatrix<double>& original, const unsigned int& row, const unsigned int& col)
+void insertSparseBlock(const Eigen::SparseMatrix<double>& ins,
+ Eigen::SparseMatrix<double>& original,
+ const unsigned int& row,
+ const unsigned int& col)
{
- for (int k=0; k<ins.outerSize(); ++k)
- for (Eigen::SparseMatrix<double>::InnerIterator iti(ins,k); iti; ++iti)
- original.coeffRef(iti.row() + row, iti.col() + col) = iti.value();
+ for (int k = 0; k < ins.outerSize(); ++k)
+ for (Eigen::SparseMatrix<double>::InnerIterator iti(ins, k); iti; ++iti)
+ original.coeffRef(iti.row() + row, iti.col() + col) = iti.value();
- original.makeCompressed();
+ original.makeCompressed();
}
-int main(int argc, char** argv)
+int main(int argc, char** argv)
{
using namespace wolf;
- //Welcome message
+ // Welcome message
std::cout << std::endl << " ========= WOLF IMPORTED .graph TEST ===========" << std::endl << std::endl;
- if (argc != 3 || atoi(argv[2]) < 0 )
+ if (argc != 3 || atoi(argv[2]) < 0)
{
std::cout << "Please call me with: [./test_wolf_imported_graph FILE_PATH MAX_VERTICES], where:" << std::endl;
std::cout << " FILE_PATH is the .graph file path" << std::endl;
@@ -66,14 +70,14 @@ int main(int argc, char** argv)
}
// auxiliar variables
- std::string file_path_ = argv[1];
+ std::string file_path_ = argv[1];
unsigned int MAX_VERTEX = atoi(argv[2]);
if (MAX_VERTEX == 0) MAX_VERTEX = 1e6;
- std::ifstream offLineFile_;
- clock_t t1;
+ std::ifstream offLineFile_;
+ clock_t t1;
ceres::Solver::Summary summary_full, summary_prun;
- Eigen::MatrixXd Sigma_ii(3,3), Sigma_ij(3,3), Sigma_jj(3,3), Sigma_z(3,3), Ji(3,3), Jj(3,3);
- double xi, yi, thi, si, ci, xj, yj;
+ Eigen::MatrixXd Sigma_ii(3, 3), Sigma_ij(3, 3), Sigma_jj(3, 3), Sigma_z(3, 3), Ji(3, 3), Jj(3, 3);
+ double xi, yi, thi, si, ci, xj, yj;
// loading variables
std::map<unsigned int, FrameBasePtr> index_2_frame_ptr_full;
@@ -81,198 +85,198 @@ int main(int argc, char** argv)
std::map<FrameBasePtr, unsigned int> frame_ptr_2_index_prun;
// Wolf problem
- ProblemPtr wolf_problem_full = new Problem(FRM_PO_2d);
- ProblemPtr wolf_problem_prun = new Problem(FRM_PO_2d);
- SensorBasePtr sensor = new SensorBase("ODOM 2d", std::make_shared<StatePoint2d>(Eigen::VectorXd::Zero(2)), std::make_shared<StateAngle>(Eigen::VectorXd::Zero(1)), std::make_shared<StateParams2>(Eigen::VectorXd::Zero(2)), 2);
+ ProblemPtr wolf_problem_full = new Problem(FRM_PO_2d);
+ ProblemPtr wolf_problem_prun = new Problem(FRM_PO_2d);
+ SensorBasePtr sensor = new SensorBase("ODOM 2d",
+ std::make_shared<StatePoint2d>(Eigen::VectorXd::Zero(2)),
+ std::make_shared<StateAngle>(Eigen::VectorXd::Zero(1)),
+ std::make_shared<StateParams2>(Eigen::VectorXd::Zero(2)),
+ 2);
- Eigen::SparseMatrix<double> Lambda(0,0);
+ Eigen::SparseMatrix<double> Lambda(0, 0);
// Ceres wrapper
ceres::Solver::Options ceres_options;
- ceres_options.minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;LINE_SEARCH
+ ceres_options.minimizer_type = ceres::TRUST_REGION; // ceres::TRUST_REGION;LINE_SEARCH
ceres_options.max_line_search_step_contraction = 1e-3;
- ceres_options.max_num_iterations = 1e4;
- SolverCeres* ceres_manager_full = new SolverCeres(wolf_problem_full, ceres_options);
- SolverCeres* ceres_manager_prun = new SolverCeres(wolf_problem_prun, ceres_options);
+ ceres_options.max_num_iterations = 1e4;
+ SolverCeres* ceres_manager_full = new SolverCeres(wolf_problem_full, ceres_options);
+ SolverCeres* ceres_manager_prun = new SolverCeres(wolf_problem_prun, ceres_options);
// load graph from .txt
offLineFile_.open(file_path_.c_str(), std::ifstream::in);
if (offLineFile_.is_open())
{
- std::string buffer;
+ std::string buffer;
unsigned int j = 0;
// Line by line
while (std::getline(offLineFile_, buffer))
{
- //std::cout << "new line:" << buffer << std::endl;
- std::string bNum;
+ // std::cout << "new line:" << buffer << std::endl;
+ std::string bNum;
unsigned int i = 0;
// VERTEX
if (buffer.at(0) == 'V')
{
- //skip rest of VERTEX word
+ // skip rest of VERTEX word
while (buffer.at(i) != ' ') i++;
- //skip white spaces
+ // skip white spaces
while (buffer.at(i) == ' ') i++;
- //vertex index
- while (buffer.at(i) != ' ')
- bNum.push_back(buffer.at(i++));
+ // vertex index
+ while (buffer.at(i) != ' ') bNum.push_back(buffer.at(i++));
unsigned int vertex_index = atoi(bNum.c_str());
bNum.clear();
- if (vertex_index <= MAX_VERTEX+1)
+ if (vertex_index <= MAX_VERTEX + 1)
{
- //skip white spaces
+ // skip white spaces
while (buffer.at(i) == ' ') i++;
// vertex pose
Eigen::Vector3d vertex_pose;
// x
- while (buffer.at(i) != ' ')
- bNum.push_back(buffer.at(i++));
+ while (buffer.at(i) != ' ') bNum.push_back(buffer.at(i++));
vertex_pose(0) = atof(bNum.c_str());
bNum.clear();
- //skip white spaces
+ // skip white spaces
while (buffer.at(i) == ' ') i++;
// y
- while (buffer.at(i) != ' ')
- bNum.push_back(buffer.at(i++));
+ while (buffer.at(i) != ' ') bNum.push_back(buffer.at(i++));
vertex_pose(1) = atof(bNum.c_str());
bNum.clear();
- //skip white spaces
+ // skip white spaces
while (buffer.at(i) == ' ') i++;
// theta
- while (i < buffer.size() && buffer.at(i) != ' ')
- bNum.push_back(buffer.at(i++));
+ while (i < buffer.size() && buffer.at(i) != ' ') bNum.push_back(buffer.at(i++));
vertex_pose(2) = atof(bNum.c_str());
bNum.clear();
// add frame to problem
- FrameBasePtr vertex_frame_ptr_full = new FrameBase(TimeStamp(0), std::make_shared<StatePoint2d>(vertex_pose.head(2)), std::make_shared<StateAngle>(vertex_pose.tail(1)));
- FrameBasePtr vertex_frame_ptr_prun = new FrameBase(TimeStamp(0), std::make_shared<StatePoint2d>(vertex_pose.head(2)), std::make_shared<StateAngle>(vertex_pose.tail(1)));
+ FrameBasePtr vertex_frame_ptr_full =
+ new FrameBase(TimeStamp(0),
+ std::make_shared<StatePoint2d>(vertex_pose.head(2)),
+ std::make_shared<StateAngle>(vertex_pose.tail(1)));
+ FrameBasePtr vertex_frame_ptr_prun =
+ new FrameBase(TimeStamp(0),
+ std::make_shared<StatePoint2d>(vertex_pose.head(2)),
+ std::make_shared<StateAngle>(vertex_pose.tail(1)));
wolf_problem_full->getTrajectory()->addFrame(vertex_frame_ptr_full);
wolf_problem_prun->getTrajectory()->addFrame(vertex_frame_ptr_prun);
// store
- index_2_frame_ptr_full[vertex_index] = vertex_frame_ptr_full;
- index_2_frame_ptr_prun[vertex_index] = vertex_frame_ptr_prun;
+ index_2_frame_ptr_full[vertex_index] = vertex_frame_ptr_full;
+ index_2_frame_ptr_prun[vertex_index] = vertex_frame_ptr_prun;
frame_ptr_2_index_prun[vertex_frame_ptr_prun] = vertex_index;
// Information matrix
- Lambda.conservativeResize(Lambda.rows()+3,Lambda.cols()+3);
+ Lambda.conservativeResize(Lambda.rows() + 3, Lambda.cols() + 3);
- //std::cout << "Added vertex! index: " << vertex_index << " id: " << vertex_frame_ptr_prun->id() << std::endl << "pose: " << vertex_pose.transpose() << std::endl;
+ // std::cout << "Added vertex! index: " << vertex_index << " id: " << vertex_frame_ptr_prun->id()
+ // << std::endl << "pose: " << vertex_pose.transpose() << std::endl;
}
}
// EDGE
else if (buffer.at(0) == 'E')
{
j++;
- //skip rest of EDGE word
+ // skip rest of EDGE word
while (buffer.at(i) != ' ') i++;
- //skip white spaces
+ // skip white spaces
while (buffer.at(i) == ' ') i++;
- //from
- while (buffer.at(i) != ' ')
- bNum.push_back(buffer.at(i++));
+ // from
+ while (buffer.at(i) != ' ') bNum.push_back(buffer.at(i++));
unsigned int edge_old = atoi(bNum.c_str());
bNum.clear();
- //skip white spaces
+ // skip white spaces
while (buffer.at(i) == ' ') i++;
- //to index
- while (buffer.at(i) != ' ')
- bNum.push_back(buffer.at(i++));
+ // to index
+ while (buffer.at(i) != ' ') bNum.push_back(buffer.at(i++));
unsigned int edge_new = atoi(bNum.c_str());
bNum.clear();
- if (edge_new <= MAX_VERTEX+1 && edge_old <= MAX_VERTEX+1 )
+ if (edge_new <= MAX_VERTEX + 1 && edge_old <= MAX_VERTEX + 1)
{
- //skip white spaces
+ // skip white spaces
while (buffer.at(i) == ' ') i++;
// edge vector
Eigen::Vector3d edge_vector;
// x
- while (buffer.at(i) != ' ')
- bNum.push_back(buffer.at(i++));
+ while (buffer.at(i) != ' ') bNum.push_back(buffer.at(i++));
edge_vector(0) = atof(bNum.c_str());
bNum.clear();
- //skip white spaces
+ // skip white spaces
while (buffer.at(i) == ' ') i++;
// y
- while (buffer.at(i) != ' ')
- bNum.push_back(buffer.at(i++));
+ while (buffer.at(i) != ' ') bNum.push_back(buffer.at(i++));
edge_vector(1) = atof(bNum.c_str());
bNum.clear();
- //skip white spaces
+ // skip white spaces
while (buffer.at(i) == ' ') i++;
// theta
- while (buffer.at(i) != ' ')
- bNum.push_back(buffer.at(i++));
+ while (buffer.at(i) != ' ') bNum.push_back(buffer.at(i++));
edge_vector(2) = atof(bNum.c_str());
bNum.clear();
- //skip white spaces
+ // skip white spaces
while (buffer.at(i) == ' ') i++;
// edge covariance
Eigen::Matrix3d edge_information;
// xx
- while (buffer.at(i) != ' ')
- bNum.push_back(buffer.at(i++));
- edge_information(0,0) = atof(bNum.c_str());
+ while (buffer.at(i) != ' ') bNum.push_back(buffer.at(i++));
+ edge_information(0, 0) = atof(bNum.c_str());
bNum.clear();
- //skip white spaces
+ // skip white spaces
while (buffer.at(i) == ' ') i++;
// xy
- while (buffer.at(i) != ' ')
- bNum.push_back(buffer.at(i++));
- edge_information(0,1) = atof(bNum.c_str());
- edge_information(1,0) = atof(bNum.c_str());
+ while (buffer.at(i) != ' ') bNum.push_back(buffer.at(i++));
+ edge_information(0, 1) = atof(bNum.c_str());
+ edge_information(1, 0) = atof(bNum.c_str());
bNum.clear();
- //skip white spaces
+ // skip white spaces
while (buffer.at(i) == ' ') i++;
// yy
- while (buffer.at(i) != ' ')
- bNum.push_back(buffer.at(i++));
- edge_information(1,1) = atof(bNum.c_str());
+ while (buffer.at(i) != ' ') bNum.push_back(buffer.at(i++));
+ edge_information(1, 1) = atof(bNum.c_str());
bNum.clear();
- //skip white spaces
+ // skip white spaces
while (buffer.at(i) == ' ') i++;
// thetatheta
- while (buffer.at(i) != ' ')
- bNum.push_back(buffer.at(i++));
- edge_information(2,2) = atof(bNum.c_str());
+ while (buffer.at(i) != ' ') bNum.push_back(buffer.at(i++));
+ edge_information(2, 2) = atof(bNum.c_str());
bNum.clear();
- //skip white spaces
+ // skip white spaces
while (buffer.at(i) == ' ') i++;
// xtheta
- while (buffer.at(i) != ' ')
- bNum.push_back(buffer.at(i++));
- edge_information(0,2) = atof(bNum.c_str());
- edge_information(2,0) = atof(bNum.c_str());
+ while (buffer.at(i) != ' ') bNum.push_back(buffer.at(i++));
+ edge_information(0, 2) = atof(bNum.c_str());
+ edge_information(2, 0) = atof(bNum.c_str());
bNum.clear();
- //skip white spaces
+ // skip white spaces
while (buffer.at(i) == ' ') i++;
// ytheta
- while (i < buffer.size() && buffer.at(i) != ' ')
- bNum.push_back(buffer.at(i++));
- edge_information(1,2) = atof(bNum.c_str());
- edge_information(2,1) = atof(bNum.c_str());
+ while (i < buffer.size() && buffer.at(i) != ' ') bNum.push_back(buffer.at(i++));
+ edge_information(1, 2) = atof(bNum.c_str());
+ edge_information(2, 1) = atof(bNum.c_str());
bNum.clear();
// add capture, feature and factor to problem
FeatureBasePtr feature_ptr_full = new FeatureBase("POSE", edge_vector, edge_information.inverse());
FeatureBasePtr feature_ptr_prun = new FeatureBase("POSE", edge_vector, edge_information.inverse());
- CaptureVoid* capture_ptr_full = new CaptureVoid(TimeStamp(0), sensor);
- CaptureVoid* capture_ptr_prun = new CaptureVoid(TimeStamp(0), sensor);
- assert(index_2_frame_ptr_full.find(edge_old) != index_2_frame_ptr_full.end() && "edge from vertex not added!");
- assert(index_2_frame_ptr_prun.find(edge_old) != index_2_frame_ptr_prun.end() && "edge from vertex not added!");
+ CaptureVoid* capture_ptr_full = new CaptureVoid(TimeStamp(0), sensor);
+ CaptureVoid* capture_ptr_prun = new CaptureVoid(TimeStamp(0), sensor);
+ assert(index_2_frame_ptr_full.find(edge_old) != index_2_frame_ptr_full.end() &&
+ "edge from vertex not added!");
+ assert(index_2_frame_ptr_prun.find(edge_old) != index_2_frame_ptr_prun.end() &&
+ "edge from vertex not added!");
FrameBasePtr frame_old_ptr_full = index_2_frame_ptr_full[edge_old];
FrameBasePtr frame_old_ptr_prun = index_2_frame_ptr_prun[edge_old];
- assert(index_2_frame_ptr_full.find(edge_new) != index_2_frame_ptr_full.end() && "edge to vertex not added!");
- assert(index_2_frame_ptr_prun.find(edge_new) != index_2_frame_ptr_prun.end() && "edge to vertex not added!");
+ assert(index_2_frame_ptr_full.find(edge_new) != index_2_frame_ptr_full.end() &&
+ "edge to vertex not added!");
+ assert(index_2_frame_ptr_prun.find(edge_new) != index_2_frame_ptr_prun.end() &&
+ "edge to vertex not added!");
FrameBasePtr frame_new_ptr_full = index_2_frame_ptr_full[edge_new];
FrameBasePtr frame_new_ptr_prun = index_2_frame_ptr_prun[edge_new];
frame_new_ptr_full->addCapture(capture_ptr_full);
@@ -283,32 +287,42 @@ int main(int argc, char** argv)
FactorOdom2d* factor_ptr_prun = new FactorOdom2d(feature_ptr_prun, frame_old_ptr_prun);
feature_ptr_full->addFactor(factor_ptr_full);
feature_ptr_prun->addFactor(factor_ptr_prun);
- //std::cout << "Added edge! " << factor_ptr_prun->id() << " from vertex " << factor_ptr_prun->getCapture()->getFrame()->id() << " to " << factor_ptr_prun->getFrameTo()->id() << std::endl;
- //std::cout << "vector " << factor_ptr_prun->getMeasurement().transpose() << std::endl;
- //std::cout << "information " << std::endl << edge_information << std::endl;
- //std::cout << "covariance " << std::endl << factor_ptr_prun->getMeasurementCovariance() << std::endl;
-
- double xi = *(frame_old_ptr_prun->getP()->get());
- double yi = *(frame_old_ptr_prun->getP()->get()+1);
- double thi = *(frame_old_ptr_prun->getO()->get());
- double si = sin(thi);
- double ci = cos(thi);
- double xj = *(frame_new_ptr_prun->getP()->get());
- double yj = *(frame_new_ptr_prun->getP()->get()+1);
- Eigen::MatrixXd Ji(3,3), Jj(3,3);
- Ji << -ci,-si,-(xj-xi)*si+(yj-yi)*ci,
- si,-ci,-(xj-xi)*ci-(yj-yi)*si,
- 0, 0, -1;
- Jj << ci, si, 0,
- -si, ci, 0,
- 0, 0, 1;
- //std::cout << "Ji" << std::endl << Ji << std::endl;
- //std::cout << "Jj" << std::endl << Jj << std::endl;
+ // std::cout << "Added edge! " << factor_ptr_prun->id() << " from vertex " <<
+ // factor_ptr_prun->getCapture()->getFrame()->id() << " to " << factor_ptr_prun->getFrameTo()->id()
+ // << std::endl; std::cout << "vector " << factor_ptr_prun->getMeasurement().transpose() <<
+ // std::endl; std::cout << "information " << std::endl << edge_information << std::endl; std::cout
+ // << "covariance " << std::endl << factor_ptr_prun->getMeasurementCovariance() << std::endl;
+
+ double xi = *(frame_old_ptr_prun->getP()->get());
+ double yi = *(frame_old_ptr_prun->getP()->get() + 1);
+ double thi = *(frame_old_ptr_prun->getO()->get());
+ double si = sin(thi);
+ double ci = cos(thi);
+ double xj = *(frame_new_ptr_prun->getP()->get());
+ double yj = *(frame_new_ptr_prun->getP()->get() + 1);
+ Eigen::MatrixXd Ji(3, 3), Jj(3, 3);
+ Ji << -ci, -si, -(xj - xi) * si + (yj - yi) * ci, si, -ci, -(xj - xi) * ci - (yj - yi) * si, 0, 0,
+ -1;
+ Jj << ci, si, 0, -si, ci, 0, 0, 0, 1;
+ // std::cout << "Ji" << std::endl << Ji << std::endl;
+ // std::cout << "Jj" << std::endl << Jj << std::endl;
Eigen::SparseMatrix<double> DeltaLambda(Lambda.rows(), Lambda.cols());
- insertSparseBlock((Ji.transpose() * edge_information * Ji).sparseView(), DeltaLambda, edge_old*3, edge_old*3);
- insertSparseBlock((Jj.transpose() * edge_information * Jj).sparseView(), DeltaLambda, edge_new*3, edge_new*3);
- insertSparseBlock((Ji.transpose() * edge_information * Jj).sparseView(), DeltaLambda, edge_old*3, edge_new*3);
- insertSparseBlock((Jj.transpose() * edge_information * Ji).sparseView(), DeltaLambda, edge_new*3, edge_old*3);
+ insertSparseBlock((Ji.transpose() * edge_information * Ji).sparseView(),
+ DeltaLambda,
+ edge_old * 3,
+ edge_old * 3);
+ insertSparseBlock((Jj.transpose() * edge_information * Jj).sparseView(),
+ DeltaLambda,
+ edge_new * 3,
+ edge_new * 3);
+ insertSparseBlock((Ji.transpose() * edge_information * Jj).sparseView(),
+ DeltaLambda,
+ edge_old * 3,
+ edge_new * 3);
+ insertSparseBlock((Jj.transpose() * edge_information * Ji).sparseView(),
+ DeltaLambda,
+ edge_new * 3,
+ edge_old * 3);
Lambda = Lambda + DeltaLambda;
}
}
@@ -321,15 +335,23 @@ int main(int argc, char** argv)
printf("\nError opening file\n");
// PRIOR
- FrameBasePtr first_frame_full = wolf_problem_full->getTrajectory()->getFirstFrame();
- FrameBasePtr first_frame_prun = wolf_problem_prun->getTrajectory()->getFirstFrame();
- CaptureFix* initial_covariance_full = new CaptureFix(TimeStamp(0), new SensorBase("ABSLOUTE POSE", nullptr, nullptr, nullptr, 0), first_frame_full->getState(), Eigen::Matrix3d::Identity() * 0.01);
- CaptureFix* initial_covariance_prun = new CaptureFix(TimeStamp(0), new SensorBase("ABSLOUTE POSE", nullptr, nullptr, nullptr, 0), first_frame_prun->getState(), Eigen::Matrix3d::Identity() * 0.01);
+ FrameBasePtr first_frame_full = wolf_problem_full->getTrajectory()->getFirstFrame();
+ FrameBasePtr first_frame_prun = wolf_problem_prun->getTrajectory()->getFirstFrame();
+ CaptureFix* initial_covariance_full = new CaptureFix(TimeStamp(0),
+ new SensorBase("ABSLOUTE POSE", nullptr, nullptr, nullptr, 0),
+ first_frame_full->getState(),
+ Eigen::Matrix3d::Identity() * 0.01);
+ CaptureFix* initial_covariance_prun = new CaptureFix(TimeStamp(0),
+ new SensorBase("ABSLOUTE POSE", nullptr, nullptr, nullptr, 0),
+ first_frame_prun->getState(),
+ Eigen::Matrix3d::Identity() * 0.01);
first_frame_full->addCapture(initial_covariance_full);
first_frame_prun->addCapture(initial_covariance_prun);
initial_covariance_full->process();
initial_covariance_prun->process();
- //std::cout << "initial covariance: factor " << initial_covariance_prun->getFeatureList().front()->getFactorFromList().front()->id() << std::endl << initial_covariance_prun->getFeatureList().front()->getMeasurementCovariance() << std::endl;
+ // std::cout << "initial covariance: factor " <<
+ // initial_covariance_prun->getFeatureList().front()->getFactorFromList().front()->id() << std::endl <<
+ // initial_covariance_prun->getFeatureList().front()->getMeasurementCovariance() << std::endl;
Eigen::SparseMatrix<double> DeltaLambda(Lambda.rows(), Lambda.cols());
insertSparseBlock((Eigen::Matrix3d::Identity() * 100).sparseView(), DeltaLambda, 0, 0);
Lambda = Lambda + DeltaLambda;
@@ -340,77 +362,94 @@ int main(int argc, char** argv)
// Manual covariance computation
t1 = clock();
Eigen::SimplicialLLT<Eigen::SparseMatrix<double>> chol(Lambda); // performs a Cholesky factorization of A
- Eigen::MatrixXd Sigma = chol.solve(Eigen::MatrixXd::Identity(Lambda.rows(), Lambda.cols()));
- double t_sigma_manual = ((double) clock() - t1) / CLOCKS_PER_SEC;
- //std::cout << "Lambda" << std::endl << Lambda << std::endl;
- //std::cout << "Sigma" << std::endl << Sigma << std::endl;
+ Eigen::MatrixXd Sigma = chol.solve(Eigen::MatrixXd::Identity(Lambda.rows(), Lambda.cols()));
+ double t_sigma_manual = ((double)clock() - t1) / CLOCKS_PER_SEC;
+ // std::cout << "Lambda" << std::endl << Lambda << std::endl;
+ // std::cout << "Sigma" << std::endl << Sigma << std::endl;
std::cout << " ceres is computing covariances..." << std::endl;
t1 = clock();
- ceres_manager_prun->computeCovariances(ALL);//ALL_MARGINALS
- double t_sigma_ceres = ((double) clock() - t1) / CLOCKS_PER_SEC;
+ ceres_manager_prun->computeCovariances(ALL); // ALL_MARGINALS
+ double t_sigma_ceres = ((double)clock() - t1) / CLOCKS_PER_SEC;
std::cout << "computed!" << std::endl;
t1 = clock();
- for (auto c_it=factors.begin(); c_it!=factors.end(); c_it++)
+ for (auto c_it = factors.begin(); c_it != factors.end(); c_it++)
{
if ((*c_it)->getCategory() != FAC_FRAME) continue;
// ii (old)
- wolf_problem_prun->getCovarianceBlock((*c_it)->getFrameOther()->getP(), (*c_it)->getFrameOther()->getP(), Sigma_ii, 0, 0);
- wolf_problem_prun->getCovarianceBlock((*c_it)->getFrameOther()->getP(), (*c_it)->getFrameOther()->getO(), Sigma_ii, 0, 2);
- wolf_problem_prun->getCovarianceBlock((*c_it)->getFrameOther()->getO(), (*c_it)->getFrameOther()->getP(), Sigma_ii, 2, 0);
- wolf_problem_prun->getCovarianceBlock((*c_it)->getFrameOther()->getO(), (*c_it)->getFrameOther()->getO(), Sigma_ii, 2, 2);
-// std::cout << "Sigma_ii" << std::endl << Sigma_ii << std::endl;
-// std::cout << "Sigma(i,i)" << std::endl << Sigma.block<3,3>(frame_ptr_2_index_prun[(*c_it)->getFrameTo()]*3, frame_ptr_2_index_prun[(*c_it)->getFrameTo()]*3) << std::endl;
+ wolf_problem_prun->getCovarianceBlock(
+ (*c_it)->getFrameOther()->getP(), (*c_it)->getFrameOther()->getP(), Sigma_ii, 0, 0);
+ wolf_problem_prun->getCovarianceBlock(
+ (*c_it)->getFrameOther()->getP(), (*c_it)->getFrameOther()->getO(), Sigma_ii, 0, 2);
+ wolf_problem_prun->getCovarianceBlock(
+ (*c_it)->getFrameOther()->getO(), (*c_it)->getFrameOther()->getP(), Sigma_ii, 2, 0);
+ wolf_problem_prun->getCovarianceBlock(
+ (*c_it)->getFrameOther()->getO(), (*c_it)->getFrameOther()->getO(), Sigma_ii, 2, 2);
+ // std::cout << "Sigma_ii" << std::endl << Sigma_ii << std::endl;
+ // std::cout << "Sigma(i,i)" << std::endl <<
+ // Sigma.block<3,3>(frame_ptr_2_index_prun[(*c_it)->getFrameTo()]*3,
+ // frame_ptr_2_index_prun[(*c_it)->getFrameTo()]*3) << std::endl;
// jj (new)
- wolf_problem_prun->getCovarianceBlock((*c_it)->getCapture()->getFrame()->getP(), (*c_it)->getCapture()->getFrame()->getP(), Sigma_jj, 0, 0);
- wolf_problem_prun->getCovarianceBlock((*c_it)->getCapture()->getFrame()->getP(), (*c_it)->getCapture()->getFrame()->getO(), Sigma_jj, 0, 2);
- wolf_problem_prun->getCovarianceBlock((*c_it)->getCapture()->getFrame()->getO(), (*c_it)->getCapture()->getFrame()->getP(), Sigma_jj, 2, 0);
- wolf_problem_prun->getCovarianceBlock((*c_it)->getCapture()->getFrame()->getO(), (*c_it)->getCapture()->getFrame()->getO(), Sigma_jj, 2, 2);
-// std::cout << "Sigma_jj" << std::endl << Sigma_jj << std::endl;
-// std::cout << "Sigma(j,j)" << std::endl << Sigma.block<3,3>(frame_ptr_2_index_prun[(*c_it)->getCapture()->getFrame()]*3, frame_ptr_2_index_prun[(*c_it)->getCapture()->getFrame()]*3) << std::endl;
+ wolf_problem_prun->getCovarianceBlock(
+ (*c_it)->getCapture()->getFrame()->getP(), (*c_it)->getCapture()->getFrame()->getP(), Sigma_jj, 0, 0);
+ wolf_problem_prun->getCovarianceBlock(
+ (*c_it)->getCapture()->getFrame()->getP(), (*c_it)->getCapture()->getFrame()->getO(), Sigma_jj, 0, 2);
+ wolf_problem_prun->getCovarianceBlock(
+ (*c_it)->getCapture()->getFrame()->getO(), (*c_it)->getCapture()->getFrame()->getP(), Sigma_jj, 2, 0);
+ wolf_problem_prun->getCovarianceBlock(
+ (*c_it)->getCapture()->getFrame()->getO(), (*c_it)->getCapture()->getFrame()->getO(), Sigma_jj, 2, 2);
+ // std::cout << "Sigma_jj" << std::endl << Sigma_jj << std::endl;
+ // std::cout << "Sigma(j,j)" << std::endl <<
+ // Sigma.block<3,3>(frame_ptr_2_index_prun[(*c_it)->getCapture()->getFrame()]*3,
+ // frame_ptr_2_index_prun[(*c_it)->getCapture()->getFrame()]*3) << std::endl;
// ij (old-new)
- wolf_problem_prun->getCovarianceBlock((*c_it)->getFrameOther()->getP(), (*c_it)->getCapture()->getFrame()->getP(), Sigma_ij, 0, 0);
- wolf_problem_prun->getCovarianceBlock((*c_it)->getFrameOther()->getP(), (*c_it)->getCapture()->getFrame()->getO(), Sigma_ij, 0, 2);
- wolf_problem_prun->getCovarianceBlock((*c_it)->getFrameOther()->getO(), (*c_it)->getCapture()->getFrame()->getP(), Sigma_ij, 2, 0);
- wolf_problem_prun->getCovarianceBlock((*c_it)->getFrameOther()->getO(), (*c_it)->getCapture()->getFrame()->getO(), Sigma_ij, 2, 2);
-// std::cout << "Sigma_ij" << std::endl << Sigma_ij << std::endl;
-// std::cout << "Sigma(i,j)" << std::endl << Sigma.block<3,3>(frame_ptr_2_index_prun[(*c_it)->getFrameTo()]*3, frame_ptr_2_index_prun[(*c_it)->getCapture()->getFrame()]*3) << std::endl;
-
- //jacobian
- xi = *(*c_it)->getFrameOther()->getP()->get();
- yi = *((*c_it)->getFrameOther()->getP()->get()+1);
+ wolf_problem_prun->getCovarianceBlock(
+ (*c_it)->getFrameOther()->getP(), (*c_it)->getCapture()->getFrame()->getP(), Sigma_ij, 0, 0);
+ wolf_problem_prun->getCovarianceBlock(
+ (*c_it)->getFrameOther()->getP(), (*c_it)->getCapture()->getFrame()->getO(), Sigma_ij, 0, 2);
+ wolf_problem_prun->getCovarianceBlock(
+ (*c_it)->getFrameOther()->getO(), (*c_it)->getCapture()->getFrame()->getP(), Sigma_ij, 2, 0);
+ wolf_problem_prun->getCovarianceBlock(
+ (*c_it)->getFrameOther()->getO(), (*c_it)->getCapture()->getFrame()->getO(), Sigma_ij, 2, 2);
+ // std::cout << "Sigma_ij" << std::endl << Sigma_ij << std::endl;
+ // std::cout << "Sigma(i,j)" << std::endl <<
+ // Sigma.block<3,3>(frame_ptr_2_index_prun[(*c_it)->getFrameTo()]*3,
+ // frame_ptr_2_index_prun[(*c_it)->getCapture()->getFrame()]*3) << std::endl;
+
+ // jacobian
+ xi = *(*c_it)->getFrameOther()->getP()->get();
+ yi = *((*c_it)->getFrameOther()->getP()->get() + 1);
thi = *(*c_it)->getFrameOther()->getO()->get();
- si = sin(thi);
- ci = cos(thi);
- xj = *(*c_it)->getCapture()->getFrame()->getP()->get();
- yj = *((*c_it)->getCapture()->getFrame()->getP()->get()+1);
-
- Ji << -ci,-si,-(xj-xi)*si+(yj-yi)*ci,
- si,-ci,-(xj-xi)*ci-(yj-yi)*si,
- 0, 0, -1;
- Jj << ci, si, 0,
- -si, ci, 0,
- 0, 0, 1;
- //std::cout << "Ji" << std::endl << Ji << std::endl;
- //std::cout << "Jj" << std::endl << Jj << std::endl;
+ si = sin(thi);
+ ci = cos(thi);
+ xj = *(*c_it)->getCapture()->getFrame()->getP()->get();
+ yj = *((*c_it)->getCapture()->getFrame()->getP()->get() + 1);
+
+ Ji << -ci, -si, -(xj - xi) * si + (yj - yi) * ci, si, -ci, -(xj - xi) * ci - (yj - yi) * si, 0, 0, -1;
+ Jj << ci, si, 0, -si, ci, 0, 0, 0, 1;
+ // std::cout << "Ji" << std::endl << Ji << std::endl;
+ // std::cout << "Jj" << std::endl << Jj << std::endl;
// Measurement covariance
Sigma_z = (*c_it)->getFeature()->getMeasurementCovariance();
- //std::cout << "Sigma_z" << std::endl << Sigma_z << std::endl;
- //std::cout << "Sigma_z.determinant() = " << Sigma_z.determinant() << std::endl;
+ // std::cout << "Sigma_z" << std::endl << Sigma_z << std::endl;
+ // std::cout << "Sigma_z.determinant() = " << Sigma_z.determinant() << std::endl;
- //std::cout << "denominador : " << std::endl << Sigma_z - (Ji * Sigma_ii * Ji.transpose() + Jj * Sigma_jj * Jj.transpose() + Ji * Sigma_ij * Jj.transpose() + Jj * Sigma_ij.transpose() * Ji.transpose()) << std::endl;
+ // std::cout << "denominador : " << std::endl << Sigma_z - (Ji * Sigma_ii * Ji.transpose() + Jj * Sigma_jj *
+ // Jj.transpose() + Ji * Sigma_ij * Jj.transpose() + Jj * Sigma_ij.transpose() * Ji.transpose()) << std::endl;
// Information gain
- double IG = 0.5 * log( Sigma_z.determinant() / (Sigma_z - (Ji * Sigma_ii * Ji.transpose() + Jj * Sigma_jj * Jj.transpose() + Ji * Sigma_ij * Jj.transpose() + Jj * Sigma_ij.transpose() * Ji.transpose())).determinant() );
- //std::cout << "IG = " << IG << std::endl;
+ double IG = 0.5 * log(Sigma_z.determinant() /
+ (Sigma_z - (Ji * Sigma_ii * Ji.transpose() + Jj * Sigma_jj * Jj.transpose() +
+ Ji * Sigma_ij * Jj.transpose() + Jj * Sigma_ij.transpose() * Ji.transpose()))
+ .determinant());
+ // std::cout << "IG = " << IG << std::endl;
- if (IG < 2)
- (*c_it)->setStatus(FAC_INACTIVE);
+ if (IG < 2) (*c_it)->setStatus(FAC_INACTIVE);
}
- double t_ig = ((double) clock() - t1) / CLOCKS_PER_SEC;
+ double t_ig = ((double)clock() - t1) / CLOCKS_PER_SEC;
std::cout << "manual sigma computation " << t_sigma_manual << "s" << std::endl;
std::cout << "ceres sigma computation " << t_sigma_ceres << "s" << std::endl;
std::cout << "information gain computation " << t_ig << "s" << std::endl;
@@ -422,14 +461,14 @@ int main(int argc, char** argv)
summary_prun = ceres_manager_prun->solve();
std::cout << summary_prun.FullReport() << std::endl;
- delete wolf_problem_full; //not necessary to delete anything more, wolf will do it!
+ delete wolf_problem_full; // not necessary to delete anything more, wolf will do it!
std::cout << "wolf_problem_ deleted!" << std::endl;
delete ceres_manager_full;
delete ceres_manager_prun;
std::cout << "ceres_manager deleted!" << std::endl;
- //End message
+ // End message
std::cout << " =========================== END ===============================" << std::endl << std::endl;
-
- //exit
+
+ // exit
return 0;
}
diff --git a/demos/hello_wolf/capture_range_bearing.cpp b/demos/hello_wolf/capture_range_bearing.cpp
index cc627493a17b3e0f8d78c05c55e89b149447f2b8..fb2aaa1e402ec254757151575e680b4130184b4e 100644
--- a/demos/hello_wolf/capture_range_bearing.cpp
+++ b/demos/hello_wolf/capture_range_bearing.cpp
@@ -17,23 +17,17 @@
//
// 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/>.
-/*
- * CaptureRangeBearing2d.cpp
- *
- * Created on: Nov 30, 2017
- * Author: jsola
- */
#include "capture_range_bearing.h"
namespace wolf
{
-
-CaptureRangeBearing::CaptureRangeBearing(const TimeStamp& _ts, const SensorBasePtr& _scanner, const Eigen::VectorXi& _ids, const Eigen::VectorXd& _ranges, const Eigen::VectorXd& _bearings) :
- CaptureBase("CaptureRangeBearing", _ts, _scanner),
- ids_(_ids),
- ranges_(_ranges),
- bearings_(_bearings)
+CaptureRangeBearing::CaptureRangeBearing(const TimeStamp& _ts,
+ const SensorBasePtr& _scanner,
+ const Eigen::VectorXi& _ids,
+ const Eigen::VectorXd& _ranges,
+ const Eigen::VectorXd& _bearings)
+ : CaptureBase("CaptureRangeBearing", _ts, _scanner), ids_(_ids), ranges_(_ranges), bearings_(_bearings)
{
//
}
diff --git a/demos/hello_wolf/capture_range_bearing.h b/demos/hello_wolf/capture_range_bearing.h
index 5600dd0fe928ec87be31ca9b28927b8572581b56..e24ec94c4aec176e5e6b5010977c0ada391be6e1 100644
--- a/demos/hello_wolf/capture_range_bearing.h
+++ b/demos/hello_wolf/capture_range_bearing.h
@@ -24,30 +24,33 @@
namespace wolf
{
-
WOLF_PTR_TYPEDEFS(CaptureRangeBearing)
using namespace Eigen;
class CaptureRangeBearing : public CaptureBase
{
- public:
- CaptureRangeBearing(const TimeStamp& _ts, const SensorBasePtr& _scanner, const Eigen::VectorXi& _ids, const Eigen::VectorXd& _ranges, const Eigen::VectorXd& _bearings);
- ~CaptureRangeBearing() override;
-
- const VectorXi& getIds () const;
- const int& getId (int _i) const;
- const Eigen::VectorXd& getRanges () const;
- const Eigen::VectorXd& getBearings () const;
- const double& getRange (int _i) const;
- const double& getBearing (int _i) const;
- Eigen::Vector2d getRangeBearing (int _i) const;
- Eigen::Matrix<double, Dynamic, 2> getRangeBearing() const;
-
- private:
- VectorXi ids_; // identifiers
- VectorXd ranges_; // ranges
- VectorXd bearings_; // bearings
+ public:
+ CaptureRangeBearing(const TimeStamp& _ts,
+ const SensorBasePtr& _scanner,
+ const Eigen::VectorXi& _ids,
+ const Eigen::VectorXd& _ranges,
+ const Eigen::VectorXd& _bearings);
+ ~CaptureRangeBearing() override;
+
+ const VectorXi& getIds() const;
+ const int& getId(int _i) const;
+ const Eigen::VectorXd& getRanges() const;
+ const Eigen::VectorXd& getBearings() const;
+ const double& getRange(int _i) const;
+ const double& getBearing(int _i) const;
+ Eigen::Vector2d getRangeBearing(int _i) const;
+ Eigen::Matrix<double, Dynamic, 2> getRangeBearing() const;
+
+ private:
+ VectorXi ids_; // identifiers
+ VectorXd ranges_; // ranges
+ VectorXd bearings_; // bearings
};
inline const Eigen::VectorXi& CaptureRangeBearing::getIds() const
@@ -80,9 +83,9 @@ inline const double& CaptureRangeBearing::getBearing(int _i) const
return bearings_(_i);
}
-inline Eigen::Matrix<double,Dynamic,2> CaptureRangeBearing::getRangeBearing() const
+inline Eigen::Matrix<double, Dynamic, 2> CaptureRangeBearing::getRangeBearing() const
{
- return (Matrix<double,Dynamic,2>() << ranges_, bearings_).finished();
+ return (Matrix<double, Dynamic, 2>() << ranges_, bearings_).finished();
}
inline Eigen::Vector2d CaptureRangeBearing::getRangeBearing(int _i) const
diff --git a/demos/hello_wolf/factor_bearing.h b/demos/hello_wolf/factor_bearing.h
index 2e4ed1d62e182e2d76b3f6add91b9c32c974133c..53caf18af0ad54cfeabbec313e08ddc1455edc69 100644
--- a/demos/hello_wolf/factor_bearing.h
+++ b/demos/hello_wolf/factor_bearing.h
@@ -24,48 +24,47 @@
namespace wolf
{
-
using namespace Eigen;
class FactorBearing : public FactorAutodiff<FactorBearing, 1, 2, 1, 2, 1, 2>
{
- public:
- FactorBearing(const FeatureBasePtr& _feature_ptr,
- const LandmarkBasePtr& _landmark_other_ptr,
- const ProcessorBasePtr& _processor_ptr,
- bool _apply_loss_function, FactorStatus _status) :
- FactorAutodiff<FactorBearing, 1, 2, 1, 2, 1, 2>("BEARING",
- TOP_LMK,
- _feature_ptr,
- nullptr,
- nullptr,
- nullptr,
- _landmark_other_ptr,
- _processor_ptr,
- _apply_loss_function,
- _status,
- _feature_ptr->getCapture()->getFrame()->getP(),
- _feature_ptr->getCapture()->getFrame()->getO(),
- _feature_ptr->getCapture()->getSensor()->getP(),
- _feature_ptr->getCapture()->getSensor()->getO(),
- _landmark_other_ptr->getP())
- {
- //
- }
-
- virtual ~FactorBearing()
- {
- //
- }
-
- template<typename T>
- bool operator ()(const T* const _p_w_r,
- const T* const _o_w_r,
- const T* const _p_r_s, // sensor position
- const T* const _o_r_s, // sensor orientation
- const T* const _lmk,
- T* _res) const;
-
+ public:
+ FactorBearing(const FeatureBasePtr& _feature_ptr,
+ const LandmarkBasePtr& _landmark_other_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status)
+ : FactorAutodiff<FactorBearing, 1, 2, 1, 2, 1, 2>("BEARING",
+ TOP_LMK,
+ _feature_ptr,
+ nullptr,
+ nullptr,
+ nullptr,
+ _landmark_other_ptr,
+ _processor_ptr,
+ _apply_loss_function,
+ _status,
+ _feature_ptr->getCapture()->getFrame()->getP(),
+ _feature_ptr->getCapture()->getFrame()->getO(),
+ _feature_ptr->getCapture()->getSensor()->getP(),
+ _feature_ptr->getCapture()->getSensor()->getO(),
+ _landmark_other_ptr->getP())
+ {
+ //
+ }
+
+ virtual ~FactorBearing()
+ {
+ //
+ }
+
+ template <typename T>
+ bool operator()(const T* const _p_w_r,
+ const T* const _o_w_r,
+ const T* const _p_r_s, // sensor position
+ const T* const _o_r_s, // sensor orientation
+ const T* const _lmk,
+ T* _res) const;
};
} /* namespace wolf */
@@ -74,44 +73,44 @@ class FactorBearing : public FactorAutodiff<FactorBearing, 1, 2, 1, 2, 1, 2>
namespace wolf
{
-
-template<typename T>
-inline bool FactorBearing::operator ()( const T* const _p_w_r,
- const T* const _o_w_r,
- const T* const _p_r_s, // sensor position
- const T* const _o_r_s, // sensor orientation
- const T* const _lmk,
- T* _res) const
+template <typename T>
+inline bool FactorBearing::operator()(const T* const _p_w_r,
+ const T* const _o_w_r,
+ const T* const _p_r_s, // sensor position
+ const T* const _o_r_s, // sensor orientation
+ const T* const _lmk,
+ T* _res) const
{
-
// 1. produce a transformation matrix to transform from robot frame to world frame
- Transform<T, 2, Isometry> H_w_r = Translation<T,2>(_p_w_r[0], _p_w_r[1]) * Rotation2D<T>(*_o_w_r) ; // Robot frame = robot-to-world transform
- Transform<T, 2, Isometry> H_r_s = Translation<T,2>(_p_r_s[0], _p_r_s[1]) * Rotation2D<T>(*_o_r_s) ; // Robot frame = robot-to-world transform
-
+ Transform<T, 2, Isometry> H_w_r =
+ Translation<T, 2>(_p_w_r[0], _p_w_r[1]) * Rotation2D<T>(*_o_w_r); // Robot frame = robot-to-world transform
+ Transform<T, 2, Isometry> H_r_s =
+ Translation<T, 2>(_p_r_s[0], _p_r_s[1]) * Rotation2D<T>(*_o_r_s); // Robot frame = robot-to-world transform
+
// Map input pointers into meaningful Eigen elements
- Map<const Matrix<T, 2, 1>> point_w(_lmk);
- Map<const Matrix<T, 1, 1>> res(_res);
+ Map<const Matrix<T, 2, 1>> point_w(_lmk);
+ Map<const Matrix<T, 1, 1>> res(_res);
// 2. Transform world point to sensor-referenced point
Matrix<T, 2, 1> point_s = (H_w_r * H_r_s).inverse() * point_w;
// 3. Get the expected bearing of the point
- T bearing = atan2(point_s(1), point_s(0));
+ T bearing = atan2(point_s(1), point_s(0));
// 4. Get the measured range-and-bearing to the point
Matrix<T, 2, 1> range_bearing = getMeasurement().cast<T>();
// 5. Get the bearing error by comparing against the bearing measurement
- T er = range_bearing(1) - bearing;
+ T er = range_bearing(1) - bearing;
if (er < T(-M_PI))
- er += T(2*M_PI);
- else if (er > T(-M_PI))
- er -= T(2*M_PI);
+ er += T(2 * M_PI);
+ else if (er > T(-M_PI))
+ er -= T(2 * M_PI);
// 6. Compute the residual by scaling according to the standard deviation of the bearing part
- *_res = er * getMeasurementSquareRootInformationUpper()(1,1);
+ *_res = er * getMeasurementSquareRootInformationUpper()(1, 1);
return true;
}
-} // namespace wolf
+} // namespace wolf
diff --git a/demos/hello_wolf/feature_range_bearing.cpp b/demos/hello_wolf/feature_range_bearing.cpp
index 73f1965f4c647cb0715d5e34bd426d18cc60d09b..e5a806203bb65b01f3160546605c10031ff612f9 100644
--- a/demos/hello_wolf/feature_range_bearing.cpp
+++ b/demos/hello_wolf/feature_range_bearing.cpp
@@ -17,20 +17,13 @@
//
// 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/>.
-/*
- * FeatureRangeBearing2d.cpp
- *
- * Created on: Nov 30, 2017
- * Author: jsola
- */
#include "feature_range_bearing.h"
namespace wolf
{
-
-FeatureRangeBearing::FeatureRangeBearing(const Eigen::VectorXd& _measurement, const Eigen::MatrixXd& _meas_covariance) :
- FeatureBase("FeatureRangeBearing", _measurement, _meas_covariance)
+FeatureRangeBearing::FeatureRangeBearing(const Eigen::VectorXd& _measurement, const Eigen::MatrixXd& _meas_covariance)
+ : FeatureBase("FeatureRangeBearing", _measurement, _meas_covariance)
{
//
}
@@ -40,5 +33,4 @@ FeatureRangeBearing::~FeatureRangeBearing()
//
}
-} // namespace wolf
-
+} // namespace wolf
diff --git a/demos/hello_wolf/feature_range_bearing.h b/demos/hello_wolf/feature_range_bearing.h
index db4e11fabc503aa966c55b31025b9ce665560030..e729b350faea2f0acbc0e44047f81d0751c65ddb 100644
--- a/demos/hello_wolf/feature_range_bearing.h
+++ b/demos/hello_wolf/feature_range_bearing.h
@@ -24,14 +24,13 @@
namespace wolf
{
-
WOLF_PTR_TYPEDEFS(FeatureRangeBearing);
class FeatureRangeBearing : public FeatureBase
{
- public:
- FeatureRangeBearing(const Eigen::VectorXd& _measurement, const Eigen::MatrixXd& _meas_covariance);
- ~FeatureRangeBearing() override;
+ public:
+ FeatureRangeBearing(const Eigen::VectorXd& _measurement, const Eigen::MatrixXd& _meas_covariance);
+ ~FeatureRangeBearing() override;
};
-} // namespace wolf
+} // namespace wolf
diff --git a/demos/hello_wolf/hello_wolf.cpp b/demos/hello_wolf/hello_wolf.cpp
index 83e5652406c836b5a574839eb54f038ce613ac0a..f5dfde2389fe74c81a949592688a052af275f36a 100644
--- a/demos/hello_wolf/hello_wolf.cpp
+++ b/demos/hello_wolf/hello_wolf.cpp
@@ -19,8 +19,8 @@
// along with this program. If not, see <http://www.gnu.org/licenses/>.
// wolf core includes
-#include "core/ceres_wrapper/solver_ceres.h"
#include "core/common/wolf.h"
+#include "core/ceres_wrapper/solver_ceres.h"
#include "core/sensor/sensor_odom.h"
#include "core/processor/processor_odom_2d.h"
#include "core/capture/capture_odom_2d.h"
@@ -108,41 +108,86 @@ int main()
using namespace wolf;
// Wolf problem and solver
- ProblemPtr problem = Problem::create("PO", 2);
- SolverCeresPtr ceres = std::make_shared<SolverCeres>(problem);
- ceres->getSolverOptions().max_num_iterations = 1000; // We depart far from solution, need a lot of iterations
+ ProblemPtr problem = Problem::create("PO", 2);
+ YAML::Node params_solver;
+ params_solver["period"] = 1;
+ params_solver["verbose"] = 2;
+ params_solver["interrupt_on_problem_change"] = false;
+ params_solver["max_num_iterations"] = 1000;
+ params_solver["n_threads"] = 2;
+ params_solver["function_tolerance"] = 0.000001;
+ params_solver["gradient_tolerance"] = 0.0000000001;
+ params_solver["minimizer"] = "LEVENBERG_MARQUARDT";
+ params_solver["use_nonmonotonic_steps"] = false;
+ params_solver["compute_cov"] = false;
+ SolverCeresPtr ceres = std::static_pointer_cast<SolverCeres>(SolverCeres::create(problem, params_solver));
// sensor odometer 2d
- ParamsSensorOdomPtr intrinsics_odo = std::make_shared<ParamsSensorOdom>();
- SpecStateSensorComposite priors_odo = {{'P', SpecStateSensor("StatePoint2d", Vector2d::Zero(), "fix")},
- {'O', SpecStateSensor("StateAngle", Vector1d::Zero(), "fix")}};
- SensorBasePtr sensor_odo = SensorBase::emplace<SensorOdom2d>(problem->getHardware(), intrinsics_odo, priors_odo);
+ YAML::Node params_sen_odo;
+ params_sen_odo["states"]["P"]["type"] = "StatePoint2d";
+ params_sen_odo["states"]["P"]["state"] = Vector2d::Zero();
+ params_sen_odo["states"]["P"]["mode"] = "fix";
+ params_sen_odo["states"]["P"]["dynamic"] = false;
+ params_sen_odo["states"]["O"]["type"] = "StateAngle";
+ params_sen_odo["states"]["O"]["state"] = Vector1d::Zero();
+ params_sen_odo["states"]["O"]["mode"] = "fix";
+ params_sen_odo["states"]["O"]["dynamic"] = false;
+
+ params_sen_odo["name"] = "Sensor Odometry";
+ params_sen_odo["k_disp_to_disp"] = 0.1;
+ params_sen_odo["k_disp_to_rot"] = 0.1;
+ params_sen_odo["k_rot_to_rot"] = 0.1;
+ params_sen_odo["min_disp_var"] = 1;
+ params_sen_odo["min_rot_var"] = 1;
+
+ SensorBasePtr sensor_odo = SensorBase::emplace<SensorOdom2d>(problem->getHardware(), params_sen_odo);
+ std::cout << "sensor_odo created\n";
// processor odometer 2d
- ParamsProcessorOdom2dPtr params_odo = std::make_shared<ParamsProcessorOdom2d>();
- params_odo->voting_active = true;
- params_odo->time_tolerance = 0.1;
- params_odo->max_time_span = 999;
- params_odo->dist_traveled = 0.95; // Will make KFs automatically every 1m displacement
- params_odo->angle_turned = 999;
- params_odo->cov_det = 999;
- params_odo->unmeasured_perturbation_std = 0.001;
- ProcessorBasePtr processor = ProcessorBase::emplace<ProcessorOdom2d>(sensor_odo, params_odo);
+ YAML::Node params_prc_odo;
+ params_prc_odo["name"] = "Processor Odometry";
+ params_prc_odo["time_tolerance"] = 0.1;
+ params_prc_odo["keyframe_vote"]["voting_active"] = true;
+ params_prc_odo["keyframe_vote"]["max_time_span"] = 999;
+ params_prc_odo["keyframe_vote"]["max_dist_traveled"] = 0.95; // Will make KFs automatically every 1m displacement
+ params_prc_odo["keyframe_vote"]["max_angle_turned"] = 999;
+ params_prc_odo["keyframe_vote"]["cov_det"] = 999;
+ params_prc_odo["keyframe_vote"]["max_buff_length"] = 999;
+ params_prc_odo["state_provider"] = true;
+ params_prc_odo["state_provider_order"] = 1;
+ params_prc_odo["unmeasured_perturbation_std"] = 0.001;
+ params_prc_odo["apply_loss_function"] = false;
+
+ ProcessorBasePtr processor = ProcessorBase::emplace<ProcessorOdom2d>(sensor_odo, params_prc_odo);
+ std::cout << "processor created\n";
// sensor Range and Bearing
- ParamsSensorRangeBearingPtr intrinsics_rb = std::make_shared<ParamsSensorRangeBearing>();
- SpecStateSensorComposite priors_rb = {{'P', SpecStateSensor("StatePoint2d", Vector2d(1, 1), "fix")},
- {'O', SpecStateSensor("StateAngle", Vector1d::Zero(), "fix")}};
- intrinsics_rb->noise_range_metres_std = 0.1;
- intrinsics_rb->noise_bearing_degrees_std = 1.0;
- SensorBasePtr sensor_rb =
- SensorBase::emplace<SensorRangeBearing>(problem->getHardware(), intrinsics_rb, priors_rb);
+ YAML::Node params_sen_rb;
+ params_sen_rb["states"]["P"]["type"] = "StatePoint2d";
+ params_sen_rb["states"]["P"]["state"] = Vector2d::Zero();
+ params_sen_rb["states"]["P"]["mode"] = "fix";
+ params_sen_rb["states"]["P"]["dynamic"] = false;
+ params_sen_rb["states"]["O"]["type"] = "StateAngle";
+ params_sen_rb["states"]["O"]["state"] = Vector1d::Zero();
+ params_sen_rb["states"]["O"]["mode"] = "fix";
+ params_sen_rb["states"]["O"]["dynamic"] = false;
+
+ params_sen_rb["name"] = "Sensor Range Bearing";
+ params_sen_rb["noise_range_metres_std"] = 0.1;
+ params_sen_rb["noise_bearing_degrees_std"] = 1.0;
+
+ SensorBasePtr sensor_rb = SensorBase::emplace<SensorRangeBearing>(problem->getHardware(), params_sen_rb);
+ std::cout << "sensor_rb created\n";
// processor Range and Bearing
- ParamsProcessorRangeBearingPtr params_rb = std::make_shared<ParamsProcessorRangeBearing>();
- params_rb->voting_active = false;
- params_rb->time_tolerance = 0.01;
- ProcessorBasePtr processor_rb = ProcessorBase::emplace<ProcessorRangeBearing>(sensor_rb, params_rb);
+ YAML::Node params_prc_rb;
+ params_prc_rb["name"] = "Processor Range Bearing";
+ params_prc_rb["voting_active"] = false;
+ params_prc_rb["time_tolerance"] = 0.01;
+ params_prc_rb["apply_loss_function"] = false;
+
+ ProcessorBasePtr processor_rb = ProcessorBase::emplace<ProcessorRangeBearing>(sensor_rb, params_prc_rb);
+ std::cout << "processor_rb created\n";
// initialize
TimeStamp t(0.0); // t : 0.0
@@ -150,23 +195,19 @@ int main()
prior.emplace('P', SpecState("StatePoint2d", Vector2d::Zero(), "factor", Vector2d::Constant(sqrt(0.1))));
prior.emplace('O', SpecState("StateAngle", Vector1d::Zero(), "factor", Vector1d::Constant(sqrt(0.1))));
FrameBasePtr KF1 = problem->setPrior(prior, t); // KF1 : (0,0,0)
- // // Vector3d x(0,0,0);
- // VectorComposite x(Vector3d(0,0,0), "PO", {2,1});
- // // Matrix3d P = Matrix3d::Identity() * 0.1;
- // VectorComposite P(Vector3d(sqrt(0.1),sqrt(0.1),sqrt(0.1)), "PO", {2,1});
- // FrameBasePtr KF1 = problem->setPriorFactor(x, P, t); // KF1 : (0,0,0)
std::static_pointer_cast<ProcessorMotion>(processor)->setOrigin(KF1);
// SELF CALIBRATION ===================================================
// SELF-CALIBRATION OF SENSOR ORIENTATION
// Uncomment this line below to achieve sensor self-calibration (of the orientation only, since the position is not
- // observable)
+ // observable):
sensor_rb->getO()->unfix();
// SELF-CALIBRATION OF SENSOR POSITION
// The position is however not observable, and thus self-calibration would not work. You can try uncommenting the
- // line below. sensor_rb->getP()->unfix();
+ // line below:
+ // sensor_rb->getP()->unfix();
// CONFIGURE ==========================================================
diff --git a/demos/hello_wolf/hello_wolf_autoconf.cpp b/demos/hello_wolf/hello_wolf_autoconf.cpp
index 4416017843e8a97b43d36701008bbf2ca907c92d..d1a65b2d5217798e66d4831f5c766523274b9398 100644
--- a/demos/hello_wolf/hello_wolf_autoconf.cpp
+++ b/demos/hello_wolf/hello_wolf_autoconf.cpp
@@ -26,9 +26,8 @@
#include "core/processor/processor_motion.h"
#include "capture_range_bearing.h"
-
int main()
- {
+{
/*
* ============= PROBLEM DEFINITION ==================
*
@@ -96,7 +95,8 @@ int main()
* - Second, using random values
* Both solutions must produce the same exact values as in the sketches above.
*
- * Optionally, the user can opt to self-calibrate the sensor's orientation (see NOTE within the code around Line 136)
+ * Optionally, the user can opt to self-calibrate the sensor's orientation (see NOTE within the code around Line
+ * 136)
*
* (c) 2017 Joan Sola @ IRI-CSIC
*/
@@ -105,7 +105,6 @@ int main()
using namespace wolf;
-
WOLF_INFO("======== CONFIGURE PROBLEM =======");
// Config file to parse. Here is where all the problem is defined:
@@ -120,38 +119,39 @@ int main()
throw std::runtime_error("Couldn't load and validate the yaml file " + config_file);
}
// Wolf problem: automatically build the left branch of the wolf tree from the contents of the params server:
- ProblemPtr problem = Problem::autoSetup(server.getNode());
-
+ ProblemPtr problem = Problem::autoSetup(server.getNode());
+
// Print problem to see its status before processing any sensor data
- problem->print(4,0,1,0);
+ problem->print(4, 0, 1, 0);
// Solver. Configure a Ceres solver
- SolverManagerPtr ceres = FactorySolver::create("SolverCeres", problem, server.getNode()["solver"]);
+ SolverManagerPtr ceres =
+ FactorySolverNode::create("SolverCeres", problem, server.getNode()["solver"], {wolf_path});
// recover sensor pointers and other stuff for later use (access by sensor name)
- SensorBasePtr sensor_odo = problem->findSensor("sen odom");
- SensorBasePtr sensor_rb = problem->findSensor("sen rb");
+ SensorBasePtr sensor_odo = problem->findSensor("sen odom");
+ SensorBasePtr sensor_rb = problem->findSensor("sen rb");
// APPLY PRIOR and SET PROCESSOR ODOM ORIGIN ===================================================
- TimeStamp t(0.0);
+ TimeStamp t(0.0);
FrameBasePtr KF1 = problem->applyPriorOptions(t);
-// std::static_pointer_cast<ProcessorMotion>(problem->getMotionProviderMap().begin()->second)->setOrigin(KF1);
+ // std::static_pointer_cast<ProcessorMotion>(problem->getMotionProviderMap().begin()->second)->setOrigin(KF1);
// SELF CALIBRATION ===================================================
// These few lines control whether we calibrate some sensor parameters or not.
// SELF-CALIBRATION OF SENSOR ORIENTATION
- // Uncomment this line below to achieve sensor self-calibration (of the orientation only, since the position is not observable)
- // sensor_rb->getO()->unfix();
+ // Uncomment this line below to achieve sensor self-calibration (of the orientation only, since the position is not
+ // observable) sensor_rb->getO()->unfix();
// SELF-CALIBRATION OF SENSOR POSITION
- // The position is however not observable, and thus self-calibration would not work. You can try uncommenting the line below.
- // sensor_rb->getP()->unfix();
+ // The position is however not observable, and thus self-calibration would not work. You can try uncommenting the
+ // line below. sensor_rb->getP()->unfix();
// CONFIGURE input data (motion and measurements) ==============================================
// Motion data
- Vector2d motion_data(1.0, 0.0); // Will advance 1m at each keyframe, will not turn.
+ Vector2d motion_data(1.0, 0.0); // Will advance 1m at each keyframe, will not turn.
Matrix2d motion_cov = 0.1 * Matrix2d::Identity();
// landmark observations data
@@ -167,69 +167,75 @@ int main()
// STEP 1 --------------------------------------------------------------
// move zero motion to accept the first keyframe and initialize the processor
- CaptureOdom2dPtr cap_motion = std::make_shared<CaptureOdom2d>(t, sensor_odo, 0*motion_data, 0*motion_cov);
- cap_motion ->process(); // KF1 : (0,0,0)
+ CaptureOdom2dPtr cap_motion = std::make_shared<CaptureOdom2d>(t, sensor_odo, 0 * motion_data, 0 * motion_cov);
+ cap_motion->process(); // KF1 : (0,0,0)
// observe lmks
- ids.resize(1); ranges.resize(1); bearings.resize(1);
- ids << 1; // will observe Lmk 1
- ranges << 1.0; // see drawing
- bearings << M_PI/2;
+ ids.resize(1);
+ ranges.resize(1);
+ bearings.resize(1);
+ ids << 1; // will observe Lmk 1
+ ranges << 1.0; // see drawing
+ bearings << M_PI / 2;
CaptureRangeBearingPtr cap_rb = std::make_shared<CaptureRangeBearing>(t, sensor_rb, ids, ranges, bearings);
- cap_rb->process(); // L1 : (1,2)
+ cap_rb->process(); // L1 : (1,2)
// STEP 2 --------------------------------------------------------------
- t += 1.0; // t : 1.0
+ t += 1.0; // t : 1.0
// motion
cap_motion = std::make_shared<CaptureOdom2d>(t, sensor_odo, motion_data, motion_cov);
- cap_motion ->process(); // KF2 : (1,0,0)
+ cap_motion->process(); // KF2 : (1,0,0)
// observe lmks
- ids.resize(2); ranges.resize(2); bearings.resize(2);
- ids << 1, 2; // will observe Lmks 1 and 2
- ranges << sqrt(2.0), 1.0; // see drawing
- bearings << 3*M_PI/4, M_PI/2;
+ ids.resize(2);
+ ranges.resize(2);
+ bearings.resize(2);
+ ids << 1, 2; // will observe Lmks 1 and 2
+ ranges << sqrt(2.0), 1.0; // see drawing
+ bearings << 3 * M_PI / 4, M_PI / 2;
- cap_rb = std::make_shared<CaptureRangeBearing>(t, sensor_rb, ids, ranges, bearings);
- cap_rb ->process(); // L1 : (1,2), L2 : (2,2)
+ cap_rb = std::make_shared<CaptureRangeBearing>(t, sensor_rb, ids, ranges, bearings);
+ cap_rb->process(); // L1 : (1,2), L2 : (2,2)
// STEP 3 --------------------------------------------------------------
- t += 1.0; // t : 2.0
+ t += 1.0; // t : 2.0
// motion
- cap_motion ->setTimeStamp(t);
- cap_motion ->process(); // KF3 : (2,0,0)
+ cap_motion->setTimeStamp(t);
+ cap_motion->process(); // KF3 : (2,0,0)
// observe lmks
- ids.resize(2); ranges.resize(2); bearings.resize(2);
- ids << 2, 3; // will observe Lmks 2 and 3
- ranges << sqrt(2.0), 1.0; // see drawing
- bearings << 3*M_PI/4, M_PI/2;
+ ids.resize(2);
+ ranges.resize(2);
+ bearings.resize(2);
+ ids << 2, 3; // will observe Lmks 2 and 3
+ ranges << sqrt(2.0), 1.0; // see drawing
+ bearings << 3 * M_PI / 4, M_PI / 2;
- cap_rb = std::make_shared<CaptureRangeBearing>(t, sensor_rb, ids, ranges, bearings);
- cap_rb ->process(); // L1 : (1,2), L2 : (2,2), L3 : (3,2)
+ cap_rb = std::make_shared<CaptureRangeBearing>(t, sensor_rb, ids, ranges, bearings);
+ cap_rb->process(); // L1 : (1,2), L2 : (2,2), L3 : (3,2)
- problem->print(1,0,1,0);
+ problem->print(1, 0, 1, 0);
// SOLVE ================================================================
// SOLVE with exact initial guess
WOLF_INFO("======== SOLVE PROBLEM WITH EXACT PRIORS =======")
std::string report = ceres->solve(wolf::SolverManager::ReportVerbosity::FULL);
- WOLF_INFO(report); // should show a very low iteration number (possibly 1)
- problem->print(1,0,1,0);
+ WOLF_INFO(report); // should show a very low iteration number (possibly 1)
+ problem->print(1, 0, 1, 0);
// PERTURB initial guess
WOLF_INFO("======== PERTURB PROBLEM PRIORS =======")
- problem->perturb(0.5); // Perturb all state blocks that are not fixed
- problem->print(1,0,1,0);
+ problem->perturb(0.5); // Perturb all state blocks that are not fixed
+ problem->print(1, 0, 1, 0);
// SOLVE again
WOLF_INFO("======== SOLVE PROBLEM WITH PERTURBED PRIORS =======")
report = ceres->solve(wolf::SolverManager::ReportVerbosity::FULL);
- WOLF_INFO(report); // should show a very high iteration number (more than 10, or than 100!)
- problem->print(1,0,1,0);
+ WOLF_INFO(report); // should show a very high iteration number (more than 10, or than 100!)
+ problem->print(1, 0, 1, 0);
// GET COVARIANCES of all states
WOLF_INFO("======== COVARIANCES OF SOLVED PROBLEM =======")
@@ -249,7 +255,7 @@ int main()
std::cout << std::endl;
WOLF_INFO("======== FINAL PRINT FOR INTERPRETATION =======")
- problem->print(4,1,1,1);
+ problem->print(4, 1, 1, 1);
/*
* ============= FIRST COMMENT ON THE RESULTS ==================
@@ -276,81 +282,83 @@ int main()
* The full message is explained below.
*
* P: wolf tree status ---------------------
- Hardware
- Sen1 SensorOdom2d "sen odom" // Sensor 1, type odometry 2D
- sb: P[Sta,Fix] = ( 0 0 ); O[Sta,Fix] = ( 0 ); // Static extrinsics, fixed position, fixed orientation (See notes 1 and 2 below)
- PrcM1 ProcessorOdom2d "prc odom" // Processor 1, type odometry 2D
- o: Cap6 - KFrm3 // origin at Capture 6, Frame 3
- l: Cap8 - Frm4 // last at Capture 8, Frame 4
- Sen2 SensorRangeBearing "sen rb" // Sensor 2, type range and bearing
- sb: P[Sta,Fix] = ( 1 1 ); O[Sta,Fix] = ( 0 ); // Static extrinsics, fixed position, estimated orientation (See notes 1 and 2 below)
- Prc2 ProcessorRangeBearing "prc rb" // Processor 2, type range and bearing
- Trajectory
- KFrm1 <-- Fac4 // KeyFrame 1, constrained by Factor 4
- P[Est] = ( -4.4e-12 1.5e-09 ) // Position is estimated
- O[Est] = ( 2.6e-09 ) // Orientation is estimated
- Cap1 CaptureVoid -> Sen- <-- // This is an "artificial" capture used to hold the features relative to the prior
- Ftr1 trk0 prior <-- // Position prior
- m = ( 0 0 )
- Fac1 FactorBlockAbsolute --> Abs
- Ftr2 trk0 prior <-- // Orientation prior
- m = ( 0 )
- Fac2 FactorBlockAbsolute --> Abs
- CapM2 CaptureOdom2d -> Sen1 <-- // Capture 2 of type motion odom 2d from sensor 1.
- buffer size : 0
- Cap4 CaptureRangeBearing -> Sen2 <--
- Ftr3 trk0 FeatureRangeBearing <--
- m = ( 1 1.6 )
- Fac3 RANGE BEARING --> Lmk1
- KFrm2 <-- Fac7
- P[Est] = ( 1 5.7e-09 )
- O[Est] = ( 5.7e-09 )
- CapM3 CaptureOdom2d -> Sen1 -> OCap2 ; OFrm1 <--// Capture 3 of type motion odom2d from sensor 1.
- // Its origin is at Capture 2; Frame 1
- buffer size : 2
- delta preint : (1 0 0)
- Ftr4 trk0 ProcessorOdom2d <--
- m = ( 1 0 0 )
- Fac4 FactorOdom2d --> Frm1
- Cap7 CaptureRangeBearing -> Sen2 <--
- Ftr5 trk0 FeatureRangeBearing <--
- m = ( 1.4 2.4 )
- Fac5 RANGE BEARING --> Lmk1
- Ftr6 trk0 FeatureRangeBearing <--
- m = ( 1 1.6 )
- Fac6 RANGE BEARING --> Lmk2
- KFrm3 <--
- P[Est] = ( 2 1.2e-08 )
- O[Est] = ( 6.6e-09 )
- CapM6 CaptureOdom2d -> Sen1 -> OCap3 ; OFrm2 <--
- buffer size : 2
- delta preint : (1 0 0)
- Ftr7 trk0 ProcessorOdom2d <--
- m = ( 1 0 0 )
- Fac7 FactorOdom2d --> Frm2
- Cap9 CaptureRangeBearing -> Sen2 <--
- Ftr8 trk0 FeatureRangeBearing <--
- m = ( 1.4 2.4 )
- Fac8 RANGE BEARING --> Lmk2
- Ftr9 trk0 FeatureRangeBearing <--
- m = ( 1 1.6 )
- Fac9 RANGE BEARING --> Lmk3
- Frm4 <--
- P[Est] = ( 2 0 )
- O[Est] = ( 0 )
- CapM8 CaptureOdom2d -> Sen1 -> OCap6 ; OFrm3 <--
- buffer size : 1
- delta preint : (0 0 0)
- Map
- Lmk1 LandmarkPoint2d <-- Fac3 Fac5 // Landmark 1 constrained by factors 3 & 5
- P[Est] = ( 1 2 )
- Lmk2 LandmarkPoint2d <-- Fac6 Fac8
- P[Est] = ( 2 2 )
- Lmk3 LandmarkPoint2d <-- Fac9
- P[Est] = ( 3 2 )
- -----------------------------------------
-
- *
+ * Hardware
+ * Sen1 SensorOdom2d "sen odom" // Sensor 1, type odometry 2D
+ * sb: P[Sta,Fix] = ( 0 0 ); O[Sta,Fix] = ( 0 ); // Static extrinsics, fixed position, fixed orientation
+ * // (See notes 1 and 2 below)
+ * PrcM1 ProcessorOdom2d "prc odom" // Processor 1, type odometry 2D
+ * o: Cap6 - KFrm3 // origin at Capture 6, Frame 3
+ * l: Cap8 - Frm4 // last at Capture 8, Frame 4
+ * Sen2 SensorRangeBearing "sen rb" // Sensor 2, type range and bearing
+ * sb: P[Sta,Fix] = ( 1 1 ); O[Sta,Fix] = ( 0 ); // Static extrinsics, fixed position, estimated orientation
+ * // (See notes 1 and 2 below)
+ * Prc2 ProcessorRangeBearing "prc rb" // Processor 2, type range and bearing
+ * Trajectory
+ * KFrm1 <-- Fac4 // KeyFrame 1, constrained by Factor 4
+ * P[Est] = ( -4.4e-12 1.5e-09 ) // Position is estimated
+ * O[Est] = ( 2.6e-09 ) // Orientation is estimated
+ * Cap1 CaptureVoid -> Sen- <-- // This is an "artificial" capture used to hold the
+ * // features relative to the prior
+ * Ftr1 trk0 prior <-- // Position prior
+ * m = ( 0 0 )
+ * Fac1 FactorBlockAbsolute --> Abs
+ * Ftr2 trk0 prior <-- // Orientation prior
+ * m = ( 0 )
+ * Fac2 FactorBlockAbsolute --> Abs
+ * CapM2 CaptureOdom2d -> Sen1 <-- // Capture 2 of type motion odom 2d from sensor 1.
+ * buffer size : 0
+ * Cap4 CaptureRangeBearing -> Sen2 <--
+ * Ftr3 trk0 FeatureRangeBearing <--
+ * m = ( 1 1.6 )
+ * Fac3 RANGE BEARING --> Lmk1
+ * KFrm2 <-- Fac7
+ * P[Est] = ( 1 5.7e-09 )
+ * O[Est] = ( 5.7e-09 )
+ * CapM3 CaptureOdom2d -> Sen1 -> OCap2 ; OFrm1 <--// Capture 3 of type motion odom2d from sensor 1.
+ * // Its origin is at Capture 2; Frame 1
+ * buffer size : 2
+ * delta preint : (1 0 0)
+ * Ftr4 trk0 ProcessorOdom2d <--
+ * m = ( 1 0 0 )
+ * Fac4 FactorOdom2d --> Frm1
+ * Cap7 CaptureRangeBearing -> Sen2 <--
+ * Ftr5 trk0 FeatureRangeBearing <--
+ * m = ( 1.4 2.4 )
+ * Fac5 RANGE BEARING --> Lmk1
+ * Ftr6 trk0 FeatureRangeBearing <--
+ * m = ( 1 1.6 )
+ * Fac6 RANGE BEARING --> Lmk2
+ * KFrm3 <--
+ * P[Est] = ( 2 1.2e-08 )
+ * O[Est] = ( 6.6e-09 )
+ * CapM6 CaptureOdom2d -> Sen1 -> OCap3 ; OFrm2 <--
+ * buffer size : 2
+ * delta preint : (1 0 0)
+ * Ftr7 trk0 ProcessorOdom2d <--
+ * m = ( 1 0 0 )
+ * Fac7 FactorOdom2d --> Frm2
+ * Cap9 CaptureRangeBearing -> Sen2 <--
+ * Ftr8 trk0 FeatureRangeBearing <--
+ * m = ( 1.4 2.4 )
+ * Fac8 RANGE BEARING --> Lmk2
+ * Ftr9 trk0 FeatureRangeBearing <--
+ * m = ( 1 1.6 )
+ * Fac9 RANGE BEARING --> Lmk3
+ * Frm4 <--
+ * P[Est] = ( 2 0 )
+ * O[Est] = ( 0 )
+ * CapM8 CaptureOdom2d -> Sen1 -> OCap6 ; OFrm3 <--
+ * buffer size : 1
+ * delta preint : (0 0 0)
+ * Map
+ * Lmk1 LandmarkPoint2d <-- Fac3 Fac5 // Landmark 1 constrained by factors 3 & 5
+ * P[Est] = ( 1 2 )
+ * Lmk2 LandmarkPoint2d <-- Fac6 Fac8
+ * P[Est] = ( 2 2 )
+ * Lmk3 LandmarkPoint2d <-- Fac9
+ * P[Est] = ( 3 2 )
+ * -----------------------------------------
+ * *
* ============= GENERAL WOLF NOTES ==================
*
* Explanatory notes, general to the Wolf architecture design:
@@ -363,7 +371,8 @@ int main()
* Fixed: they are used as constant values, never estimated
* Estimated: they are estimated by the solver iteratively
*
- * Therefore, in the case of Sensors, each block of parameters can be Static/Dynamic and Fixed/Estimated. This produces 4 combinations:
+ * Therefore, in the case of Sensors, each block of parameters can be Static/Dynamic and Fixed/Estimated. This
+ * produces 4 combinations:
*
* 1 Fixed + Static : general case of calibrated sensor.
* Example: rigidly fixed sensor with calibrated parameters
diff --git a/demos/hello_wolf/landmark_point_2d.cpp b/demos/hello_wolf/landmark_point_2d.cpp
index e5dc4bafa4fd87ee6a937f6960ba75aedf40f1d9..481ca4eeaf8829c471b071fedb8610a17b78b84e 100644
--- a/demos/hello_wolf/landmark_point_2d.cpp
+++ b/demos/hello_wolf/landmark_point_2d.cpp
@@ -23,9 +23,8 @@
namespace wolf
{
-
-LandmarkPoint2d::LandmarkPoint2d(int _id, const Eigen::Vector2d& _xy) :
- LandmarkBase("LandmarkPoint2d", std::make_shared<StatePoint2d>(_xy))
+LandmarkPoint2d::LandmarkPoint2d(int _id, const Eigen::Vector2d& _xy)
+ : LandmarkBase("LandmarkPoint2d", std::make_shared<StatePoint2d>(_xy))
{
setId(_id);
}
diff --git a/demos/hello_wolf/landmark_point_2d.h b/demos/hello_wolf/landmark_point_2d.h
index d86c3e151a3b7bfa46836216d9d83fdd1f040113..753370bf79aaf3f23ab26dfac42fc81dba0b6476 100644
--- a/demos/hello_wolf/landmark_point_2d.h
+++ b/demos/hello_wolf/landmark_point_2d.h
@@ -24,14 +24,13 @@
namespace wolf
{
-
WOLF_PTR_TYPEDEFS(LandmarkPoint2d);
class LandmarkPoint2d : public LandmarkBase
{
- public:
- LandmarkPoint2d(int _id, const Eigen::Vector2d& _xy);
- ~LandmarkPoint2d() override;
+ public:
+ LandmarkPoint2d(int _id, const Eigen::Vector2d& _xy);
+ ~LandmarkPoint2d() override;
};
} /* namespace wolf */
diff --git a/demos/hello_wolf/processor_range_bearing.cpp b/demos/hello_wolf/processor_range_bearing.cpp
index ae6db2157b3510c078d3c76d7bcd07e73a95146f..5475edd8941efaa89b364b513481c1de6fa8c69f 100644
--- a/demos/hello_wolf/processor_range_bearing.cpp
+++ b/demos/hello_wolf/processor_range_bearing.cpp
@@ -17,12 +17,6 @@
//
// 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/>.
-/*
- * processor_range_bearing.cpp
- *
- * Created on: Nov 30, 2017
- * Author: jsola
- */
#include "processor_range_bearing.h"
#include "capture_range_bearing.h"
@@ -32,9 +26,8 @@
namespace wolf
{
-
-ProcessorRangeBearing::ProcessorRangeBearing(ParamsProcessorBasePtr _params) :
- ProcessorBase("ProcessorRangeBearing", 2, _params)
+ProcessorRangeBearing::ProcessorRangeBearing(const YAML::Node& _params)
+ : ProcessorBase("ProcessorRangeBearing", 2, _params)
{
//
}
@@ -49,21 +42,22 @@ void ProcessorRangeBearing::processCapture(CaptureBasePtr _capture)
// 1. get KF
FrameBasePtr keyframe(nullptr);
- if ( !buffer_frame_.empty() )
+ if (!buffer_frame_.empty())
{
// KeyFrame Callback received
- keyframe = buffer_frame_.select( _capture->getTimeStamp(), params_->time_tolerance );
+ keyframe = buffer_frame_.select(_capture->getTimeStamp(), getTimeTolerance());
- buffer_frame_.removeUpTo( _capture->getTimeStamp() );
+ buffer_frame_.removeUpTo(_capture->getTimeStamp());
- assert( keyframe && "Callback KF is not close enough to _capture!");
+ assert(keyframe && "Callback KF is not close enough to _capture!");
}
if (!keyframe)
{
// No KeyFrame callback received -- we assume a KF is available to hold this _capture (checked in assert below)
keyframe = getProblem()->closestFrameToTimeStamp(_capture->getTimeStamp());
- assert( (fabs(keyframe->getTimeStamp() - _capture->getTimeStamp()) < params_->time_tolerance) && "Could not find a KF close enough to _capture!");
+ assert((fabs(keyframe->getTimeStamp() - _capture->getTimeStamp()) < getTimeTolerance()) &&
+ "Could not find a KF close enough to _capture!");
}
// 2. cast incoming capture to the range-and-bearing type, add it to the keyframe
@@ -74,14 +68,14 @@ void ProcessorRangeBearing::processCapture(CaptureBasePtr _capture)
for (SizeEigen i = 0; i < capture_rb->getIds().size(); i++)
{
// extract info
- int id = capture_rb->getId (i);
- double range = capture_rb->getRange (i);
- double bearing = capture_rb->getBearing(i);
+ int id = capture_rb->getId(i);
+ double range = capture_rb->getRange(i);
+ double bearing = capture_rb->getBearing(i);
// 4. create or recover landmark
LandmarkPoint2dPtr lmk;
- auto lmk_it = known_lmks.find(id);
- if ( lmk_it != known_lmks.end() )
+ auto lmk_it = known_lmks.find(id);
+ if (lmk_it != known_lmks.end())
{
// known landmarks : recover landmark
lmk = std::static_pointer_cast<LandmarkPoint2d>(lmk_it->second);
@@ -98,25 +92,16 @@ void ProcessorRangeBearing::processCapture(CaptureBasePtr _capture)
}
// 5. create feature
- Vector2d measurement_rb(range,bearing);
- auto ftr = FeatureBase::emplace<FeatureRangeBearing>(capture_rb,
- measurement_rb,
- getSensor()->computeNoiseCov(measurement_rb));
+ Vector2d measurement_rb(range, bearing);
+ auto ftr = FeatureBase::emplace<FeatureRangeBearing>(
+ capture_rb, measurement_rb, getSensor()->computeNoiseCov(measurement_rb));
// 6. create factor
auto prc = shared_from_this();
- auto fac = FactorBase::emplace<FactorRangeBearing>(ftr,
- capture_rb,
- ftr,
- lmk,
- prc,
- false,
- FAC_ACTIVE);
+ auto fac = FactorBase::emplace<FactorRangeBearing>(ftr, capture_rb, ftr, lmk, prc, false, FAC_ACTIVE);
}
-
}
-
Eigen::Vector2d ProcessorRangeBearing::observe(const Eigen::Vector2d& lmk_w) const
{
return polar(toSensor(lmk_w));
@@ -129,14 +114,14 @@ Eigen::Vector2d ProcessorRangeBearing::invObserve(double r, double b) const
ProcessorRangeBearing::Trf ProcessorRangeBearing::transform(const Eigen::Vector3d& _pose) const
{
- Trf H = Eigen::Translation<double,2>(_pose(0), _pose(1)) * Eigen::Rotation2D<double>(_pose(2)) ;
+ Trf H = Eigen::Translation<double, 2>(_pose(0), _pose(1)) * Eigen::Rotation2D<double>(_pose(2));
return H;
}
ProcessorRangeBearing::Trf ProcessorRangeBearing::transform(const Eigen::Vector2d& _position,
const Eigen::Vector1d& _orientation) const
{
- Trf H = Eigen::Translation<double,2>(_position(0), _position(1)) * Eigen::Rotation2D<double>(_orientation(0)) ;
+ Trf H = Eigen::Translation<double, 2>(_position(0), _position(1)) * Eigen::Rotation2D<double>(_orientation(0));
return H;
}
@@ -148,7 +133,7 @@ Eigen::Vector2d ProcessorRangeBearing::fromSensor(const Eigen::Vector2d& lmk_s)
Eigen::Vector2d ProcessorRangeBearing::toSensor(const Eigen::Vector2d& lmk_w) const
{
-// Trf H_w_r = transform(getSensor()->getP()->getState(), getSensor()->getO()->getState());
+ // Trf H_w_r = transform(getSensor()->getP()->getState(), getSensor()->getO()->getState());
Trf H_w_r = transform(getProblem()->getState().vector("PO"));
return (H_w_r * H_r_s).inverse() * lmk_w;
}
@@ -168,11 +153,11 @@ Eigen::Vector2d ProcessorRangeBearing::rect(double range, double bearing) const
bool ProcessorRangeBearing::storeKeyFrame(FrameBasePtr _frame_ptr)
{
- return true;
+ return true;
}
bool ProcessorRangeBearing::storeCapture(CaptureBasePtr _cap_ptr)
{
- return false;
+ return false;
}
} /* namespace wolf */
@@ -181,5 +166,4 @@ bool ProcessorRangeBearing::storeCapture(CaptureBasePtr _cap_ptr)
namespace wolf
{
WOLF_REGISTER_PROCESSOR(ProcessorRangeBearing)
-} // namespace wolf
-
+} // namespace wolf
diff --git a/demos/hello_wolf/processor_range_bearing.h b/demos/hello_wolf/processor_range_bearing.h
index 4782534ab7f0bf1d00c95f23cf09518ef912c162..64e98aaf45e8a7a2c5ba4300ce1e9ba1440149bc 100644
--- a/demos/hello_wolf/processor_range_bearing.h
+++ b/demos/hello_wolf/processor_range_bearing.h
@@ -28,79 +28,72 @@
namespace wolf
{
-
-WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorRangeBearing);
-
-struct ParamsProcessorRangeBearing : public ParamsProcessorBase
-{
- // We do not need special parameters, but in case you need they should be defined here.
- ParamsProcessorRangeBearing()
- {
- //DEFINED FOR COMPATIBILITY PURPOSES. TO BE REMOVED IN THE FUTURE.
- }
- ParamsProcessorRangeBearing(const YAML::Node& _n) :
- ParamsProcessorBase(_n)
- {
- //
- }
- std::string print() const
- {
- return ParamsProcessorBase::print();
- }
-};
-
using namespace Eigen;
WOLF_PTR_TYPEDEFS(ProcessorRangeBearing);
class ProcessorRangeBearing : public ProcessorBase
{
- public:
- typedef Eigen::Transform<double, 2, Eigen::Isometry> Trf;
-
- ProcessorRangeBearing(ParamsProcessorBasePtr _params);
- ~ProcessorRangeBearing() override {/* empty */}
- void configure(SensorBasePtr _sensor) override;
-
- // Factory method for high level API
- WOLF_PROCESSOR_CREATE(ProcessorRangeBearing, ParamsProcessorRangeBearing);
-
- protected:
- // Implementation of pure virtuals from ProcessorBase
- void processCapture (CaptureBasePtr _capture) override;
- void processKeyFrame (FrameBasePtr _keyframe_ptr) override {};
- bool triggerInCapture (CaptureBasePtr) const override { return true;};
- bool triggerInKeyFrame (FrameBasePtr _keyframe_ptr) const override {return false;}
- bool voteForKeyFrame () const override {return false;}
-
- /** \brief store key frame
- *
- * Returns true if the key frame should be stored
- */
- bool storeKeyFrame(FrameBasePtr) override;
-
- /** \brief store capture
- *
- * Returns true if the capture should be stored
- */
- bool storeCapture(CaptureBasePtr) override;
-
- private:
- // control variables
- Trf H_r_s; // transformation matrix, robot to sensor
- std::map<int, LandmarkBasePtr> known_lmks; // all observed lmks so far
-
- protected:
- // helper methods -- to be used only here -- they would be better off in a separate library e.g. range_bearing_tools.h
- Eigen::Vector2d observe (const Eigen::Vector2d& lmk_w) const;
- Eigen::Vector2d invObserve (double r, double b) const;
- private:
- // helper methods -- to be used only here -- they would be better off in a separate library e.g. range_bearing_tools.h
- Trf transform (const Eigen::Vector3d& _pose) const;
- Trf transform (const Eigen::Vector2d& _position, const Eigen::Vector1d& _orientation) const;
- Eigen::Vector2d fromSensor (const Eigen::Vector2d& lmk_s) const;
- Eigen::Vector2d toSensor (const Eigen::Vector2d& lmk_w) const;
- Eigen::Vector2d polar (const Eigen::Vector2d& rect) const;
- Eigen::Vector2d rect (double range, double bearing) const;
+ public:
+ typedef Eigen::Transform<double, 2, Eigen::Isometry> Trf;
+
+ ProcessorRangeBearing(const YAML::Node& _params);
+ ~ProcessorRangeBearing() override
+ { /* empty */
+ }
+ void configure(SensorBasePtr _sensor) override;
+
+ // Factory method for high level API
+ WOLF_PROCESSOR_CREATE(ProcessorRangeBearing);
+
+ protected:
+ // Implementation of pure virtuals from ProcessorBase
+ void processCapture(CaptureBasePtr _capture) override;
+ void processKeyFrame(FrameBasePtr _keyframe_ptr) override{};
+ bool triggerInCapture(CaptureBasePtr) const override
+ {
+ return true;
+ };
+ bool triggerInKeyFrame(FrameBasePtr _keyframe_ptr) const override
+ {
+ return false;
+ }
+ bool voteForKeyFrame() const override
+ {
+ return false;
+ }
+
+ /** \brief store key frame
+ *
+ * Returns true if the key frame should be stored
+ */
+ bool storeKeyFrame(FrameBasePtr) override;
+
+ /** \brief store capture
+ *
+ * Returns true if the capture should be stored
+ */
+ bool storeCapture(CaptureBasePtr) override;
+
+ private:
+ // control variables
+ Trf H_r_s; // transformation matrix, robot to sensor
+ std::map<int, LandmarkBasePtr> known_lmks; // all observed lmks so far
+
+ protected:
+ // helper methods -- to be used only here -- they would be better off in a separate library e.g.
+ // range_bearing_tools.h
+ Eigen::Vector2d observe(const Eigen::Vector2d& lmk_w) const;
+ Eigen::Vector2d invObserve(double r, double b) const;
+
+ private:
+ // helper methods -- to be used only here -- they would be better off in a separate library e.g.
+ // range_bearing_tools.h
+ Trf transform(const Eigen::Vector3d& _pose) const;
+ Trf transform(const Eigen::Vector2d& _position, const Eigen::Vector1d& _orientation) const;
+ Eigen::Vector2d fromSensor(const Eigen::Vector2d& lmk_s) const;
+ Eigen::Vector2d toSensor(const Eigen::Vector2d& lmk_w) const;
+ Eigen::Vector2d polar(const Eigen::Vector2d& rect) const;
+ Eigen::Vector2d rect(double range, double bearing) const;
};
inline void ProcessorRangeBearing::configure(SensorBasePtr _sensor)
diff --git a/demos/hello_wolf/schema/SensorRangeBearing.schema b/demos/hello_wolf/schema/SensorRangeBearing.schema
index afb6d7a4f115fe9642f7e217b9080d6294440d18..1d510289843e258df3be307754fb0b66ff203a2a 100644
--- a/demos/hello_wolf/schema/SensorRangeBearing.schema
+++ b/demos/hello_wolf/schema/SensorRangeBearing.schema
@@ -11,6 +11,11 @@ noise_bearing_degrees_std:
_doc: Standard deviation of the noise of the bearing measurements (degrees)
states:
+ keys:
+ _value: PO
+ _mandatory: false
+ _type: string
+ _doc: The keys corresponding to the states of the sensor.
P:
follow: SpecStateSensorP2d.schema
O:
diff --git a/demos/hello_wolf/sensor_range_bearing.cpp b/demos/hello_wolf/sensor_range_bearing.cpp
index 11f233a250c6e06266e434ca727fc64f8a8ccb74..2aea3a8c42a4fa8b5fa10891beaa07fec5dbf985 100644
--- a/demos/hello_wolf/sensor_range_bearing.cpp
+++ b/demos/hello_wolf/sensor_range_bearing.cpp
@@ -20,14 +20,14 @@
#include "sensor_range_bearing.h"
-namespace wolf{
-
+namespace wolf
+{
WOLF_PTR_TYPEDEFS(SensorRangeBearing);
-SensorRangeBearing::SensorRangeBearing(ParamsSensorRangeBearingPtr _params,
- const SpecStateSensorComposite& _priors) :
- SensorBase("SensorRangeBearing", 2, _params, _priors),
- params_rb_(_params)
+SensorRangeBearing::SensorRangeBearing(const YAML::Node& _params)
+ : SensorBase("SensorRangeBearing", 2, _params),
+ noise_range_metres_std_(_params["noise_range_metres_std"].as<double>()),
+ noise_bearing_degrees_std_(_params["noise_bearing_degrees_std"].as<double>())
{
}
@@ -36,10 +36,12 @@ SensorRangeBearing::~SensorRangeBearing()
//
}
-Eigen::MatrixXd SensorRangeBearing::computeNoiseCov(const Eigen::VectorXd & _data) const
+Eigen::MatrixXd SensorRangeBearing::computeNoiseCov(const Eigen::VectorXd& _data) const
{
- return (Eigen::Vector2d() << params_rb_->noise_range_metres_std,
- params_rb_->noise_bearing_degrees_std).finished().cwiseAbs2().asDiagonal();
+ return (Eigen::Vector2d() << noise_range_metres_std_, noise_bearing_degrees_std_)
+ .finished()
+ .cwiseAbs2()
+ .asDiagonal();
}
} /* namespace wolf */
@@ -49,4 +51,4 @@ Eigen::MatrixXd SensorRangeBearing::computeNoiseCov(const Eigen::VectorXd & _dat
namespace wolf
{
WOLF_REGISTER_SENSOR(SensorRangeBearing)
-} // namespace wolf
+} // namespace wolf
diff --git a/demos/hello_wolf/sensor_range_bearing.h b/demos/hello_wolf/sensor_range_bearing.h
index 7bdc52c2ad57c01181708d3cf2356f9322c14610..aca391c4a1794f622d9c6196c0ff363a93e0810e 100644
--- a/demos/hello_wolf/sensor_range_bearing.h
+++ b/demos/hello_wolf/sensor_range_bearing.h
@@ -25,41 +25,18 @@
namespace wolf
{
-WOLF_STRUCT_PTR_TYPEDEFS(ParamsSensorRangeBearing);
-
-struct ParamsSensorRangeBearing : public ParamsSensorBase
-{
- double noise_range_metres_std = 0.05;
- double noise_bearing_degrees_std = 0.5;
-
- ParamsSensorRangeBearing() = default;
- ParamsSensorRangeBearing(const YAML::Node& _n) : ParamsSensorBase(_n)
- {
- noise_range_metres_std = _n["noise_range_metres_std"].as<double>();
- noise_bearing_degrees_std = _n["noise_bearing_degrees_std"].as<double>();
- }
- std::string getStatesKeys() const override
- {
- return "PO";
- }
- std::string print() const
- {
- return ParamsSensorBase::print() + "\n" + "noise_range_metres_std: " + toString(noise_range_metres_std) +
- "\n" + "noise_bearing_degrees_std: " + toString(noise_bearing_degrees_std) + "\n";
- }
-};
-
WOLF_PTR_TYPEDEFS(SensorRangeBearing)
class SensorRangeBearing : public SensorBase
{
private:
- ParamsSensorRangeBearingPtr params_rb_;
+ double noise_range_metres_std_ = 0.05;
+ double noise_bearing_degrees_std_ = 0.5;
- SensorRangeBearing(ParamsSensorRangeBearingPtr _params, const SpecStateSensorComposite& _priors);
+ SensorRangeBearing(const YAML::Node& _params);
public:
- WOLF_SENSOR_CREATE(SensorRangeBearing, ParamsSensorRangeBearing);
+ WOLF_SENSOR_CREATE(SensorRangeBearing);
~SensorRangeBearing() override;
diff --git a/demos/hello_wolf/yaml/processor_odom_2d.yaml b/demos/hello_wolf/yaml/processor_odom_2d.yaml
index ded2d752badc3ee26d0a774121607952ebfc1c7f..6d9e0e8867ea8bef83d321c215e21dc9e9a983e3 100644
--- a/demos/hello_wolf/yaml/processor_odom_2d.yaml
+++ b/demos/hello_wolf/yaml/processor_odom_2d.yaml
@@ -5,8 +5,8 @@ unmeasured_perturbation_std: 0.001
keyframe_vote:
voting_active: true
max_time_span: 999
- dist_traveled: 0.95
- angle_turned: 999
+ max_dist_traveled: 0.95
+ max_angle_turned: 999
max_buff_length: 999
cov_det: 999
apply_loss_function: true
diff --git a/demos/solver/test_SPQR.cpp b/demos/solver/test_SPQR.cpp
index 341204219b60d056bf955c0a1640a71416e61f23..010ecb423dd1d324d8472578c5e8f4888b957834 100644
--- a/demos/solver/test_SPQR.cpp
+++ b/demos/solver/test_SPQR.cpp
@@ -17,12 +17,6 @@
//
// 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/>.
-/*
- * test_SPQR.cpp
- *
- * Created on: Jun 18, 2015
- * Author: jvallve
- */
#include <iostream>
#include <Eigen/SPQRSupport>
@@ -31,20 +25,20 @@
using namespace Eigen;
-int main (int argc, char **argv)
+int main(int argc, char **argv)
{
///////////////////////////////////////////////////////////////////////
// Eigen Support SPQR
- SPQR < SparseMatrix<double> > solver;
- //solver.setSPQROrdering(0); // no ordering -> segmentation fault
+ SPQR<SparseMatrix<double> > solver;
+ // solver.setSPQROrdering(0); // no ordering -> segmentation fault
- SparseMatrix<double> matA(4,3);
- matA.coeffRef(0,0) = 0.1;
- matA.coeffRef(1,0) = 0.4;
- matA.coeffRef(1,1) = 0.2;
- matA.coeffRef(2,1) = 0.4;
- matA.coeffRef(2,2) = 0.2;
- matA.coeffRef(3,2) = 0.1;
+ SparseMatrix<double> matA(4, 3);
+ matA.coeffRef(0, 0) = 0.1;
+ matA.coeffRef(1, 0) = 0.4;
+ matA.coeffRef(1, 1) = 0.2;
+ matA.coeffRef(2, 1) = 0.4;
+ matA.coeffRef(2, 2) = 0.2;
+ matA.coeffRef(3, 2) = 0.1;
std::cout << "matA: " << std::endl << matA << std::endl;
@@ -66,46 +60,46 @@ int main (int argc, char **argv)
///////////////////////////////////////////////////////////////////////
// Directly in suitesparse
- cholmod_common Common, *cc ;
- cholmod_sparse A ;
- cholmod_dense *X, *B, *Residual ;
- double rnorm, one [2] = {1,0}, minusone [2] = {-1,0} ;
- int mtype ;
+ cholmod_common Common, *cc;
+ cholmod_sparse A;
+ cholmod_dense *X, *B, *Residual;
+ double rnorm, one[2] = {1, 0}, minusone[2] = {-1, 0};
+ int mtype;
// start CHOLMOD
- cc = &Common ;
- cholmod_l_start (cc) ;
+ cc = &Common;
+ cholmod_l_start(cc);
// load A
A = viewAsCholmod(matA);
- //A = (cholmod_sparse *) cholmod_l_read_matrix (stdin, 1, &mtype, cc) ;
+ // A = (cholmod_sparse *) cholmod_l_read_matrix (stdin, 1, &mtype, cc) ;
std::cout << "A.xtype " << A.xtype << std::endl;
std::cout << "A.nrow " << A.nrow << std::endl;
std::cout << "A.ncol " << A.ncol << std::endl;
// B = ones (size (A,1),1)
- B = cholmod_l_ones (A.nrow, 1, A.xtype, cc) ;
+ B = cholmod_l_ones(A.nrow, 1, A.xtype, cc);
std::cout << "2" << std::endl;
// X = A\B
- //X = SuiteSparseQR <double> (0, SPQR_DEFAULT_TOL, &A, B, cc) ;
- X = SuiteSparseQR <double> (&A, B, cc);
+ // X = SuiteSparseQR <double> (0, SPQR_DEFAULT_TOL, &A, B, cc) ;
+ X = SuiteSparseQR<double>(&A, B, cc);
std::cout << "3" << std::endl;
// rnorm = norm (B-A*X)
- Residual = cholmod_l_copy_dense (B, cc) ;
+ Residual = cholmod_l_copy_dense(B, cc);
std::cout << "4" << std::endl;
- cholmod_l_sdmult (&A, 0, minusone, one, X, Residual, cc) ;
+ cholmod_l_sdmult(&A, 0, minusone, one, X, Residual, cc);
std::cout << "5" << std::endl;
- rnorm = cholmod_l_norm_dense (Residual, 2, cc) ;
- printf ("2-norm of residual: %8.1e\n", rnorm) ;
- printf ("rank %ld\n", cc->SPQR_istat [4]) ;
+ rnorm = cholmod_l_norm_dense(Residual, 2, cc);
+ printf("2-norm of residual: %8.1e\n", rnorm);
+ printf("rank %ld\n", cc->SPQR_istat[4]);
// free everything and finish CHOLMOD
- cholmod_l_free_dense (&Residual, cc) ;
- //cholmod_l_free_sparse (A, cc) ;
- cholmod_l_free_dense (&X, cc) ;
- cholmod_l_free_dense (&B, cc) ;
- cholmod_l_finish (cc) ;
- return (0) ;
+ cholmod_l_free_dense(&Residual, cc);
+ // cholmod_l_free_sparse (A, cc) ;
+ cholmod_l_free_dense(&X, cc);
+ cholmod_l_free_dense(&B, cc);
+ cholmod_l_finish(cc);
+ return (0);
}
diff --git a/demos/solver/test_ccolamd.cpp b/demos/solver/test_ccolamd.cpp
index 1f168493ddefd4c6d03ca5f8079ebf7c3a74f856..5140f30ccd03340f34af8d8e5a53c17900a36a56 100644
--- a/demos/solver/test_ccolamd.cpp
+++ b/demos/solver/test_ccolamd.cpp
@@ -17,17 +17,11 @@
//
// 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/>.
-/*
- * test_ccolamd.cpp
- *
- * Created on: Jun 11, 2015
- * Author: jvallve
- */
// Wolf includes
#include "core/common/wolf.h"
-//std includes
+// std includes
#include <cstdlib>
#include <iostream>
#include <fstream>
@@ -48,7 +42,7 @@
using namespace Eigen;
using namespace wolf;
-//main
+// main
int main(int argc, char *argv[])
{
if (argc != 2 || atoi(argv[1]) < 1)
@@ -60,25 +54,25 @@ int main(int argc, char *argv[])
}
SizeEigen size = atoi(argv[1]);
- SparseMatrix<double, ColMajor, SizeEigen> A(size, size), Aordered(size, size);
- CholmodSupernodalLLT < SparseMatrix<double, ColMajor, SizeEigen> > solver;
- PermutationMatrix<Dynamic, Dynamic, SizeEigen> perm(size);
- CCOLAMDOrdering<SizeEigen> ordering;
+ SparseMatrix<double, ColMajor, SizeEigen> A(size, size), Aordered(size, size);
+ CholmodSupernodalLLT<SparseMatrix<double, ColMajor, SizeEigen> > solver;
+ PermutationMatrix<Dynamic, Dynamic, SizeEigen> perm(size);
+ CCOLAMDOrdering<SizeEigen> ordering;
Matrix<SizeEigen, Dynamic, 1> ordering_factors = Matrix<SizeEigen, Dynamic, 1>::Ones(size);
- VectorXd b(size), bordered(size), xordered(size), x(size);
- clock_t t1, t2, t3;
- double time1, time2, time3;
+ VectorXd b(size), bordered(size), xordered(size), x(size);
+ clock_t t1, t2, t3;
+ double time1, time2, time3;
// BUILD THE PROBLEM ----------------------------
- //Fill A & b
+ // Fill A & b
A.insert(0, 0) = 5;
- b(0) = 1;
+ b(0) = 1;
for (int i = 1; i < size; i++)
{
- A.insert(i, i) = 5;
+ A.insert(i, i) = 5;
A.insert(i, i - 1) = 1;
A.insert(i - 1, i) = 1;
- b(i) = i + 1;
+ b(i) = i + 1;
}
A.insert(size - 1, 0) = 2;
A.insert(0, size - 1) = 2;
@@ -95,15 +89,15 @@ int main(int argc, char *argv[])
std::cout << "decomposition failed" << std::endl;
return 0;
}
- x = solver.solve(b);
- time1 = ((double) clock() - t1) / CLOCKS_PER_SEC;
+ x = solver.solve(b);
+ time1 = ((double)clock() - t1) / CLOCKS_PER_SEC;
std::cout << "solved in " << time1 << "seconds" << std::endl;
std::cout << "x = " << x.transpose() << std::endl;
// SOLVING AFTER REORDERING ------------------------------------
// ordering factors
- ordering_factors(size-1) = 2;
- ordering_factors(0) = 2;
+ ordering_factors(size - 1) = 2;
+ ordering_factors(0) = 2;
// ordering
t2 = clock();
@@ -127,7 +121,7 @@ int main(int argc, char *argv[])
return 0;
}
xordered = solver.solve(bordered);
- time2 = ((double) clock() - t2) / CLOCKS_PER_SEC;
+ time2 = ((double)clock() - t2) / CLOCKS_PER_SEC;
std::cout << "solved in " << time2 << "seconds" << std::endl;
std::cout << "x = " << (perm.inverse() * xordered).transpose() << std::endl;
std::cout << "x = " << x.transpose() << " (solution without reordering)" << std::endl;
@@ -141,9 +135,8 @@ int main(int argc, char *argv[])
std::cout << "decomposition failed" << std::endl;
return 0;
}
- x = solver2.solve(b);
- time3 = ((double) clock() - t3) / CLOCKS_PER_SEC;
+ x = solver2.solve(b);
+ time3 = ((double)clock() - t3) / CLOCKS_PER_SEC;
std::cout << "solved in " << time3 << "seconds" << std::endl;
std::cout << "x = " << x.transpose() << std::endl;
}
-
diff --git a/demos/solver/test_ccolamd_blocks.cpp b/demos/solver/test_ccolamd_blocks.cpp
index 11e1148ea7df58ea9878a4fd3c0250ac6d83238c..b3011a4c80ace99f688f34526549447abf416387 100644
--- a/demos/solver/test_ccolamd_blocks.cpp
+++ b/demos/solver/test_ccolamd_blocks.cpp
@@ -17,14 +17,8 @@
//
// 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/>.
-/*
- * test_ccolamd_blocks.cpp
- *
- * Created on: Jun 12, 2015
- * Author: jvallve
- */
-
-//std includes
+
+// std includes
#include <cstdlib>
#include <iostream>
#include <fstream>
@@ -44,36 +38,43 @@
using namespace Eigen;
-void eraseSparseBlock(SparseMatrix<double>& original, const unsigned int& row, const unsigned int& Nrows, const unsigned int& col, const unsigned int& Ncols)
+void eraseSparseBlock(SparseMatrix<double>& original,
+ const unsigned int& row,
+ const unsigned int& Nrows,
+ const unsigned int& col,
+ const unsigned int& Ncols)
{
- for (unsigned int i = row; i < row + Nrows; i++)
- for (unsigned int j = col; j < row + Ncols; j++)
- original.coeffRef(i,j) = 0.0;
+ for (unsigned int i = row; i < row + Nrows; i++)
+ for (unsigned int j = col; j < row + Ncols; j++) original.coeffRef(i, j) = 0.0;
- original.makeCompressed();
+ original.makeCompressed();
}
-void addSparseBlock(const MatrixXd& ins, SparseMatrix<double>& original, const unsigned int& row, const unsigned int& col)
+void addSparseBlock(const MatrixXd& ins,
+ SparseMatrix<double>& original,
+ const unsigned int& row,
+ const unsigned int& col)
{
- for (unsigned int r=0; r<ins.rows(); ++r)
- for (unsigned int c = 0; c < ins.cols(); c++)
- original.coeffRef(r + row, c + col) += ins(r,c);
+ for (unsigned int r = 0; r < ins.rows(); ++r)
+ for (unsigned int c = 0; c < ins.cols(); c++) original.coeffRef(r + row, c + col) += ins(r, c);
}
-void permutation_2_block_permutation(const PermutationMatrix<Dynamic, Dynamic, int> &perm, PermutationMatrix<Dynamic, Dynamic, int> &perm_blocks, const int dim, const int size)
+void permutation_2_block_permutation(const PermutationMatrix<Dynamic, Dynamic, int>& perm,
+ PermutationMatrix<Dynamic, Dynamic, int>& perm_blocks,
+ const int dim,
+ const int size)
{
ArrayXXi idx(dim, size);
idx.row(0) = dim * perm.indices().transpose();
- for (int i = 1; i<dim; i++)
- idx.row(i) = idx.row(i-1) + 1;
- Map<ArrayXi> idx_blocks(idx.data(), dim*size, 1);
+ for (int i = 1; i < dim; i++) idx.row(i) = idx.row(i - 1) + 1;
+ Map<ArrayXi> idx_blocks(idx.data(), dim * size, 1);
perm_blocks.indices() = idx_blocks;
}
-//main
-int main(int argc, char *argv[])
+// main
+int main(int argc, char* argv[])
{
- if (argc != 3 || atoi(argv[1]) < 1|| atoi(argv[2]) < 1)
+ if (argc != 3 || atoi(argv[1]) < 1 || atoi(argv[2]) < 1)
{
std::cout << "Please call me with: [./test_ccolamd SIZE DIM], where:" << std::endl;
std::cout << " - SIZE: integer size of the problem" << std::endl;
@@ -82,43 +83,45 @@ int main(int argc, char *argv[])
return -1;
}
int size = atoi(argv[1]);
- int dim = atoi(argv[2]);
-
- SparseMatrix<double> H(size * dim, size * dim), H_ordered(size * dim, size * dim), H_block_ordered(size * dim, size * dim);
- SparseMatrix<int> FactorMatrix(size,size);
- CholmodSupernodalLLT < SparseMatrix<double> > solver, solver2, solver3;
- PermutationMatrix<Dynamic, Dynamic, int> perm(size), perm_blocks(size * dim);
- CCOLAMDOrdering<int> ordering;
- VectorXi block_ordering_factors = VectorXi::Ones(size);
- VectorXi ordering_factors = VectorXi::Ones(size*dim);
- VectorXd b(size * dim), b_ordered(size * dim), b_block_ordered(size * dim), x_block_ordered(size * dim), x_ordered(size * dim), x(size * dim);
+ int dim = atoi(argv[2]);
+
+ SparseMatrix<double> H(size * dim, size * dim), H_ordered(size * dim, size * dim),
+ H_block_ordered(size * dim, size * dim);
+ SparseMatrix<int> FactorMatrix(size, size);
+ CholmodSupernodalLLT<SparseMatrix<double> > solver, solver2, solver3;
+ PermutationMatrix<Dynamic, Dynamic, int> perm(size), perm_blocks(size * dim);
+ CCOLAMDOrdering<int> ordering;
+ VectorXi block_ordering_factors = VectorXi::Ones(size);
+ VectorXi ordering_factors = VectorXi::Ones(size * dim);
+ VectorXd b(size * dim), b_ordered(size * dim), b_block_ordered(size * dim), x_block_ordered(size * dim),
+ x_ordered(size * dim), x(size * dim);
clock_t t1, t2, t3;
- double time1, time2, time3;
+ double time1, time2, time3;
MatrixXd omega = MatrixXd::Constant(dim, dim, 0.1) + MatrixXd::Identity(dim, dim);
// BUILD THE PROBLEM ----------------------------
- //Fill H & b
+ // Fill H & b
for (int i = 0; i < size; i++)
{
- addSparseBlock(5*omega, H, i*dim, i*dim);
- FactorMatrix.insert(i,i) = 1;
+ addSparseBlock(5 * omega, H, i * dim, i * dim);
+ FactorMatrix.insert(i, i) = 1;
if (i > 0)
{
- addSparseBlock(omega, H, i*dim, (i-1)*dim);
- addSparseBlock(omega, H, (i-1)*dim, i*dim);
- FactorMatrix.insert(i,i-1) = 1;
- FactorMatrix.insert(i-1,i) = 1;
+ addSparseBlock(omega, H, i * dim, (i - 1) * dim);
+ addSparseBlock(omega, H, (i - 1) * dim, i * dim);
+ FactorMatrix.insert(i, i - 1) = 1;
+ FactorMatrix.insert(i - 1, i) = 1;
}
- b.segment(i*dim, dim) = VectorXd::Constant(dim, i+1);
+ b.segment(i * dim, dim) = VectorXd::Constant(dim, i + 1);
}
- addSparseBlock(2*omega, H, 0, (size - 1)*dim);
- addSparseBlock(2*omega, H, (size-1)*dim, 0);
- FactorMatrix.insert(0,size-1) = 1;
- FactorMatrix.insert(size-1,0) = 1;
+ addSparseBlock(2 * omega, H, 0, (size - 1) * dim);
+ addSparseBlock(2 * omega, H, (size - 1) * dim, 0);
+ FactorMatrix.insert(0, size - 1) = 1;
+ FactorMatrix.insert(size - 1, 0) = 1;
std::cout << "Solving factor graph:" << std::endl;
- std::cout << "Factors: " << std::endl << FactorMatrix * MatrixXi::Identity(size,size) << std::endl << std::endl;
+ std::cout << "Factors: " << std::endl << FactorMatrix * MatrixXi::Identity(size, size) << std::endl << std::endl;
// SOLVING WITHOUT REORDERING ------------------------------------
// solve Hx = b
@@ -129,13 +132,13 @@ int main(int argc, char *argv[])
std::cout << "decomposition failed" << std::endl;
return 0;
}
- x = solver.solve(b);
- time1 = ((double) clock() - t1) / CLOCKS_PER_SEC;
+ x = solver.solve(b);
+ time1 = ((double)clock() - t1) / CLOCKS_PER_SEC;
// SOLVING AFTER REORDERING ------------------------------------
// ordering factors
- ordering_factors.segment(dim * (size-1), dim) = VectorXi::Constant(dim,2);
- ordering_factors.segment(0, dim) = VectorXi::Constant(dim,2);
+ ordering_factors.segment(dim * (size - 1), dim) = VectorXi::Constant(dim, 2);
+ ordering_factors.segment(0, dim) = VectorXi::Constant(dim, 2);
// variable ordering
t2 = clock();
@@ -157,23 +160,25 @@ int main(int argc, char *argv[])
}
x_ordered = solver2.solve(b_ordered);
x_ordered = perm.inverse() * x_ordered;
- time2 = ((double) clock() - t2) / CLOCKS_PER_SEC;
+ time2 = ((double)clock() - t2) / CLOCKS_PER_SEC;
// SOLVING AFTER BLOCK REORDERING ------------------------------------
// ordering factors
- block_ordering_factors(size-1) = 2;
- block_ordering_factors(0) = 2;
+ block_ordering_factors(size - 1) = 2;
+ block_ordering_factors(0) = 2;
// block ordering
t3 = clock();
FactorMatrix.makeCompressed();
std::cout << "Reordering using Block CCOLAMD:" << std::endl;
- std::cout << "block_ordering_factors = " << std::endl << block_ordering_factors.transpose() << std::endl << std::endl;
+ std::cout << "block_ordering_factors = " << std::endl
+ << block_ordering_factors.transpose() << std::endl
+ << std::endl;
ordering(FactorMatrix, perm_blocks, block_ordering_factors.data());
// variable ordering
- permutation_2_block_permutation(perm_blocks, perm , dim, size);
+ permutation_2_block_permutation(perm_blocks, perm, dim, size);
b_block_ordered = perm * b;
H_block_ordered = H.twistedBy(perm);
@@ -186,14 +191,13 @@ int main(int argc, char *argv[])
}
x_block_ordered = solver3.solve(b_block_ordered);
x_block_ordered = perm.inverse() * x_block_ordered;
- time3 = ((double) clock() - t3) / CLOCKS_PER_SEC;
+ time3 = ((double)clock() - t3) / CLOCKS_PER_SEC;
// RESULTS ------------------------------------
- std::cout << "NO REORDERING: solved in " << time1*1e3 << " ms" << std::endl;
- std::cout << "REORDERING: solved in " << time2*1e3 << " ms" << std::endl;
- std::cout << "BLOCK REORDERING: solved in " << time3*1e3 << " ms" << std::endl;
- //std::cout << "x = " << x.transpose() << std::endl;
- //std::cout << "x = " << x_ordered.transpose() << std::endl;
- //std::cout << "x = " << x_block_ordered.transpose() << std::endl;
+ std::cout << "NO REORDERING: solved in " << time1 * 1e3 << " ms" << std::endl;
+ std::cout << "REORDERING: solved in " << time2 * 1e3 << " ms" << std::endl;
+ std::cout << "BLOCK REORDERING: solved in " << time3 * 1e3 << " ms" << std::endl;
+ // std::cout << "x = " << x.transpose() << std::endl;
+ // std::cout << "x = " << x_ordered.transpose() << std::endl;
+ // std::cout << "x = " << x_block_ordered.transpose() << std::endl;
}
-
diff --git a/demos/solver/test_iQR.cpp b/demos/solver/test_iQR.cpp
index e880992a348bc212764cd3a99048b7f02f770807..4b09adc757924a167df88bcb74130db1f5dffa6a 100644
--- a/demos/solver/test_iQR.cpp
+++ b/demos/solver/test_iQR.cpp
@@ -17,21 +17,8 @@
//
// 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/>.
-/*
- * test_iQR.cpp
- *
- * Created on: Jun 17, 2015
- * Author: jvallve
- */
-
-/*
- * test_ccolamd_blocks.cpp
- *
- * Created on: Jun 12, 2015
- * Author: jvallve
- */
-
-//std includes
+
+// std includes
#include <cstdlib>
#include <iostream>
#include <fstream>
@@ -53,72 +40,76 @@ using namespace Eigen;
class block_pruning
{
- public:
- int col, row, Nrows, Ncols;
- block_pruning(int _col, int _row, int _Nrows, int _Ncols) :
- col(_col),
- row(_row),
- Nrows(_Nrows),
- Ncols(_Ncols)
- {
- //
- }
- bool operator()(int i, int j, double) const
- {
- return (i < row || i > row + Nrows-1) || (j < col || j > col + Ncols-1);
- }
+ public:
+ int col, row, Nrows, Ncols;
+ block_pruning(int _col, int _row, int _Nrows, int _Ncols) : col(_col), row(_row), Nrows(_Nrows), Ncols(_Ncols)
+ {
+ //
+ }
+ bool operator()(int i, int j, double) const
+ {
+ return (i < row || i > row + Nrows - 1) || (j < col || j > col + Ncols - 1);
+ }
};
-void eraseSparseBlock(SparseMatrix<double>& original, const unsigned int& row, const unsigned int& col, const unsigned int& Nrows, const unsigned int& Ncols)
+void eraseSparseBlock(SparseMatrix<double>& original,
+ const unsigned int& row,
+ const unsigned int& col,
+ const unsigned int& Nrows,
+ const unsigned int& Ncols)
{
// prune all non-zero elements that not satisfy the 'keep' operand
// elements that are not in the block rows or are not in the block columns should be kept
- //original.prune([](int i, int j, double) { return (i < row || i > row + Nrows-1) || (j < col || j > col + Ncols-1); });
+ // original.prune([](int i, int j, double) { return (i < row || i > row + Nrows-1) || (j < col || j > col +
+ // Ncols-1); });
block_pruning bp(row, col, Nrows, Ncols);
original.prune(bp);
-// for (unsigned int i = row; i < row + Nrows; i++)
-// for (unsigned int j = col; j < row + Ncols; j++)
-// original.coeffRef(i,j) = 0.0;
-//
-// original.prune(0);
+ // for (unsigned int i = row; i < row + Nrows; i++)
+ // for (unsigned int j = col; j < row + Ncols; j++)
+ // original.coeffRef(i,j) = 0.0;
+ //
+ // original.prune(0);
}
-void addSparseBlock(const MatrixXd& ins, SparseMatrix<double>& original, const unsigned int& row, const unsigned int& col)
+void addSparseBlock(const MatrixXd& ins,
+ SparseMatrix<double>& original,
+ const unsigned int& row,
+ const unsigned int& col)
{
- for (unsigned int r=0; r<ins.rows(); ++r)
- for (unsigned int c = 0; c < ins.cols(); c++)
- if (ins(r,c) != 0)
- original.coeffRef(r + row, c + col) += ins(r,c);
+ for (unsigned int r = 0; r < ins.rows(); ++r)
+ for (unsigned int c = 0; c < ins.cols(); c++)
+ if (ins(r, c) != 0) original.coeffRef(r + row, c + col) += ins(r, c);
}
-void permutation_2_block_permutation(const PermutationMatrix<Dynamic, Dynamic, int> &perm_nodes, PermutationMatrix<Dynamic, Dynamic, int> &perm_variables, const int dim)
+void permutation_2_block_permutation(const PermutationMatrix<Dynamic, Dynamic, int>& perm_nodes,
+ PermutationMatrix<Dynamic, Dynamic, int>& perm_variables,
+ const int dim)
{
ArrayXXi idx(dim, perm_nodes.indices().rows());
idx.row(0) = dim * perm_nodes.indices().transpose();
- for (int i = 1; i<dim; i++)
- idx.row(i) = idx.row(i-1) + 1;
- Map<ArrayXi> idx_blocks(idx.data(), dim*perm_nodes.indices().rows(), 1);
+ for (int i = 1; i < dim; i++) idx.row(i) = idx.row(i - 1) + 1;
+ Map<ArrayXi> idx_blocks(idx.data(), dim * perm_nodes.indices().rows(), 1);
perm_variables.indices() = idx_blocks;
}
-void augment_permutation(PermutationMatrix<Dynamic, Dynamic, int> &perm, const int new_size)
+void augment_permutation(PermutationMatrix<Dynamic, Dynamic, int>& perm, const int new_size)
{
- int old_size = perm.indices().size();
- int dim = new_size - old_size;
+ int old_size = perm.indices().size();
+ int dim = new_size - old_size;
VectorXi new_indices(new_size);
- new_indices.head(old_size)= perm.indices();
- new_indices.tail(dim) = ArrayXi::LinSpaced(dim, old_size, new_size-1);
+ new_indices.head(old_size) = perm.indices();
+ new_indices.tail(dim) = ArrayXi::LinSpaced(dim, old_size, new_size - 1);
perm.resize(new_size);
perm.indices() = new_indices;
}
-//main
-int main(int argc, char *argv[])
+// main
+int main(int argc, char* argv[])
{
- if (argc != 3 || atoi(argv[1]) < 1|| atoi(argv[2]) < 1)
+ if (argc != 3 || atoi(argv[1]) < 1 || atoi(argv[2]) < 1)
{
std::cout << "Please call me with: [./test_iQR SIZE DIM], where:" << std::endl;
std::cout << " - SIZE: integer size of the problem" << std::endl;
@@ -127,32 +118,27 @@ int main(int argc, char *argv[])
return -1;
}
int size = atoi(argv[1]);
- int dim = atoi(argv[2]);
+ int dim = atoi(argv[2]);
// Problem variables
- SparseQR < SparseMatrix<double>, NaturalOrdering<int>> solver_ordered, solver_unordered, solver_ordered_partial;
-
- MatrixXd omega = MatrixXd::Constant(dim, dim, 0.1) + MatrixXd::Identity(dim, dim);
- SparseMatrix<double> A(0,0),
- A_ordered(0,0),
- R(0,0);
- VectorXd b,
- x,
- x_ordered,
- x_ordered_partial;
- int n_measurements = 0;
- int n_nodes = 0;
+ SparseQR<SparseMatrix<double>, NaturalOrdering<int>> solver_ordered, solver_unordered, solver_ordered_partial;
+
+ MatrixXd omega = MatrixXd::Constant(dim, dim, 0.1) + MatrixXd::Identity(dim, dim);
+ SparseMatrix<double> A(0, 0), A_ordered(0, 0), R(0, 0);
+ VectorXd b, x, x_ordered, x_ordered_partial;
+ int n_measurements = 0;
+ int n_nodes = 0;
// ordering variables
- SparseMatrix<int> A_nodes_ordered(0,0);
+ SparseMatrix<int> A_nodes_ordered(0, 0);
PermutationMatrix<Dynamic, Dynamic, int> acc_permutation_nodes_matrix(0);
CCOLAMDOrdering<int> ordering, partial_ordering;
- VectorXi nodes_ordering_factors;
+ VectorXi nodes_ordering_factors;
// results variables
clock_t t_ordering, t_solving_ordered_full, t_solving_unordered, t_solving_ordered_partial, t4;
- double time_ordering=0, time_solving_unordered=0, time_solving_ordered=0, time_solving_ordered_partial=0;
+ double time_ordering = 0, time_solving_unordered = 0, time_solving_ordered = 0, time_solving_ordered_partial = 0;
std::cout << "STARTING INCREMENTAL QR TEST" << std::endl << std::endl;
@@ -161,20 +147,20 @@ int main(int argc, char *argv[])
for (int i = 0; i < size; i++)
{
std::vector<int> meas(0);
- if (i == 0) //prior
+ if (i == 0) // prior
{
meas.push_back(0);
measurements.push_back(meas);
meas.clear();
}
- else //odometry
+ else // odometry
{
- meas.push_back(i-1);
+ meas.push_back(i - 1);
meas.push_back(i);
measurements.push_back(meas);
meas.clear();
}
- if (i > size / 2) // loop closures
+ if (i > size / 2) // loop closures
{
meas.push_back(0);
meas.push_back(i);
@@ -191,22 +177,22 @@ int main(int argc, char *argv[])
// AUGMENT THE PROBLEM ----------------------------
n_measurements++;
- while (n_nodes < measurement.back()+1)
+ while (n_nodes < measurement.back() + 1)
{
n_nodes++;
// Resize accumulated permutations
augment_permutation(acc_permutation_nodes_matrix, n_nodes);
// Resize state
- x.conservativeResize(n_nodes*dim);
- x_ordered.conservativeResize(n_nodes*dim);
- x_ordered_partial.conservativeResize(n_nodes*dim);
+ x.conservativeResize(n_nodes * dim);
+ x_ordered.conservativeResize(n_nodes * dim);
+ x_ordered_partial.conservativeResize(n_nodes * dim);
}
- A.conservativeResize(n_measurements*dim,n_nodes*dim);
- A_ordered.conservativeResize(n_measurements*dim,n_nodes*dim);
- R.conservativeResize(n_nodes*dim,n_nodes*dim);
- b.conservativeResize(n_measurements*dim);
- A_nodes_ordered.conservativeResize(n_measurements,n_nodes);
+ A.conservativeResize(n_measurements * dim, n_nodes * dim);
+ A_ordered.conservativeResize(n_measurements * dim, n_nodes * dim);
+ R.conservativeResize(n_nodes * dim, n_nodes * dim);
+ b.conservativeResize(n_measurements * dim);
+ A_nodes_ordered.conservativeResize(n_measurements, n_nodes);
// ADD MEASUREMENTS
int min_ordered_node = n_nodes;
@@ -214,31 +200,32 @@ int main(int argc, char *argv[])
{
int ordered_node = acc_permutation_nodes_matrix.indices()(measurement.at(j));
- addSparseBlock(2*omega, A, A.rows()-dim, measurement.at(j) * dim);
- addSparseBlock(2*omega, A_ordered, A_ordered.rows()-dim, ordered_node * dim);
+ addSparseBlock(2 * omega, A, A.rows() - dim, measurement.at(j) * dim);
+ addSparseBlock(2 * omega, A_ordered, A_ordered.rows() - dim, ordered_node * dim);
- A_nodes_ordered.coeffRef(A_nodes_ordered.rows()-1, ordered_node) = 1;
+ A_nodes_ordered.coeffRef(A_nodes_ordered.rows() - 1, ordered_node) = 1;
- b.segment(b.size() - dim, dim) = VectorXd::LinSpaced(dim, b.size()-dim, b.size()-1);
+ b.segment(b.size() - dim, dim) = VectorXd::LinSpaced(dim, b.size() - dim, b.size() - 1);
// store minimum ordered node
- if (min_ordered_node > ordered_node)
- min_ordered_node = ordered_node;
+ if (min_ordered_node > ordered_node) min_ordered_node = ordered_node;
}
-// std::cout << "Solving Ax = b" << std::endl;
-// std::cout << "A_nodes_ordered: " << std::endl << MatrixXi::Identity(A_nodes_ordered.rows(), A_nodes_ordered.rows()) * A_nodes_ordered << std::endl << std::endl;
-// std::cout << "A: " << std::endl << MatrixXd::Identity(A.rows(), A.rows()) * A << std::endl << std::endl;
-// std::cout << "A_ordered: " << std::endl << MatrixXd::Identity(A.rows(), A.rows()) * A_ordered << std::endl << std::endl;
-// std::cout << "b: " << std::endl << b.transpose() << std::endl << std::endl;
+ // std::cout << "Solving Ax = b" << std::endl;
+ // std::cout << "A_nodes_ordered: " << std::endl << MatrixXi::Identity(A_nodes_ordered.rows(),
+ // A_nodes_ordered.rows()) * A_nodes_ordered << std::endl << std::endl; std::cout << "A: " << std::endl
+ // << MatrixXd::Identity(A.rows(), A.rows()) * A << std::endl << std::endl; std::cout << "A_ordered: "
+ // << std::endl << MatrixXd::Identity(A.rows(), A.rows()) * A_ordered << std::endl << std::endl;
+ // std::cout << "b: " << std::endl << b.transpose() << std::endl << std::endl;
// BLOCK REORDERING ------------------------------------
- t_ordering = clock();
- int ordered_nodes = n_nodes - min_ordered_node;
+ t_ordering = clock();
+ int ordered_nodes = n_nodes - min_ordered_node;
int unordered_nodes = n_nodes - ordered_nodes;
- if (n_nodes > 1 && ordered_nodes > 2) // only reordering when involved nodes in the measurement are not the two last ones
+ if (n_nodes > 1 &&
+ ordered_nodes > 2) // only reordering when involved nodes in the measurement are not the two last ones
{
// SUBPROBLEM ORDERING (from first node variable to last one)
- std::cout << "ordering partial problem: " << min_ordered_node << " to "<< n_nodes - 1 << std::endl;
+ std::cout << "ordering partial problem: " << min_ordered_node << " to " << n_nodes - 1 << std::endl;
SparseMatrix<int> sub_A_nodes_ordered = A_nodes_ordered.rightCols(ordered_nodes);
// partial ordering factors
@@ -247,26 +234,36 @@ int main(int argc, char *argv[])
// computing nodes partial ordering
A_nodes_ordered.makeCompressed();
PermutationMatrix<Dynamic, Dynamic, int> partial_permutation_nodes_matrix(ordered_nodes);
- partial_ordering(sub_A_nodes_ordered, partial_permutation_nodes_matrix, nodes_partial_ordering_factors.data());
+ partial_ordering(
+ sub_A_nodes_ordered, partial_permutation_nodes_matrix, nodes_partial_ordering_factors.data());
// node ordering to variable ordering
PermutationMatrix<Dynamic, Dynamic, int> partial_permutation_matrix(A_ordered.cols());
- permutation_2_block_permutation(partial_permutation_nodes_matrix, partial_permutation_matrix , dim);
+ permutation_2_block_permutation(partial_permutation_nodes_matrix, partial_permutation_matrix, dim);
// apply partial orderings
- A_nodes_ordered.rightCols(ordered_nodes) = (A_nodes_ordered.rightCols(ordered_nodes) * partial_permutation_nodes_matrix.transpose()).sparseView();
- A_ordered.rightCols(ordered_nodes * dim) = (A_ordered.rightCols(ordered_nodes * dim) * partial_permutation_matrix.transpose()).sparseView();
- R.rightCols(ordered_nodes * dim) = (R.rightCols(ordered_nodes * dim) * partial_permutation_matrix.transpose()).sparseView();
+ A_nodes_ordered.rightCols(ordered_nodes) =
+ (A_nodes_ordered.rightCols(ordered_nodes) * partial_permutation_nodes_matrix.transpose()).sparseView();
+ A_ordered.rightCols(ordered_nodes * dim) =
+ (A_ordered.rightCols(ordered_nodes * dim) * partial_permutation_matrix.transpose()).sparseView();
+ R.rightCols(ordered_nodes * dim) =
+ (R.rightCols(ordered_nodes * dim) * partial_permutation_matrix.transpose()).sparseView();
// ACCUMULATING PERMUTATIONS
- PermutationMatrix<Dynamic, Dynamic, int> permutation_nodes_matrix(VectorXi::LinSpaced(n_nodes, 0, n_nodes - 1)); // identity permutation
- permutation_nodes_matrix.indices().tail(ordered_nodes) = partial_permutation_nodes_matrix.indices() + VectorXi::Constant(ordered_nodes, n_nodes - ordered_nodes);
+ PermutationMatrix<Dynamic, Dynamic, int> permutation_nodes_matrix(
+ VectorXi::LinSpaced(n_nodes, 0, n_nodes - 1)); // identity permutation
+ permutation_nodes_matrix.indices().tail(ordered_nodes) =
+ partial_permutation_nodes_matrix.indices() +
+ VectorXi::Constant(ordered_nodes, n_nodes - ordered_nodes);
acc_permutation_nodes_matrix = permutation_nodes_matrix * acc_permutation_nodes_matrix;
}
- time_ordering += ((double) clock() - t_ordering) / CLOCKS_PER_SEC;
- // std::cout << "incrementally ordered A Block structure: " << std::endl << MatrixXi::Identity(A_nodes_ordered.rows(), A_nodes_ordered.rows()) * A_nodes_ordered << std::endl << std::endl;
- //std::cout << "ordered A: " << std::endl << MatrixXd::Identity(A_ordered.rows(), A_ordered.rows()) * A_ordered << std::endl << std::endl;
- //std::cout << "b: " << std::endl << b.transpose() << std::endl << std::endl;
+ time_ordering += ((double)clock() - t_ordering) / CLOCKS_PER_SEC;
+ // std::cout << "incrementally ordered A Block structure: " << std::endl <<
+ // MatrixXi::Identity(A_nodes_ordered.rows(), A_nodes_ordered.rows()) * A_nodes_ordered << std::endl <<
+ // std::endl;
+ // std::cout << "ordered A: " << std::endl << MatrixXd::Identity(A_ordered.rows(), A_ordered.rows()) *
+ // A_ordered << std::endl << std::endl; std::cout << "b: " << std::endl << b.transpose() << std::endl <<
+ // std::endl;
// SOLVING
// solving ordered subproblem
@@ -276,21 +273,27 @@ int main(int argc, char *argv[])
// finding measurements block
SparseMatrix<int> measurements_to_initial = A_nodes_ordered.col(min_ordered_node);
-// std::cout << "measurements_to_initial " << measurements_to_initial << std::endl;
-// std::cout << "measurements_to_initial.innerIndex()[measurements_to_initial.outerIndex()[0]] " << measurements_to_initial.innerIndex()[measurements_to_initial.outerIndex()[0]] << std::endl;
+ // std::cout << "measurements_to_initial " << measurements_to_initial << std::endl;
+ // std::cout << "measurements_to_initial.innerIndex()[measurements_to_initial.outerIndex()[0]] " <<
+ // measurements_to_initial.innerIndex()[measurements_to_initial.outerIndex()[0]] << std::endl;
int initial_measurement = measurements_to_initial.innerIndex()[measurements_to_initial.outerIndex()[0]];
- SparseMatrix<double> A_ordered_partial = A_ordered.bottomRightCorner((n_nodes - initial_measurement) * dim, ordered_nodes * dim);
+ SparseMatrix<double> A_ordered_partial =
+ A_ordered.bottomRightCorner((n_nodes - initial_measurement) * dim, ordered_nodes * dim);
solver_ordered_partial.compute(A_ordered_partial);
if (solver_ordered_partial.info() != Success)
{
std::cout << "decomposition failed" << std::endl;
return 0;
}
- std::cout << "R new" << std::endl << MatrixXd::Identity(ordered_nodes * dim, ordered_nodes * dim) * solver_ordered_partial.matrixR() << std::endl;
+ std::cout << "R new" << std::endl
+ << MatrixXd::Identity(ordered_nodes * dim, ordered_nodes * dim) * solver_ordered_partial.matrixR()
+ << std::endl;
x_ordered_partial.tail(ordered_nodes * dim) = solver_ordered_partial.solve(b.tail(ordered_nodes * dim));
- std::cout << "x_ordered_partial.tail(ordered_nodes * dim)" << std::endl << x_ordered_partial.tail(ordered_nodes * dim).transpose() << std::endl;
- // store new part of R (equivalent to R.bottomRightCorner(ordered_nodes * dim, ordered_nodes * dim) = solver3.matrixR();)
+ std::cout << "x_ordered_partial.tail(ordered_nodes * dim)" << std::endl
+ << x_ordered_partial.tail(ordered_nodes * dim).transpose() << std::endl;
+ // store new part of R (equivalent to R.bottomRightCorner(ordered_nodes * dim, ordered_nodes * dim) =
+ // solver3.matrixR();)
eraseSparseBlock(R, unordered_nodes * dim, unordered_nodes * dim, ordered_nodes * dim, ordered_nodes * dim);
addSparseBlock(solver_ordered_partial.matrixR(), R, unordered_nodes * dim, unordered_nodes * dim);
std::cout << "R" << std::endl << MatrixXd::Identity(R.rows(), R.rows()) * R << std::endl;
@@ -310,13 +313,14 @@ int main(int argc, char *argv[])
std::cout << "decomposition failed" << std::endl;
return 0;
}
- x_ordered_partial.head(unordered_nodes * dim) = solver_ordered_partial.solve(b.head(unordered_nodes * dim) - R2 * x_ordered_partial.tail(ordered_nodes * dim));
+ x_ordered_partial.head(unordered_nodes * dim) = solver_ordered_partial.solve(
+ b.head(unordered_nodes * dim) - R2 * x_ordered_partial.tail(ordered_nodes * dim));
}
// undo ordering
PermutationMatrix<Dynamic, Dynamic, int> acc_permutation_matrix(A_ordered.cols());
- permutation_2_block_permutation(acc_permutation_nodes_matrix, acc_permutation_matrix , dim);
+ permutation_2_block_permutation(acc_permutation_nodes_matrix, acc_permutation_matrix, dim);
x_ordered_partial = acc_permutation_matrix.inverse() * x_ordered_partial;
- time_solving_ordered_partial += ((double) clock() - t_solving_ordered_partial) / CLOCKS_PER_SEC;
+ time_solving_ordered_partial += ((double)clock() - t_solving_ordered_partial) / CLOCKS_PER_SEC;
// SOLVING
// full ordered problem
@@ -330,12 +334,13 @@ int main(int argc, char *argv[])
return 0;
}
x_ordered = solver_ordered.solve(b);
- std::cout << "solver_ordered.matrixR()" << std::endl << MatrixXd::Identity(A_ordered.cols(), A_ordered.cols()) * solver_ordered.matrixR() << std::endl;
+ std::cout << "solver_ordered.matrixR()" << std::endl
+ << MatrixXd::Identity(A_ordered.cols(), A_ordered.cols()) * solver_ordered.matrixR() << std::endl;
// undo ordering
PermutationMatrix<Dynamic, Dynamic, int> acc_permutation_matrix2(A_ordered.cols());
- permutation_2_block_permutation(acc_permutation_nodes_matrix, acc_permutation_matrix2 , dim);
+ permutation_2_block_permutation(acc_permutation_nodes_matrix, acc_permutation_matrix2, dim);
x_ordered = acc_permutation_matrix.inverse() * x_ordered;
- time_solving_ordered += ((double) clock() - t_solving_ordered_full) / CLOCKS_PER_SEC;
+ time_solving_ordered += ((double)clock() - t_solving_ordered_full) / CLOCKS_PER_SEC;
// WITHOUT ORDERING
t_solving_unordered = clock();
@@ -346,24 +351,29 @@ int main(int argc, char *argv[])
std::cout << "decomposition failed" << std::endl;
return 0;
}
- //std::cout << "no ordering? " << solver_unordered.colsPermutation().indices().transpose() << std::endl;
+ // std::cout << "no ordering? " << solver_unordered.colsPermutation().indices().transpose() << std::endl;
x = solver_unordered.solve(b);
- std::cout << "solver_unordered.matrixR()" << std::endl << MatrixXd::Identity(A.cols(), A.cols()) * solver_unordered.matrixR() << std::endl;
- time_solving_unordered += ((double) clock() - t_solving_unordered) / CLOCKS_PER_SEC;
+ std::cout << "solver_unordered.matrixR()" << std::endl
+ << MatrixXd::Identity(A.cols(), A.cols()) * solver_unordered.matrixR() << std::endl;
+ time_solving_unordered += ((double)clock() - t_solving_unordered) / CLOCKS_PER_SEC;
// RESULTS ------------------------------------
std::cout << "========================= RESULTS " << i << ":" << std::endl;
- std::cout << "NO REORDERING: solved in " << time_solving_unordered*1e3 << " ms | " << solver_unordered.matrixR().nonZeros() << " nonzeros in R"<< std::endl;
- std::cout << "BLOCK REORDERING: solved in " << time_solving_ordered*1e3 << " ms | " << solver_ordered.matrixR().nonZeros() << " nonzeros in R"<< std::endl;
- std::cout << "BLOCK REORDERING 2: solved in " << time_solving_ordered_partial*1e3 << " ms | " << R.nonZeros() << " nonzeros in R"<< std::endl;
+ std::cout << "NO REORDERING: solved in " << time_solving_unordered * 1e3 << " ms | "
+ << solver_unordered.matrixR().nonZeros() << " nonzeros in R" << std::endl;
+ std::cout << "BLOCK REORDERING: solved in " << time_solving_ordered * 1e3 << " ms | "
+ << solver_ordered.matrixR().nonZeros() << " nonzeros in R" << std::endl;
+ std::cout << "BLOCK REORDERING 2: solved in " << time_solving_ordered_partial * 1e3 << " ms | " << R.nonZeros()
+ << " nonzeros in R" << std::endl;
std::cout << "x = " << x.transpose() << std::endl;
std::cout << "x_ordered = " << x_ordered.transpose() << std::endl;
std::cout << "x_ordered_partial = " << x_ordered_partial.transpose() << std::endl;
- if ((x_ordered_partial-x_ordered).maxCoeff() < 1e-10)
- std::cout << "Both solutions are equals (tolerance " << (x_ordered_partial-x_ordered).maxCoeff() << ")" << std::endl;
+ if ((x_ordered_partial - x_ordered).maxCoeff() < 1e-10)
+ std::cout << "Both solutions are equals (tolerance " << (x_ordered_partial - x_ordered).maxCoeff() << ")"
+ << std::endl;
else
- std::cout << "DIFFERENT SOLUTIONS!!!!!!!! max difference " << (x_ordered_partial-x_ordered).maxCoeff() << std::endl;
+ std::cout << "DIFFERENT SOLUTIONS!!!!!!!! max difference " << (x_ordered_partial - x_ordered).maxCoeff()
+ << std::endl;
}
}
-
diff --git a/demos/solver/test_iQR_wolf.cpp b/demos/solver/test_iQR_wolf.cpp
index 06fdf707ef5b899f9cbded62e29cf9447dc8c6d4..ec7ecc7512014f8a91477c2e24ae131756f36564 100644
--- a/demos/solver/test_iQR_wolf.cpp
+++ b/demos/solver/test_iQR_wolf.cpp
@@ -17,14 +17,8 @@
//
// 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/>.
-/*
- * test_iQR_wolf.cpp
- *
- * Created on: Jun 17, 2015
- * Author: jvallve
- */
-
-//std includes
+
+// std includes
#include <cstdlib>
#include <string>
#include <iostream>
@@ -47,457 +41,477 @@ using namespace Eigen;
class block_pruning
{
- public:
- int col, row, Nrows, Ncols;
- block_pruning(int _col, int _row, int _Nrows, int _Ncols) :
- col(_col),
- row(_row),
- Nrows(_Nrows),
- Ncols(_Ncols)
- {
- //
- }
- bool operator()(int i, int j, double) const
- {
- return (i < row || i > row + Nrows-1) || (j < col || j > col + Ncols-1);
- }
+ public:
+ int col, row, Nrows, Ncols;
+ block_pruning(int _col, int _row, int _Nrows, int _Ncols) : col(_col), row(_row), Nrows(_Nrows), Ncols(_Ncols)
+ {
+ //
+ }
+ bool operator()(int i, int j, double) const
+ {
+ return (i < row || i > row + Nrows - 1) || (j < col || j > col + Ncols - 1);
+ }
};
-void eraseSparseBlock(SparseMatrix<double, ColMajor>& original, const unsigned int& row, const unsigned int& col, const unsigned int& Nrows, const unsigned int& Ncols)
+void eraseSparseBlock(SparseMatrix<double, ColMajor>& original,
+ const unsigned int& row,
+ const unsigned int& col,
+ const unsigned int& Nrows,
+ const unsigned int& Ncols)
{
// prune all non-zero elements that not satisfy the 'keep' operand
// elements that are not in the block rows or are not in the block columns should be kept
- //original.prune([](int i, int j, double) { return (i < row || i > row + Nrows-1) || (j < col || j > col + Ncols-1); });
+ // original.prune([](int i, int j, double) { return (i < row || i > row + Nrows-1) || (j < col || j > col +
+ // Ncols-1); });
block_pruning bp(row, col, Nrows, Ncols);
original.prune(bp);
-// for (unsigned int i = row; i < row + Nrows; i++)
-// for (unsigned int j = col; j < row + Ncols; j++)
-// original.coeffRef(i,j) = 0.0;
-//
-// original.prune(0);
+ // for (unsigned int i = row; i < row + Nrows; i++)
+ // for (unsigned int j = col; j < row + Ncols; j++)
+ // original.coeffRef(i,j) = 0.0;
+ //
+ // original.prune(0);
}
-void addSparseBlock(const MatrixXd& ins, SparseMatrix<double, ColMajor>& original, const unsigned int& row, const unsigned int& col)
+void addSparseBlock(const MatrixXd& ins,
+ SparseMatrix<double, ColMajor>& original,
+ const unsigned int& row,
+ const unsigned int& col)
{
- for (unsigned int r=0; r<ins.rows(); ++r)
- for (unsigned int c = 0; c < ins.cols(); c++)
- if (ins(r,c) != 0)
- original.coeffRef(r + row, c + col) += ins(r,c);
+ for (unsigned int r = 0; r < ins.rows(); ++r)
+ for (unsigned int c = 0; c < ins.cols(); c++)
+ if (ins(r, c) != 0) original.coeffRef(r + row, c + col) += ins(r, c);
}
struct node
{
- public:
- int id;
- int dim;
- int location;
- int order;
-
- node(const int _id, const int _dim, const int _location, const int _order) :
- id(_id),
- dim(_dim),
- location(_location),
- order(_order)
- {
-
- }
+ public:
+ int id;
+ int dim;
+ int location;
+ int order;
+
+ node(const int _id, const int _dim, const int _location, const int _order)
+ : id(_id), dim(_dim), location(_location), order(_order)
+ {
+ }
};
struct measurement
{
- public:
- std::vector<MatrixXd> jacobians;
- std::vector<int> nodes_idx;
- VectorXd error;
- int dim;
- bool odometry_type;
- int location;
-
- measurement(const MatrixXd & _jacobian1, const int _idx1, const VectorXd &_error, const int _meas_dim, bool _odometry_type=false) :
- jacobians({_jacobian1}),
- nodes_idx({_idx1}),
- error(_error),
- dim(_meas_dim),
- odometry_type(_odometry_type),
- location(0)
- {
- //jacobians.push_back(_jacobian1);
- }
-
- measurement(const MatrixXd & _jacobian1, const int _idx1, const MatrixXd & _jacobian2, const int _idx2, const VectorXd &_error, const int _meas_dim, bool _odometry_type=false) :
- jacobians({_jacobian1, _jacobian2}),
- nodes_idx({_idx1, _idx2}),
- error(_error),
- dim(_meas_dim),
- odometry_type(_odometry_type),
- location(0)
- {
+ public:
+ std::vector<MatrixXd> jacobians;
+ std::vector<int> nodes_idx;
+ VectorXd error;
+ int dim;
+ bool odometry_type;
+ int location;
+
+ measurement(const MatrixXd& _jacobian1,
+ const int _idx1,
+ const VectorXd& _error,
+ const int _meas_dim,
+ bool _odometry_type = false)
+ : jacobians({_jacobian1}),
+ nodes_idx({_idx1}),
+ error(_error),
+ dim(_meas_dim),
+ odometry_type(_odometry_type),
+ location(0)
+ {
+ // jacobians.push_back(_jacobian1);
+ }
- }
+ measurement(const MatrixXd& _jacobian1,
+ const int _idx1,
+ const MatrixXd& _jacobian2,
+ const int _idx2,
+ const VectorXd& _error,
+ const int _meas_dim,
+ bool _odometry_type = false)
+ : jacobians({_jacobian1, _jacobian2}),
+ nodes_idx({_idx1, _idx2}),
+ error(_error),
+ dim(_meas_dim),
+ odometry_type(_odometry_type),
+ location(0)
+ {
+ }
};
class SolverQR
{
- protected:
- std::string name_;
- SparseQR < SparseMatrix<double, ColMajor>, NaturalOrdering<int>> solver_;
- SparseMatrix<double, ColMajor> A_, R_;
- VectorXd b_, x_incr_;
- int n_measurements;
- int n_nodes_;
- std::vector<node> nodes_;
- std::vector<measurement> measurements_;
-
- // ordering
- SparseMatrix<int, ColMajor> A_nodes_;
- PermutationMatrix<Dynamic, Dynamic, int> acc_node_permutation_;
-
- CCOLAMDOrdering<int> orderer_;
- VectorXi node_ordering_restrictions_;
- int first_ordered_node_;
-
- // time
- clock_t t_ordering_, t_solving_, t_managing_;
- double time_ordering_, time_solving_, time_managing_;
-
- public:
- SolverQR(const std::string &_name) :
- name_(_name),
- A_(0,0),
- R_(0,0),
-// b_(0),
-// x_(0),
- n_measurements(0),
- n_nodes_(0),
- A_nodes_(0,0),
- acc_node_permutation_(0),
-// nodes_(0),
-// measurements_(0),
- first_ordered_node_(0),
- t_ordering_(0),
- t_solving_(0),
- t_managing_(0),
- time_ordering_(0),
- time_solving_(0),
- time_managing_(0)
- {
- //
- }
+ protected:
+ std::string name_;
+ SparseQR<SparseMatrix<double, ColMajor>, NaturalOrdering<int>> solver_;
+ SparseMatrix<double, ColMajor> A_, R_;
+ VectorXd b_, x_incr_;
+ int n_measurements;
+ int n_nodes_;
+ std::vector<node> nodes_;
+ std::vector<measurement> measurements_;
+
+ // ordering
+ SparseMatrix<int, ColMajor> A_nodes_;
+ PermutationMatrix<Dynamic, Dynamic, int> acc_node_permutation_;
+
+ CCOLAMDOrdering<int> orderer_;
+ VectorXi node_ordering_restrictions_;
+ int first_ordered_node_;
+
+ // time
+ clock_t t_ordering_, t_solving_, t_managing_;
+ double time_ordering_, time_solving_, time_managing_;
+
+ public:
+ SolverQR(const std::string& _name)
+ : name_(_name),
+ A_(0, 0),
+ R_(0, 0),
+ // b_(0),
+ // x_(0),
+ n_measurements(0),
+ n_nodes_(0),
+ A_nodes_(0, 0),
+ acc_node_permutation_(0),
+ // nodes_(0),
+ // measurements_(0),
+ first_ordered_node_(0),
+ t_ordering_(0),
+ t_solving_(0),
+ t_managing_(0),
+ time_ordering_(0),
+ time_solving_(0),
+ time_managing_(0)
+ {
+ //
+ }
- virtual ~SolverQR()
- {
-
- }
+ virtual ~SolverQR() {}
- void add_state_unit(const int node_dim, const int node_idx)
- {
- t_managing_ = clock();
+ void add_state_unit(const int node_dim, const int node_idx)
+ {
+ t_managing_ = clock();
- n_nodes_++;
- nodes_.push_back(node(node_idx, node_dim, x_incr_.size(), n_nodes_-1));
+ n_nodes_++;
+ nodes_.push_back(node(node_idx, node_dim, x_incr_.size(), n_nodes_ - 1));
- // Resize accumulated permutations
- augment_permutation(acc_node_permutation_, n_nodes_);
+ // Resize accumulated permutations
+ augment_permutation(acc_node_permutation_, n_nodes_);
- // Resize state
- x_incr_.conservativeResize(x_incr_.size() + node_dim);
+ // Resize state
+ x_incr_.conservativeResize(x_incr_.size() + node_dim);
- // Resize problem
- A_.conservativeResize(A_.rows(), A_.cols() + node_dim);
- R_.conservativeResize(R_.cols() + node_dim, R_.cols() + node_dim);
- //A_nodes_.conservativeResize(n_measurements, n_nodes); // not necessary
+ // Resize problem
+ A_.conservativeResize(A_.rows(), A_.cols() + node_dim);
+ R_.conservativeResize(R_.cols() + node_dim, R_.cols() + node_dim);
+ // A_nodes_.conservativeResize(n_measurements, n_nodes); // not necessary
- time_managing_ += ((double) clock() - t_managing_) / CLOCKS_PER_SEC;
- }
+ time_managing_ += ((double)clock() - t_managing_) / CLOCKS_PER_SEC;
+ }
- void addFactor(const measurement& _meas)
+ void addFactor(const measurement& _meas)
+ {
+ t_managing_ = clock();
+
+ assert(_meas.jacobians.size() == _meas.nodes_idx.size());
+ assert(_meas.error.size() == _meas.dim);
+
+ n_measurements++;
+ measurements_.push_back(_meas);
+ measurements_.back().location = A_.rows();
+
+ // Resize problem
+ A_.conservativeResize(A_.rows() + _meas.dim, A_.cols());
+ b_.conservativeResize(b_.size() + _meas.dim);
+ A_nodes_.conservativeResize(n_measurements, n_nodes_);
+
+ // ADD MEASUREMENTS
+ first_ordered_node_ = n_nodes_;
+ for (unsigned int j = 0; j < _meas.nodes_idx.size(); j++)
{
- t_managing_ = clock();
+ assert(acc_node_permutation_.indices()(_meas.nodes_idx.at(j)) == nodes_.at(_meas.nodes_idx.at(j)).order);
- assert(_meas.jacobians.size() == _meas.nodes_idx.size());
- assert(_meas.error.size() == _meas.dim);
+ int ordered_node =
+ nodes_.at(_meas.nodes_idx.at(j)).order; // acc_permutation_nodes_.indices()(_nodes_idx.at(j));
- n_measurements++;
- measurements_.push_back(_meas);
- measurements_.back().location = A_.rows();
+ addSparseBlock(
+ _meas.jacobians.at(j), A_, A_.rows() - _meas.dim, nodes_.at(_meas.nodes_idx.at(j)).location);
- // Resize problem
- A_.conservativeResize(A_.rows() + _meas.dim, A_.cols());
- b_.conservativeResize(b_.size() + _meas.dim);
- A_nodes_.conservativeResize(n_measurements,n_nodes_);
+ A_nodes_.coeffRef(A_nodes_.rows() - 1, ordered_node) = 1;
- // ADD MEASUREMENTS
- first_ordered_node_ = n_nodes_;
- for (unsigned int j = 0; j < _meas.nodes_idx.size(); j++)
- {
- assert(acc_node_permutation_.indices()(_meas.nodes_idx.at(j)) == nodes_.at(_meas.nodes_idx.at(j)).order);
+ assert(_meas.jacobians.at(j).cols() == nodes_.at(_meas.nodes_idx.at(j)).dim);
+ assert(_meas.jacobians.at(j).rows() == _meas.dim);
- int ordered_node = nodes_.at(_meas.nodes_idx.at(j)).order;//acc_permutation_nodes_.indices()(_nodes_idx.at(j));
+ // store minimum ordered node
+ if (first_ordered_node_ > ordered_node) first_ordered_node_ = ordered_node;
+ }
- addSparseBlock(_meas.jacobians.at(j), A_, A_.rows()-_meas.dim, nodes_.at(_meas.nodes_idx.at(j)).location);
+ // error
+ b_.tail(_meas.dim) = _meas.error;
- A_nodes_.coeffRef(A_nodes_.rows()-1, ordered_node) = 1;
+ time_managing_ += ((double)clock() - t_managing_) / CLOCKS_PER_SEC;
+ }
- assert(_meas.jacobians.at(j).cols() == nodes_.at(_meas.nodes_idx.at(j)).dim);
- assert(_meas.jacobians.at(j).rows() == _meas.dim);
+ void ordering(const int& _first_ordered_node)
+ {
+ t_ordering_ = clock();
- // store minimum ordered node
- if (first_ordered_node_ > ordered_node)
- first_ordered_node_ = ordered_node;
- }
+ // full problem ordering
+ if (_first_ordered_node == 0)
+ {
+ // ordering ordering factors
+ node_ordering_restrictions_.resize(n_nodes_);
+ node_ordering_restrictions_ = A_nodes_.bottomRows(1).transpose();
+
+ // computing nodes partial ordering_
+ A_nodes_.makeCompressed();
+ PermutationMatrix<Dynamic, Dynamic, int> incr_permutation_nodes(n_nodes_);
+ orderer_(A_nodes_, incr_permutation_nodes, node_ordering_restrictions_.data());
+
+ // node ordering to variable ordering
+ PermutationMatrix<Dynamic, Dynamic, int> incr_permutation(A_.cols());
+ nodePermutation2VariablesPermutation(incr_permutation_nodes, incr_permutation);
- // error
- b_.tail(_meas.dim) = _meas.error;
+ // apply partial_ordering orderings
+ A_nodes_ = (A_nodes_ * incr_permutation_nodes.transpose()).sparseView();
+ A_ = (A_ * incr_permutation.transpose()).sparseView();
- time_managing_ += ((double) clock() - t_managing_) / CLOCKS_PER_SEC;
+ // ACCUMULATING PERMUTATIONS
+ accumulatePermutation(incr_permutation_nodes);
}
- void ordering(const int & _first_ordered_node)
+ // partial ordering
+ else
{
- t_ordering_ = clock();
-
- // full problem ordering
- if (_first_ordered_node == 0)
+ int ordered_nodes = n_nodes_ - _first_ordered_node;
+ int unordered_nodes = n_nodes_ - ordered_nodes;
+ if (ordered_nodes > 2) // only reordering when involved nodes in the measurement are not the two last ones
{
- // ordering ordering factors
- node_ordering_restrictions_.resize(n_nodes_);
- node_ordering_restrictions_ = A_nodes_.bottomRows(1).transpose();
+ // SUBPROBLEM ORDERING (from first node variable to last one)
+ // std::cout << "ordering partial_ordering problem: " << _first_ordered_node << " to "<< n_nodes_ - 1
+ // << std::endl;
+ SparseMatrix<int> sub_A_nodes_ = A_nodes_.rightCols(ordered_nodes);
+
+ // _partial_ordering ordering_ factors
+ node_ordering_restrictions_.resize(ordered_nodes);
+ node_ordering_restrictions_ = sub_A_nodes_.bottomRows(1).transpose();
// computing nodes partial ordering_
- A_nodes_.makeCompressed();
- PermutationMatrix<Dynamic, Dynamic, int> incr_permutation_nodes(n_nodes_);
- orderer_(A_nodes_, incr_permutation_nodes, node_ordering_restrictions_.data());
+ sub_A_nodes_.makeCompressed();
+ PermutationMatrix<Dynamic, Dynamic, int> partial_permutation_nodes(ordered_nodes);
+ orderer_(sub_A_nodes_, partial_permutation_nodes, node_ordering_restrictions_.data());
// node ordering to variable ordering
- PermutationMatrix<Dynamic, Dynamic, int> incr_permutation(A_.cols());
- nodePermutation2VariablesPermutation(incr_permutation_nodes, incr_permutation);
+ PermutationMatrix<Dynamic, Dynamic, int> partial_permutation(A_.cols());
+ nodePermutation2VariablesPermutation(partial_permutation_nodes, partial_permutation);
// apply partial_ordering orderings
- A_nodes_ = (A_nodes_ * incr_permutation_nodes.transpose()).sparseView();
- A_ = (A_ * incr_permutation.transpose()).sparseView();
+ int ordered_variables = A_.cols() - nodes_.at(_first_ordered_node).location;
+ A_nodes_.rightCols(ordered_nodes) =
+ (A_nodes_.rightCols(ordered_nodes) * partial_permutation_nodes.transpose()).sparseView();
+ A_.rightCols(ordered_variables) =
+ (A_.rightCols(ordered_variables) * partial_permutation.transpose()).sparseView();
+ R_.rightCols(ordered_variables) =
+ (R_.rightCols(ordered_variables) * partial_permutation.transpose()).sparseView();
// ACCUMULATING PERMUTATIONS
- accumulatePermutation(incr_permutation_nodes);
+ accumulatePermutation(partial_permutation_nodes);
}
+ }
+ time_ordering_ += ((double)clock() - t_ordering_) / CLOCKS_PER_SEC;
+ }
+
+ bool solve(const int mode)
+ {
+ bool batch = (mode != 2 || first_ordered_node_ == 0);
+ bool order = (mode != 0 && n_nodes_ > 1);
- // partial ordering
- else
+ // BATCH
+ if (batch)
+ {
+ // REORDER
+ if (order) ordering(0);
+
+ // print_problem();
+
+ // SOLVE
+ t_solving_ = clock();
+ A_.makeCompressed();
+ solver_.compute(A_);
+ if (solver_.info() != Success)
{
- int ordered_nodes = n_nodes_ - _first_ordered_node;
- int unordered_nodes = n_nodes_ - ordered_nodes;
- if (ordered_nodes > 2) // only reordering when involved nodes in the measurement are not the two last ones
- {
- // SUBPROBLEM ORDERING (from first node variable to last one)
- //std::cout << "ordering partial_ordering problem: " << _first_ordered_node << " to "<< n_nodes_ - 1 << std::endl;
- SparseMatrix<int> sub_A_nodes_ = A_nodes_.rightCols(ordered_nodes);
-
- // _partial_ordering ordering_ factors
- node_ordering_restrictions_.resize(ordered_nodes);
- node_ordering_restrictions_ = sub_A_nodes_.bottomRows(1).transpose();
-
- // computing nodes partial ordering_
- sub_A_nodes_.makeCompressed();
- PermutationMatrix<Dynamic, Dynamic, int> partial_permutation_nodes(ordered_nodes);
- orderer_(sub_A_nodes_, partial_permutation_nodes, node_ordering_restrictions_.data());
-
- // node ordering to variable ordering
- PermutationMatrix<Dynamic, Dynamic, int> partial_permutation(A_.cols());
- nodePermutation2VariablesPermutation(partial_permutation_nodes, partial_permutation);
-
- // apply partial_ordering orderings
- int ordered_variables = A_.cols() - nodes_.at(_first_ordered_node).location;
- A_nodes_.rightCols(ordered_nodes) = (A_nodes_.rightCols(ordered_nodes) * partial_permutation_nodes.transpose()).sparseView();
- A_.rightCols(ordered_variables) = (A_.rightCols(ordered_variables) * partial_permutation.transpose()).sparseView();
- R_.rightCols(ordered_variables) = (R_.rightCols(ordered_variables) * partial_permutation.transpose()).sparseView();
-
- // ACCUMULATING PERMUTATIONS
- accumulatePermutation(partial_permutation_nodes);
- }
+ std::cout << "decomposition failed" << std::endl;
+ return 0;
}
- time_ordering_ += ((double) clock() - t_ordering_) / CLOCKS_PER_SEC;
+ x_incr_ = solver_.solve(b_);
+ R_ = solver_.matrixR();
+ // std::cout << "R" << std::endl << MatrixXd::Identity(R_.cols(), R_.cols()) * R_ << std::endl;
+ time_solving_ += ((double)clock() - t_solving_) / CLOCKS_PER_SEC;
}
-
- bool solve(const int mode)
+ // INCREMENTAL
+ else
{
- bool batch = (mode !=2 || first_ordered_node_ == 0);
- bool order = (mode !=0 && n_nodes_ > 1);
-
- // BATCH
- if (batch)
+ // REORDER SUBPROBLEM
+ ordering(first_ordered_node_);
+ // print_problem();
+
+ // SOLVE ORDERED SUBPROBLEM
+ t_solving_ = clock();
+ A_nodes_.makeCompressed();
+ A_.makeCompressed();
+
+ // finding measurements block
+ SparseMatrix<int> measurements_to_initial = A_nodes_.col(first_ordered_node_);
+ // std::cout << "measurements_to_initial " << measurements_to_initial << std::endl;
+ // std::cout << "measurements_to_initial.innerIndex()[measurements_to_initial.outerIndex()[0]] " <<
+ // measurements_to_initial.innerIndex()[measurements_to_initial.outerIndex()[0]] << std::endl;
+ int first_ordered_measurement =
+ measurements_to_initial.innerIndex()[measurements_to_initial.outerIndex()[0]];
+ int ordered_measurements = A_.rows() - measurements_.at(first_ordered_measurement).location;
+ int ordered_variables = A_.cols() - nodes_.at(first_ordered_node_).location;
+ int unordered_variables = nodes_.at(first_ordered_node_).location;
+
+ SparseMatrix<double, ColMajor> A_partial = A_.bottomRightCorner(ordered_measurements, ordered_variables);
+ solver_.compute(A_partial);
+ if (solver_.info() != Success)
{
- // REORDER
- if (order)
- ordering(0);
-
- //print_problem();
-
- // SOLVE
- t_solving_ = clock();
- A_.makeCompressed();
- solver_.compute(A_);
- if (solver_.info() != Success)
- {
- std::cout << "decomposition failed" << std::endl;
- return 0;
- }
- x_incr_ = solver_.solve(b_);
- R_ = solver_.matrixR();
- //std::cout << "R" << std::endl << MatrixXd::Identity(R_.cols(), R_.cols()) * R_ << std::endl;
- time_solving_ += ((double) clock() - t_solving_) / CLOCKS_PER_SEC;
+ std::cout << "decomposition failed" << std::endl;
+ return 0;
}
- // INCREMENTAL
- else
+ // std::cout << "R new" << std::endl << MatrixXd::Identity(A_partial.cols(), A_partial.cols()) *
+ // solver_.matrixR() << std::endl;
+ x_incr_.tail(ordered_variables) = solver_.solve(b_.tail(ordered_measurements));
+
+ // store new part of R
+ eraseSparseBlock(R_, unordered_variables, unordered_variables, ordered_variables, ordered_variables);
+ // std::cout << "R" << std::endl << MatrixXd::Identity(R_.rows(), R_.rows()) * R_ << std::endl;
+ addSparseBlock(solver_.matrixR(), R_, unordered_variables, unordered_variables);
+ // std::cout << "R" << std::endl << MatrixXd::Identity(R_.rows(), R_.rows()) * R_ << std::endl;
+ R_.makeCompressed();
+
+ // solving not ordered subproblem
+ if (unordered_variables > 0)
{
- // REORDER SUBPROBLEM
- ordering(first_ordered_node_);
- //print_problem();
-
- // SOLVE ORDERED SUBPROBLEM
- t_solving_= clock();
- A_nodes_.makeCompressed();
- A_.makeCompressed();
-
- // finding measurements block
- SparseMatrix<int> measurements_to_initial = A_nodes_.col(first_ordered_node_);
- // std::cout << "measurements_to_initial " << measurements_to_initial << std::endl;
- // std::cout << "measurements_to_initial.innerIndex()[measurements_to_initial.outerIndex()[0]] " << measurements_to_initial.innerIndex()[measurements_to_initial.outerIndex()[0]] << std::endl;
- int first_ordered_measurement = measurements_to_initial.innerIndex()[measurements_to_initial.outerIndex()[0]];
- int ordered_measurements = A_.rows() - measurements_.at(first_ordered_measurement).location;
- int ordered_variables = A_.cols() - nodes_.at(first_ordered_node_).location;
- int unordered_variables = nodes_.at(first_ordered_node_).location;
-
- SparseMatrix<double, ColMajor> A_partial = A_.bottomRightCorner(ordered_measurements, ordered_variables);
- solver_.compute(A_partial);
+ // std::cout << "--------------------- solving unordered part" << std::endl;
+ SparseMatrix<double, ColMajor> R1 = R_.topLeftCorner(unordered_variables, unordered_variables);
+ // std::cout << "R1" << std::endl << MatrixXd::Identity(R1.rows(), R1.rows()) * R1 << std::endl;
+ SparseMatrix<double, ColMajor> R2 = R_.topRightCorner(unordered_variables, ordered_variables);
+ // std::cout << "R2" << std::endl << MatrixXd::Identity(R2.rows(), R2.rows()) * R2 << std::endl;
+ solver_.compute(R1);
if (solver_.info() != Success)
{
std::cout << "decomposition failed" << std::endl;
return 0;
}
- //std::cout << "R new" << std::endl << MatrixXd::Identity(A_partial.cols(), A_partial.cols()) * solver_.matrixR() << std::endl;
- x_incr_.tail(ordered_variables) = solver_.solve(b_.tail(ordered_measurements));
-
- // store new part of R
- eraseSparseBlock(R_, unordered_variables, unordered_variables, ordered_variables, ordered_variables);
- //std::cout << "R" << std::endl << MatrixXd::Identity(R_.rows(), R_.rows()) * R_ << std::endl;
- addSparseBlock(solver_.matrixR(), R_, unordered_variables, unordered_variables);
- //std::cout << "R" << std::endl << MatrixXd::Identity(R_.rows(), R_.rows()) * R_ << std::endl;
- R_.makeCompressed();
-
- // solving not ordered subproblem
- if (unordered_variables > 0)
- {
- //std::cout << "--------------------- solving unordered part" << std::endl;
- SparseMatrix<double, ColMajor> R1 = R_.topLeftCorner(unordered_variables, unordered_variables);
- //std::cout << "R1" << std::endl << MatrixXd::Identity(R1.rows(), R1.rows()) * R1 << std::endl;
- SparseMatrix<double, ColMajor> R2 = R_.topRightCorner(unordered_variables, ordered_variables);
- //std::cout << "R2" << std::endl << MatrixXd::Identity(R2.rows(), R2.rows()) * R2 << std::endl;
- solver_.compute(R1);
- if (solver_.info() != Success)
- {
- std::cout << "decomposition failed" << std::endl;
- return 0;
- }
- x_incr_.head(unordered_variables) = solver_.solve(b_.head(unordered_variables) - R2 * x_incr_.tail(ordered_variables));
- }
-
+ x_incr_.head(unordered_variables) =
+ solver_.solve(b_.head(unordered_variables) - R2 * x_incr_.tail(ordered_variables));
}
- // UNDO ORDERING FOR RESULT
- PermutationMatrix<Dynamic, Dynamic, int> acc_permutation(A_.cols());
- nodePermutation2VariablesPermutation(acc_node_permutation_, acc_permutation); // TODO via pointers
- x_incr_ = acc_permutation.inverse() * x_incr_;
+ }
+ // UNDO ORDERING FOR RESULT
+ PermutationMatrix<Dynamic, Dynamic, int> acc_permutation(A_.cols());
+ nodePermutation2VariablesPermutation(acc_node_permutation_, acc_permutation); // TODO via pointers
+ x_incr_ = acc_permutation.inverse() * x_incr_;
- time_solving_ += ((double) clock() - t_solving_) / CLOCKS_PER_SEC;
+ time_solving_ += ((double)clock() - t_solving_) / CLOCKS_PER_SEC;
- return 1;
- }
+ return 1;
+ }
- void nodePermutation2VariablesPermutation(const PermutationMatrix<Dynamic, Dynamic, int> &_perm_nodes, PermutationMatrix<Dynamic, Dynamic, int> &perm_variables)
- {
- ArrayXi locations = perm_nodes_2_locations(_perm_nodes);
+ void nodePermutation2VariablesPermutation(const PermutationMatrix<Dynamic, Dynamic, int>& _perm_nodes,
+ PermutationMatrix<Dynamic, Dynamic, int>& perm_variables)
+ {
+ ArrayXi locations = perm_nodes_2_locations(_perm_nodes);
- int last_idx = 0;
- for (unsigned int i = 0; i<locations.size(); i++)
- {
- perm_variables.indices().segment(last_idx, nodes_.at(i).dim) = VectorXi::LinSpaced(nodes_.at(i).dim, locations(i), locations(i)+nodes_.at(i).dim-1);
- last_idx += nodes_.at(i).dim;
- }
+ int last_idx = 0;
+ for (unsigned int i = 0; i < locations.size(); i++)
+ {
+ perm_variables.indices().segment(last_idx, nodes_.at(i).dim) =
+ VectorXi::LinSpaced(nodes_.at(i).dim, locations(i), locations(i) + nodes_.at(i).dim - 1);
+ last_idx += nodes_.at(i).dim;
}
+ }
- ArrayXi perm_nodes_2_locations(const PermutationMatrix<Dynamic, Dynamic, int> &_perm_nodes)
- {
- ArrayXi indices = _perm_nodes.indices().array();
+ ArrayXi perm_nodes_2_locations(const PermutationMatrix<Dynamic, Dynamic, int>& _perm_nodes)
+ {
+ ArrayXi indices = _perm_nodes.indices().array();
- for (unsigned int i = 0; i<indices.size(); i++)
- indices = (indices > indices(i)).select(indices + nodes_.at(i).dim-1, indices);
+ for (unsigned int i = 0; i < indices.size(); i++)
+ indices = (indices > indices(i)).select(indices + nodes_.at(i).dim - 1, indices);
- return indices;
- }
+ return indices;
+ }
- void augment_permutation(PermutationMatrix<Dynamic, Dynamic, int> &perm, const int new_size)
- {
- int old_size = perm.indices().size();
- int dim = new_size - old_size;
- VectorXi new_indices(new_size);
- new_indices.head(old_size)= perm.indices();
- new_indices.tail(dim) = ArrayXi::LinSpaced(dim, old_size, new_size-1);
- perm.resize(new_size);
- perm.indices() = new_indices;
- }
+ void augment_permutation(PermutationMatrix<Dynamic, Dynamic, int>& perm, const int new_size)
+ {
+ int old_size = perm.indices().size();
+ int dim = new_size - old_size;
+ VectorXi new_indices(new_size);
+ new_indices.head(old_size) = perm.indices();
+ new_indices.tail(dim) = ArrayXi::LinSpaced(dim, old_size, new_size - 1);
+ perm.resize(new_size);
+ perm.indices() = new_indices;
+ }
- void accumulatePermutation(const PermutationMatrix<Dynamic, Dynamic, int> &perm)
+ void accumulatePermutation(const PermutationMatrix<Dynamic, Dynamic, int>& perm)
+ {
+ printName();
+ // std::cout << std::endl << "old acc_permutation_nodes_ " << acc_permutation_nodes_.indices().transpose() <<
+ // std::endl; std::cout << "incr perm " << perm.indices().transpose() << std::endl;
+
+ // acumulate permutation
+ if (perm.size() == acc_node_permutation_.size()) // full permutation
+ acc_node_permutation_ = perm * acc_node_permutation_;
+ else // partial permutation
{
- printName();
- //std::cout << std::endl << "old acc_permutation_nodes_ " << acc_permutation_nodes_.indices().transpose() << std::endl;
- //std::cout << "incr perm " << perm.indices().transpose() << std::endl;
-
- // acumulate permutation
- if (perm.size() == acc_node_permutation_.size()) //full permutation
- acc_node_permutation_ = perm * acc_node_permutation_;
- else //partial permutation
- {
- PermutationMatrix<Dynamic, Dynamic, int> incr_permutation_nodes(VectorXi::LinSpaced(n_nodes_, 0, n_nodes_ - 1)); // identity permutation
- incr_permutation_nodes.indices().tail(perm.size()) = perm.indices() + VectorXi::Constant(perm.size(), n_nodes_ - perm.size());
- //std::cout << "incr perm " << incr_permutation_nodes.indices().transpose() << std::endl;
- acc_node_permutation_ = incr_permutation_nodes * acc_node_permutation_;
- }
- //std::cout << "new acc_permutation_nodes_ " << acc_permutation_nodes_.indices().transpose() << std::endl;
-
- // update nodes orders and locations
- ArrayXi locations = perm_nodes_2_locations(acc_node_permutation_);
- for (unsigned int i = 0; i < nodes_.size(); i++)
- {
- nodes_.at(i).order = acc_node_permutation_.indices()(i);
- nodes_.at(i).location = locations(i);
- }
+ PermutationMatrix<Dynamic, Dynamic, int> incr_permutation_nodes(
+ VectorXi::LinSpaced(n_nodes_, 0, n_nodes_ - 1)); // identity permutation
+ incr_permutation_nodes.indices().tail(perm.size()) =
+ perm.indices() + VectorXi::Constant(perm.size(), n_nodes_ - perm.size());
+ // std::cout << "incr perm " << incr_permutation_nodes.indices().transpose() << std::endl;
+ acc_node_permutation_ = incr_permutation_nodes * acc_node_permutation_;
}
+ // std::cout << "new acc_permutation_nodes_ " << acc_permutation_nodes_.indices().transpose() << std::endl;
- void printName()
+ // update nodes orders and locations
+ ArrayXi locations = perm_nodes_2_locations(acc_node_permutation_);
+ for (unsigned int i = 0; i < nodes_.size(); i++)
{
- std::cout << name_;
+ nodes_.at(i).order = acc_node_permutation_.indices()(i);
+ nodes_.at(i).location = locations(i);
}
+ }
- void printResults()
- {
- printName();
- std::cout << " solved in " << time_solving_*1e3 << " ms | " << R_.nonZeros() << " nonzeros in R"<< std::endl;
- std::cout << "x = " << x_incr_.transpose() << std::endl;
- }
+ void printName()
+ {
+ std::cout << name_;
+ }
- void printProblem()
- {
- printName();
- std::cout << std::endl << "A_nodes_: " << std::endl << MatrixXi::Identity(A_nodes_.rows(), A_nodes_.rows()) * A_nodes_ << std::endl << std::endl;
- std::cout << "A_: " << std::endl << MatrixXd::Identity(A_.rows(), A_.rows()) * A_ << std::endl << std::endl;
- std::cout << "b_: " << std::endl << b_.transpose() << std::endl << std::endl;
- }
+ void printResults()
+ {
+ printName();
+ std::cout << " solved in " << time_solving_ * 1e3 << " ms | " << R_.nonZeros() << " nonzeros in R"
+ << std::endl;
+ std::cout << "x = " << x_incr_.transpose() << std::endl;
+ }
+
+ void printProblem()
+ {
+ printName();
+ std::cout << std::endl
+ << "A_nodes_: " << std::endl
+ << MatrixXi::Identity(A_nodes_.rows(), A_nodes_.rows()) * A_nodes_ << std::endl
+ << std::endl;
+ std::cout << "A_: " << std::endl << MatrixXd::Identity(A_.rows(), A_.rows()) * A_ << std::endl << std::endl;
+ std::cout << "b_: " << std::endl << b_.transpose() << std::endl << std::endl;
+ }
};
-//main
-int main(int argc, char *argv[])
+// main
+int main(int argc, char* argv[])
{
- if (argc != 3 || atoi(argv[1]) < 1|| atoi(argv[2]) < 1)
+ if (argc != 3 || atoi(argv[1]) < 1 || atoi(argv[2]) < 1)
{
std::cout << "Please call me with: [./test_iQR SIZE DIM], where:" << std::endl;
std::cout << " - SIZE: integer size of the problem" << std::endl;
@@ -506,7 +520,7 @@ int main(int argc, char *argv[])
return -1;
}
int size = atoi(argv[1]);
- int dim = atoi(argv[2]);
+ int dim = atoi(argv[2]);
// Problems
SolverQR solver_ordered("FULL ORDERED");
@@ -517,7 +531,7 @@ int main(int argc, char *argv[])
// results variables
clock_t t_ordering, t_solving_ordered_full, t_solving_unordered, t_solving_ordered_partial, t4;
- double time_ordering=0, time_solving_unordered=0, time_solving_ordered=0, time_solving_ordered_partial=0;
+ double time_ordering = 0, time_solving_unordered = 0, time_solving_ordered = 0, time_solving_ordered_partial = 0;
std::cout << "STARTING INCREMENTAL QR TEST" << std::endl << std::endl;
@@ -526,14 +540,16 @@ int main(int argc, char *argv[])
for (int i = 0; i < size; i++)
{
std::vector<int> meas(0);
- if (i == 0) //prior
- measurements.push_back(measurement(omega, 0, VectorXd::LinSpaced(dim, 0, dim-1), dim));
+ if (i == 0) // prior
+ measurements.push_back(measurement(omega, 0, VectorXd::LinSpaced(dim, 0, dim - 1), dim));
- else //odometry
- measurements.push_back(measurement(2*omega, i-1, 2*omega, i, VectorXd::LinSpaced(dim, dim * i, dim * (i+1)-1), dim, true));
+ else // odometry
+ measurements.push_back(measurement(
+ 2 * omega, i - 1, 2 * omega, i, VectorXd::LinSpaced(dim, dim * i, dim * (i + 1) - 1), dim, true));
- if (i > size / 2) //loop closures
- measurements.push_back(measurement(4*omega, 0, 4*omega, i, VectorXd::LinSpaced(dim, dim * i, dim * (i+1)-1), dim));
+ if (i > size / 2) // loop closures
+ measurements.push_back(
+ measurement(4 * omega, 0, 4 * omega, i, VectorXd::LinSpaced(dim, dim * i, dim * (i + 1) - 1), dim));
}
// INCREMENTAL LOOP
@@ -542,11 +558,11 @@ int main(int argc, char *argv[])
std::cout << "========================= MEASUREMENT " << i << ":" << std::endl;
// AUGMENT THE PROBLEM ----------------------------
- if (measurements.at(i).odometry_type || i == 0) // if odometry, augment the problem
+ if (measurements.at(i).odometry_type || i == 0) // if odometry, augment the problem
{
solver_unordered.add_state_unit(dim, i);
solver_ordered.add_state_unit(dim, i);
- solver_ordered_partial.add_state_unit(dim,i);
+ solver_ordered_partial.add_state_unit(dim, i);
}
// ADD MEASUREMENTS
@@ -570,10 +586,11 @@ int main(int argc, char *argv[])
solver_ordered.printResults();
solver_ordered_partial.printResults();
-// if ((x_ordered_partial-x_ordered).maxCoeff() < 1e-10)
-// std::cout << "Both solutions are equals (tolerance " << (x_ordered_partial-x_ordered).maxCoeff() << ")" << std::endl;
-// else
-// std::cout << "DIFFERENT SOLUTIONS!!!!!!!! max difference " << (x_ordered_partial-x_ordered).maxCoeff() << std::endl;
+ // if ((x_ordered_partial-x_ordered).maxCoeff() < 1e-10)
+ // std::cout << "Both solutions are equals (tolerance " << (x_ordered_partial-x_ordered).maxCoeff()
+ // << ")" << std::endl;
+ // else
+ // std::cout << "DIFFERENT SOLUTIONS!!!!!!!! max difference " <<
+ // (x_ordered_partial-x_ordered).maxCoeff() << std::endl;
}
}
-
diff --git a/demos/solver/test_iQR_wolf2.cpp b/demos/solver/test_iQR_wolf2.cpp
index cb47a37cd5215e1db37e390cfc34e384ea7c4e4e..d87c9455bebcb9f916825be3da4fc78c32749043 100644
--- a/demos/solver/test_iQR_wolf2.cpp
+++ b/demos/solver/test_iQR_wolf2.cpp
@@ -17,14 +17,8 @@
//
// 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/>.
-/*
- * test_iQR_wolf.cpp
- *
- * Created on: Jun 17, 2015
- * Author: jvallve
- */
-
-//std includes
+
+// std includes
#include <cstdlib>
#include <string>
#include <iostream>
@@ -35,7 +29,7 @@
#include <ctime>
#include <queue>
-//Wolf includes
+// Wolf includes
#include "core/state_block/state_block.h"
#include "core/factor/factor_base.h"
#include "core/sensor/sensor_laser_2D.h"
@@ -44,76 +38,76 @@
// wolf solver
#include "solver/qr_solver.h"
-//C includes for sleep, time and main args
+// C includes for sleep, time and main args
#include "unistd.h"
-//faramotics includes
+// faramotics includes
#include "faramotics/dynamicSceneRender.h"
#include "faramotics/rangeScan2D.h"
#include "btr-headers/pose3d.h"
-//Ceres includes
+// Ceres includes
#include "glog/logging.h"
#include "ceres_wrapper/solver_ceres.h"
#include "iri-algorithms/laser_scan_utils/corner_detector.h"
#include "iri-algorithms/laser_scan_utils/entities.h"
-//function travel around
-void motionCampus(unsigned int ii, Cpose3d & pose, double& displacement_, double& rotation_)
+// function travel around
+void motionCampus(unsigned int ii, Cpose3d& pose, double& displacement_, double& rotation_)
{
if (ii <= 120)
{
displacement_ = 0.1;
- rotation_ = 0;
+ rotation_ = 0;
}
else if ((ii > 120) && (ii <= 170))
{
displacement_ = 0.2;
- rotation_ = 1.8 * M_PI / 180;
+ rotation_ = 1.8 * M_PI / 180;
}
else if ((ii > 170) && (ii <= 220))
{
displacement_ = 0;
- rotation_ = -1.8 * M_PI / 180;
+ rotation_ = -1.8 * M_PI / 180;
}
else if ((ii > 220) && (ii <= 310))
{
displacement_ = 0.1;
- rotation_ = 0;
+ rotation_ = 0;
}
else if ((ii > 310) && (ii <= 487))
{
displacement_ = 0.1;
- rotation_ = -1. * M_PI / 180;
+ rotation_ = -1. * M_PI / 180;
}
else if ((ii > 487) && (ii <= 600))
{
displacement_ = 0.2;
- rotation_ = 0;
+ rotation_ = 0;
}
else if ((ii > 600) && (ii <= 700))
{
displacement_ = 0.1;
- rotation_ = -1. * M_PI / 180;
+ rotation_ = -1. * M_PI / 180;
}
else if ((ii > 700) && (ii <= 780))
{
displacement_ = 0;
- rotation_ = -1. * M_PI / 180;
+ rotation_ = -1. * M_PI / 180;
}
else
{
displacement_ = 0.3;
- rotation_ = 0.0 * M_PI / 180;
+ rotation_ = 0.0 * M_PI / 180;
}
pose.moveForward(displacement_);
pose.rt.setEuler(pose.rt.head() + rotation_, pose.rt.pitch(), pose.rt.roll());
}
-//main
-int main(int argc, char *argv[])
+// main
+int main(int argc, char* argv[])
{
using namespace Eigen;
using namespace wolf;
@@ -123,84 +117,100 @@ int main(int argc, char *argv[])
{
std::cout << "Please call me with: [./test_ceres_manager NI MODE], where:" << std::endl;
std::cout << " - NI is the number of iterations (0 < NI < 1100)" << std::endl;
- std::cout << " - MODE is the solver mode (0 batch (no ordering), 1 batch (ordering), 2 incremental" << std::endl;
+ std::cout << " - MODE is the solver mode (0 batch (no ordering), 1 batch (ordering), 2 incremental"
+ << std::endl;
std::cout << "EXIT due to bad user input" << std::endl << std::endl;
return -1;
}
- bool complex_angle = false;
- unsigned int solving_mode = (unsigned int) atoi(argv[2]); //solving mode
- unsigned int n_execution = (unsigned int) atoi(argv[1]); //number of iterations of the whole execution
+ bool complex_angle = false;
+ unsigned int solving_mode = (unsigned int)atoi(argv[2]); // solving mode
+ unsigned int n_execution = (unsigned int)atoi(argv[1]); // number of iterations of the whole execution
// INITIALIZATION ============================================================================================
- //init random generators
- double odom_std_factor = 0.1;
- double gps_std = 10;
- std::default_random_engine generator(1);
+ // init random generators
+ double odom_std_factor = 0.1;
+ double gps_std = 10;
+ std::default_random_engine generator(1);
std::normal_distribution<double> gaussian_distribution(0.0, 1);
// Faramotics stuff
- Cpose3d viewPoint, devicePose, laser1Pose, laser2Pose, estimated_vehicle_pose, estimated_laser_1_pose, estimated_laser_2_pose;
- vector < Cpose3d > devicePoses;
- vector<float> scan1, scan2;
- string modelFileName;
+ Cpose3d viewPoint, devicePose, laser1Pose, laser2Pose, estimated_vehicle_pose, estimated_laser_1_pose,
+ estimated_laser_2_pose;
+ vector<Cpose3d> devicePoses;
+ vector<float> scan1, scan2;
+ string modelFileName;
- //model and initial view point
+ // model and initial view point
modelFileName = "/home/jvallve/iri-lab/faramotics/models/campusNordUPC.obj";
- //modelFileName = "/home/acoromin/dev/br/faramotics/models/campusNordUPC.obj";
- //modelFileName = "/home/andreu/dev/faramotics/models/campusNordUPC.obj";
+ // modelFileName = "/home/acoromin/dev/br/faramotics/models/campusNordUPC.obj";
+ // modelFileName = "/home/andreu/dev/faramotics/models/campusNordUPC.obj";
devicePose.setPose(2, 8, 0.2, 0, 0, 0);
viewPoint.setPose(devicePose);
viewPoint.moveForward(10);
- viewPoint.rt.setEuler(viewPoint.rt.head() + M_PI / 2, viewPoint.rt.pitch() + 30. * M_PI / 180., viewPoint.rt.roll());
+ viewPoint.rt.setEuler(
+ viewPoint.rt.head() + M_PI / 2, viewPoint.rt.pitch() + 30. * M_PI / 180., viewPoint.rt.roll());
viewPoint.moveForward(-15);
- //glut initialization
+ // glut initialization
faramotics::initGLUT(argc, argv);
- //create a viewer for the 3D model and scan points
- CdynamicSceneRender* myRender = new CdynamicSceneRender(1200, 700, 90 * M_PI / 180, 90 * 700.0 * M_PI / (1200.0 * 180.0), 0.2, 100);
- myRender->loadAssimpModel(modelFileName, true); //with wireframe
- //create scanner and load 3D model
- CrangeScan2D* myScanner = new CrangeScan2D(HOKUYO_UTM30LX_180DEG); //HOKUYO_UTM30LX_180DEG or LEUZE_RS4
+ // create a viewer for the 3D model and scan points
+ CdynamicSceneRender* myRender =
+ new CdynamicSceneRender(1200, 700, 90 * M_PI / 180, 90 * 700.0 * M_PI / (1200.0 * 180.0), 0.2, 100);
+ myRender->loadAssimpModel(modelFileName, true); // with wireframe
+ // create scanner and load 3D model
+ CrangeScan2D* myScanner = new CrangeScan2D(HOKUYO_UTM30LX_180DEG); // HOKUYO_UTM30LX_180DEG or LEUZE_RS4
myScanner->loadAssimpModel(modelFileName);
- //variables
+ // variables
Eigen::Vector3d odom_reading;
Eigen::Vector2d gps_fix_reading;
- Eigen::VectorXd pose_odom(3); //current odometry integred pose
- Eigen::VectorXd ground_truth(n_execution * 3); //all true poses
- Eigen::VectorXd odom_trajectory(n_execution * 3); //open loop trajectory
+ Eigen::VectorXd pose_odom(3); // current odometry integred pose
+ Eigen::VectorXd ground_truth(n_execution * 3); // all true poses
+ Eigen::VectorXd odom_trajectory(n_execution * 3); // open loop trajectory
Eigen::VectorXd mean_times = Eigen::VectorXd::Zero(7);
- clock_t t1, t2;
+ clock_t t1, t2;
// Wolf manager initialization
Eigen::Vector3d odom_pose = Eigen::Vector3d::Zero();
- Eigen::Vector3d gps_pose = Eigen::Vector3d::Zero();
- Eigen::Vector4d laser_1_pose, laser_2_pose; //xyz + theta
- laser_1_pose << 1.2, 0, 0, 0; //laser 1
- laser_2_pose << -1.2, 0, 0, M_PI; //laser 2
- SensorOdom2D odom_sensor(std::make_shared<StatePoint2d>(odom_pose.head(2)), std::make_shared<StateAngle>(odom_pose.tail(1)), odom_std_factor, odom_std_factor);
- SensorGPSFix gps_sensor(std::make_shared<StatePoint2d>(gps_pose.head(2)), std::make_shared<StateAngle>(gps_pose.tail(1)), gps_std);
- SensorLaser2D laser_1_sensor(std::make_shared<StatePoint2d>(laser_1_pose.head(2)), std::make_shared<StateAngle>(laser_1_pose.tail(1)), laserscanutils::LaserScanParams({M_PI/2,-M_PI/2, -M_PI/720,0.01,0.2,100,0.01,0.01}));
- SensorLaser2D laser_2_sensor(std::make_shared<StatePoint2d>(laser_2_pose.head(2)), std::make_shared<StateAngle>(laser_2_pose.tail(1)), laserscanutils::LaserScanParams({M_PI/2,-M_PI/2, -M_PI/720,0.01,0.2,100,0.01,0.01}));
+ Eigen::Vector3d gps_pose = Eigen::Vector3d::Zero();
+ Eigen::Vector4d laser_1_pose, laser_2_pose; // xyz + theta
+ laser_1_pose << 1.2, 0, 0, 0; // laser 1
+ laser_2_pose << -1.2, 0, 0, M_PI; // laser 2
+ SensorOdom2D odom_sensor(std::make_shared<StatePoint2d>(odom_pose.head(2)),
+ std::make_shared<StateAngle>(odom_pose.tail(1)),
+ odom_std_factor,
+ odom_std_factor);
+ SensorGPSFix gps_sensor(
+ std::make_shared<StatePoint2d>(gps_pose.head(2)), std::make_shared<StateAngle>(gps_pose.tail(1)), gps_std);
+ SensorLaser2D laser_1_sensor(
+ std::make_shared<StatePoint2d>(laser_1_pose.head(2)),
+ std::make_shared<StateAngle>(laser_1_pose.tail(1)),
+ laserscanutils::LaserScanParams({M_PI / 2, -M_PI / 2, -M_PI / 720, 0.01, 0.2, 100, 0.01, 0.01}));
+ SensorLaser2D laser_2_sensor(
+ std::make_shared<StatePoint2d>(laser_2_pose.head(2)),
+ std::make_shared<StateAngle>(laser_2_pose.tail(1)),
+ laserscanutils::LaserScanParams({M_PI / 2, -M_PI / 2, -M_PI / 720, 0.01, 0.2, 100, 0.01, 0.01}));
// Initial pose
pose_odom << 2, 8, 0;
- ground_truth.head(3) = pose_odom;
+ ground_truth.head(3) = pose_odom;
odom_trajectory.head(3) = pose_odom;
- WolfManager* wolf_manager_QR = new WolfManager(FRM_PO_2D, &odom_sensor, pose_odom, Eigen::Matrix3d::Identity() * 0.01, n_execution*10, 0.01);
- WolfManager* wolf_manager_ceres = new WolfManager(FRM_PO_2D, &odom_sensor, pose_odom, Eigen::Matrix3d::Identity() * 0.01, n_execution*10, 0.01);
+ WolfManager* wolf_manager_QR = new WolfManager(
+ FRM_PO_2D, &odom_sensor, pose_odom, Eigen::Matrix3d::Identity() * 0.01, n_execution * 10, 0.01);
+ WolfManager* wolf_manager_ceres = new WolfManager(
+ FRM_PO_2D, &odom_sensor, pose_odom, Eigen::Matrix3d::Identity() * 0.01, n_execution * 10, 0.01);
// Ceres initialization
ceres::Solver::Options ceres_options;
- ceres_options.minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;LINE_SEARCH
+ ceres_options.minimizer_type = ceres::TRUST_REGION; // ceres::TRUST_REGION;LINE_SEARCH
ceres_options.max_line_search_step_contraction = 1e-3;
// ceres_options.minimizer_progress_to_stdout = false;
// ceres_options.line_search_direction_type = ceres::LBFGS;
// ceres_options.max_num_iterations = 100;
- SolverCeres* ceres_manager = new SolverCeres(wolf_manager_ceres->getProblem(), ceres_options);
- std::ofstream log_file, landmark_file; //output file
+ SolverCeres* ceres_manager = new SolverCeres(wolf_manager_ceres->getProblem(), ceres_options);
+ std::ofstream log_file, landmark_file; // output file
// Own solver
SolverQR solver_(wolf_manager_QR->getProblem());
@@ -210,11 +220,11 @@ int main(int argc, char *argv[])
// START TRAJECTORY ============================================================================================
for (unsigned int step = 1; step < n_execution; step++)
{
- //get init time
+ // get init time
t2 = clock();
// ROBOT MOVEMENT ---------------------------
- //std::cout << "ROBOT MOVEMENT..." << std::endl;
+ // std::cout << "ROBOT MOVEMENT..." << std::endl;
// moves the device position
t1 = clock();
motionCampus(step, devicePose, odom_reading(0), odom_reading(2));
@@ -222,14 +232,16 @@ int main(int argc, char *argv[])
devicePoses.push_back(devicePose);
// SENSOR DATA ---------------------------
- //std::cout << "SENSOR DATA..." << std::endl;
+ // std::cout << "SENSOR DATA..." << std::endl;
// store groundtruth
ground_truth.segment(step * 3, 3) << devicePose.pt(0), devicePose.pt(1), devicePose.rt.head();
// compute odometry
- odom_reading(0) += gaussian_distribution(generator) * odom_std_factor * (odom_reading(0) == 0 ? 1e-6 : odom_reading(0));
+ odom_reading(0) +=
+ gaussian_distribution(generator) * odom_std_factor * (odom_reading(0) == 0 ? 1e-6 : odom_reading(0));
odom_reading(1) += gaussian_distribution(generator) * odom_std_factor * 1e-6;
- odom_reading(2) += gaussian_distribution(generator) * odom_std_factor * (odom_reading(2) == 0 ? 1e-6 : odom_reading(2));
+ odom_reading(2) +=
+ gaussian_distribution(generator) * odom_std_factor * (odom_reading(2) == 0 ? 1e-6 : odom_reading(2));
// odometry integration
pose_odom(0) = pose_odom(0) + odom_reading(0) * cos(pose_odom(2)) - odom_reading(1) * sin(pose_odom(2));
@@ -242,7 +254,7 @@ int main(int argc, char *argv[])
gps_fix_reading(0) += gaussian_distribution(generator) * gps_std;
gps_fix_reading(1) += gaussian_distribution(generator) * gps_std;
- //compute scans
+ // compute scans
scan1.clear();
scan2.clear();
// scan 1
@@ -255,94 +267,115 @@ int main(int argc, char *argv[])
laser2Pose.rt.setEuler(laser2Pose.rt.head() + M_PI, laser2Pose.rt.pitch(), laser2Pose.rt.roll());
myScanner->computeScan(laser2Pose, scan2);
- mean_times(0) += ((double) clock() - t1) / CLOCKS_PER_SEC;
+ mean_times(0) += ((double)clock() - t1) / CLOCKS_PER_SEC;
// ADD CAPTURES ---------------------------
- //std::cout << "ADD CAPTURES..." << std::endl;
+ // std::cout << "ADD CAPTURES..." << std::endl;
// adding new sensor captures
- wolf_manager_QR->addCapture(new CaptureOdom2D(TimeStamp(), TimeStamp(), &odom_sensor, odom_reading)); //, odom_std_factor * Eigen::MatrixXd::Identity(2,2)));
- wolf_manager_QR->addCapture(new CaptureGPSFix(TimeStamp(), &gps_sensor, gps_fix_reading, gps_std * gps_std * Eigen::MatrixXd::Identity(3,3)));
- wolf_manager_ceres->addCapture(new CaptureOdom2D(TimeStamp(), TimeStamp(), &odom_sensor, odom_reading)); //, odom_std_factor * Eigen::MatrixXd::Identity(2,2)));
- wolf_manager_ceres->addCapture(new CaptureGPSFix(TimeStamp(), &gps_sensor, gps_fix_reading, gps_std * gps_std * Eigen::MatrixXd::Identity(3,3)));
- //wolf_manager->addCapture(new CaptureLaser2D(TimeStamp(), &laser_1_sensor, scan1));
- //wolf_manager->addCapture(new CaptureLaser2D(TimeStamp(), &laser_2_sensor, scan2));
+ wolf_manager_QR->addCapture(
+ new CaptureOdom2D(TimeStamp(),
+ TimeStamp(),
+ &odom_sensor,
+ odom_reading)); //, odom_std_factor * Eigen::MatrixXd::Identity(2,2)));
+ wolf_manager_QR->addCapture(new CaptureGPSFix(
+ TimeStamp(), &gps_sensor, gps_fix_reading, gps_std * gps_std * Eigen::MatrixXd::Identity(3, 3)));
+ wolf_manager_ceres->addCapture(
+ new CaptureOdom2D(TimeStamp(),
+ TimeStamp(),
+ &odom_sensor,
+ odom_reading)); //, odom_std_factor * Eigen::MatrixXd::Identity(2,2)));
+ wolf_manager_ceres->addCapture(new CaptureGPSFix(
+ TimeStamp(), &gps_sensor, gps_fix_reading, gps_std * gps_std * Eigen::MatrixXd::Identity(3, 3)));
+ // wolf_manager->addCapture(new CaptureLaser2D(TimeStamp(), &laser_1_sensor, scan1));
+ // wolf_manager->addCapture(new CaptureLaser2D(TimeStamp(), &laser_2_sensor, scan2));
// updating problem
wolf_manager_QR->update();
wolf_manager_ceres->update();
// UPDATING SOLVER ---------------------------
- //std::cout << "UPDATING..." << std::endl;
+ // std::cout << "UPDATING..." << std::endl;
// update state units and factors in ceres
solver_.update();
// PRINT PROBLEM
- //solver_.printProblem();
+ // solver_.printProblem();
// SOLVE OPTIMIZATION ---------------------------
- //std::cout << "SOLVING..." << std::endl;
+ // std::cout << "SOLVING..." << std::endl;
ceres::Solver::Summary summary = ceres_manager->solve();
solver_.solve(solving_mode);
std::cout << "========================= RESULTS " << step << ":" << std::endl;
- //solver_.printResults();
+ // solver_.printResults();
std::cout << "QR vehicle pose " << wolf_manager_QR->getVehiclePose().transpose() << std::endl;
std::cout << "ceres vehicle pose " << wolf_manager_ceres->getVehiclePose().transpose() << std::endl;
// COMPUTE COVARIANCES ---------------------------
- //std::cout << "COMPUTING COVARIANCES..." << std::endl;
+ // std::cout << "COMPUTING COVARIANCES..." << std::endl;
// TODO
// DRAWING STUFF ---------------------------
// draw detected corners
-// std::list < laserscanutils::Corner > corner_list;
-// std::vector<double> corner_vector;
-// CaptureLaser2D last_scan(TimeStamp(), &laser_1_sensor, scan1);
-// last_scan.extractCorners(corner_list);
-// for (std::list<laserscanutils::Corner>::iterator corner_it = corner_list.begin(); corner_it != corner_list.end(); corner_it++)
-// {
-// corner_vector.push_back(corner_it->pt_(0));
-// corner_vector.push_back(corner_it->pt_(1));
-// }
-// myRender->drawCorners(laser1Pose, corner_vector);
+ // std::list < laserscanutils::Corner > corner_list;
+ // std::vector<double> corner_vector;
+ // CaptureLaser2D last_scan(TimeStamp(), &laser_1_sensor, scan1);
+ // last_scan.extractCorners(corner_list);
+ // for (std::list<laserscanutils::Corner>::iterator corner_it = corner_list.begin(); corner_it !=
+ // corner_list.end(); corner_it++)
+ // {
+ // corner_vector.push_back(corner_it->pt_(0));
+ // corner_vector.push_back(corner_it->pt_(1));
+ // }
+ // myRender->drawCorners(laser1Pose, corner_vector);
// draw landmarks
std::vector<double> landmark_vector;
for (auto landmark : wolf_manager_QR->getProblem()->getMap()->getLandmarkList())
{
double* position_ptr = landmark->getP()->get();
- landmark_vector.push_back(*position_ptr); //x
- landmark_vector.push_back(*(position_ptr + 1)); //y
- landmark_vector.push_back(0.2); //z
+ landmark_vector.push_back(*position_ptr); // x
+ landmark_vector.push_back(*(position_ptr + 1)); // y
+ landmark_vector.push_back(0.2); // z
}
myRender->drawLandmarks(landmark_vector);
// draw localization and sensors
- estimated_vehicle_pose.setPose(wolf_manager_QR->getVehiclePose()(0), wolf_manager_QR->getVehiclePose()(1), 0.2, wolf_manager_QR->getVehiclePose()(2), 0, 0);
+ estimated_vehicle_pose.setPose(wolf_manager_QR->getVehiclePose()(0),
+ wolf_manager_QR->getVehiclePose()(1),
+ 0.2,
+ wolf_manager_QR->getVehiclePose()(2),
+ 0,
+ 0);
estimated_laser_1_pose.setPose(estimated_vehicle_pose);
estimated_laser_1_pose.moveForward(laser_1_pose(0));
estimated_laser_2_pose.setPose(estimated_vehicle_pose);
// instead of laser 2 we draw ceres solution
- //estimated_laser_2_pose.moveForward(laser_2_pose(0));
- //estimated_laser_2_pose.rt.setEuler(estimated_laser_2_pose.rt.head() + M_PI, estimated_laser_2_pose.rt.pitch(), estimated_laser_2_pose.rt.roll());
- estimated_laser_2_pose.setPose(wolf_manager_ceres->getVehiclePose()(0), wolf_manager_ceres->getVehiclePose()(1), 0.2, wolf_manager_ceres->getVehiclePose()(2), 0, 0);
-
- myRender->drawPoseAxisVector( { estimated_vehicle_pose, estimated_laser_1_pose, estimated_laser_2_pose });
-
- //Set view point and render the scene
- //locate visualization view point, somewhere behind the device
+ // estimated_laser_2_pose.moveForward(laser_2_pose(0));
+ // estimated_laser_2_pose.rt.setEuler(estimated_laser_2_pose.rt.head() + M_PI,
+ // estimated_laser_2_pose.rt.pitch(), estimated_laser_2_pose.rt.roll());
+ estimated_laser_2_pose.setPose(wolf_manager_ceres->getVehiclePose()(0),
+ wolf_manager_ceres->getVehiclePose()(1),
+ 0.2,
+ wolf_manager_ceres->getVehiclePose()(2),
+ 0,
+ 0);
+
+ myRender->drawPoseAxisVector({estimated_vehicle_pose, estimated_laser_1_pose, estimated_laser_2_pose});
+
+ // Set view point and render the scene
+ // locate visualization view point, somewhere behind the device
myRender->setViewPoint(viewPoint);
myRender->drawPoseAxis(devicePose);
- myRender->drawScan(laser1Pose, scan1, 180. * M_PI / 180., 90. * M_PI / 180.); //draw scan
+ myRender->drawScan(laser1Pose, scan1, 180. * M_PI / 180., 90. * M_PI / 180.); // draw scan
myRender->render();
// TIME MANAGEMENT ---------------------------
- double dt = ((double) clock() - t2) / CLOCKS_PER_SEC;
+ double dt = ((double)clock() - t2) / CLOCKS_PER_SEC;
mean_times(6) += dt;
- if (dt < 0.1)
- usleep(100000 - 1e6 * dt);
+ if (dt < 0.1) usleep(100000 - 1e6 * dt);
-// std::cout << "\nTree after step..." << std::endl;
-// wolf_manager->getProblem()->print();
+ // std::cout << "\nTree after step..." << std::endl;
+ // wolf_manager->getProblem()->print();
}
// DISPLAY RESULTS ============================================================================================
@@ -356,8 +389,8 @@ int main(int argc, char *argv[])
std::cout << " results drawing: " << mean_times(5) << std::endl;
std::cout << " loop time: " << mean_times(6) << std::endl;
-// std::cout << "\nTree before deleting..." << std::endl;
-// wolf_manager->getProblem()->print();
+ // std::cout << "\nTree before deleting..." << std::endl;
+ // wolf_manager->getProblem()->print();
// Draw Final result -------------------------
std::cout << "Drawing final results..." << std::endl;
@@ -365,15 +398,15 @@ int main(int argc, char *argv[])
for (auto landmark : wolf_manager_QR->getProblem()->getMap()->getLandmarkList())
{
double* position_ptr = landmark->getP()->get();
- landmark_vector.push_back(*position_ptr); //x
- landmark_vector.push_back(*(position_ptr + 1)); //y
- landmark_vector.push_back(0.2); //z
+ landmark_vector.push_back(*position_ptr); // x
+ landmark_vector.push_back(*(position_ptr + 1)); // y
+ landmark_vector.push_back(0.2); // z
}
myRender->drawLandmarks(landmark_vector);
-// viewPoint.setPose(devicePoses.front());
-// viewPoint.moveForward(10);
-// viewPoint.rt.setEuler( viewPoint.rt.head()+M_PI/4, viewPoint.rt.pitch()+20.*M_PI/180., viewPoint.rt.roll() );
-// viewPoint.moveForward(-10);
+ // viewPoint.setPose(devicePoses.front());
+ // viewPoint.moveForward(10);
+ // viewPoint.rt.setEuler( viewPoint.rt.head()+M_PI/4, viewPoint.rt.pitch()+20.*M_PI/180., viewPoint.rt.roll() );
+ // viewPoint.moveForward(-10);
myRender->setViewPoint(viewPoint);
myRender->render();
@@ -381,12 +414,13 @@ int main(int argc, char *argv[])
std::cout << "Printing results in a file..." << std::endl;
// Vehicle poses
std::cout << "Vehicle poses..." << std::endl;
- int i = 0;
+ int i = 0;
Eigen::VectorXd state_poses(wolf_manager_QR->getProblem()->getTrajectory()->size() * 3);
for (auto frame : wolf_manager_QR->getProblem()->getTrajectory()->getFrameMap())
{
if (complex_angle)
- state_poses.segment(i, 3) << *frame->getP()->get(), *(frame->getP()->get() + 1), atan2(*frame->getO()->get(), *(frame->getO()->get() + 1));
+ state_poses.segment(i, 3) << *frame->getP()->get(), *(frame->getP()->get() + 1),
+ atan2(*frame->getO()->get(), *(frame->getO()->get() + 1));
else
state_poses.segment(i, 3) << *frame->getP()->get(), *(frame->getP()->get() + 1), *frame->getO()->get();
i += 3;
@@ -404,28 +438,33 @@ int main(int argc, char *argv[])
}
// Print log files
- std::string filepath = getenv("HOME") + (complex_angle ? std::string("/Desktop/log_file_3.txt") : std::string("/Desktop/log_file_2.txt"));
- log_file.open(filepath, std::ofstream::out); //open log file
+ std::string filepath = getenv("HOME") + (complex_angle ? std::string("/Desktop/log_file_3.txt")
+ : std::string("/Desktop/log_file_2.txt"));
+ log_file.open(filepath, std::ofstream::out); // open log file
if (log_file.is_open())
{
log_file << 0 << std::endl;
for (unsigned int ii = 0; ii < n_execution; ii++)
- log_file << state_poses.segment(ii * 3, 3).transpose() << "\t" << ground_truth.segment(ii * 3, 3).transpose() << "\t" << (state_poses.segment(ii * 3, 3) - ground_truth.segment(ii * 3, 3)).transpose() << "\t" << odom_trajectory.segment(ii * 3, 3).transpose() << std::endl;
- log_file.close(); //close log file
+ log_file << state_poses.segment(ii * 3, 3).transpose() << "\t"
+ << ground_truth.segment(ii * 3, 3).transpose() << "\t"
+ << (state_poses.segment(ii * 3, 3) - ground_truth.segment(ii * 3, 3)).transpose() << "\t"
+ << odom_trajectory.segment(ii * 3, 3).transpose() << std::endl;
+ log_file.close(); // close log file
std::cout << std::endl << "Result file " << filepath << std::endl;
}
else
std::cout << std::endl << "Failed to write the log file " << filepath << std::endl;
- std::string filepath2 = getenv("HOME") + (complex_angle ? std::string("/Desktop/landmarks_file_3.txt") : std::string("/Desktop/landmarks_file_2.txt"));
- landmark_file.open(filepath2, std::ofstream::out); //open log file
+ std::string filepath2 = getenv("HOME") + (complex_angle ? std::string("/Desktop/landmarks_file_3.txt")
+ : std::string("/Desktop/landmarks_file_2.txt"));
+ landmark_file.open(filepath2, std::ofstream::out); // open log file
if (landmark_file.is_open())
{
for (unsigned int ii = 0; ii < landmarks.size(); ii += 2)
landmark_file << landmarks.segment(ii, 2).transpose() << std::endl;
- landmark_file.close(); //close log file
+ landmark_file.close(); // close log file
std::cout << std::endl << "Landmark file " << filepath << std::endl;
}
else
@@ -442,7 +481,6 @@ int main(int argc, char *argv[])
std::cout << " ========= END ===========" << std::endl << std::endl;
- //exit
+ // exit
return 0;
}
-
diff --git a/demos/solver/test_incremental_ccolamd_blocks.cpp b/demos/solver/test_incremental_ccolamd_blocks.cpp
index c7d83a011f2ebc90305fc2d8cb07b822feb566cb..1e099edfead3a8569d5934222a7f6c9c63827fe7 100644
--- a/demos/solver/test_incremental_ccolamd_blocks.cpp
+++ b/demos/solver/test_incremental_ccolamd_blocks.cpp
@@ -17,14 +17,8 @@
//
// 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/>.
-/*
- * test_ccolamd_blocks.cpp
- *
- * Created on: Jun 12, 2015
- * Author: jvallve
- */
-
-//std includes
+
+// std includes
#include <cstdlib>
#include <iostream>
#include <fstream>
@@ -44,38 +38,45 @@
using namespace Eigen;
-void eraseSparseBlock(SparseMatrix<double>& original, const unsigned int& row, const unsigned int& Nrows, const unsigned int& col, const unsigned int& Ncols)
+void eraseSparseBlock(SparseMatrix<double>& original,
+ const unsigned int& row,
+ const unsigned int& Nrows,
+ const unsigned int& col,
+ const unsigned int& Ncols)
{
- for (unsigned int i = row; i < row + Nrows; i++)
- for (unsigned int j = col; j < row + Ncols; j++)
- original.coeffRef(i,j) = 0.0;
+ for (unsigned int i = row; i < row + Nrows; i++)
+ for (unsigned int j = col; j < row + Ncols; j++) original.coeffRef(i, j) = 0.0;
- original.makeCompressed();
+ original.makeCompressed();
}
-void addSparseBlock(const MatrixXd& ins, SparseMatrix<double>& original, const unsigned int& row, const unsigned int& col)
+void addSparseBlock(const MatrixXd& ins,
+ SparseMatrix<double>& original,
+ const unsigned int& row,
+ const unsigned int& col)
{
- for (unsigned int r=0; r<ins.rows(); ++r)
- for (unsigned int c = 0; c < ins.cols(); c++)
- if (ins(r,c) != 0)
- original.coeffRef(r + row, c + col) += ins(r,c);
+ for (unsigned int r = 0; r < ins.rows(); ++r)
+ for (unsigned int c = 0; c < ins.cols(); c++)
+ if (ins(r, c) != 0) original.coeffRef(r + row, c + col) += ins(r, c);
}
-void permutation_2_block_permutation(const PermutationMatrix<Dynamic, Dynamic, int> &perm, PermutationMatrix<Dynamic, Dynamic, int> &perm_blocks, const int dim, const int size)
+void permutation_2_block_permutation(const PermutationMatrix<Dynamic, Dynamic, int>& perm,
+ PermutationMatrix<Dynamic, Dynamic, int>& perm_blocks,
+ const int dim,
+ const int size)
{
ArrayXXi idx(dim, size);
idx.row(0) = dim * perm.indices().transpose();
- for (int i = 1; i<dim; i++)
- idx.row(i) = idx.row(i-1) + 1;
- Map<ArrayXi> idx_blocks(idx.data(), dim*size, 1);
+ for (int i = 1; i < dim; i++) idx.row(i) = idx.row(i - 1) + 1;
+ Map<ArrayXi> idx_blocks(idx.data(), dim * size, 1);
perm_blocks.indices() = idx_blocks;
}
-//main
-int main(int argc, char *argv[])
+// main
+int main(int argc, char* argv[])
{
- if (argc != 3 || atoi(argv[1]) < 1|| atoi(argv[2]) < 1)
+ if (argc != 3 || atoi(argv[1]) < 1 || atoi(argv[2]) < 1)
{
std::cout << "Please call me with: [./test_ccolamd SIZE DIM], where:" << std::endl;
std::cout << " - SIZE: integer size of the problem" << std::endl;
@@ -84,43 +85,34 @@ int main(int argc, char *argv[])
return -1;
}
int size = atoi(argv[1]);
- int dim = atoi(argv[2]);
+ int dim = atoi(argv[2]);
// Problem variables
- //CholmodSupernodalLLT < SparseMatrix<double> > solver, solver2, solver3;
- SparseLU < SparseMatrix<double>, NaturalOrdering<int> > solver, solver2, solver3;
- MatrixXd omega = MatrixXd::Constant(dim, dim, 0.1) + MatrixXd::Identity(dim, dim);
- SparseMatrix<double> H(dim,dim),
- H_ordered(dim,dim),
- H_b_ordered(dim,dim);
- VectorXd b(dim),
- b_ordered(dim),
- b_b_ordered(dim),
- x_b_ordered(dim),
- x_ordered(dim),
- x(dim);
+ // CholmodSupernodalLLT < SparseMatrix<double> > solver, solver2, solver3;
+ SparseLU<SparseMatrix<double>, NaturalOrdering<int> > solver, solver2, solver3;
+ MatrixXd omega = MatrixXd::Constant(dim, dim, 0.1) + MatrixXd::Identity(dim, dim);
+ SparseMatrix<double> H(dim, dim), H_ordered(dim, dim), H_b_ordered(dim, dim);
+ VectorXd b(dim), b_ordered(dim), b_b_ordered(dim), x_b_ordered(dim), x_ordered(dim), x(dim);
// ordering variables
- SparseMatrix<int> factors(1,1), factors_ordered(1,1);
- ArrayXi acc_permutation(dim),
- acc_permutation_b(dim),
- acc_permutation_factors(1);
- acc_permutation = ArrayXi::LinSpaced(dim,0,dim-1);
- acc_permutation_b = acc_permutation;
+ SparseMatrix<int> factors(1, 1), factors_ordered(1, 1);
+ ArrayXi acc_permutation(dim), acc_permutation_b(dim), acc_permutation_factors(1);
+ acc_permutation = ArrayXi::LinSpaced(dim, 0, dim - 1);
+ acc_permutation_b = acc_permutation;
acc_permutation_factors(0) = 0;
CCOLAMDOrdering<int> ordering;
- VectorXi factor_ordering_factors(1);
- VectorXi ordering_factors(1);
+ VectorXi factor_ordering_factors(1);
+ VectorXi ordering_factors(1);
// results variables
clock_t t1, t2, t3;
- double time1=0, time2=0, time3=0;
+ double time1 = 0, time2 = 0, time3 = 0;
// INITIAL STATE
- addSparseBlock(5*omega, H, 0, 0);
- factors.insert(0,0) = 1;
- b.head(dim) = VectorXd::LinSpaced(Sequential, dim, 0, dim-1);
+ addSparseBlock(5 * omega, H, 0, 0);
+ factors.insert(0, 0) = 1;
+ b.head(dim) = VectorXd::LinSpaced(Sequential, dim, 0, dim - 1);
std::cout << "STARTING INCREMENTAL TEST" << std::endl << std::endl;
@@ -129,40 +121,43 @@ int main(int argc, char *argv[])
{
std::cout << "========================= STEP " << i << ":" << std::endl;
// AUGMENT THE PROBLEM ----------------------------
- H.conservativeResize((i+1)*dim,(i+1)*dim);
- H_ordered.conservativeResize((i+1)*dim,(i+1)*dim);
- H_b_ordered.conservativeResize((i+1)*dim,(i+1)*dim);
- b.conservativeResize((i+1)*dim);
- b_ordered.conservativeResize((i+1)*dim);
- b_b_ordered.conservativeResize((i+1)*dim);
- x.conservativeResize((i+1)*dim);
- x_ordered.conservativeResize((i+1)*dim);
- x_b_ordered.conservativeResize((i+1)*dim);
- factors.conservativeResize(i+1, i+1);
+ H.conservativeResize((i + 1) * dim, (i + 1) * dim);
+ H_ordered.conservativeResize((i + 1) * dim, (i + 1) * dim);
+ H_b_ordered.conservativeResize((i + 1) * dim, (i + 1) * dim);
+ b.conservativeResize((i + 1) * dim);
+ b_ordered.conservativeResize((i + 1) * dim);
+ b_b_ordered.conservativeResize((i + 1) * dim);
+ x.conservativeResize((i + 1) * dim);
+ x_ordered.conservativeResize((i + 1) * dim);
+ x_b_ordered.conservativeResize((i + 1) * dim);
+ factors.conservativeResize(i + 1, i + 1);
// Odometry
- addSparseBlock(5*omega, H, i*dim, i*dim);
- addSparseBlock(omega, H, i*dim, (i-1)*dim);
- addSparseBlock(omega, H, (i-1)*dim, i*dim);
- factors.insert(i,i) = 1;
- factors.insert(i,i-1) = 1;
- factors.insert(i-1,i) = 1;
+ addSparseBlock(5 * omega, H, i * dim, i * dim);
+ addSparseBlock(omega, H, i * dim, (i - 1) * dim);
+ addSparseBlock(omega, H, (i - 1) * dim, i * dim);
+ factors.insert(i, i) = 1;
+ factors.insert(i, i - 1) = 1;
+ factors.insert(i - 1, i) = 1;
// Loop Closure
- if (i == size-1)
+ if (i == size - 1)
{
- addSparseBlock(2*omega, H, 0, i*dim);
- addSparseBlock(2*omega, H, i*dim, 0);
- factors.insert(0,i) = 1;
- factors.insert(i,0) = 1;
+ addSparseBlock(2 * omega, H, 0, i * dim);
+ addSparseBlock(2 * omega, H, i * dim, 0);
+ factors.insert(0, i) = 1;
+ factors.insert(i, 0) = 1;
}
// r.h.v
- b.segment(i*dim, dim) = VectorXd::LinSpaced(Sequential, dim, dim*i, dim *(i+1)-1);
+ b.segment(i * dim, dim) = VectorXd::LinSpaced(Sequential, dim, dim * i, dim * (i + 1) - 1);
std::cout << "Solving factor graph:" << std::endl;
- std::cout << "Factors: " << std::endl << factors * MatrixXi::Identity((i+1), (i+1)) << std::endl << std::endl;
-// std::cout << "H: " << std::endl << H * MatrixXd::Identity(dim*(i+1), dim*(i+1)) << std::endl << std::endl;
+ std::cout << "Factors: " << std::endl
+ << factors * MatrixXi::Identity((i + 1), (i + 1)) << std::endl
+ << std::endl;
+ // std::cout << "H: " << std::endl << H * MatrixXd::Identity(dim*(i+1), dim*(i+1)) << std::endl <<
+ // std::endl;
// SOLVING WITHOUT REORDERING ------------------------------------
// solve Hx = b
@@ -174,33 +169,34 @@ int main(int argc, char *argv[])
return 0;
}
x = solver.solve(b);
- time1 += ((double) clock() - t1) / CLOCKS_PER_SEC;
+ time1 += ((double)clock() - t1) / CLOCKS_PER_SEC;
// SOLVING WITH REORDERING ------------------------------------
// Order with previous orderings
- acc_permutation.conservativeResize(dim*(i+1));
- acc_permutation.tail(dim) = ArrayXi::LinSpaced(dim,dim*i,dim*(i+1)-1);
- PermutationMatrix<Dynamic, Dynamic, int> acc_permutation_matrix(dim*(i+1));
+ acc_permutation.conservativeResize(dim * (i + 1));
+ acc_permutation.tail(dim) = ArrayXi::LinSpaced(dim, dim * i, dim * (i + 1) - 1);
+ PermutationMatrix<Dynamic, Dynamic, int> acc_permutation_matrix(dim * (i + 1));
acc_permutation_matrix.indices() = acc_permutation;
- b_ordered = acc_permutation_matrix * b;
- H_ordered = H.twistedBy(acc_permutation_matrix);
+ b_ordered = acc_permutation_matrix * b;
+ H_ordered = H.twistedBy(acc_permutation_matrix);
// ordering factors
- ordering_factors.resize(dim*(i+1));
- ordering_factors = ((H_ordered.rightCols(3) * MatrixXd::Ones(3,1)).array() == 0).select(VectorXi::Zero(dim*(i+1)),VectorXi::Ones(dim*(i+1)));
+ ordering_factors.resize(dim * (i + 1));
+ ordering_factors = ((H_ordered.rightCols(3) * MatrixXd::Ones(3, 1)).array() == 0)
+ .select(VectorXi::Zero(dim * (i + 1)), VectorXi::Ones(dim * (i + 1)));
// variable ordering
t2 = clock();
H_ordered.makeCompressed();
- PermutationMatrix<Dynamic, Dynamic, int> permutation_matrix(dim*(i+1));
+ PermutationMatrix<Dynamic, Dynamic, int> permutation_matrix(dim * (i + 1));
ordering(H_ordered, permutation_matrix, ordering_factors.data());
// applying ordering
acc_permutation_matrix = permutation_matrix * acc_permutation_matrix;
- acc_permutation = acc_permutation_matrix.indices();
- b_ordered = permutation_matrix * b_ordered;
- H_ordered = H_ordered.twistedBy(permutation_matrix);
+ acc_permutation = acc_permutation_matrix.indices();
+ b_ordered = permutation_matrix * b_ordered;
+ H_ordered = H_ordered.twistedBy(permutation_matrix);
// solve Hx = b
solver2.compute(H_ordered);
@@ -211,22 +207,22 @@ int main(int argc, char *argv[])
}
x_ordered = solver2.solve(b_ordered);
x_ordered = acc_permutation_matrix.inverse() * x_ordered;
- time2 += ((double) clock() - t2) / CLOCKS_PER_SEC;
+ time2 += ((double)clock() - t2) / CLOCKS_PER_SEC;
// SOLVING WITH BLOCK REORDERING ------------------------------------
// Order with previous orderings
- acc_permutation_b.conservativeResize(dim*(i+1));
- acc_permutation_b.tail(dim) = ArrayXi::LinSpaced(dim,dim*i,dim*(i+1)-1);
- PermutationMatrix<Dynamic, Dynamic, int> acc_permutation_b_matrix(dim*(i+1));
+ acc_permutation_b.conservativeResize(dim * (i + 1));
+ acc_permutation_b.tail(dim) = ArrayXi::LinSpaced(dim, dim * i, dim * (i + 1) - 1);
+ PermutationMatrix<Dynamic, Dynamic, int> acc_permutation_b_matrix(dim * (i + 1));
acc_permutation_b_matrix.indices() = acc_permutation_b;
- b_b_ordered = acc_permutation_b_matrix * b;
- H_b_ordered = H.twistedBy(acc_permutation_b_matrix);
+ b_b_ordered = acc_permutation_b_matrix * b;
+ H_b_ordered = H.twistedBy(acc_permutation_b_matrix);
- acc_permutation_factors.conservativeResize(i+1);
+ acc_permutation_factors.conservativeResize(i + 1);
acc_permutation_factors(i) = i;
- PermutationMatrix<Dynamic, Dynamic, int> acc_permutation_factors_matrix(dim*(i+1));
+ PermutationMatrix<Dynamic, Dynamic, int> acc_permutation_factors_matrix(dim * (i + 1));
acc_permutation_factors_matrix.indices() = acc_permutation_factors;
- factors_ordered = factors.twistedBy(acc_permutation_factors_matrix);
+ factors_ordered = factors.twistedBy(acc_permutation_factors_matrix);
// ordering factors
factor_ordering_factors.resize(i);
@@ -236,17 +232,17 @@ int main(int argc, char *argv[])
t3 = clock();
factors_ordered.makeCompressed();
- PermutationMatrix<Dynamic, Dynamic, int> permutation_factors_matrix(i+1);
+ PermutationMatrix<Dynamic, Dynamic, int> permutation_factors_matrix(i + 1);
ordering(factors_ordered, permutation_factors_matrix, factor_ordering_factors.data());
// applying ordering
- permutation_2_block_permutation(permutation_factors_matrix, permutation_matrix , dim, i+1);
+ permutation_2_block_permutation(permutation_factors_matrix, permutation_matrix, dim, i + 1);
acc_permutation_factors_matrix = permutation_factors_matrix * acc_permutation_factors_matrix;
- acc_permutation_factors = acc_permutation_factors_matrix.indices();
- acc_permutation_b_matrix = permutation_matrix * acc_permutation_b_matrix;
- acc_permutation_b = acc_permutation_b_matrix.indices();
- b_b_ordered = permutation_matrix * b_b_ordered;
- H_b_ordered = H_b_ordered.twistedBy(permutation_matrix);
+ acc_permutation_factors = acc_permutation_factors_matrix.indices();
+ acc_permutation_b_matrix = permutation_matrix * acc_permutation_b_matrix;
+ acc_permutation_b = acc_permutation_b_matrix.indices();
+ b_b_ordered = permutation_matrix * b_b_ordered;
+ H_b_ordered = H_b_ordered.twistedBy(permutation_matrix);
// solve Hx = b
solver3.compute(H_b_ordered);
@@ -257,25 +253,24 @@ int main(int argc, char *argv[])
}
x_b_ordered = solver3.solve(b_b_ordered);
x_b_ordered = acc_permutation_b_matrix.inverse() * x_b_ordered;
- time3 += ((double) clock() - t3) / CLOCKS_PER_SEC;
+ time3 += ((double)clock() - t3) / CLOCKS_PER_SEC;
// RESULTS ------------------------------------
std::cout << "========================= RESULTS " << i << ":" << std::endl;
- std::cout << "NO REORDERING: solved in " << time1*1e3 << " ms" << std::endl;
- std::cout << "REORDERING: solved in " << time2*1e3 << " ms" << std::endl;
- std::cout << "BLOCK REORDERING: solved in " << time3*1e3 << " ms" << std::endl;
+ std::cout << "NO REORDERING: solved in " << time1 * 1e3 << " ms" << std::endl;
+ std::cout << "REORDERING: solved in " << time2 * 1e3 << " ms" << std::endl;
+ std::cout << "BLOCK REORDERING: solved in " << time3 * 1e3 << " ms" << std::endl;
std::cout << "x1 = " << x.transpose() << std::endl;
std::cout << "x2 = " << x_ordered.transpose() << std::endl;
std::cout << "x3 = " << x_b_ordered.transpose() << std::endl;
}
// RESULTS ------------------------------------
- std::cout << "NO REORDERING: solved in " << time1*1e3 << " ms" << std::endl;
- std::cout << "REORDERING: solved in " << time2*1e3 << " ms" << std::endl;
- std::cout << "BLOCK REORDERING: solved in " << time3*1e3 << " ms" << std::endl;
+ std::cout << "NO REORDERING: solved in " << time1 * 1e3 << " ms" << std::endl;
+ std::cout << "REORDERING: solved in " << time2 * 1e3 << " ms" << std::endl;
+ std::cout << "BLOCK REORDERING: solved in " << time3 * 1e3 << " ms" << std::endl;
- //std::cout << "x = " << x.transpose() << std::endl;
- //std::cout << "x = " << x_ordered.transpose() << std::endl;
- //std::cout << "x = " << x_b_ordered.transpose() << std::endl;
+ // std::cout << "x = " << x.transpose() << std::endl;
+ // std::cout << "x = " << x_ordered.transpose() << std::endl;
+ // std::cout << "x = " << x_b_ordered.transpose() << std::endl;
}
-
diff --git a/demos/solver/test_permutations.cpp b/demos/solver/test_permutations.cpp
index 0e43cd1f0e6ed7b683add470bc9589b87cfd7558..887522e03b5efeb2899c74a3568d3928b06304eb 100644
--- a/demos/solver/test_permutations.cpp
+++ b/demos/solver/test_permutations.cpp
@@ -17,14 +17,8 @@
//
// 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/>.
-/*
- * test_permutations.cpp
- *
- * Created on: Jun 15, 2015
- * Author: jvallve
- */
-
-//std includes
+
+// std includes
#include <cstdlib>
#include <iostream>
#include <fstream>
@@ -39,7 +33,7 @@
using namespace Eigen;
-//main
+// main
int main(int argc, char *argv[])
{
PermutationMatrix<Dynamic, Dynamic, int> P1(5), P2(5), P3(5), P4(5);
@@ -47,8 +41,8 @@ int main(int argc, char *argv[])
P2.setIdentity();
P3.setIdentity();
- VectorXd a = VectorXd::LinSpaced(5,1,5);
- MatrixXd A= a.asDiagonal();
+ VectorXd a = VectorXd::LinSpaced(5, 1, 5);
+ MatrixXd A = a.asDiagonal();
SparseMatrix<double> B = A.sparseView();
B.makeCompressed();
@@ -72,35 +66,42 @@ int main(int argc, char *argv[])
std::cout << "(P1 * B).bottomRows(1)" << std::endl << C.bottomRows(1) << std::endl << std::endl;
std::cout << "Post-multiplying: Permutating cols" << std::endl;
- std::cout << "A * P1.transpose()" << std::endl << A * P1.transpose()<< std::endl << std::endl;
- std::cout << "B * P1.transpose()" << std::endl << B * P1.transpose()<< std::endl << std::endl;
+ std::cout << "A * P1.transpose()" << std::endl << A * P1.transpose() << std::endl << std::endl;
+ std::cout << "B * P1.transpose()" << std::endl << B * P1.transpose() << std::endl << std::endl;
std::cout << "Pre&post-multiplying:" << std::endl;
std::cout << "P1 * A * P1.transpose()" << std::endl << P1 * A * P1.transpose() << std::endl << std::endl;
- std::cout << "P2 * P1 * A * P1.transpose() * P2.transpose()" << std::endl << P2 * P1 * A * P1.transpose() * P2.transpose() << std::endl << std::endl;
- std::cout << "P1 * P2 * A * P2.transpose() * P1.transpose()" << std::endl << P1 * P2 * A * P2.transpose() * P1.transpose() << std::endl << std::endl;
+ std::cout << "P2 * P1 * A * P1.transpose() * P2.transpose()" << std::endl
+ << P2 * P1 * A * P1.transpose() * P2.transpose() << std::endl
+ << std::endl;
+ std::cout << "P1 * P2 * A * P2.transpose() * P1.transpose()" << std::endl
+ << P1 * P2 * A * P2.transpose() * P1.transpose() << std::endl
+ << std::endl;
P3 = P1 * P2;
std::cout << "Permutation P3 = P1 * P2" << std::endl << P3.indices().transpose() << std::endl << std::endl;
- std::cout << "P3 * A * P3.transpose()" << std::endl << P3 * A * P3.transpose() << std::endl << std::endl;
+ std::cout << "P3 * A * P3.transpose()" << std::endl << P3 * A * P3.transpose() << std::endl << std::endl;
std::cout << "PERMUTATING INDICES" << std::endl;
ArrayXi acc_permutations(5);
- acc_permutations << 0,1,2,3,4;
+ acc_permutations << 0, 1, 2, 3, 4;
std::cout << "acc_permutations: " << acc_permutations.transpose() << std::endl;
std::cout << "P1: " << P1.indices().transpose() << std::endl;
std::cout << "P1 * acc_permutations: " << (P1 * acc_permutations.matrix()).transpose() << std::endl;
- std::cout << "P1.inverse() * acc_permutations: " << (P1.inverse() * acc_permutations.matrix()).transpose() << std::endl;
+ std::cout << "P1.inverse() * acc_permutations: " << (P1.inverse() * acc_permutations.matrix()).transpose()
+ << std::endl;
std::cout << "P2: " << P2.indices().transpose() << std::endl;
std::cout << "P2 * (P1 * acc_permutations): " << (P2 * (P1 * acc_permutations.matrix())).transpose() << std::endl;
- std::cout << "(P2 * P1).inverse() * acc_permutations): " << ((P2 * P1).inverse() * acc_permutations.matrix()).transpose() << std::endl;
+ std::cout << "(P2 * P1).inverse() * acc_permutations): "
+ << ((P2 * P1).inverse() * acc_permutations.matrix()).transpose() << std::endl;
P4 = P1 * P2 * P3;
std::cout << "Permutation P4 = P1 * P2 * P3: " << P4.indices().transpose() << std::endl;
- std::cout << "P3 * (P2 * (P1 * acc_permutations)): " << (P3 * (P2 * (P1 * acc_permutations.matrix()))).transpose() << std::endl;
+ std::cout << "P3 * (P2 * (P1 * acc_permutations)): " << (P3 * (P2 * (P1 * acc_permutations.matrix()))).transpose()
+ << std::endl;
std::cout << "accumulated permutations can not be stored in vectors..." << std::endl;
std::cout << std::endl << "PARTIALL PERMUTATIONS" << std::endl;
@@ -110,10 +111,13 @@ int main(int argc, char *argv[])
std::cout << "P5 (equivalent to P1 but partiall): " << std::endl << P5.indices().transpose() << std::endl;
std::cout << "P2: " << P2.indices().transpose() << std::endl << std::endl;
std::cout << "A * P2.transpose(): " << std::endl << A * P2.transpose() << std::endl << std::endl;
- std::cout << "(A * P2.transpose()).rightCols(2) * P5.transpose(): " << std::endl << (A * P2.transpose()).rightCols(2) * P5.transpose() << std::endl << std::endl;
+ std::cout << "(A * P2.transpose()).rightCols(2) * P5.transpose(): " << std::endl
+ << (A * P2.transpose()).rightCols(2) * P5.transpose() << std::endl
+ << std::endl;
PermutationMatrix<Dynamic, Dynamic, int> P6 = P2;
- P6.indices().tail(2) = P5 * P6.indices().tail(2);
- std::cout << "P6 = P2, P6.indices().tail(2) = P5 * P6.indices().tail(2): " << P6.indices().transpose() << std::endl << std::endl;
+ P6.indices().tail(2) = P5 * P6.indices().tail(2);
+ std::cout << "P6 = P2, P6.indices().tail(2) = P5 * P6.indices().tail(2): " << P6.indices().transpose() << std::endl
+ << std::endl;
std::cout << "A * P6.transpose(): " << std::endl << A * P6.transpose() << std::endl << std::endl;
std::cout << "(P1 * P2): " << std::endl << (P1 * P2).indices().transpose() << std::endl << std::endl;
std::cout << "A * (P1 * P2).transpose(): " << std::endl << A * (P1 * P2).transpose() << std::endl << std::endl;
@@ -128,5 +132,5 @@ int main(int argc, char *argv[])
std::cout << "mapped_a = " << mapped_a.transpose() << std::endl << std::endl;
std::cout << "maps are affected of the reorderings in mapped vectors" << std::endl;
-// Map<>
+ // Map<>
}
diff --git a/include/core/capture/capture_diff_drive.h b/include/core/capture/capture_diff_drive.h
index 2310e147c6ba61dd0171bc64549c9fd4dc5837a8..63c7ed69e4940c9a922f9c1f2c596bd24a961897 100644
--- a/include/core/capture/capture_diff_drive.h
+++ b/include/core/capture/capture_diff_drive.h
@@ -20,11 +20,11 @@
#pragma once
-//wolf includes
+// wolf includes
#include "core/capture/capture_motion.h"
-namespace wolf {
-
+namespace wolf
+{
WOLF_PTR_TYPEDEFS(CaptureDiffDrive)
/**
@@ -34,22 +34,20 @@ WOLF_PTR_TYPEDEFS(CaptureDiffDrive)
*/
class CaptureDiffDrive : public CaptureMotion
{
-
-public:
-
- /**
- * \brief Constructor
- **/
- CaptureDiffDrive(const TimeStamp& _ts,
- const SensorBasePtr& _sensor_ptr,
- const Eigen::VectorXd& _data,
- const Eigen::MatrixXd& _data_cov,
- CaptureBasePtr _capture_origin_ptr,
- StateBlockPtr _p_ptr = nullptr,
- StateBlockPtr _o_ptr = nullptr,
- StateBlockPtr _intr_ptr = nullptr);
-
- ~CaptureDiffDrive() override = default;
+ public:
+ /**
+ * \brief Constructor
+ **/
+ CaptureDiffDrive(const TimeStamp& _ts,
+ const SensorBasePtr& _sensor_ptr,
+ const Eigen::VectorXd& _data,
+ const Eigen::MatrixXd& _data_cov,
+ CaptureBasePtr _capture_origin_ptr,
+ StateBlockPtr _p_ptr = nullptr,
+ StateBlockPtr _o_ptr = nullptr,
+ StateBlockPtr _intr_ptr = nullptr);
+
+ ~CaptureDiffDrive() override = default;
};
-} // namespace wolf
+} // namespace wolf
diff --git a/include/core/capture/capture_landmarks_external.h b/include/core/capture/capture_landmarks_external.h
index f186ae1e9ad9e7c4cdcf08173e92415ad282a354..3dffe5bf7668e70b43a560136753a94cb9d7d895 100644
--- a/include/core/capture/capture_landmarks_external.h
+++ b/include/core/capture/capture_landmarks_external.h
@@ -20,41 +20,46 @@
#pragma once
-//Wolf includes
+// Wolf includes
#include "core/capture/capture_base.h"
-namespace wolf {
-
+namespace wolf
+{
struct LandmarkDetection
{
- int id; // id of landmark
+ int id; // id 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<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 Eigen::VectorXd& _detection,
+ const Eigen::MatrixXd& _cov,
+ const double& quality);
};
-} //namespace wolf
+} // namespace wolf
diff --git a/include/core/capture/capture_motion.h b/include/core/capture/capture_motion.h
index 51de061db9c8d3b007f71d742352e210400b3ef5..b00f186bd42cfe8fec38481da63b5eda5d0e1cbc 100644
--- a/include/core/capture/capture_motion.h
+++ b/include/core/capture/capture_motion.h
@@ -30,10 +30,9 @@
#include <iterator>
#include <utility>
-namespace wolf {
-
+namespace wolf
+{
WOLF_PTR_TYPEDEFS(CaptureMotion);
-
/** \brief Base class for motion Captures.
*
@@ -49,79 +48,82 @@ WOLF_PTR_TYPEDEFS(CaptureMotion);
* - until the frame of this capture.
*
* Once a keyframe is generated, this buffer is frozen and kept in the Capture for eventual later uses.
- * It is then used to compute the factor that links the Frame of this capture to the previous key-frame in the Trajectory.
+ * It is then used to compute the factor that links the Frame of this capture to the previous key-frame in the
+ * Trajectory.
*/
class CaptureMotion : public CaptureBase
{
-
- // public interface:
-
- public:
- CaptureMotion(const std::string & _type,
- const TimeStamp& _ts,
- SensorBasePtr _sensor_ptr,
- const Eigen::VectorXd& _data,
- CaptureBasePtr _capture_origin_ptr);
-
- CaptureMotion(const std::string & _type,
- const TimeStamp& _ts,
- SensorBasePtr _sensor_ptr,
- const Eigen::VectorXd& _data,
- const Eigen::MatrixXd& _data_cov,
- CaptureBasePtr _capture_origin_ptr,
- StateBlockPtr _p_ptr = nullptr,
- StateBlockPtr _o_ptr = nullptr,
- StateBlockPtr _intr_ptr = nullptr);
-
- ~CaptureMotion() override;
-
- // Type
- bool isMotion() const override { return true; }
-
- // Data
- const Eigen::VectorXd& getData() const;
- const Eigen::MatrixXd& getDataCovariance() const;
- void setData(const Eigen::VectorXd& _data);
- void setDataCovariance(const Eigen::MatrixXd& _data_cov);
-
- // Buffer
- MotionBuffer& getBuffer();
- const MotionBuffer& getBuffer() const;
-
- // Buffer's initial conditions for pre-integration
- VectorXd getCalibrationPreint() const;
- void setCalibrationPreint(const VectorXd& _calib_preint);
- MatrixXd getJacobianCalib() const;
- MatrixXd getJacobianCalib(const TimeStamp& _ts) const;
-
- // Get delta preintegrated, and corrected for changes on calibration params
- VectorXd getDeltaPreint() const;
- VectorXd getDeltaPreint(const TimeStamp& _ts) const;
- MatrixXd getDeltaPreintCov() const;
- MatrixXd getDeltaPreintCov(const TimeStamp& _ts) const;
-
- // Origin frame and capture
- CaptureBasePtr getOriginCapture();
- CaptureBaseConstPtr getOriginCapture() const;
- void setOriginCapture(CaptureBasePtr _capture_origin_ptr);
-
- bool containsTimeStamp(const TimeStamp& _ts, double _time_tolerance) const;
-
- void printHeader(int depth, //
- bool factored_by, //
- bool metric, //
- bool state_blocks,
- std::ostream& stream ,
- std::string _tabs = "") const override;
-
- // member data:
- private:
- Eigen::VectorXd data_; ///< Motion data in form of vector mandatory
- Eigen::MatrixXd data_cov_; ///< Motion data covariance
- Eigen::VectorXd calib_preint_; ///< Calibration parameters used during pre-integration
- MotionBuffer buffer_; ///< Buffer of motions between this Capture and the next one.
- CaptureBaseWPtr capture_origin_ptr_; ///< Pointer to the origin capture of the motion
+ // public interface:
+
+ public:
+ CaptureMotion(const std::string& _type,
+ const TimeStamp& _ts,
+ SensorBasePtr _sensor_ptr,
+ const Eigen::VectorXd& _data,
+ CaptureBasePtr _capture_origin_ptr);
+
+ CaptureMotion(const std::string& _type,
+ const TimeStamp& _ts,
+ SensorBasePtr _sensor_ptr,
+ const Eigen::VectorXd& _data,
+ const Eigen::MatrixXd& _data_cov,
+ CaptureBasePtr _capture_origin_ptr,
+ StateBlockPtr _p_ptr = nullptr,
+ StateBlockPtr _o_ptr = nullptr,
+ StateBlockPtr _intr_ptr = nullptr);
+
+ ~CaptureMotion() override;
+
+ // Type
+ bool isMotion() const override
+ {
+ return true;
+ }
+
+ // Data
+ const Eigen::VectorXd& getData() const;
+ const Eigen::MatrixXd& getDataCovariance() const;
+ void setData(const Eigen::VectorXd& _data);
+ void setDataCovariance(const Eigen::MatrixXd& _data_cov);
+
+ // Buffer
+ MotionBuffer& getBuffer();
+ const MotionBuffer& getBuffer() const;
+
+ // Buffer's initial conditions for pre-integration
+ VectorXd getCalibrationPreint() const;
+ void setCalibrationPreint(const VectorXd& _calib_preint);
+ MatrixXd getJacobianCalib() const;
+ MatrixXd getJacobianCalib(const TimeStamp& _ts) const;
+
+ // Get delta preintegrated, and corrected for changes on calibration params
+ VectorXd getDeltaPreint() const;
+ VectorXd getDeltaPreint(const TimeStamp& _ts) const;
+ MatrixXd getDeltaPreintCov() const;
+ MatrixXd getDeltaPreintCov(const TimeStamp& _ts) const;
+
+ // Origin frame and capture
+ CaptureBasePtr getOriginCapture();
+ CaptureBaseConstPtr getOriginCapture() const;
+ void setOriginCapture(CaptureBasePtr _capture_origin_ptr);
+
+ bool containsTimeStamp(const TimeStamp& _ts, double _time_tolerance) const;
+
+ void printHeader(int depth, //
+ bool factored_by, //
+ bool metric, //
+ bool state_blocks,
+ std::ostream& stream,
+ std::string _tabs = "") const override;
+
+ // member data:
+ private:
+ Eigen::VectorXd data_; ///< Motion data in form of vector mandatory
+ Eigen::MatrixXd data_cov_; ///< Motion data covariance
+ Eigen::VectorXd calib_preint_; ///< Calibration parameters used during pre-integration
+ MotionBuffer buffer_; ///< Buffer of motions between this Capture and the next one.
+ CaptureBaseWPtr capture_origin_ptr_; ///< Pointer to the origin capture of the motion
};
inline const Eigen::VectorXd& CaptureMotion::getData() const
@@ -212,4 +214,4 @@ inline MatrixXd CaptureMotion::getDeltaPreintCov(const TimeStamp& _ts) const
return getBuffer().getMotion(_ts).delta_integr_cov_;
}
-} // namespace wolf
+} // namespace wolf
diff --git a/include/core/capture/capture_odom_2d.h b/include/core/capture/capture_odom_2d.h
index b111f605c200856e77c84ce84867ae9152fc0a9d..6b52e5c357cf4646ee0003708b76dd93621a63e6 100644
--- a/include/core/capture/capture_odom_2d.h
+++ b/include/core/capture/capture_odom_2d.h
@@ -26,25 +26,23 @@
namespace wolf
{
-
WOLF_PTR_TYPEDEFS(CaptureOdom2d);
class CaptureOdom2d : public CaptureMotion
{
- public:
- CaptureOdom2d(const TimeStamp& _init_ts,
- SensorBasePtr _sensor_ptr,
- const Eigen::VectorXd& _data,
- CaptureBasePtr _capture_origin_ptr = nullptr);
-
- CaptureOdom2d(const TimeStamp& _init_ts,
- SensorBasePtr _sensor_ptr,
- const Eigen::VectorXd& _data,
- const Eigen::MatrixXd& _data_cov,
- CaptureBasePtr _capture_origin_ptr = nullptr);
-
- ~CaptureOdom2d() override;
-
+ public:
+ CaptureOdom2d(const TimeStamp& _init_ts,
+ SensorBasePtr _sensor_ptr,
+ const Eigen::VectorXd& _data,
+ CaptureBasePtr _capture_origin_ptr = nullptr);
+
+ CaptureOdom2d(const TimeStamp& _init_ts,
+ SensorBasePtr _sensor_ptr,
+ const Eigen::VectorXd& _data,
+ const Eigen::MatrixXd& _data_cov,
+ CaptureBasePtr _capture_origin_ptr = nullptr);
+
+ ~CaptureOdom2d() override;
};
} /* namespace wolf */
diff --git a/include/core/capture/capture_odom_3d.h b/include/core/capture/capture_odom_3d.h
index 13463ab08db4a543588f774045bdb64c5f33a069..e93dc3c05a464b7b9e6d0433369ec709031b9be2 100644
--- a/include/core/capture/capture_odom_3d.h
+++ b/include/core/capture/capture_odom_3d.h
@@ -26,25 +26,23 @@
namespace wolf
{
-
WOLF_PTR_TYPEDEFS(CaptureOdom3d);
class CaptureOdom3d : public CaptureMotion
{
- public:
- CaptureOdom3d(const TimeStamp& _init_ts,
- SensorBasePtr _sensor_ptr,
- const Eigen::VectorXd& _data,
- CaptureBasePtr _capture_origin_ptr = nullptr);
-
- CaptureOdom3d(const TimeStamp& _init_ts,
- SensorBasePtr _sensor_ptr,
- const Eigen::VectorXd& _data,
- const Eigen::MatrixXd& _data_cov,
- CaptureBasePtr _capture_origin_ptr = nullptr);
-
- ~CaptureOdom3d() override;
-
+ public:
+ CaptureOdom3d(const TimeStamp& _init_ts,
+ SensorBasePtr _sensor_ptr,
+ const Eigen::VectorXd& _data,
+ CaptureBasePtr _capture_origin_ptr = nullptr);
+
+ CaptureOdom3d(const TimeStamp& _init_ts,
+ SensorBasePtr _sensor_ptr,
+ const Eigen::VectorXd& _data,
+ const Eigen::MatrixXd& _data_cov,
+ CaptureBasePtr _capture_origin_ptr = nullptr);
+
+ ~CaptureOdom3d() override;
};
} /* namespace wolf */
diff --git a/include/core/capture/capture_pose.h b/include/core/capture/capture_pose.h
index 64b95810032c5fd58a624ef795018d1d026a8d56..7ff804a0116dc79f537427e150b4fbe976a3e8af 100644
--- a/include/core/capture/capture_pose.h
+++ b/include/core/capture/capture_pose.h
@@ -20,40 +20,46 @@
#pragma once
-//Wolf includes
+// Wolf includes
#include "core/capture/capture_base.h"
#include "core/factor/factor_pose_2d.h"
#include "core/factor/factor_pose_3d.h"
#include "core/feature/feature_pose.h"
-//std includes
+// std includes
//
-namespace wolf {
-
+namespace wolf
+{
WOLF_PTR_TYPEDEFS(CapturePose);
-//class CapturePose
+// class CapturePose
class CapturePose : public CaptureBase
{
- protected:
- Eigen::VectorXd data_; ///< Raw data.
- Eigen::MatrixXd data_covariance_; ///< Noise of the capture.
-
- public:
+ protected:
+ Eigen::VectorXd data_; ///< Raw data.
+ Eigen::MatrixXd data_covariance_; ///< Noise of the capture.
- CapturePose(const TimeStamp& _ts,
- SensorBasePtr _sensor_ptr,
- const Eigen::VectorXd& _data,
- const Eigen::MatrixXd& _data_covariance);
+ public:
+ CapturePose(const TimeStamp& _ts,
+ SensorBasePtr _sensor_ptr,
+ const Eigen::VectorXd& _data,
+ const Eigen::MatrixXd& _data_covariance);
- ~CapturePose() override;
+ ~CapturePose() override;
- virtual void emplaceFeatureAndFactor(const ProcessorBasePtr& _processor=nullptr, bool _apply_loss_function=false);
- Eigen::VectorXd getData() const { return data_;}
- Eigen::MatrixXd getDataCovariance() const { return data_covariance_;}
- void setData(const Eigen::VectorXd& _data);
- void setDataCovariance(const Eigen::MatrixXd& _data_covariance);
+ virtual void emplaceFeatureAndFactor(const ProcessorBasePtr& _processor = nullptr,
+ bool _apply_loss_function = false);
+ Eigen::VectorXd getData() const
+ {
+ return data_;
+ }
+ Eigen::MatrixXd getDataCovariance() const
+ {
+ return data_covariance_;
+ }
+ void setData(const Eigen::VectorXd& _data);
+ void setDataCovariance(const Eigen::MatrixXd& _data_covariance);
};
-} //namespace wolf
+} // namespace wolf
diff --git a/include/core/capture/capture_void.h b/include/core/capture/capture_void.h
index 8360c6216851440df783fafd443818d1fd28f950..1b5653cba1f0b25cfae052b033fd92f290b6c641 100644
--- a/include/core/capture/capture_void.h
+++ b/include/core/capture/capture_void.h
@@ -20,21 +20,19 @@
#pragma once
-//Wolf includes
+// Wolf includes
#include "core/capture/capture_base.h"
-namespace wolf {
-
+namespace wolf
+{
WOLF_PTR_TYPEDEFS(CaptureVoid);
-
-
-//class CaptureVoid
+
+// class CaptureVoid
class CaptureVoid : public CaptureBase
{
- public:
- CaptureVoid(const TimeStamp& _ts, SensorBasePtr _sensor_ptr);
- ~CaptureVoid() override;
-
+ public:
+ CaptureVoid(const TimeStamp& _ts, SensorBasePtr _sensor_ptr);
+ ~CaptureVoid() override;
};
-} // namespace wolf
+} // namespace wolf
diff --git a/include/core/ceres_wrapper/cost_function_wrapper.h b/include/core/ceres_wrapper/cost_function_wrapper.h
index 0fa474d37fa178119385e1f05d57fc433f4e46dc..d60fe7937a5c9ce2931194f666173d41ff265512 100644
--- a/include/core/ceres_wrapper/cost_function_wrapper.h
+++ b/include/core/ceres_wrapper/cost_function_wrapper.h
@@ -30,40 +30,35 @@
// EIGEN
#include <Eigen/StdVector>
-namespace wolf {
-
+namespace wolf
+{
WOLF_PTR_TYPEDEFS(CostFunctionWrapper);
class CostFunctionWrapper : public ceres::CostFunction
{
- private:
-
- FactorBasePtr factor_ptr_;
+ private:
+ FactorBasePtr factor_ptr_;
- public:
+ public:
+ CostFunctionWrapper(FactorBasePtr _factor_ptr);
- CostFunctionWrapper(FactorBasePtr _factor_ptr);
+ ~CostFunctionWrapper() override;
- ~CostFunctionWrapper() override;
+ bool Evaluate(const double* const* parameters, double* residuals, double** jacobians) const override;
- bool Evaluate(const double* const * parameters, double* residuals, double** jacobians) const override;
-
- FactorBasePtr getFactor() const;
+ FactorBasePtr getFactor() const;
};
-inline CostFunctionWrapper::CostFunctionWrapper(FactorBasePtr _factor_ptr) :
- ceres::CostFunction(), factor_ptr_(_factor_ptr)
+inline CostFunctionWrapper::CostFunctionWrapper(FactorBasePtr _factor_ptr)
+ : ceres::CostFunction(), factor_ptr_(_factor_ptr)
{
- for (auto st_block_size : factor_ptr_->getStateSizes())
- mutable_parameter_block_sizes()->push_back(st_block_size);
+ for (auto st_block_size : factor_ptr_->getStateSizes()) mutable_parameter_block_sizes()->push_back(st_block_size);
set_num_residuals(factor_ptr_->getSize());
}
-inline CostFunctionWrapper::~CostFunctionWrapper()
-{
-}
+inline CostFunctionWrapper::~CostFunctionWrapper() {}
-inline bool CostFunctionWrapper::Evaluate(const double* const * parameters, double* residuals, double** jacobians) const
+inline bool CostFunctionWrapper::Evaluate(const double* const* parameters, double* residuals, double** jacobians) const
{
return factor_ptr_->evaluate(parameters, residuals, jacobians);
}
@@ -73,4 +68,4 @@ inline FactorBasePtr CostFunctionWrapper::getFactor() const
return factor_ptr_;
}
-} // namespace wolf
+} // namespace wolf
diff --git a/include/core/ceres_wrapper/create_numeric_diff_cost_function.h b/include/core/ceres_wrapper/create_numeric_diff_cost_function.h
index 4447ff577fef3529060f1c22cf8b628485c6b9de..cbde48524e2231eb837a2245052dacea6930ac91 100644
--- a/include/core/ceres_wrapper/create_numeric_diff_cost_function.h
+++ b/include/core/ceres_wrapper/create_numeric_diff_cost_function.h
@@ -26,35 +26,58 @@
// Factors
#include "core/factor/factor_base.h"
-namespace wolf {
-
+namespace wolf
+{
// Wolf ceres auto_diff creator
template <class T>
-std::shared_ptr<ceres::NumericDiffCostFunction<T, ceres::CENTRAL, T::residualSize,
- T::block0Size,T::block1Size,T::block2Size,T::block3Size,T::block4Size,
- T::block5Size,T::block6Size,T::block7Size,T::block8Size,T::block9Size> > createNumericDiffCostFunctionCeres(FactorBasePtr _factor_ptr)
+std::shared_ptr<ceres::NumericDiffCostFunction<T,
+ ceres::CENTRAL,
+ T::residualSize,
+ T::block0Size,
+ T::block1Size,
+ T::block2Size,
+ T::block3Size,
+ T::block4Size,
+ T::block5Size,
+ T::block6Size,
+ T::block7Size,
+ T::block8Size,
+ T::block9Size> >
+createNumericDiffCostFunctionCeres(FactorBasePtr _factor_ptr)
{
- return std::make_shared<ceres::NumericDiffCostFunction<T, ceres::CENTRAL, T::residualSize,
- T::block0Size,T::block1Size,T::block2Size,T::block3Size,T::block4Size,
- T::block5Size,T::block6Size,T::block7Size,T::block8Size,T::block9Size> >(std::static_pointer_cast<T>(_factor_ptr).get());
+ return std::make_shared<ceres::NumericDiffCostFunction<T,
+ ceres::CENTRAL,
+ T::residualSize,
+ T::block0Size,
+ T::block1Size,
+ T::block2Size,
+ T::block3Size,
+ T::block4Size,
+ T::block5Size,
+ T::block6Size,
+ T::block7Size,
+ T::block8Size,
+ T::block9Size> >(
+ std::static_pointer_cast<T>(_factor_ptr).get());
};
inline std::shared_ptr<ceres::CostFunction> createNumericDiffCostFunction(FactorBasePtr _fac_ptr)
{
-// switch (_fac_ptr->getTypeId())
-// {
-// // just for testing
-// case FAC_ODOM_2d:
-// return createNumericDiffCostFunctionCeres<FactorOdom2d>(_fac_ptr);
-//
-// /* For adding a new factor, add the #include and a case:
-// case FAC_ENUM:
-// return createNumericDiffCostFunctionCeres<FactorType>(_fac_ptr);
-// */
-//
-// default:
- throw std::invalid_argument( "Unknown factor type! Please add it in the file: ceres_wrapper/create_Numeric_diff_cost_function.h" );
-// }
+ // switch (_fac_ptr->getTypeId())
+ // {
+ // // just for testing
+ // case FAC_ODOM_2d:
+ // return createNumericDiffCostFunctionCeres<FactorOdom2d>(_fac_ptr);
+ //
+ // /* For adding a new factor, add the #include and a case:
+ // case FAC_ENUM:
+ // return createNumericDiffCostFunctionCeres<FactorType>(_fac_ptr);
+ // */
+ //
+ // default:
+ throw std::invalid_argument(
+ "Unknown factor type! Please add it in the file: ceres_wrapper/create_Numeric_diff_cost_function.h");
+ // }
}
-} // namespace wolf
+} // namespace wolf
diff --git a/include/core/ceres_wrapper/iteration_update_callback.h b/include/core/ceres_wrapper/iteration_update_callback.h
index dd01fb5bb69966078ebf32d66141766c5430c8bf..4a88d7e7c172b2a2e50b072a09f63aaffbf47b16 100644
--- a/include/core/ceres_wrapper/iteration_update_callback.h
+++ b/include/core/ceres_wrapper/iteration_update_callback.h
@@ -23,48 +23,50 @@
#include "core/problem/problem.h"
#include "ceres/ceres.h"
-namespace wolf {
-
+namespace wolf
+{
class IterationUpdateCallback : public ceres::IterationCallback
{
- public:
- explicit IterationUpdateCallback(ProblemPtr _problem, const int& min_num_iterations, bool verbose = false)
- : problem_(_problem)
- , min_num_iterations_(min_num_iterations)
- , initial_cost_(0)
- , verbose_(verbose)
- {}
+ public:
+ explicit IterationUpdateCallback(ProblemPtr _problem, const int& min_num_iterations, bool verbose = false)
+ : problem_(_problem), min_num_iterations_(min_num_iterations), initial_cost_(0), verbose_(verbose)
+ {
+ }
- ~IterationUpdateCallback() {}
+ ~IterationUpdateCallback() {}
- ceres::CallbackReturnType operator()(const ceres::IterationSummary& summary) override
- {
- if (summary.iteration == 0)
- initial_cost_ = summary.cost;
+ ceres::CallbackReturnType operator()(const ceres::IterationSummary& summary) override
+ {
+ if (summary.iteration == 0)
+ initial_cost_ = summary.cost;
- else if (summary.iteration >= min_num_iterations_ and
- (problem_->getStateBlockNotificationMapSize() != 0 or
- problem_->getFactorNotificationMapSize() != 0))
+ else if (summary.iteration >= min_num_iterations_ and
+ (problem_->getStateBlockNotificationMapSize() != 0 or problem_->getFactorNotificationMapSize() != 0))
+ {
+ if (summary.cost >= initial_cost_)
+ {
+ WOLF_INFO_COND(
+ verbose_,
+ "Stopping solver to update the problem! ABORTING since cost didn't decrease. Iteration ",
+ summary.iteration);
+ return ceres::SOLVER_ABORT;
+ }
+ else
{
- if (summary.cost >= initial_cost_)
- {
- WOLF_INFO_COND(verbose_, "Stopping solver to update the problem! ABORTING since cost didn't decrease. Iteration ", summary.iteration);
- return ceres::SOLVER_ABORT;
- }
- else
- {
- WOLF_INFO_COND(verbose_, "Stopping solver to update the problem! SUCCESS since cost decreased. Iteration ", summary.iteration);
- return ceres::SOLVER_TERMINATE_SUCCESSFULLY;
- }
+ WOLF_INFO_COND(verbose_,
+ "Stopping solver to update the problem! SUCCESS since cost decreased. Iteration ",
+ summary.iteration);
+ return ceres::SOLVER_TERMINATE_SUCCESSFULLY;
}
- return ceres::SOLVER_CONTINUE;
}
+ return ceres::SOLVER_CONTINUE;
+ }
- private:
- ProblemPtr problem_;
- int min_num_iterations_;
- double initial_cost_;
- bool verbose_;
+ private:
+ ProblemPtr problem_;
+ int min_num_iterations_;
+ double initial_cost_;
+ bool verbose_;
};
-}
+} // namespace wolf
diff --git a/include/core/ceres_wrapper/local_parametrization_wrapper.h b/include/core/ceres_wrapper/local_parametrization_wrapper.h
index bce52b1fade779278c7fbe04c55f00fd70abd282..03cd030e8d070d11295cffe79e4ca154f2c009b1 100644
--- a/include/core/ceres_wrapper/local_parametrization_wrapper.h
+++ b/include/core/ceres_wrapper/local_parametrization_wrapper.h
@@ -21,48 +21,48 @@
#pragma once
// Fwd refs
-namespace wolf{
+namespace wolf
+{
class LocalParametrizationBase;
}
-//Ceres includes
+// Ceres includes
#include "core/common/wolf.h"
#include "ceres/ceres.h"
-namespace wolf {
-
+namespace wolf
+{
class LocalParametrizationWrapper : public ceres::LocalParameterization
{
- private:
- LocalParametrizationBasePtr local_parametrization_ptr_;
+ private:
+ LocalParametrizationBasePtr local_parametrization_ptr_;
- public:
+ public:
+ LocalParametrizationWrapper(LocalParametrizationBasePtr _local_parametrization_ptr);
- LocalParametrizationWrapper(LocalParametrizationBasePtr _local_parametrization_ptr);
+ ~LocalParametrizationWrapper() override = default;
- ~LocalParametrizationWrapper() override = default;
+ bool Plus(const double* x_raw, const double* delta_raw, double* x_plus_delta_raw) const override;
- bool Plus(const double* x_raw, const double* delta_raw, double* x_plus_delta_raw) const override;
+ bool ComputeJacobian(const double* x, double* jacobian) const override;
- bool ComputeJacobian(const double* x, double* jacobian) const override;
+ int GlobalSize() const override;
- int GlobalSize() const override;
+ int LocalSize() const override;
- int LocalSize() const override;
-
- LocalParametrizationBasePtr getLocalParametrization() const;
+ LocalParametrizationBasePtr getLocalParametrization() const;
};
using LocalParametrizationWrapperPtr = std::shared_ptr<LocalParametrizationWrapper>;
-} // namespace wolf
+} // namespace wolf
#include "core/state_block/local_parametrization_base.h"
-namespace wolf {
-
-inline LocalParametrizationWrapper::LocalParametrizationWrapper(LocalParametrizationBasePtr _local_parametrization_ptr) :
- local_parametrization_ptr_(_local_parametrization_ptr)
+namespace wolf
+{
+inline LocalParametrizationWrapper::LocalParametrizationWrapper(LocalParametrizationBasePtr _local_parametrization_ptr)
+ : local_parametrization_ptr_(_local_parametrization_ptr)
{
}
@@ -81,4 +81,4 @@ inline LocalParametrizationBasePtr LocalParametrizationWrapper::getLocalParametr
return local_parametrization_ptr_;
}
-} // namespace wolf
+} // namespace wolf
diff --git a/include/core/ceres_wrapper/qr_manager.h b/include/core/ceres_wrapper/qr_manager.h
index 7b961de2b9794de7941155f44e1ebde52602f632..6087e4bf218d7753ffcf0e7de8798852aac220ab 100644
--- a/include/core/ceres_wrapper/qr_manager.h
+++ b/include/core/ceres_wrapper/qr_manager.h
@@ -25,48 +25,45 @@
namespace wolf
{
-
class QRManager : public SolverManager
{
- protected:
- Eigen::SparseQR<Eigen::SparseMatrixd, Eigen::COLAMDOrdering<int>> solver_;
- Eigen::SparseQR<Eigen::SparseMatrixd, Eigen::NaturalOrdering<int>> covariance_solver_;
- Eigen::SparseMatrixd A_;
- Eigen::VectorXd b_;
- std::map<StateBlockPtr, unsigned int> sb_2_col_;
- std::map<FactorBasePtr, unsigned int> fac_2_row_;
- bool any_state_block_removed_;
- unsigned int new_state_blocks_;
- unsigned int N_batch_;
- bool pending_changes_;
-
- public:
-
- QRManager(ProblemPtr _wolf_problem, const unsigned int& _N_batch);
+ protected:
+ Eigen::SparseQR<Eigen::SparseMatrixd, Eigen::COLAMDOrdering<int>> solver_;
+ Eigen::SparseQR<Eigen::SparseMatrixd, Eigen::NaturalOrdering<int>> covariance_solver_;
+ Eigen::SparseMatrixd A_;
+ Eigen::VectorXd b_;
+ std::map<StateBlockPtr, unsigned int> sb_2_col_;
+ std::map<FactorBasePtr, unsigned int> fac_2_row_;
+ bool any_state_block_removed_;
+ unsigned int new_state_blocks_;
+ unsigned int N_batch_;
+ bool pending_changes_;
- virtual ~QRManager();
+ public:
+ QRManager(ProblemPtr _wolf_problem, const unsigned int& _N_batch);
- virtual std::string solve(const unsigned int& _report_level);
+ virtual ~QRManager();
- virtual void computeCovariances(CovarianceBlocksToBeComputed _blocks = ROBOT_LANDMARKS);
+ virtual std::string solve(const unsigned int& _report_level);
- virtual void computeCovariances(const std::vector<StateBlockPtr>& _sb_list);
+ virtual void computeCovariances(CovarianceBlocksToBeComputed _blocks = ROBOT_LANDMARKS);
- private:
+ virtual void computeCovariances(const std::vector<StateBlockPtr>& _sb_list);
- bool computeDecomposition();
+ private:
+ bool computeDecomposition();
- virtual void addFactor(FactorBasePtr _fac_ptr);
+ virtual void addFactor(FactorBasePtr _fac_ptr);
- virtual void removeFactor(FactorBasePtr _fac_ptr);
+ virtual void removeFactor(FactorBasePtr _fac_ptr);
- virtual void addStateBlock(StateBlockPtr _st_ptr);
+ virtual void addStateBlock(StateBlockPtr _st_ptr);
- virtual void removeStateBlock(StateBlockPtr _st_ptr);
+ virtual void removeStateBlock(StateBlockPtr _st_ptr);
- virtual void updateStateBlockStatus(StateBlockPtr _st_ptr);
+ virtual void updateStateBlockStatus(StateBlockPtr _st_ptr);
- void relinearizeFactor(FactorBasePtr _fac_ptr);
+ void relinearizeFactor(FactorBasePtr _fac_ptr);
};
} /* namespace wolf */
diff --git a/include/core/ceres_wrapper/solver_ceres.h b/include/core/ceres_wrapper/solver_ceres.h
index 7d3047dd84810175b6c9a3ba2ea3c98be47fabd8..54618a68261c0512d0f300fffd5553f49d1d1d78 100644
--- a/include/core/ceres_wrapper/solver_ceres.h
+++ b/include/core/ceres_wrapper/solver_ceres.h
@@ -20,194 +20,107 @@
#pragma once
-//Ceres includes
+// Ceres includes
#include "ceres/jet.h"
#include "ceres/ceres.h"
#include "glog/logging.h"
-//wolf includes
+// wolf includes
#include "core/solver/solver_manager.h"
#include "yaml-cpp/yaml.h"
-namespace ceres {
-typedef std::shared_ptr<CostFunction> CostFunctionPtr;
+namespace ceres
+{
+typedef std::shared_ptr<CostFunction> CostFunctionPtr;
}
-namespace wolf {
-
+namespace wolf
+{
WOLF_PTR_TYPEDEFS(SolverCeres);
WOLF_PTR_TYPEDEFS(LocalParametrizationWrapper);
-WOLF_STRUCT_PTR_TYPEDEFS(ParamsCeres);
-
-struct ParamsCeres : public ParamsSolver
-{
- bool interrupt_on_problem_change = false;
- int min_num_iterations = 1;
- ceres::Solver::Options solver_options;
- ceres::Problem::Options problem_options;
- ceres::Covariance::Options covariance_options;
-
- ParamsCeres() :
- ParamsSolver()
- {
- loadHardcodedValues();
- }
-
- ParamsCeres(const YAML::Node& _input_node) :
- ParamsSolver(_input_node)
- {
- loadHardcodedValues();
-
- // interrupt solver whenever the problem is updated (via ceres::iterationCallback)
- interrupt_on_problem_change = _input_node["interrupt_on_problem_change"].as<bool>();
- if (interrupt_on_problem_change)
- min_num_iterations = _input_node["min_num_iterations"].as<int>();
-
- // CERES SOLVER OPTIONS
- solver_options.max_num_iterations = _input_node["max_num_iterations"].as<int>();
- solver_options.function_tolerance = _input_node["function_tolerance"].as<double>();
- solver_options.gradient_tolerance = _input_node["gradient_tolerance"].as<double>();
- solver_options.num_threads = _input_node["n_threads"].as<int>();
- covariance_options.num_threads = solver_options.num_threads;
-
- // minimizer type
- std::string minimizer = _input_node["minimizer"].as<std::string>();
- if (minimizer == "LEVENBERG_MARQUARDT" or minimizer == "levenberg_marquardt")
- {
- solver_options.minimizer_type = ceres::TRUST_REGION;
- solver_options.trust_region_strategy_type = ceres::LEVENBERG_MARQUARDT;
- }
- else if (minimizer == "DOGLEG" or minimizer == "dogleg")
- {
- solver_options.minimizer_type = ceres::TRUST_REGION;
- solver_options.trust_region_strategy_type = ceres::DOGLEG;
- }
- else if (minimizer == "LBFGS" or minimizer == "lbfgs")
- {
- solver_options.minimizer_type = ceres::LINE_SEARCH;
- solver_options.line_search_direction_type = ceres::LBFGS;
- }
- else if (minimizer == "BFGS" or minimizer == "bfgs")
- {
- solver_options.minimizer_type = ceres::LINE_SEARCH;
- solver_options.line_search_direction_type = ceres::BFGS;
- }
- else
- {
- throw std::runtime_error("ParamsCeres: Wrong parameter 'minimizer'. Should be 'LEVENBERG_MARQUARDT', 'DOGLEG', 'LBFGS' or 'BFGS' (upper or lowercase)");
- }
-
- // specific options for TRUST REGION
- if (solver_options.minimizer_type == ceres::TRUST_REGION)
- {
- solver_options.use_nonmonotonic_steps = _input_node["use_nonmonotonic_steps"].as<bool>();
- if (solver_options.use_nonmonotonic_steps)
- solver_options.max_consecutive_nonmonotonic_steps = _input_node["max_consecutive_nonmonotonic_steps"].as<int>();
- }
- }
-
- void loadHardcodedValues()
- {
- solver_options = ceres::Solver::Options();
- problem_options = ceres::Problem::Options();
- covariance_options = ceres::Covariance::Options();
- problem_options.cost_function_ownership = ceres::DO_NOT_TAKE_OWNERSHIP;
- problem_options.loss_function_ownership = ceres::TAKE_OWNERSHIP;
- problem_options.local_parameterization_ownership = ceres::DO_NOT_TAKE_OWNERSHIP;
-
- covariance_options.apply_loss_function = false;
- }
-
- ~ParamsCeres() override = default;
-};
class SolverCeres : public SolverManager
{
- protected:
-
- std::map<FactorBasePtr, ceres::ResidualBlockId> fac_2_residual_idx_;
- std::map<FactorBasePtr, ceres::CostFunctionPtr> fac_2_costfunction_;
-
- // this map is only for handling automatic destruction of localParametrizationWrapper objects
- std::map<StateBlockPtr, LocalParametrizationWrapperPtr> state_blocks_local_param_;
-
- ceres::Solver::Summary summary_;
- std::unique_ptr<ceres::Problem> ceres_problem_;
- std::unique_ptr<ceres::Covariance> covariance_;
-
- ParamsCeresPtr params_ceres_;
+ protected:
+ std::map<FactorBasePtr, ceres::ResidualBlockId> fac_2_residual_idx_;
+ std::map<FactorBasePtr, ceres::CostFunctionPtr> fac_2_costfunction_;
- // profiling
- unsigned int n_iter_acc_, n_iter_max_;
- unsigned int n_convergence_, n_interrupted_, n_no_convergence_;
+ // this map is only for handling automatic destruction of localParametrizationWrapper objects
+ std::map<StateBlockPtr, LocalParametrizationWrapperPtr> state_blocks_local_param_;
- public:
+ ceres::Solver::Summary summary_;
+ std::unique_ptr<ceres::Problem> ceres_problem_;
+ std::unique_ptr<ceres::Covariance> covariance_;
- SolverCeres(const ProblemPtr& _wolf_problem);
+ // profiling
+ unsigned int n_iter_acc_, n_iter_max_;
+ unsigned int n_convergence_, n_interrupted_, n_no_convergence_;
- SolverCeres(const ProblemPtr& _wolf_problem,
- const ParamsCeresPtr& _params);
+ public:
+ SolverCeres(const ProblemPtr& _wolf_problem, const YAML::Node& _params);
- WOLF_SOLVER_CREATE(SolverCeres, ParamsCeres);
+ WOLF_SOLVER_CREATE(SolverCeres);
- ~SolverCeres() override;
+ ~SolverCeres() override;
- ceres::Solver::Summary getSummary();
+ ceres::Solver::Summary getSummary();
- std::unique_ptr<ceres::Problem>& getCeresProblem();
+ std::unique_ptr<ceres::Problem>& getCeresProblem();
- bool computeCovariancesDerived(CovarianceBlocksToBeComputed _blocks
- = CovarianceBlocksToBeComputed::ROBOT_LANDMARKS) override;
+ bool computeCovariancesDerived(
+ CovarianceBlocksToBeComputed _blocks = CovarianceBlocksToBeComputed::ROBOT_LANDMARKS) override;
- bool computeCovariancesDerived(const std::vector<StateBlockPtr>& st_list) override;
+ bool computeCovariancesDerived(const std::vector<StateBlockPtr>& st_list) override;
- bool hasConverged() override;
- bool wasStopped() override;
- unsigned int iterations() override;
- double initialCost() override;
- double finalCost() override;
- double totalTime() override;
+ bool hasConverged() override;
+ bool wasStopped() override;
+ unsigned int iterations() override;
+ double initialCost() override;
+ double finalCost() override;
+ double totalTime() override;
- ceres::Solver::Options& getSolverOptions();
+ ceres::Solver::Options& getSolverOptions();
- const Eigen::SparseMatrixd computeHessian() const;
+ const Eigen::SparseMatrixd computeHessian() const;
- virtual int numStateBlocksDerived() const override;
+ virtual int numStateBlocksDerived() const override;
- virtual int numFactorsDerived() const override;
+ virtual int numFactorsDerived() const override;
- protected:
+ protected:
+ bool checkDerived(std::string prefix = "") const override;
- bool checkDerived(std::string prefix="") const override;
+ std::string solveDerived(const ReportVerbosity report_level) override;
- std::string solveDerived(const ReportVerbosity report_level) override;
+ void addFactorDerived(const FactorBasePtr& fac_ptr) override;
- void addFactorDerived(const FactorBasePtr& fac_ptr) override;
+ void removeFactorDerived(const FactorBasePtr& fac_ptr) override;
- void removeFactorDerived(const FactorBasePtr& fac_ptr) override;
+ void addStateBlockDerived(const StateBlockPtr& state_ptr) override;
- void addStateBlockDerived(const StateBlockPtr& state_ptr) override;
+ void removeStateBlockDerived(const StateBlockPtr& state_ptr) override;
- void removeStateBlockDerived(const StateBlockPtr& state_ptr) override;
+ void updateStateBlockStatusDerived(const StateBlockPtr& state_ptr) override;
- void updateStateBlockStatusDerived(const StateBlockPtr& state_ptr) override;
+ void updateStateBlockLocalParametrizationDerived(const StateBlockPtr& state_ptr) override;
- void updateStateBlockLocalParametrizationDerived(const StateBlockPtr& state_ptr) override;
+ ceres::CostFunctionPtr createCostFunction(const FactorBasePtr& _fac_ptr);
- ceres::CostFunctionPtr createCostFunction(const FactorBasePtr& _fac_ptr);
+ bool isFactorRegisteredDerived(const FactorBasePtr& fac_ptr) const override;
- bool isFactorRegisteredDerived(const FactorBasePtr& fac_ptr) const override;
+ bool isStateBlockRegisteredDerived(const StateBlockPtr& state_ptr) const override;
- bool isStateBlockRegisteredDerived(const StateBlockPtr& state_ptr) const override;
+ bool isStateBlockFixedDerived(const StateBlockPtr& st) override;
- bool isStateBlockFixedDerived(const StateBlockPtr& st) override;
+ bool hasThisLocalParametrizationDerived(const StateBlockPtr& st,
+ const LocalParametrizationBasePtr& local_param) override;
- bool hasThisLocalParametrizationDerived(const StateBlockPtr& st,
- const LocalParametrizationBasePtr& local_param) override;
+ bool hasLocalParametrizationDerived(const StateBlockPtr& st) const override;
- bool hasLocalParametrizationDerived(const StateBlockPtr& st) const override;
+ void printProfilingDerived(std::ostream& _stream) const override;
- void printProfilingDerived(std::ostream& _stream) const override;
+ // PARAMS
+ ceres::Solver::Options solver_options_;
};
inline ceres::Solver::Summary SolverCeres::getSummary()
@@ -222,26 +135,23 @@ inline std::unique_ptr<ceres::Problem>& SolverCeres::getCeresProblem()
inline ceres::Solver::Options& SolverCeres::getSolverOptions()
{
- return params_ceres_->solver_options;
+ return solver_options_;
}
inline bool SolverCeres::isFactorRegisteredDerived(const FactorBasePtr& fac_ptr) const
{
- return fac_2_residual_idx_.count(fac_ptr) == 1 and
- fac_2_costfunction_.count(fac_ptr) == 1;
+ return fac_2_residual_idx_.count(fac_ptr) == 1 and fac_2_costfunction_.count(fac_ptr) == 1;
}
inline bool SolverCeres::isStateBlockRegisteredDerived(const StateBlockPtr& state_ptr) const
{
- if (state_blocks_.count(state_ptr) == 0)
- return false;
+ if (state_blocks_.count(state_ptr) == 0) return false;
return ceres_problem_->HasParameterBlock(getAssociatedMemBlockPtr(state_ptr));
}
inline bool SolverCeres::isStateBlockFixedDerived(const StateBlockPtr& st)
{
- if (state_blocks_.count(st) == 0)
- return false;
+ if (state_blocks_.count(st) == 0) return false;
return ceres_problem_->IsParameterBlockConstant(getAssociatedMemBlockPtr(st));
};
@@ -260,4 +170,4 @@ inline int SolverCeres::numFactorsDerived() const
return ceres_problem_->NumResidualBlocks();
};
-} // namespace wolf
+} // namespace wolf
diff --git a/include/core/ceres_wrapper/sparse_utils.h b/include/core/ceres_wrapper/sparse_utils.h
index aa8d3ec2b2877ce127601e8d0e77dcfed602ddef..173c2b88958112023c494ac63b6d4a321974b554 100644
--- a/include/core/ceres_wrapper/sparse_utils.h
+++ b/include/core/ceres_wrapper/sparse_utils.h
@@ -25,62 +25,80 @@
namespace wolf
{
-
-void eraseBlockRow(Eigen::SparseMatrix<double, Eigen::RowMajor>& A, const unsigned int& _row, const unsigned int& _n_rows)
+void eraseBlockRow(Eigen::SparseMatrix<double, Eigen::RowMajor>& A,
+ const unsigned int& _row,
+ const unsigned int& _n_rows)
{
- A.middleRows(_row,_n_rows) = Eigen::SparseMatrixd(_n_rows,A.cols());
+ A.middleRows(_row, _n_rows) = Eigen::SparseMatrixd(_n_rows, A.cols());
A.makeCompressed();
}
-void eraseBlockRow(Eigen::SparseMatrix<double, Eigen::ColMajor>& A, const unsigned int& _row, const unsigned int& _n_rows)
+void eraseBlockRow(Eigen::SparseMatrix<double, Eigen::ColMajor>& A,
+ const unsigned int& _row,
+ const unsigned int& _n_rows)
{
A.prune([&](int i, int, double) { return i >= _row && i < _row + _n_rows; });
A.makeCompressed();
}
-void eraseBlockCol(Eigen::SparseMatrix<double, Eigen::ColMajor>& A, const unsigned int& _col, const unsigned int& _n_cols)
+void eraseBlockCol(Eigen::SparseMatrix<double, Eigen::ColMajor>& A,
+ const unsigned int& _col,
+ const unsigned int& _n_cols)
{
- A.middleCols(_col,_n_cols) = Eigen::SparseMatrixd(A.rows(),_n_cols);
+ A.middleCols(_col, _n_cols) = Eigen::SparseMatrixd(A.rows(), _n_cols);
A.makeCompressed();
}
-void eraseBlockCol(Eigen::SparseMatrix<double, Eigen::RowMajor>& A, const unsigned int& _col, const unsigned int& _n_cols)
+void eraseBlockCol(Eigen::SparseMatrix<double, Eigen::RowMajor>& A,
+ const unsigned int& _col,
+ const unsigned int& _n_cols)
{
A.prune([&](int, int j, double) { return j >= _col && j < _col + _n_cols; });
A.makeCompressed();
}
-template<int _Options, typename _StorageIndex>
-void assignSparseBlock(const Eigen::MatrixXd& ins, Eigen::SparseMatrix<double,_Options,_StorageIndex>& original, const unsigned int& row, const unsigned int& col)
+template <int _Options, typename _StorageIndex>
+void assignSparseBlock(const Eigen::MatrixXd& ins,
+ Eigen::SparseMatrix<double, _Options, _StorageIndex>& original,
+ const unsigned int& row,
+ const unsigned int& col)
{
for (auto ins_row = 0; ins_row < ins.rows(); ins_row++)
for (auto ins_col = 0; ins_col < ins.cols(); ins_col++)
- original.coeffRef(row+ins_row, col+ins_col) = ins(ins_row,ins_col);
+ original.coeffRef(row + ins_row, col + ins_col) = ins(ins_row, ins_col);
original.makeCompressed();
}
-template<int _Options, typename _StorageIndex>
-void addSparseBlock(const Eigen::MatrixXd& ins, Eigen::SparseMatrix<double,_Options,_StorageIndex>& original, const unsigned int& row, const unsigned int& col)
+template <int _Options, typename _StorageIndex>
+void addSparseBlock(const Eigen::MatrixXd& ins,
+ Eigen::SparseMatrix<double, _Options, _StorageIndex>& original,
+ const unsigned int& row,
+ const unsigned int& col)
{
for (auto ins_row = 0; ins_row < ins.rows(); ins_row++)
for (auto ins_col = 0; ins_col < ins.cols(); ins_col++)
- original.coeffRef(row+ins_row, col+ins_col) += ins(ins_row,ins_col);
+ original.coeffRef(row + ins_row, col + ins_col) += ins(ins_row, ins_col);
original.makeCompressed();
}
-template<int _Options, typename _StorageIndex>
-void insertSparseBlock(const Eigen::MatrixXd& ins, Eigen::SparseMatrix<double,_Options,_StorageIndex>& original, const unsigned int& row, const unsigned int& col)
+template <int _Options, typename _StorageIndex>
+void insertSparseBlock(const Eigen::MatrixXd& ins,
+ Eigen::SparseMatrix<double, _Options, _StorageIndex>& original,
+ const unsigned int& row,
+ const unsigned int& col)
{
for (auto ins_row = 0; ins_row < ins.rows(); ins_row++)
for (auto ins_col = 0; ins_col < ins.cols(); ins_col++)
- original.insert(row+ins_row, col+ins_col) = ins(ins_row,ins_col);
+ original.insert(row + ins_row, col + ins_col) = ins(ins_row, ins_col);
original.makeCompressed();
}
-void assignBlockRow(Eigen::SparseMatrix<double, Eigen::RowMajor>& A, const Eigen::SparseMatrix<double, Eigen::RowMajor>& ins, const unsigned int& _row)
+void assignBlockRow(Eigen::SparseMatrix<double, Eigen::RowMajor>& A,
+ const Eigen::SparseMatrix<double, Eigen::RowMajor>& ins,
+ const unsigned int& _row)
{
assert(A.rows() >= _row + ins.rows() && A.cols() == ins.cols());
A.middleRows(_row, ins.rows()) = ins;
@@ -89,10 +107,10 @@ void assignBlockRow(Eigen::SparseMatrix<double, Eigen::RowMajor>& A, const Eigen
Eigen::SparseMatrixd createBlockDiagonal(const std::vector<Eigen::MatrixXd>& _diag_blocs)
{
- unsigned int dim = _diag_blocs.front().rows();
+ unsigned int dim = _diag_blocs.front().rows();
unsigned int size = dim * _diag_blocs.size();
- Eigen::SparseMatrixd M(size,size);
+ Eigen::SparseMatrixd M(size, size);
unsigned int pos = 0;
for (const Eigen::MatrixXd& omega_k : _diag_blocs)
@@ -106,13 +124,14 @@ Eigen::SparseMatrixd createBlockDiagonal(const std::vector<Eigen::MatrixXd>& _di
return M;
}
-template<int _Options, typename _StorageIndex>
-void getDiagonalBlocks(const Eigen::SparseMatrix<double,_Options,_StorageIndex>& _M, std::vector<Eigen::MatrixXd>& diag_, const unsigned int& dim)
+template <int _Options, typename _StorageIndex>
+void getDiagonalBlocks(const Eigen::SparseMatrix<double, _Options, _StorageIndex>& _M,
+ std::vector<Eigen::MatrixXd>& diag_,
+ const unsigned int& dim)
{
assert(_M.rows() % dim == 0 && "number of rows must be multiple of dimension");
diag_.clear();
- for (auto i = 0; i < _M.rows(); i += dim)
- diag_.push_back(Eigen::MatrixXd(_M.block(i,i,dim,dim)));
+ for (auto i = 0; i < _M.rows(); i += dim) diag_.push_back(Eigen::MatrixXd(_M.block(i, i, dim, dim)));
}
-} // namespace wolf
+} // namespace wolf
diff --git a/include/core/ceres_wrapper/wolf_jet.h b/include/core/ceres_wrapper/wolf_jet.h
index a4a5888dd49ad928e06b00e405c1e5b7fdd0f67c..962be7f6e8602f781dbe95c5af8e2d76b875bbab 100644
--- a/include/core/ceres_wrapper/wolf_jet.h
+++ b/include/core/ceres_wrapper/wolf_jet.h
@@ -25,8 +25,8 @@
#include "core/math/rotations.h"
-namespace ceres{
-
+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)
@@ -39,11 +39,11 @@ namespace ceres{
using std::remainder;
-template<int N>
+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);
+ res.a = remainder(_x.a, _y);
return res;
}
-}
+} // namespace ceres
diff --git a/include/core/common/factory.h b/include/core/common/factory.h
index d7a874faf080aec8959040255798ff7987b019ed..f91e26ba4390ec614cc3d52668fd5af42e7cdf59 100644
--- a/include/core/common/factory.h
+++ b/include/core/common/factory.h
@@ -31,7 +31,6 @@
namespace wolf
{
-
/** \brief Singleton template factory
*
* This template class implements a generic factory as a singleton.
@@ -50,20 +49,20 @@ namespace wolf
*
* For example, you may use as __TypeBase__ the following types:
* - LandmarkBase: the Factory creates landmarks deriving from LandmarkBase and returns base pointers
- * ````LandmarkBasePtr```` to them
- * - ParamsSensorBase: the Factory creates intrinsic parameters deriving from ParamsSensorBase and returns base
- * pointers ````ParamsSensorBasePtr```` to them
+ * ````LandmarkBasePtr```` to them
+ * - SensorBase: the Factory creates sensor deriving from SensorBase and returns base
+ * pointers ````SensorBasePtr```` to them
* - XxxBase: the Factory creates objects deriving from XxxBase and returns pointers ````XxxBasePtr```` to them.
*
* - The class is variadic-templatized on the types of the input parameter of the creator, __TypeInput__:
- * - ````std::string```` is used when the input parameter is a file name from which to read data
- * (typically a YAML file).
- * - ````std::string, YAML::Node``` is used when nodes are build from parameters in a YAML node.
+ * - ````std::string```` is used when the input parameter is a file name from which to read data
+ * (typically a YAML file).
+ * - ````std::string, YAML::Node``` is used when nodes are build from parameters in a YAML node.
* - Any other variadic list of inputs is possible.
*
* - Examples of specific factories existing in Wolf are in ````factory_landmark.h````, ````factory_map.h````,
* ````factory_processor.h````, ````factory_sensor.h````, ````factory_solver.h````, ````factory_state_block.h````,
- * ````factory_tree_manager.h````
+ * ````factory_tree_manager.h````
*
* ### Operation of the factory
*
@@ -243,11 +242,11 @@ namespace wolf
*
* ### More information
* - FactorySensorNode: typedef of this template to create Sensor classes deriving from SensorBase directly
- * from YAML files.
+ * from YAML files.
* - FactoryProcessor: typedef of this template to create processor classes deriving from
- * ProcessorBase directly from YAML files.
+ * ProcessorBase directly from YAML files.
* - FactoryLandmark: typedef of this template to create landmarks deriving from LandmarkBase directly from YAML
- * nodes.
+ * nodes.
* - Problem::loadMap() : to load a maps directly from YAML files.
* - You can also check the code in the example file ````src/examples/test_map_yaml.cpp````.
*
@@ -288,7 +287,7 @@ class Factory
static Factory& get();
public:
- Factory(const Factory&) = delete;
+ Factory(const Factory&) = delete;
void operator=(Factory const&) = delete;
private:
@@ -391,7 +390,6 @@ inline std::list<std::string> Factory<TypeBase, TypeInput...>::getRegisteredKeys
namespace wolf
{
-
#ifdef __GNUC__
#define WOLF_UNUSED __attribute__((used))
#elif defined _MSC_VER
diff --git a/include/core/common/node_base.h b/include/core/common/node_base.h
index 4873417445ee629ab006dae22727223c367f52dc..d47d4b939f09fa7863aedbeac78be8d3c6bc8943 100644
--- a/include/core/common/node_base.h
+++ b/include/core/common/node_base.h
@@ -24,8 +24,8 @@
#include "core/common/wolf.h"
#include "core/utils/check_log.h"
-namespace wolf {
-
+namespace wolf
+{
/** \brief Base class for Nodes
*
* Base class for all Nodes in the Wolf tree. Each node has
@@ -46,7 +46,8 @@ namespace wolf {
* - "MAP"
* - "LANDMARK"
*
- * - A unique type, which is a subcategory of the above. The list here cannot be exhaustive, but a few examples follow:
+ * - A unique type, which is a subcategory of the above. The list here cannot be exhaustive, but a few examples
+ * follow:
* - "SensorCamera"
* - "SensorLaser2d"
* - "LandmarkPoint3d"
@@ -73,54 +74,56 @@ namespace wolf {
class NodeBase
{
- friend Problem;
- private:
- static unsigned int node_id_count_; ///< Object counter (acts as simple ID factory)
+ friend Problem;
+
+ private:
+ static unsigned int node_id_count_; ///< Object counter (acts as simple ID factory)
- struct Serializer;
+ struct Serializer;
- protected:
- ProblemWPtr problem_ptr_;
+ protected:
+ ProblemWPtr problem_ptr_;
- unsigned int node_id_; ///< Node id. It is unique over the whole Wolf Tree
- std::string node_category_; ///< Text label identifying the category of node ("SENSOR", "FEATURE", etc)
- std::string node_type_; ///< Text label identifying the type or subcategory of node ("Pin Hole", "Point 2d", etc)
- std::string node_name_; ///< Text label identifying each specific object ("left camera", "LIDAR 1", "PointGrey", "Andrew", etc)
+ unsigned int node_id_; ///< Node id. It is unique over the whole Wolf Tree
+ std::string node_category_; ///< Text label identifying the category of node ("SENSOR", "FEATURE", etc)
+ std::string node_type_; ///< Text label identifying the type or subcategory of node ("Pin Hole", "Point 2d", etc)
+ std::string node_name_; ///< Text label identifying each specific object ("left camera", "LIDAR 1", "PointGrey",
+ ///< "Andrew", etc)
- bool is_removing_; ///< A flag for safely removing nodes from the Wolf tree. See remove().
+ bool is_removing_; ///< A flag for safely removing nodes from the Wolf tree. See remove().
- virtual void setProblem(ProblemPtr _prob_ptr);
- public:
+ virtual void setProblem(ProblemPtr _prob_ptr);
- NodeBase(const std::string& _category, const std::string& _type = "Undefined", const std::string& _name = "");
- virtual ~NodeBase() = default;
+ public:
+ NodeBase(const std::string& _category, const std::string& _type = "Undefined", const std::string& _name = "");
+ virtual ~NodeBase() = default;
- unsigned int nodeId() const;
- std::string getCategory() const;
- std::string getType() const;
- std::string getName() const;
- bool isRemoving() const;
- virtual bool hasChildren() const = 0;
+ unsigned int nodeId() const;
+ std::string getCategory() const;
+ std::string getType() const;
+ std::string getName() const;
+ bool isRemoving() const;
+ virtual bool hasChildren() const = 0;
- void setType(const std::string& _type);
- void setName(const std::string& _name);
- ProblemConstPtr getProblem() const;
- ProblemPtr getProblem();
+ void setType(const std::string& _type);
+ void setName(const std::string& _name);
+ ProblemConstPtr getProblem() const;
+ ProblemPtr getProblem();
};
-} // namespace wolf
+} // namespace wolf
#include <iostream>
-namespace wolf{
-
-inline NodeBase::NodeBase(const std::string& _category, const std::string& _type, const std::string& _name) :
- problem_ptr_(), // nullptr
- node_id_(++node_id_count_),
- node_category_(_category),
- node_type_(_type),
- node_name_(_name),
- is_removing_(false)
+namespace wolf
+{
+inline NodeBase::NodeBase(const std::string& _category, const std::string& _type, const std::string& _name)
+ : problem_ptr_(), // nullptr
+ node_id_(++node_id_count_),
+ node_category_(_category),
+ node_type_(_type),
+ node_name_(_name),
+ is_removing_(false)
{
//
}
@@ -175,4 +178,4 @@ inline void NodeBase::setProblem(ProblemPtr _prob_ptr)
problem_ptr_ = _prob_ptr;
}
-} // namespace wolf
+} // namespace wolf
diff --git a/include/core/common/node_state_blocks.h b/include/core/common/node_state_blocks.h
index df220589e15e4f519c37e36798ad5b9a8332080d..ea5d6ed8d0100f03328c88f9d19bde1921baf9d9 100644
--- a/include/core/common/node_state_blocks.h
+++ b/include/core/common/node_state_blocks.h
@@ -100,12 +100,12 @@ class NodeStateBlocks : public NodeBase, public std::enable_shared_from_this<Nod
* \param _size state segment size (-1: whole state) (not used in quaternions)
*
**/
- void emplaceFactorPrior(const char& _key,
- const Eigen::VectorXd& _x,
- const Eigen::MatrixXd& _cov,
- unsigned int _start_idx = 0);
+ void emplaceFactorStateBlock(const char& _key,
+ const Eigen::VectorXd& _x,
+ const Eigen::MatrixXd& _cov,
+ unsigned int _start_idx = 0);
- void applyPriors(const SpecStateComposite& _specs);
+ void emplacePriors(const SpecStateComposite& _specs);
FactorBaseConstPtrSet getFactoredBySet() const;
FactorBasePtrSet getFactoredBySet();
@@ -160,9 +160,7 @@ class NodeStateBlocks : public NodeBase, public std::enable_shared_from_this<Nod
bool _state_blocks,
std::ostream& _stream,
std::string _tabs) const;
- CheckLog checkStatesAndFactoredBy(bool _verbose,
- std::ostream& _stream,
- std::string _tabs) const;
+ CheckLog checkStatesAndFactoredBy(bool _verbose, std::ostream& _stream, std::string _tabs) const;
private:
Composite<StateBlockPtr> state_block_composite_;
@@ -180,7 +178,6 @@ class NodeStateBlocks : public NodeBase, public std::enable_shared_from_this<Nod
namespace wolf
{
-
inline NodeStateBlocks::~NodeStateBlocks()
{
//
diff --git a/include/core/common/time_stamp.h b/include/core/common/time_stamp.h
index 36aa22711dab73deb1fd9962ceed68b2acd46e1b..f0f91e051fc9cea2c0140d76e58735ac272ec16e 100644
--- a/include/core/common/time_stamp.h
+++ b/include/core/common/time_stamp.h
@@ -20,230 +20,229 @@
#pragma once
-//wolf includes
+// wolf includes
#include "core/common/wolf.h"
-//C, std
+// C, std
#include <sys/time.h>
#include <iostream>
#include <iomanip>
static const unsigned int NANOSECS = 1000000000;
-namespace wolf {
-
+namespace wolf
+{
/**
* \brief TimeStamp implements basic functionalities for time stamps
- *
+ *
* TimeStamp implements basic functionalities for time stamps
*/
class TimeStamp
{
- protected:
- unsigned long int time_stamp_nano_; ///< Time stamp. Expressed in nanoseconds from 1th jan 1970.
- bool is_valid_; // time stamp has a valid value
-
- public:
- /** \brief Constructor
- *
- * Constructor without arguments. Sets time stamp to now.
- *
- */
- TimeStamp();
-
- /** \brief Copy constructor
- *
- * Copy constructor
- *
- */
- TimeStamp(const TimeStamp& _ts);
-
- /** \brief Constructor with argument
- *
- * Constructor with arguments
- *
- */
- TimeStamp(const double& _ts);
-
- /** \brief Constructor from sec and nsec
- *
- * Constructor from sec and nsec
- *
- */
- TimeStamp(const unsigned long int& _sec, const unsigned long int& _nsec);
-
- /** \brief Destructor
- *
- * Destructor
- *
- */
- ~TimeStamp();
-
- /** \brief Value of time stamp is valid
- *
- */
- static TimeStamp Invalid ( );
- bool ok ( ) const;
- void setOk ( );
- void setNOk ( );
-
- static TimeStamp Now();
-
- /** \brief Time stamp to now
- */
- void setToNow();
-
- /** \brief Set time stamp
- *
- * Sets time stamp to a given value passed as a timeval struct
- */
- void set(const timeval& ts);
-
- /** \brief Set time stamp
- *
- * Sets time stamp to a given value passed as a two-integer (seconds and nanoseconds)
- *
- */
- void set(const unsigned long int& sec, const unsigned long int& nanosec);
-
- /** \brief Set time stamp
- *
- * Sets time stamp to a given value passed as a scalar_t (seconds)
- *
- */
- void set(const double& ts);
-
- /** \brief Get time stamp
- *
- * Returns time stamp
- *
- */
- double get() const;
-
- /** \brief Get time stamp (only seconds)
- *
- * Returns seconds of time stamp
- *
- */
- unsigned long int getSeconds() const;
-
- /** \brief Get time stamp (only nano seconds)
- *
- * Returns nanoseconds part of time stamp
- *
- */
- unsigned long int getNanoSeconds() const;
-
- /** \brief Assignement operator
- *
- * Assignement operator
- *
- */
- void operator =(const double& ts);
-
- /** \brief Assignement operator
- *
- * Assignement operator given a scalar_t (seconds)
- *
- */
- void operator =(const TimeStamp& ts);
-
- /** \brief comparison operator
- *
- * Comparison operator
- *
- */
- bool operator !=(const TimeStamp& ts) const;
- bool operator ==(const TimeStamp& ts) const;
- bool operator <(const TimeStamp& ts) const;
- bool operator >(const TimeStamp& ts) const;
-
- /** \brief comparison operator
- *
- * Comparison operator
- *
- */
- bool operator <=(const TimeStamp& ts) const;
- bool operator >=(const TimeStamp& ts) const;
-
- /** \brief Add-assign operator given a scalar_t (seconds)
- */
- void operator +=(const double& dt);
-
- /** \brief Add-assign operator given a scalar_t (seconds)
- */
- TimeStamp operator +(const double& dt) const;
-
- /** \brief Substraction-assign operator given a scalar_t (seconds)
- */
- void operator -=(const double& dt);
-
- /** \brief difference operator
- *
- * difference operator that returns a scalar_t (seconds)
- *
- */
- TimeStamp operator -(const double& ts) const;
-
- /** \brief difference operator
- *
- * difference operator that returns a Timestamp (seconds)
- *
- */
- double operator -(const TimeStamp& ts) const;
-
- /** \brief Prints time stamp to a given ostream
- *
- * Prints time stamp to a given ostream
- *
- */
- void print(std::ostream & ost = std::cout) const;
-
- friend std::ostream& operator<<(std::ostream& os, const TimeStamp& _ts);
-
+ protected:
+ unsigned long int time_stamp_nano_; ///< Time stamp. Expressed in nanoseconds from 1th jan 1970.
+ bool is_valid_; // time stamp has a valid value
+
+ public:
+ /** \brief Constructor
+ *
+ * Constructor without arguments. Sets time stamp to now.
+ *
+ */
+ TimeStamp();
+
+ /** \brief Copy constructor
+ *
+ * Copy constructor
+ *
+ */
+ TimeStamp(const TimeStamp& _ts);
+
+ /** \brief Constructor with argument
+ *
+ * Constructor with arguments
+ *
+ */
+ TimeStamp(const double& _ts);
+
+ /** \brief Constructor from sec and nsec
+ *
+ * Constructor from sec and nsec
+ *
+ */
+ TimeStamp(const unsigned long int& _sec, const unsigned long int& _nsec);
+
+ /** \brief Destructor
+ *
+ * Destructor
+ *
+ */
+ ~TimeStamp();
+
+ /** \brief Value of time stamp is valid
+ *
+ */
+ static TimeStamp Invalid();
+ bool ok() const;
+ void setOk();
+ void setNOk();
+
+ static TimeStamp Now();
+
+ /** \brief Time stamp to now
+ */
+ void setToNow();
+
+ /** \brief Set time stamp
+ *
+ * Sets time stamp to a given value passed as a timeval struct
+ */
+ void set(const timeval& ts);
+
+ /** \brief Set time stamp
+ *
+ * Sets time stamp to a given value passed as a two-integer (seconds and nanoseconds)
+ *
+ */
+ void set(const unsigned long int& sec, const unsigned long int& nanosec);
+
+ /** \brief Set time stamp
+ *
+ * Sets time stamp to a given value passed as a scalar_t (seconds)
+ *
+ */
+ void set(const double& ts);
+
+ /** \brief Get time stamp
+ *
+ * Returns time stamp
+ *
+ */
+ double get() const;
+
+ /** \brief Get time stamp (only seconds)
+ *
+ * Returns seconds of time stamp
+ *
+ */
+ unsigned long int getSeconds() const;
+
+ /** \brief Get time stamp (only nano seconds)
+ *
+ * Returns nanoseconds part of time stamp
+ *
+ */
+ unsigned long int getNanoSeconds() const;
+
+ /** \brief Assignement operator
+ *
+ * Assignement operator
+ *
+ */
+ void operator=(const double& ts);
+
+ /** \brief Assignement operator
+ *
+ * Assignement operator given a scalar_t (seconds)
+ *
+ */
+ void operator=(const TimeStamp& ts);
+
+ /** \brief comparison operator
+ *
+ * Comparison operator
+ *
+ */
+ bool operator!=(const TimeStamp& ts) const;
+ bool operator==(const TimeStamp& ts) const;
+ bool operator<(const TimeStamp& ts) const;
+ bool operator>(const TimeStamp& ts) const;
+
+ /** \brief comparison operator
+ *
+ * Comparison operator
+ *
+ */
+ bool operator<=(const TimeStamp& ts) const;
+ bool operator>=(const TimeStamp& ts) const;
+
+ /** \brief Add-assign operator given a scalar_t (seconds)
+ */
+ void operator+=(const double& dt);
+
+ /** \brief Add-assign operator given a scalar_t (seconds)
+ */
+ TimeStamp operator+(const double& dt) const;
+
+ /** \brief Substraction-assign operator given a scalar_t (seconds)
+ */
+ void operator-=(const double& dt);
+
+ /** \brief difference operator
+ *
+ * difference operator that returns a scalar_t (seconds)
+ *
+ */
+ TimeStamp operator-(const double& ts) const;
+
+ /** \brief difference operator
+ *
+ * difference operator that returns a Timestamp (seconds)
+ *
+ */
+ double operator-(const TimeStamp& ts) const;
+
+ /** \brief Prints time stamp to a given ostream
+ *
+ * Prints time stamp to a given ostream
+ *
+ */
+ void print(std::ostream& ost = std::cout) const;
+
+ friend std::ostream& operator<<(std::ostream& os, const TimeStamp& _ts);
};
-inline wolf::TimeStamp TimeStamp::Invalid ( )
+inline wolf::TimeStamp TimeStamp::Invalid()
{
return TimeStamp(-1.0);
}
-inline bool TimeStamp::ok ( ) const
+inline bool TimeStamp::ok() const
{
return is_valid_;
}
-inline void TimeStamp::setOk ( )
+inline void TimeStamp::setOk()
{
is_valid_ = true;
}
-inline void TimeStamp::setNOk ( )
+inline void TimeStamp::setNOk()
{
is_valid_ = false;
}
inline void TimeStamp::set(const double& ts)
{
- time_stamp_nano_ = (ts >= 0 ? (unsigned long int)(ts*NANOSECS) : 0);
- is_valid_ = (ts >= 0);
+ time_stamp_nano_ = (ts >= 0 ? (unsigned long int)(ts * NANOSECS) : 0);
+ is_valid_ = (ts >= 0);
}
inline void TimeStamp::set(const unsigned long int& sec, const unsigned long int& nanosec)
{
- time_stamp_nano_ = sec*NANOSECS+nanosec;
- is_valid_ = true;
+ time_stamp_nano_ = sec * NANOSECS + nanosec;
+ is_valid_ = true;
}
inline void TimeStamp::set(const timeval& ts)
{
- time_stamp_nano_ = (unsigned long int)(ts.tv_sec*NANOSECS) + (unsigned long int)(ts.tv_usec*1000);
- is_valid_ = (ts.tv_sec >= 0 and ts.tv_usec >= 0);
+ time_stamp_nano_ = (unsigned long int)(ts.tv_sec * NANOSECS) + (unsigned long int)(ts.tv_usec * 1000);
+ is_valid_ = (ts.tv_sec >= 0 and ts.tv_usec >= 0);
}
inline double TimeStamp::get() const
{
- return ((double)( time_stamp_nano_))*1e-9;
+ return ((double)(time_stamp_nano_)) * 1e-9;
}
inline unsigned long int TimeStamp::getSeconds() const
@@ -256,56 +255,57 @@ inline unsigned long int TimeStamp::getNanoSeconds() const
return time_stamp_nano_ % NANOSECS;
}
-inline void TimeStamp::operator =(const TimeStamp& ts)
+inline void TimeStamp::operator=(const TimeStamp& ts)
{
time_stamp_nano_ = ts.time_stamp_nano_;
- is_valid_ = ts.is_valid_;
+ is_valid_ = ts.is_valid_;
}
-inline void TimeStamp::operator =(const double& ts)
+inline void TimeStamp::operator=(const double& ts)
{
- time_stamp_nano_ = (unsigned long int)(ts*NANOSECS);
- is_valid_ = (ts >= 0);
+ time_stamp_nano_ = (unsigned long int)(ts * NANOSECS);
+ is_valid_ = (ts >= 0);
}
-inline bool TimeStamp::operator ==(const TimeStamp& ts) const
+inline bool TimeStamp::operator==(const TimeStamp& ts) const
{
return (time_stamp_nano_ == ts.time_stamp_nano_);
}
-inline bool TimeStamp::operator !=(const TimeStamp& ts) const
+inline bool TimeStamp::operator!=(const TimeStamp& ts) const
{
return (time_stamp_nano_ != ts.time_stamp_nano_);
}
-inline bool TimeStamp::operator <(const TimeStamp& ts) const
+inline bool TimeStamp::operator<(const TimeStamp& ts) const
{
return (time_stamp_nano_ < ts.time_stamp_nano_);
}
-inline bool TimeStamp::operator >(const TimeStamp& ts) const
+inline bool TimeStamp::operator>(const TimeStamp& ts) const
{
return (time_stamp_nano_ > ts.time_stamp_nano_);
}
-inline bool TimeStamp::operator <=(const TimeStamp& ts) const
+inline bool TimeStamp::operator<=(const TimeStamp& ts) const
{
return (time_stamp_nano_ <= ts.time_stamp_nano_);
}
-inline bool TimeStamp::operator >=(const TimeStamp& ts) const
+inline bool TimeStamp::operator>=(const TimeStamp& ts) const
{
return (time_stamp_nano_ >= ts.time_stamp_nano_);
}
-inline void TimeStamp::operator +=(const double& dt)
+inline void TimeStamp::operator+=(const double& dt)
{
- time_stamp_nano_ += (unsigned long int)(dt*NANOSECS);
+ time_stamp_nano_ += (unsigned long int)(dt * NANOSECS);
}
-inline double TimeStamp::operator -(const TimeStamp& ts) const
+inline double TimeStamp::operator-(const TimeStamp& ts) const
{
- return double((long int)(time_stamp_nano_ - ts.time_stamp_nano_))*1e-9; // long int cast fix overflow in case of negative substraction result
+ return double((long int)(time_stamp_nano_ - ts.time_stamp_nano_)) *
+ 1e-9; // long int cast fix overflow in case of negative substraction result
}
-} // namespace wolf
+} // namespace wolf
diff --git a/include/core/common/wolf.h b/include/core/common/wolf.h
index 0c7c0d83b939f7543391f5909997a0326758f8e1..4a40d5a8d149808a2599a69bed37c1d35ccdbd83 100644
--- a/include/core/common/wolf.h
+++ b/include/core/common/wolf.h
@@ -23,28 +23,33 @@
// Enable project-specific definitions and macros
#include "core/internal/config.h"
#include "core/utils/logging.h"
+#include "core/utils/string_utils.h"
// Folder Registry
#include "core/utils/folder_registry.h"
-//includes from Eigen lib
+// includes from Eigen lib
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <Eigen/Sparse>
#include <libgen.h>
-//includes from std lib
+// includes from std lib
#include <list>
#include <map>
-#include <memory> // shared_ptr and weak_ptr
+#include <memory> // shared_ptr and weak_ptr
#include <set>
// System specifics
#include <sys/stat.h>
-namespace wolf {
+// YAML
+#include "yaml-cpp/yaml.h"
+#include "yaml-schema-cpp/yaml_conversion.hpp"
+namespace wolf
+{
/**
* \brief Vector and Matrices size type for the Wolf project
*
@@ -64,15 +69,15 @@ typedef std::size_t SizeStd;
#define M_TORAD 0.017453292519943295769236907684886127134 // pi / 180
#define M_TODEG 57.295779513082320876798154814105170332 // 180 / pi
-namespace Constants{
-
+namespace Constants
+{
// Wolf standard tolerance
const double EPS = 1e-8;
// Wolf smmmmall tolerance
const double EPS_SMALL = 1e-16;
-}
+} // namespace Constants
-} // namespace wolf
+} // namespace wolf
///////////////////////////////////////////
// Construct types for any scalar defined in the typedef double above
@@ -89,30 +94,30 @@ const double EPS_SMALL = 1e-16;
namespace Eigen // Eigen namespace extension
{
// 1. Vectors and Matrices
-typedef Matrix<double, 1, 1> Matrix1d; ///< 1x1 matrix of real double type
-typedef Matrix<double, 5, 5> Matrix5d; ///< 5x5 matrix of real double type
-typedef Matrix<double, 6, 6> Matrix6d; ///< 6x6 matrix of real double type
-typedef Matrix<double, 7, 7> Matrix7d; ///< 7x7 matrix of real double type
-typedef Matrix<double, 8, 8> Matrix8d; ///< 8x8 matrix of real double type
-typedef Matrix<double, 9, 9> Matrix9d; ///< 9x9 matrix of real double type
-typedef Matrix<double, 10, 10> Matrix10d; ///< 10x10 matrix of real double type
-typedef Matrix<double, 1, 1> Vector1d; ///< 1-vector of real double type
-typedef Matrix<double, 5, 1> Vector5d; ///< 5-vector of real double type
-typedef Matrix<double, 6, 1> Vector6d; ///< 6-vector of real double type
-typedef Matrix<double, 7, 1> Vector7d; ///< 7-vector of real double type
-typedef Matrix<double, 8, 1> Vector8d; ///< 8-vector of real double type
-typedef Matrix<double, 9, 1> Vector9d; ///< 9-vector of real double type
-typedef Matrix<double, 10, 1> Vector10d; ///< 10-vector of real double type
+typedef Matrix<double, 1, 1> Matrix1d; ///< 1x1 matrix of real double type
+typedef Matrix<double, 5, 5> Matrix5d; ///< 5x5 matrix of real double type
+typedef Matrix<double, 6, 6> Matrix6d; ///< 6x6 matrix of real double type
+typedef Matrix<double, 7, 7> Matrix7d; ///< 7x7 matrix of real double type
+typedef Matrix<double, 8, 8> Matrix8d; ///< 8x8 matrix of real double type
+typedef Matrix<double, 9, 9> Matrix9d; ///< 9x9 matrix of real double type
+typedef Matrix<double, 10, 10> Matrix10d; ///< 10x10 matrix of real double type
+typedef Matrix<double, 1, 1> Vector1d; ///< 1-vector of real double type
+typedef Matrix<double, 5, 1> Vector5d; ///< 5-vector of real double type
+typedef Matrix<double, 6, 1> Vector6d; ///< 6-vector of real double type
+typedef Matrix<double, 7, 1> Vector7d; ///< 7-vector of real double type
+typedef Matrix<double, 8, 1> Vector8d; ///< 8-vector of real double type
+typedef Matrix<double, 9, 1> Vector9d; ///< 9-vector of real double type
+typedef Matrix<double, 10, 1> Vector10d; ///< 10-vector of real double type
// 2. Sparse matrix
typedef SparseMatrix<double> SparseMatrixd;
// 3. Row major matrix
-typedef Matrix<double,Dynamic,Dynamic,RowMajor> MatrixRowXd; ///< dynamic rowmajor matrix of real double type
-}
-
-namespace wolf {
+typedef Matrix<double, Dynamic, Dynamic, RowMajor> MatrixRowXd; ///< dynamic rowmajor matrix of real double type
+} // namespace Eigen
+namespace wolf
+{
//////////////////////////////////////////////////////////
/** Check Eigen Matrix sizes, both statically and dynamically
*
@@ -121,7 +126,8 @@ namespace wolf {
* The WOLF project implements many template functions using Eigen Matrix and Quaterniond, in different versions
* (Static size, Dynamic size, Map, Matrix expression).
*
- * Eigen provides some macros for STATIC assert of matrix sizes, the most common of them are (see Eigen's StaticAssert.h):
+ * Eigen provides some macros for STATIC assert of matrix sizes, the most common of them are (see Eigen's
+ * StaticAssert.h):
*
* EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE
* EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE
@@ -129,7 +135,8 @@ namespace wolf {
*
* but they do not work if the evaluated types are of dynamic size.
*
- * In order to achieve full templatization over both dynamic and static sizes, we use extensively a prototype of this kind:
+ * In order to achieve full templatization over both dynamic and static sizes, we use extensively a prototype of this
+ * kind:
*
* template<typename Derived>
* inline Eigen::Matrix<typename Derived::Scalar, 3, 3> function(const Eigen::MatrixBase<Derived>& _v){
@@ -146,43 +153,43 @@ namespace wolf {
* The function : MatrixSizeCheck <Rows, Cols>::check(M) checks that the Matrix M is of size ( Rows x Cols ).
* This check is performed statically or dynamically, depending on the type of argument provided.
*/
-template<int Size, int DesiredSize>
+template <int Size, int DesiredSize>
struct StaticSizeCheck
{
- template<typename T>
- StaticSizeCheck(const T&)
- {
- static_assert(Size == DesiredSize, "Size of static Vector or Matrix does not match");
- }
+ template <typename T>
+ StaticSizeCheck(const T&)
+ {
+ static_assert(Size == DesiredSize, "Size of static Vector or Matrix does not match");
+ }
};
//
-template<int DesiredSize>
+template <int DesiredSize>
struct StaticSizeCheck<Eigen::Dynamic, DesiredSize>
{
- template<typename T>
- StaticSizeCheck(const T& t)
- {
- assert(t == DesiredSize && "Size of dynamic Vector or Matrix does not match");
- }
+ template <typename T>
+ StaticSizeCheck(const T& t)
+ {
+ assert(t == DesiredSize && "Size of dynamic Vector or Matrix does not match");
+ }
};
//
-template<int DesiredR, int DesiredC>
+template <int DesiredR, int DesiredC>
struct MatrixSizeCheck
{
- /** \brief Assert matrix size dynamically or statically
- *
- * @param t the variable for which we wish to assert the size.
- *
- * Usage: to assert that t is size (Rows x Cols)
- *
- * MatrixSizeCheck<Rows, Cols>::check(t);
- */
- template<typename T>
- static void check(const T& t)
- {
- StaticSizeCheck<T::RowsAtCompileTime, DesiredR>(t.rows());
- StaticSizeCheck<T::ColsAtCompileTime, DesiredC>(t.cols());
- }
+ /** \brief Assert matrix size dynamically or statically
+ *
+ * @param t the variable for which we wish to assert the size.
+ *
+ * Usage: to assert that t is size (Rows x Cols)
+ *
+ * MatrixSizeCheck<Rows, Cols>::check(t);
+ */
+ template <typename T>
+ static void check(const T& t)
+ {
+ StaticSizeCheck<T::RowsAtCompileTime, DesiredR>(t.rows());
+ StaticSizeCheck<T::ColsAtCompileTime, DesiredC>(t.cols());
+ }
};
//
@@ -192,58 +199,57 @@ struct MatrixSizeCheck
// TYPEDEFS FOR POINTERS, LISTS AND ITERATORS IN THE WOLF TREE
/////////////////////////////////////////////////////////////////////////
-#define WOLF_DECLARED_PTR_TYPEDEFS(Name) \
- typedef std::shared_ptr<Name> Name##Ptr; \
- typedef std::shared_ptr<const Name> Name##ConstPtr; \
- typedef std::weak_ptr<Name> Name##WPtr; \
- typedef std::weak_ptr<const Name> Name##ConstWPtr; \
-
-#define WOLF_PTR_TYPEDEFS(ClassName) \
- class ClassName; \
- WOLF_DECLARED_PTR_TYPEDEFS(ClassName); \
-
-#define WOLF_STRUCT_PTR_TYPEDEFS(StructName) \
- struct StructName; \
- WOLF_DECLARED_PTR_TYPEDEFS(StructName); \
-
-#define WOLF_LIST_TYPEDEFS(ClassName) \
- class ClassName; \
- typedef std::list<ClassName##Ptr> ClassName##PtrList; \
- typedef ClassName##PtrList::iterator ClassName##PtrListIter; \
- typedef ClassName##PtrList::const_iterator ClassName##PtrListConstIter; \
- typedef ClassName##PtrList::reverse_iterator ClassName##PtrListRevIter; \
- typedef std::list<ClassName##WPtr> ClassName##WPtrList; \
- typedef ClassName##WPtrList::iterator ClassName##WPtrListIter; \
- typedef ClassName##WPtrList::const_iterator ClassName##WPtrListConstIter; \
- typedef ClassName##WPtrList::reverse_iterator ClassName##WPtrListRevIter; \
- typedef std::list<ClassName##ConstPtr> ClassName##ConstPtrList; \
- typedef ClassName##ConstPtrList::iterator ClassName##ConstPtrListIter; \
- typedef ClassName##ConstPtrList::const_iterator ClassName##ConstPtrListConstIter; \
- typedef ClassName##ConstPtrList::reverse_iterator ClassName##ConstPtrListRevIter; \
- typedef std::list<ClassName##ConstWPtr> ClassName##ConstWPtrList; \
- typedef ClassName##ConstWPtrList::iterator ClassName##ConstWPtrListIter; \
- typedef ClassName##ConstWPtrList::const_iterator ClassName##ConstWPtrListConstIter; \
- typedef ClassName##ConstWPtrList::reverse_iterator ClassName##ConstWPtrListRevIter; \
-
-#define WOLF_SET_TYPEDEFS(ClassName) \
- class ClassName; \
- typedef std::set<ClassName##Ptr> ClassName##PtrSet; \
- typedef ClassName##PtrSet::iterator ClassName##PtrSetIter; \
- typedef ClassName##PtrSet::const_iterator ClassName##PtrSetConstIter; \
- typedef ClassName##PtrSet::reverse_iterator ClassName##PtrSetRevIter; \
- typedef std::set<ClassName##WPtr> ClassName##WPtrSet; \
- typedef ClassName##WPtrSet::iterator ClassName##WPtrSetIter; \
- typedef ClassName##WPtrSet::const_iterator ClassName##WPtrSetConstIter; \
- typedef ClassName##WPtrSet::reverse_iterator ClassName##WPtrSetRevIter; \
- typedef std::set<ClassName##ConstPtr> ClassName##ConstPtrSet; \
- typedef ClassName##ConstPtrSet::iterator ClassName##ConstPtrSetIter; \
- typedef ClassName##ConstPtrSet::const_iterator ClassName##ConstPtrSetConstIter; \
- typedef ClassName##ConstPtrSet::reverse_iterator ClassName##ConstPtrSetRevIter; \
- typedef std::set<ClassName##ConstWPtr> ClassName##ConstWPtrSet; \
- typedef ClassName##ConstWPtrSet::iterator ClassName##ConstWPtrSetIter; \
- typedef ClassName##ConstWPtrSet::const_iterator ClassName##ConstWPtrSetConstIter; \
- typedef ClassName##ConstWPtrSet::reverse_iterator ClassName##ConstWPtrSetRevIter; \
-
+#define WOLF_DECLARED_PTR_TYPEDEFS(Name) \
+ typedef std::shared_ptr<Name> Name##Ptr; \
+ typedef std::shared_ptr<const Name> Name##ConstPtr; \
+ typedef std::weak_ptr<Name> Name##WPtr; \
+ typedef std::weak_ptr<const Name> Name##ConstWPtr;
+
+#define WOLF_PTR_TYPEDEFS(ClassName) \
+ class ClassName; \
+ WOLF_DECLARED_PTR_TYPEDEFS(ClassName);
+
+#define WOLF_STRUCT_PTR_TYPEDEFS(StructName) \
+ struct StructName; \
+ WOLF_DECLARED_PTR_TYPEDEFS(StructName);
+
+#define WOLF_LIST_TYPEDEFS(ClassName) \
+ class ClassName; \
+ typedef std::list<ClassName##Ptr> ClassName##PtrList; \
+ typedef ClassName##PtrList::iterator ClassName##PtrListIter; \
+ typedef ClassName##PtrList::const_iterator ClassName##PtrListConstIter; \
+ typedef ClassName##PtrList::reverse_iterator ClassName##PtrListRevIter; \
+ typedef std::list<ClassName##WPtr> ClassName##WPtrList; \
+ typedef ClassName##WPtrList::iterator ClassName##WPtrListIter; \
+ typedef ClassName##WPtrList::const_iterator ClassName##WPtrListConstIter; \
+ typedef ClassName##WPtrList::reverse_iterator ClassName##WPtrListRevIter; \
+ typedef std::list<ClassName##ConstPtr> ClassName##ConstPtrList; \
+ typedef ClassName##ConstPtrList::iterator ClassName##ConstPtrListIter; \
+ typedef ClassName##ConstPtrList::const_iterator ClassName##ConstPtrListConstIter; \
+ typedef ClassName##ConstPtrList::reverse_iterator ClassName##ConstPtrListRevIter; \
+ typedef std::list<ClassName##ConstWPtr> ClassName##ConstWPtrList; \
+ typedef ClassName##ConstWPtrList::iterator ClassName##ConstWPtrListIter; \
+ typedef ClassName##ConstWPtrList::const_iterator ClassName##ConstWPtrListConstIter; \
+ typedef ClassName##ConstWPtrList::reverse_iterator ClassName##ConstWPtrListRevIter;
+
+#define WOLF_SET_TYPEDEFS(ClassName) \
+ class ClassName; \
+ typedef std::set<ClassName##Ptr> ClassName##PtrSet; \
+ typedef ClassName##PtrSet::iterator ClassName##PtrSetIter; \
+ typedef ClassName##PtrSet::const_iterator ClassName##PtrSetConstIter; \
+ typedef ClassName##PtrSet::reverse_iterator ClassName##PtrSetRevIter; \
+ typedef std::set<ClassName##WPtr> ClassName##WPtrSet; \
+ typedef ClassName##WPtrSet::iterator ClassName##WPtrSetIter; \
+ typedef ClassName##WPtrSet::const_iterator ClassName##WPtrSetConstIter; \
+ typedef ClassName##WPtrSet::reverse_iterator ClassName##WPtrSetRevIter; \
+ typedef std::set<ClassName##ConstPtr> ClassName##ConstPtrSet; \
+ typedef ClassName##ConstPtrSet::iterator ClassName##ConstPtrSetIter; \
+ typedef ClassName##ConstPtrSet::const_iterator ClassName##ConstPtrSetConstIter; \
+ typedef ClassName##ConstPtrSet::reverse_iterator ClassName##ConstPtrSetRevIter; \
+ typedef std::set<ClassName##ConstWPtr> ClassName##ConstWPtrSet; \
+ typedef ClassName##ConstWPtrSet::iterator ClassName##ConstWPtrSetIter; \
+ typedef ClassName##ConstWPtrSet::const_iterator ClassName##ConstWPtrSetConstIter; \
+ typedef ClassName##ConstWPtrSet::reverse_iterator ClassName##ConstWPtrSetRevIter;
// NodeBase
WOLF_PTR_TYPEDEFS(NodeBase);
@@ -287,7 +293,7 @@ WOLF_PTR_TYPEDEFS(FrameBase);
WOLF_LIST_TYPEDEFS(FrameBase);
class TimeStamp;
-typedef std::map<TimeStamp, FrameBasePtr> FramePtrMap;
+typedef std::map<TimeStamp, FrameBasePtr> FramePtrMap;
typedef std::map<TimeStamp, FrameBaseConstPtr> FrameConstPtrMap;
// - Capture
@@ -328,15 +334,16 @@ WOLF_PTR_TYPEDEFS(LocalParametrizationBase);
inline bool file_exists(const std::string& _name)
{
struct stat buffer;
- return (stat (_name.c_str(), &buffer) == 0);
+ return (stat(_name.c_str(), &buffer) == 0);
}
-inline const Eigen::Vector3d gravity(void) {
- return Eigen::Vector3d(0,0,-9.806);
+inline const Eigen::Vector3d gravity(void)
+{
+ return Eigen::Vector3d(0, 0, -9.806);
}
// ===================================================
// Schema folder registry
WOLF_REGISTER_FOLDER(_WOLF_SCHEMA_DIR);
-} // namespace wolf
+} // namespace wolf
diff --git a/include/core/composite/composite.h b/include/core/composite/composite.h
index 957d333cf544fb6972a3afe93dd9c7dd47671b07..20a974049d6598f45cafd0ef549378558ab0a687 100644
--- a/include/core/composite/composite.h
+++ b/include/core/composite/composite.h
@@ -87,21 +87,35 @@ inline Composite<T>::Composite(const YAML::Node& _n, std::string _keys)
throw std::runtime_error("Composite: constructor with a non-map yaml node");
}
+ // load _keys from node or input param
+ if (_keys == ALL_KEYS and _n["keys"]) _keys = _n["keys"].as<std::string>();
+
// check existence of _keys in node
if (_keys != ALL_KEYS)
for (auto key : _keys)
- {
if (not _n[key])
{
throw std::runtime_error(std::string("Composite: In constructor. Missing state for key '") + key +
"'.");
}
- }
// iterate node map pairs
for (auto node_pair : _n)
{
- char key = node_pair.first.as<char>();
+ // ignore "keys"
+ if (node_pair.first.as<std::string>() == "keys") continue;
+
+ // check that keys are char type
+ char key;
+ try
+ {
+ key = node_pair.first.as<char>();
+ }
+ catch (const std::exception& e)
+ {
+ throw std::runtime_error("In Composite constructor: There is a key of type different than char: " +
+ node_pair.first.as<std::string>() + ". Error: " + e.what());
+ }
// discard if not in _keys (if _keys is not ALL_KEYS)
if (_keys != ALL_KEYS and //
@@ -114,9 +128,11 @@ inline Composite<T>::Composite(const YAML::Node& _n, std::string _keys)
}
catch (const std::exception& e)
{
+ YAML::Emitter em;
+ em << node_pair.second;
throw std::runtime_error(
std::string("Composite: In constructor. Failed to emplace the entry for key '") + key +
- "' with error: " + e.what());
+ "' with error: " + e.what() + "\nNode:\n" + em.c_str());
}
}
}
diff --git a/include/core/composite/matrix_composite.h b/include/core/composite/matrix_composite.h
index 3ae0fec997ab17a949e905a0cfaf34fe089b49fa..92ddbca72eaf1145f9a2699da76a89c39f136b3e 100644
--- a/include/core/composite/matrix_composite.h
+++ b/include/core/composite/matrix_composite.h
@@ -28,191 +28,178 @@
namespace wolf
{
-
-class MatrixComposite : public std::unordered_map < char, std::unordered_map < char, Eigen::MatrixXd > >
+class MatrixComposite : public std::unordered_map<char, std::unordered_map<char, Eigen::MatrixXd> >
{
-
- private:
- std::unordered_map < char, unsigned int> size_rows_, size_cols_;
- unsigned int rows() const;
- unsigned int cols() const;
- void sizeAndIndices(const StateKeys & _struct_rows,
- const StateKeys & _struct_cols,
- std::unordered_map < char, unsigned int>& _indices_rows,
- std::unordered_map < char, unsigned int>& _indices_cols,
- unsigned int& _rows,
- unsigned int& _cols) const;
-
- public:
- MatrixComposite() {};
- MatrixComposite(const StateKeys& _row_structure,
- const StateKeys& _col_structure);
- MatrixComposite(const StateKeys& _row_structure,
- const std::list<int>& _row_sizes,
- const StateKeys& _col_structure,
- const std::list<int>& _col_sizes);
-
- /**
- * \brief Construct from Eigen::VectorXd and structure
- *
- * Usage:
- *
- * VectorXd vec_eigen(1,2,3,4,5);
- *
- * VectorComposite vec_comp( vec_eigen, "ab", {2,3} ); // vec_eigen must be at least size 5 !!
- *
- * result:
- *
- * vec_comp["a"].transpose(); // = (1,2);
- * vec_comp["b"].transpose(); // = (3,4,5);
- */
- MatrixComposite(const MatrixXd& _m,
- const StateKeys& _row_structure,
- const std::list<int>& _row_sizes,
- const StateKeys& _col_structure,
- const std::list<int>& _col_sizes);
- ~MatrixComposite() = default;
-
- bool check() const;
- static MatrixComposite zero(const StateKeys& _row_structure,
- const std::list<int>& _row_sizes,
- const StateKeys& _col_structure,
- const std::list<int>& _col_sizes);
-
- static MatrixComposite identity(const StateKeys& _structure,
- const std::list<int>& _sizes);
-
- unsigned int count(const char &_row,
- const char &_col) const;
-
- bool emplace(const char &_row,
- const char &_col,
- const Eigen::MatrixXd &_mat_blk);
-
- // throw error if queried block is not present
- bool at(const char &_row,
- const char &_col,
- Eigen::MatrixXd &_mat_blk) const;
- const MatrixXd& at(const char &_row,
- const char &_col) const;
- MatrixXd& at(const char &_row,
- const char &_col);
-
- // make some shadowed API available
- using std::unordered_map < char, std::unordered_map < char, Eigen::MatrixXd > >::at;
- using std::unordered_map < char, std::unordered_map < char, Eigen::MatrixXd > >::count;
-
-
- MatrixXd matrix(const StateKeys & _struct_rows,
- const StateKeys & _struct_cols) const;
-
- MatrixComposite operator * (double scalar_right) const;
- MatrixComposite operator + (const MatrixComposite & _second) const;
- MatrixComposite operator - (const MatrixComposite & _second) const;
- MatrixComposite operator - (void) const;
-
- /**
- * \brief Matrix product
- *
- * This function computes the matrix product M * N
- *
- * It assumes that the second matrix's blocks are compatible with the blocks of this matrix,
- * both in their structure and their individual sizes.
- *
- * For example: Let us call 'this' matrix with the name 'M'.
- *
- * The matrix 'M' maps the "PO" space into a new space "VW":
- * M["V"]["P"] M["V"]["O"]
- * M["W"]["P"] M["W"]["O"]
- *
- * The second matrix N has blocks "PO" in vertical arrangement,
- * and "XY" in horizontal arrangement:
- * N["P"]["X"] N["P"]["Y"]
- * N["O"]["X"] N["O"]["Y"]
- *
- * Also, assume that the sizes of the blocks "P" and "O" are the same in M and N.
- *
- * Then, the result is a matrix S = M * N with block arrangements "VW" and "XY"
- * S["V"]["X"] S["V"]["Y"]
- * S["W"]["X"] S["W"]["Y"]
- */
- MatrixComposite operator *(const MatrixComposite & _second) const;
-
- /**
- * \brief Propagate a given covariances matrix, with 'this' being a Jacobian matrix
- *
- * This function computes the product J * Q * J.transpose.
- *
- * It considers that this is a Jacobian matrix, and that the provided matrix
- * is a proper covariance (e.g. symmmetric and semi-positive).
- * It also considers that the Jacobian blocks are compatible with the blocks
- * of the provided covariance, both in their structure and their individual sizes.
- *
- * If these conditions are not satisfied, the product will fail and throw,
- * or give aberrant results at best.
- *
- * -----
- *
- * For example: Let us call 'this' a Jacobian matrix with the name 'J'.
- *
- * This Jacobian 'J' maps the "PO" blocks into "VW":
- *
- * J["V"]["P"] J["V"]["O"]
- * J["W"]["P"] J["W"]["O"]
- *
- * The provided matrix is a covariances matrix Q.
- * Q has blocks "P", "O" in both vertical and horizontal arrangements:
- *
- * Q["P"]["P"] Q["P"]["O"]
- * Q["O"]["P"] Q["O"]["O"]
- *
- * Also, it is required that the sizes of the blocks "P", "O" are the same in Q and J.
- *
- * Now, upon a call to
- *
- * MatrixComposite S = J.propagate(Q);
- *
- * the result is a covariances matrix S with blocks "V" and "W"
- *
- * S["V"]["V"] S["V"]["W"]
- * S["W"]["V"] S["W"]["W"]
- */
- MatrixComposite propagate(const MatrixComposite & _Cov); // This performs this * _Cov * this.tr
-
- /**
- * \brief Matrix-vector product
- *
- * This function computes the matrix product M * N
- *
- * It assumes that the second matrix's blocks are compatible with the blocks of this matrix,
- * both in their structure and their individual sizes.
- *
- * For example: Let us call 'this' matrix with the name 'M'.
- *
- * The matrix 'M' maps the "PO" space into a new space "VW":
- * M["V"]["P"] M["V"]["O"]
- * M["W"]["P"] M["W"]["O"]
- *
- * The second vector V has blocks "PO" in vertical arrangement,
- * V["P"]
- * V["O"]
- *
- * Also, assume that the sizes of the blocks "P" and "O" are the same in M and V.
- *
- * Then, the result is a vector S = M * V with block arrangement "VW"
- * S["V"]
- * S["W"]
- */
- VectorComposite operator *(const VectorComposite & _second) const;
-
- friend std::ostream& operator << (std::ostream & _os, const MatrixComposite & _M);
-
- friend MatrixComposite operator * (double scalar_left, const MatrixComposite& M);
+ private:
+ std::unordered_map<char, unsigned int> size_rows_, size_cols_;
+ unsigned int rows() const;
+ unsigned int cols() const;
+ void sizeAndIndices(const StateKeys& _struct_rows,
+ const StateKeys& _struct_cols,
+ std::unordered_map<char, unsigned int>& _indices_rows,
+ std::unordered_map<char, unsigned int>& _indices_cols,
+ unsigned int& _rows,
+ unsigned int& _cols) const;
+
+ public:
+ MatrixComposite(){};
+ MatrixComposite(const StateKeys& _row_structure, const StateKeys& _col_structure);
+ MatrixComposite(const StateKeys& _row_structure,
+ const std::list<int>& _row_sizes,
+ const StateKeys& _col_structure,
+ const std::list<int>& _col_sizes);
+
+ /**
+ * \brief Construct from Eigen::VectorXd and structure
+ *
+ * Usage:
+ *
+ * VectorXd vec_eigen(1,2,3,4,5);
+ *
+ * VectorComposite vec_comp( vec_eigen, "ab", {2,3} ); // vec_eigen must be at least size 5 !!
+ *
+ * result:
+ *
+ * vec_comp["a"].transpose(); // = (1,2);
+ * vec_comp["b"].transpose(); // = (3,4,5);
+ */
+ MatrixComposite(const MatrixXd& _m,
+ const StateKeys& _row_structure,
+ const std::list<int>& _row_sizes,
+ const StateKeys& _col_structure,
+ const std::list<int>& _col_sizes);
+ ~MatrixComposite() = default;
+
+ bool check() const;
+ static MatrixComposite zero(const StateKeys& _row_structure,
+ const std::list<int>& _row_sizes,
+ const StateKeys& _col_structure,
+ const std::list<int>& _col_sizes);
+
+ static MatrixComposite identity(const StateKeys& _structure, const std::list<int>& _sizes);
+
+ unsigned int count(const char& _row, const char& _col) const;
+
+ bool emplace(const char& _row, const char& _col, const Eigen::MatrixXd& _mat_blk);
+
+ // throw error if queried block is not present
+ bool at(const char& _row, const char& _col, Eigen::MatrixXd& _mat_blk) const;
+ const MatrixXd& at(const char& _row, const char& _col) const;
+ MatrixXd& at(const char& _row, const char& _col);
+
+ // make some shadowed API available
+ using std::unordered_map<char, std::unordered_map<char, Eigen::MatrixXd> >::at;
+ using std::unordered_map<char, std::unordered_map<char, Eigen::MatrixXd> >::count;
+
+ MatrixXd matrix(const StateKeys& _struct_rows, const StateKeys& _struct_cols) const;
+
+ MatrixComposite operator*(double scalar_right) const;
+ MatrixComposite operator+(const MatrixComposite& _second) const;
+ MatrixComposite operator-(const MatrixComposite& _second) const;
+ MatrixComposite operator-(void) const;
+
+ /**
+ * \brief Matrix product
+ *
+ * This function computes the matrix product M * N
+ *
+ * It assumes that the second matrix's blocks are compatible with the blocks of this matrix,
+ * both in their structure and their individual sizes.
+ *
+ * For example: Let us call 'this' matrix with the name 'M'.
+ *
+ * The matrix 'M' maps the "PO" space into a new space "VW":
+ * M["V"]["P"] M["V"]["O"]
+ * M["W"]["P"] M["W"]["O"]
+ *
+ * The second matrix N has blocks "PO" in vertical arrangement,
+ * and "XY" in horizontal arrangement:
+ * N["P"]["X"] N["P"]["Y"]
+ * N["O"]["X"] N["O"]["Y"]
+ *
+ * Also, assume that the sizes of the blocks "P" and "O" are the same in M and N.
+ *
+ * Then, the result is a matrix S = M * N with block arrangements "VW" and "XY"
+ * S["V"]["X"] S["V"]["Y"]
+ * S["W"]["X"] S["W"]["Y"]
+ */
+ MatrixComposite operator*(const MatrixComposite& _second) const;
+
+ /**
+ * \brief Propagate a given covariances matrix, with 'this' being a Jacobian matrix
+ *
+ * This function computes the product J * Q * J.transpose.
+ *
+ * It considers that this is a Jacobian matrix, and that the provided matrix
+ * is a proper covariance (e.g. symmmetric and semi-positive).
+ * It also considers that the Jacobian blocks are compatible with the blocks
+ * of the provided covariance, both in their structure and their individual sizes.
+ *
+ * If these conditions are not satisfied, the product will fail and throw,
+ * or give aberrant results at best.
+ *
+ * -----
+ *
+ * For example: Let us call 'this' a Jacobian matrix with the name 'J'.
+ *
+ * This Jacobian 'J' maps the "PO" blocks into "VW":
+ *
+ * J["V"]["P"] J["V"]["O"]
+ * J["W"]["P"] J["W"]["O"]
+ *
+ * The provided matrix is a covariances matrix Q.
+ * Q has blocks "P", "O" in both vertical and horizontal arrangements:
+ *
+ * Q["P"]["P"] Q["P"]["O"]
+ * Q["O"]["P"] Q["O"]["O"]
+ *
+ * Also, it is required that the sizes of the blocks "P", "O" are the same in Q and J.
+ *
+ * Now, upon a call to
+ *
+ * MatrixComposite S = J.propagate(Q);
+ *
+ * the result is a covariances matrix S with blocks "V" and "W"
+ *
+ * S["V"]["V"] S["V"]["W"]
+ * S["W"]["V"] S["W"]["W"]
+ */
+ MatrixComposite propagate(const MatrixComposite& _Cov); // This performs this * _Cov * this.tr
+
+ /**
+ * \brief Matrix-vector product
+ *
+ * This function computes the matrix product M * N
+ *
+ * It assumes that the second matrix's blocks are compatible with the blocks of this matrix,
+ * both in their structure and their individual sizes.
+ *
+ * For example: Let us call 'this' matrix with the name 'M'.
+ *
+ * The matrix 'M' maps the "PO" space into a new space "VW":
+ * M["V"]["P"] M["V"]["O"]
+ * M["W"]["P"] M["W"]["O"]
+ *
+ * The second vector V has blocks "PO" in vertical arrangement,
+ * V["P"]
+ * V["O"]
+ *
+ * Also, assume that the sizes of the blocks "P" and "O" are the same in M and V.
+ *
+ * Then, the result is a vector S = M * V with block arrangement "VW"
+ * S["V"]
+ * S["W"]
+ */
+ VectorComposite operator*(const VectorComposite& _second) const;
+
+ friend std::ostream& operator<<(std::ostream& _os, const MatrixComposite& _M);
+
+ friend MatrixComposite operator*(double scalar_left, const MatrixComposite& M);
};
//////////// IMPLEMENTATION ////////////
-inline unsigned int MatrixComposite::count(const char &_row, const char &_col) const
+inline unsigned int MatrixComposite::count(const char& _row, const char& _col) const
{
const auto& rit = this->find(_row);
@@ -223,4 +210,4 @@ inline unsigned int MatrixComposite::count(const char &_row, const char &_col) c
return rit->second.count(_col);
}
-}
+} // namespace wolf
diff --git a/include/core/composite/spec_state_composite.h b/include/core/composite/spec_state_composite.h
index 3f0e2139cc29bfc755c760974c0cb36511d247df..6ba838167df5d113a889b3e3ab4db030c29a8281 100644
--- a/include/core/composite/spec_state_composite.h
+++ b/include/core/composite/spec_state_composite.h
@@ -171,7 +171,7 @@ inline bool SpecState::isFactor() const
// CONVERSION TO YAML
namespace YAML
{
-template<>
+template <>
struct convert<wolf::SpecState>
{
static Node encode(const wolf::SpecState& spec)
diff --git a/include/core/composite/spec_state_sensor_composite.h b/include/core/composite/spec_state_sensor_composite.h
index 1821e727313378461662bbe7940fe01463539742..351d2999010e94588faab2e87b5c80bf9c1fe70b 100644
--- a/include/core/composite/spec_state_sensor_composite.h
+++ b/include/core/composite/spec_state_sensor_composite.h
@@ -49,7 +49,7 @@ class SpecStateSensor : public SpecState
void check() const override;
- std::string print(const std::string& _spaces = "") const override;
+ std::string print(const std::string& _spaces = "") const override;
friend std::ostream& operator<<(std::ostream& _os, const wolf::SpecStateSensor& _x);
YAML::Node toYaml() const;
diff --git a/include/core/composite/state_composite.h b/include/core/composite/state_composite.h
deleted file mode 100644
index 4804335678bdda7361b97a9d984d694684534f09..0000000000000000000000000000000000000000
--- a/include/core/composite/state_composite.h
+++ /dev/null
@@ -1,117 +0,0 @@
-// WOLF - Copyright (C) 2020,2021,2022,2023
-// Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
-// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
-// Joan Vallvé Navarro (jvallve@iri.upc.edu)
-// All rights reserved.
-//
-// This file is part of WOLF: http://www.iri.upc.edu/wolf
-// WOLF is free software: you can redistribute it and/or modify
-// it under the terms of the GNU Lesser General Public License as published by
-// 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/>.
-
-#pragma once
-
-#include "core/common/wolf.h"
-#include "core/composite/vector_composite.h"
-
-#include <unordered_map>
-#include <iostream>
-
-namespace wolf
-{
-
-using std::string;
-using std::unordered_map;
-using namespace Eigen;
-
-typedef std::unordered_map<char, StateBlockPtr> StateBlockMap;
-typedef StateBlockMap::const_iterator StateBlockMapCIter;
-
-class StateBlockComposite
-{
- public:
- StateBlockComposite() = default;
- ~StateBlockComposite() = default;
-
- const StateBlockMap& getStateBlockMap() const;
-
- StateBlockPtr add(const char& _sb_type, const StateBlockPtr& _sb);
-
- // Emplace derived state blocks (angle, quaternion, etc).
- template <typename SB, typename... Args>
- std::shared_ptr<SB> emplace(const char& _sb_type, Args&&... _args_of_derived_state_block_constructor);
-
- unsigned int remove(const char& _sb_type);
- bool has(const char& _sb_type) const
- {
- return state_block_map_.count(_sb_type) > 0;
- }
- bool has(const StateBlockPtr& _sb) const;
- StateBlockPtr at(const char& _sb_type) const;
- void set(const char& _sb_type, const StateBlockPtr& _sb);
- void set(const char& _sb_type, const VectorXd& _vec);
- void setVectors(const StateKeys& _structure, const std::list<VectorXd>& _vectors);
- bool key(const StateBlockPtr& _sb, char& _key) const;
- char key(const StateBlockPtr& _sb) const;
- StateBlockMapCIter find(const StateBlockPtr& _sb) const;
-
- // identity and zero (they are the same with different names)
- void setIdentity(bool _notify = true);
- void setZero(bool _notify = true);
- VectorComposite identity() const;
- VectorComposite zero() const;
-
- // Plus operator
- void plus(const VectorComposite& _dx);
-
- /**
- * \brief perturb all states states with random noise
- * following an uniform distribution in [ -amplitude, amplitude ]
- */
- void perturb(double amplitude = 0.01);
-
- // These act on all state blocks. Per-block action must be done through state_block.fix() or through extended API
- // in derived classes of this.
- void fix();
- void unfix();
- bool isFixed() const;
-
- // Get composite of state vectors (not blocks)
- VectorComposite getState() const;
- bool getState(VectorComposite& _state) const;
- void setState(const VectorComposite& _state);
-
- // Get compact Eigen vector and related things
- SizeEigen stateSize() const;
- SizeEigen stateSize(const StateKeys& _structure) const;
- VectorXd stateVector(const StateKeys& _structure) const;
- void stateVector(const StateKeys& _structure, VectorXd& _vector) const;
-
- private:
- StateBlockMap state_block_map_;
-};
-
-//////////// IMPLEMENTATION ////////////
-template <typename SB, typename... Args>
-inline std::shared_ptr<SB> wolf::StateBlockComposite::emplace(const char& _sb_type,
- Args&&... _args_of_derived_state_block_constructor)
-{
- assert(state_block_map_.count(_sb_type) == 0 &&
- "Trying to add a state block with an existing type! Use setStateBlock instead.");
- std::shared_ptr<SB> sb = std::make_shared<SB>(std::forward<Args>(_args_of_derived_state_block_constructor)...);
-
- add(_sb_type, sb);
-
- return sb;
-}
-
-} // namespace wolf
diff --git a/include/core/composite/type_composite.h b/include/core/composite/type_composite.h
index 848ca52e1239cec96c97619ac82de22dd5cffdaa..3587eaa99391f3f1161fad8f21b73370781c727f 100644
--- a/include/core/composite/type_composite.h
+++ b/include/core/composite/type_composite.h
@@ -25,38 +25,9 @@
namespace wolf
{
-
typedef Composite<std::string> TypeComposite;
-void checkTypeComposite(const TypeComposite& _types, SizeEigen _dim = 0);
-bool isTypeCompositeValid(const TypeComposite& _types, SizeEigen _dim = 0);
-
-// template <>
-// inline Composite<std::string>::Composite(const YAML::Node& _n)
-// {
-// if (not _n.IsMap())
-// {
-// throw std::runtime_error("SpecStateComposite: constructor with a non-map yaml node");
-// }
-
-// for (auto spec_pair : _n)
-// {
-// this->emplace(spec_pair.first.as<char>(), spec_pair.second["type"].as<std::string>());
-// }
-// }
-
-// template <>
-// inline YAML::Node Composite<std::string>::toYaml() const
-// {
-// YAML::Node n;
-// for (auto&& pair : *this)
-// {
-// YAML::Node n_type;
-// n_type["type"] = pair.second;
-// n[pair.first] = n_type;
-// }
-
-// return n;
-// }
+void checkTypeComposite(const TypeComposite& _types, SizeEigen _dim = 0);
+bool isTypeCompositeValid(const TypeComposite& _types, SizeEigen _dim = 0);
inline bool isTypeCompositeValid(const TypeComposite& _types, SizeEigen _dim)
{
@@ -64,7 +35,7 @@ inline bool isTypeCompositeValid(const TypeComposite& _types, SizeEigen _dim)
{
checkTypeComposite(_types, _dim);
}
- catch(const std::exception& e)
+ catch (const std::exception& e)
{
return false;
}
@@ -120,7 +91,9 @@ inline void checkTypeComposite(const TypeComposite& _types, SizeEigen _dim)
}
else
{
- throw std::runtime_error("checkTypeComposite(_types, _dim) bad value for '_dim', should be 2 for 2D, 3 for 3D, or 0 for not defined");
+ throw std::runtime_error(
+ "checkTypeComposite(_types, _dim) bad value for '_dim', should be 2 for 2D, 3 for 3D, or 0 for not "
+ "defined");
}
}
diff --git a/include/core/composite/vector_composite.h b/include/core/composite/vector_composite.h
index d632d5bccd0e5e496c091588b58f360a12e02da7..84ae2785801172af3e6471015c65699b131c343c 100644
--- a/include/core/composite/vector_composite.h
+++ b/include/core/composite/vector_composite.h
@@ -30,54 +30,53 @@
namespace wolf
{
-
using namespace Eigen;
class VectorComposite : public Composite<Eigen::VectorXd>
{
- public:
- using Composite<Eigen::VectorXd>::Composite;
-
- /**
- * \brief Construct from Eigen::VectorXd and keys
- *
- * Usage:
- *
- * VectorXd vec_eigen(1,2,3,4,5);
- *
- * VectorComposite vec_comp("ab", vec_eigen, {2,3} ); // vec_eigen must be at least size 5 !!
- *
- * result:
- *
- * vec_comp["a"].transpose(); // = (1,2);
- * vec_comp["b"].transpose(); // = (3,4,5);
- */
- VectorComposite(const StateKeys& _keys, const VectorXd& _v, const std::list<int>& _sizes);
- VectorComposite(const StateKeys& _keys, const std::list<VectorXd>& _vectors);
+ public:
+ using Composite<Eigen::VectorXd>::Composite;
+
+ /**
+ * \brief Construct from Eigen::VectorXd and keys
+ *
+ * Usage:
+ *
+ * VectorXd vec_eigen(1,2,3,4,5);
+ *
+ * VectorComposite vec_comp("ab", vec_eigen, {2,3} ); // vec_eigen must be at least size 5 !!
+ *
+ * result:
+ *
+ * vec_comp["a"].transpose(); // = (1,2);
+ * vec_comp["b"].transpose(); // = (3,4,5);
+ */
+ VectorComposite(const StateKeys& _keys, const VectorXd& _v, const std::list<int>& _sizes);
+ VectorComposite(const StateKeys& _keys, const std::list<VectorXd>& _vectors);
+
+ Eigen::VectorXd vector(const StateKeys& _keys) const;
- Eigen::VectorXd vector(const StateKeys& _keys) const;
+ /**
+ * \brief set from Eigen::VectorXd and keys
+ *
+ * Usage:
+ *
+ * Eigen::VectorXd vec_eigen;
+ * wolf::VectorComposite vec_comp;
+ *
+ * vec_comp.set("ab", vec_eigen, {2,3} ); // vec_eigen must be at least size 5 !!
+ *
+ * result:
+ *
+ * vec_comp["a"].transpose(); // = (1,2);
+ * vec_comp["b"].transpose(); // = (3,4,5);
+ */
+ void set(const StateKeys& _keys, const VectorXd& _v, const std::list<int>& _sizes);
+ void set(const StateKeys& _keys, const std::list<VectorXd>& _vectors);
- /**
- * \brief set from Eigen::VectorXd and keys
- *
- * Usage:
- *
- * Eigen::VectorXd vec_eigen;
- * wolf::VectorComposite vec_comp;
- *
- * vec_comp.set("ab", vec_eigen, {2,3} ); // vec_eigen must be at least size 5 !!
- *
- * result:
- *
- * vec_comp["a"].transpose(); // = (1,2);
- * vec_comp["b"].transpose(); // = (3,4,5);
- */
- void set(const StateKeys& _keys, const VectorXd& _v, const std::list<int>& _sizes);
- void set(const StateKeys& _keys, const std::list<VectorXd>& _vectors);
-
- void setZero();
+ void setZero();
- friend std::ostream& operator <<(std::ostream &_os, const wolf::VectorComposite &_x);
+ friend std::ostream& operator<<(std::ostream& _os, const wolf::VectorComposite& _x);
};
-}
+} // namespace wolf
diff --git a/include/core/factor/factor_autodiff.h b/include/core/factor/factor_autodiff.h
index 68fcf9f170bc084bb1f35083e829b7f1e14d5cef..45412f5cc77fe67fd11df1707146968e850ff557 100644
--- a/include/core/factor/factor_autodiff.h
+++ b/include/core/factor/factor_autodiff.h
@@ -36,15 +36,15 @@ namespace wolf
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 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,
@@ -85,7 +85,7 @@ class FactorAutodiff : public FactorBase
const ProcessorBasePtr& _processor_ptr,
const std::vector<StateBlockPtr>& _state_ptrs,
bool _apply_loss_function,
- FactorStatus _status = FAC_ACTIVE)
+ FactorStatus _status = FAC_ACTIVE)
: FactorBase(_type,
_top,
JAC_AUTO,
@@ -105,7 +105,8 @@ class FactorAutodiff : public FactorBase
for (auto i = 0; i < n_blocks; i++)
{
assert(_state_ptrs.at(i) && ("FactorAutodiff: nullptr state block " + toString(i)).c_str());
- assert(state_block_sizes_.at(i) == sizes_template.at(i) && ("FactorAutodiff: inconsistent state block size " + toString(i)).c_str());
+ assert(state_block_sizes_.at(i) == sizes_template.at(i) &&
+ ("FactorAutodiff: inconsistent state block size " + toString(i)).c_str());
}
// initialize jets
@@ -335,7 +336,7 @@ class FactorAutodiff<FacT, RES, B0, B1, B2, B3, B4, B5, B6, B7, B8, B9, B10, B11
const ProcessorBasePtr& _processor_ptr,
const std::vector<StateBlockPtr>& _state_ptrs,
bool _apply_loss_function,
- FactorStatus _status = FAC_ACTIVE)
+ FactorStatus _status = FAC_ACTIVE)
: FactorBase(_type,
_top,
JAC_AUTO,
@@ -355,7 +356,8 @@ class FactorAutodiff<FacT, RES, B0, B1, B2, B3, B4, B5, B6, B7, B8, B9, B10, B11
for (auto i = 0; i < n_blocks; i++)
{
assert(_state_ptrs.at(i) && ("FactorAutodiff: nullptr state block " + toString(i)).c_str());
- assert(state_block_sizes_.at(i) == sizes_template.at(i) && ("FactorAutodiff: inconsistent state block size " + toString(i)).c_str());
+ assert(state_block_sizes_.at(i) == sizes_template.at(i) &&
+ ("FactorAutodiff: inconsistent state block size " + toString(i)).c_str());
}
// initialize jets
@@ -561,7 +563,7 @@ class FactorAutodiff<FacT, RES, B0, B1, B2, B3, B4, B5, B6, B7, B8, B9, B10, B11
const ProcessorBasePtr& _processor_ptr,
const std::vector<StateBlockPtr>& _state_ptrs,
bool _apply_loss_function,
- FactorStatus _status = FAC_ACTIVE)
+ FactorStatus _status = FAC_ACTIVE)
: FactorBase(_type,
_top,
JAC_AUTO,
@@ -581,9 +583,10 @@ class FactorAutodiff<FacT, RES, B0, B1, B2, B3, B4, B5, B6, B7, B8, B9, B10, B11
for (auto i = 0; i < n_blocks; i++)
{
assert(_state_ptrs.at(i) && ("FactorAutodiff: nullptr state block " + toString(i)).c_str());
- assert(state_block_sizes_.at(i) == sizes_template.at(i) && ("FactorAutodiff: inconsistent state block size " + toString(i)).c_str());
+ assert(state_block_sizes_.at(i) == sizes_template.at(i) &&
+ ("FactorAutodiff: inconsistent state block size " + toString(i)).c_str());
}
-
+
// initialize jets
unsigned int last_jet_idx = 0;
for (unsigned int i = 0; i < B0; i++) jets_0_[i] = WolfJet(0, last_jet_idx++);
@@ -780,7 +783,7 @@ class FactorAutodiff<FacT, RES, B0, B1, B2, B3, B4, B5, B6, B7, B8, B9, B10, B11
const ProcessorBasePtr& _processor_ptr,
const std::vector<StateBlockPtr>& _state_ptrs,
bool _apply_loss_function,
- FactorStatus _status = FAC_ACTIVE)
+ FactorStatus _status = FAC_ACTIVE)
: FactorBase(_type,
_top,
JAC_AUTO,
@@ -800,9 +803,10 @@ class FactorAutodiff<FacT, RES, B0, B1, B2, B3, B4, B5, B6, B7, B8, B9, B10, B11
for (auto i = 0; i < n_blocks; i++)
{
assert(_state_ptrs.at(i) && ("FactorAutodiff: nullptr state block " + toString(i)).c_str());
- assert(state_block_sizes_.at(i) == sizes_template.at(i) && ("FactorAutodiff: inconsistent state block size " + toString(i)).c_str());
+ assert(state_block_sizes_.at(i) == sizes_template.at(i) &&
+ ("FactorAutodiff: inconsistent state block size " + toString(i)).c_str());
}
-
+
// initialize jets
unsigned int last_jet_idx = 0;
for (unsigned int i = 0; i < B0; i++) jets_0_[i] = WolfJet(0, last_jet_idx++);
@@ -992,7 +996,7 @@ class FactorAutodiff<FacT, RES, B0, B1, B2, B3, B4, B5, B6, B7, B8, B9, B10, 0,
const ProcessorBasePtr& _processor_ptr,
const std::vector<StateBlockPtr>& _state_ptrs,
bool _apply_loss_function,
- FactorStatus _status = FAC_ACTIVE)
+ FactorStatus _status = FAC_ACTIVE)
: FactorBase(_type,
_top,
JAC_AUTO,
@@ -1012,9 +1016,10 @@ class FactorAutodiff<FacT, RES, B0, B1, B2, B3, B4, B5, B6, B7, B8, B9, B10, 0,
for (auto i = 0; i < n_blocks; i++)
{
assert(_state_ptrs.at(i) && ("FactorAutodiff: nullptr state block " + toString(i)).c_str());
- assert(state_block_sizes_.at(i) == sizes_template.at(i) && ("FactorAutodiff: inconsistent state block size " + toString(i)).c_str());
+ assert(state_block_sizes_.at(i) == sizes_template.at(i) &&
+ ("FactorAutodiff: inconsistent state block size " + toString(i)).c_str());
}
-
+
// initialize jets
unsigned int last_jet_idx = 0;
for (unsigned int i = 0; i < B0; i++) jets_0_[i] = WolfJet(0, last_jet_idx++);
@@ -1197,7 +1202,7 @@ class FactorAutodiff<FacT, RES, B0, B1, B2, B3, B4, B5, B6, B7, B8, B9, 0, 0, 0,
const ProcessorBasePtr& _processor_ptr,
const std::vector<StateBlockPtr>& _state_ptrs,
bool _apply_loss_function,
- FactorStatus _status = FAC_ACTIVE)
+ FactorStatus _status = FAC_ACTIVE)
: FactorBase(_type,
_top,
JAC_AUTO,
@@ -1217,9 +1222,10 @@ class FactorAutodiff<FacT, RES, B0, B1, B2, B3, B4, B5, B6, B7, B8, B9, 0, 0, 0,
for (auto i = 0; i < n_blocks; i++)
{
assert(_state_ptrs.at(i) && ("FactorAutodiff: nullptr state block " + toString(i)).c_str());
- assert(state_block_sizes_.at(i) == sizes_template.at(i) && ("FactorAutodiff: inconsistent state block size " + toString(i)).c_str());
+ assert(state_block_sizes_.at(i) == sizes_template.at(i) &&
+ ("FactorAutodiff: inconsistent state block size " + toString(i)).c_str());
}
-
+
// initialize jets
unsigned int last_jet_idx = 0;
for (unsigned int i = 0; i < B0; i++) jets_0_[i] = WolfJet(0, last_jet_idx++);
@@ -1395,7 +1401,7 @@ class FactorAutodiff<FacT, RES, B0, B1, B2, B3, B4, B5, B6, B7, B8, 0, 0, 0, 0,
const ProcessorBasePtr& _processor_ptr,
const std::vector<StateBlockPtr>& _state_ptrs,
bool _apply_loss_function,
- FactorStatus _status = FAC_ACTIVE)
+ FactorStatus _status = FAC_ACTIVE)
: FactorBase(_type,
_top,
JAC_AUTO,
@@ -1415,9 +1421,10 @@ class FactorAutodiff<FacT, RES, B0, B1, B2, B3, B4, B5, B6, B7, B8, 0, 0, 0, 0,
for (auto i = 0; i < n_blocks; i++)
{
assert(_state_ptrs.at(i) && ("FactorAutodiff: nullptr state block " + toString(i)).c_str());
- assert(state_block_sizes_.at(i) == sizes_template.at(i) && ("FactorAutodiff: inconsistent state block size " + toString(i)).c_str());
+ assert(state_block_sizes_.at(i) == sizes_template.at(i) &&
+ ("FactorAutodiff: inconsistent state block size " + toString(i)).c_str());
}
-
+
// initialize jets
unsigned int last_jet_idx = 0;
for (unsigned int i = 0; i < B0; i++) jets_0_[i] = WolfJet(0, last_jet_idx++);
@@ -1586,7 +1593,7 @@ class FactorAutodiff<FacT, RES, B0, B1, B2, B3, B4, B5, B6, B7, 0, 0, 0, 0, 0, 0
const ProcessorBasePtr& _processor_ptr,
const std::vector<StateBlockPtr>& _state_ptrs,
bool _apply_loss_function,
- FactorStatus _status = FAC_ACTIVE)
+ FactorStatus _status = FAC_ACTIVE)
: FactorBase(_type,
_top,
JAC_AUTO,
@@ -1606,9 +1613,10 @@ class FactorAutodiff<FacT, RES, B0, B1, B2, B3, B4, B5, B6, B7, 0, 0, 0, 0, 0, 0
for (auto i = 0; i < n_blocks; i++)
{
assert(_state_ptrs.at(i) && ("FactorAutodiff: nullptr state block " + toString(i)).c_str());
- assert(state_block_sizes_.at(i) == sizes_template.at(i) && ("FactorAutodiff: inconsistent state block size " + toString(i)).c_str());
+ assert(state_block_sizes_.at(i) == sizes_template.at(i) &&
+ ("FactorAutodiff: inconsistent state block size " + toString(i)).c_str());
}
-
+
// initialize jets
unsigned int last_jet_idx = 0;
for (unsigned int i = 0; i < B0; i++) jets_0_[i] = WolfJet(0, last_jet_idx++);
@@ -1762,7 +1770,7 @@ class FactorAutodiff<FacT, RES, B0, B1, B2, B3, B4, B5, B6, 0, 0, 0, 0, 0, 0, 0,
const ProcessorBasePtr& _processor_ptr,
const std::vector<StateBlockPtr>& _state_ptrs,
bool _apply_loss_function,
- FactorStatus _status = FAC_ACTIVE)
+ FactorStatus _status = FAC_ACTIVE)
: FactorBase(_type,
_top,
JAC_AUTO,
@@ -1782,9 +1790,10 @@ class FactorAutodiff<FacT, RES, B0, B1, B2, B3, B4, B5, B6, 0, 0, 0, 0, 0, 0, 0,
for (auto i = 0; i < n_blocks; i++)
{
assert(_state_ptrs.at(i) && ("FactorAutodiff: nullptr state block " + toString(i)).c_str());
- assert(state_block_sizes_.at(i) == sizes_template.at(i) && ("FactorAutodiff: inconsistent state block size " + toString(i)).c_str());
+ assert(state_block_sizes_.at(i) == sizes_template.at(i) &&
+ ("FactorAutodiff: inconsistent state block size " + toString(i)).c_str());
}
-
+
// initialize jets
unsigned int last_jet_idx = 0;
for (unsigned int i = 0; i < B0; i++) jets_0_[i] = WolfJet(0, last_jet_idx++);
@@ -1932,7 +1941,7 @@ class FactorAutodiff<FacT, RES, B0, B1, B2, B3, B4, B5, 0, 0, 0, 0, 0, 0, 0, 0,
const ProcessorBasePtr& _processor_ptr,
const std::vector<StateBlockPtr>& _state_ptrs,
bool _apply_loss_function,
- FactorStatus _status = FAC_ACTIVE)
+ FactorStatus _status = FAC_ACTIVE)
: FactorBase(_type,
_top,
JAC_AUTO,
@@ -1952,9 +1961,10 @@ class FactorAutodiff<FacT, RES, B0, B1, B2, B3, B4, B5, 0, 0, 0, 0, 0, 0, 0, 0,
for (auto i = 0; i < n_blocks; i++)
{
assert(_state_ptrs.at(i) && ("FactorAutodiff: nullptr state block " + toString(i)).c_str());
- assert(state_block_sizes_.at(i) == sizes_template.at(i) && ("FactorAutodiff: inconsistent state block size " + toString(i)).c_str());
+ assert(state_block_sizes_.at(i) == sizes_template.at(i) &&
+ ("FactorAutodiff: inconsistent state block size " + toString(i)).c_str());
}
-
+
// initialize jets
unsigned int last_jet_idx = 0;
for (unsigned int i = 0; i < B0; i++) jets_0_[i] = WolfJet(0, last_jet_idx++);
@@ -2096,7 +2106,7 @@ class FactorAutodiff<FacT, RES, B0, B1, B2, B3, B4, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0
const ProcessorBasePtr& _processor_ptr,
const std::vector<StateBlockPtr>& _state_ptrs,
bool _apply_loss_function,
- FactorStatus _status = FAC_ACTIVE)
+ FactorStatus _status = FAC_ACTIVE)
: FactorBase(_type,
_top,
JAC_AUTO,
@@ -2116,9 +2126,10 @@ class FactorAutodiff<FacT, RES, B0, B1, B2, B3, B4, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0
for (auto i = 0; i < n_blocks; i++)
{
assert(_state_ptrs.at(i) && ("FactorAutodiff: nullptr state block " + toString(i)).c_str());
- assert(state_block_sizes_.at(i) == sizes_template.at(i) && ("FactorAutodiff: inconsistent state block size " + toString(i)).c_str());
+ assert(state_block_sizes_.at(i) == sizes_template.at(i) &&
+ ("FactorAutodiff: inconsistent state block size " + toString(i)).c_str());
}
-
+
// initialize jets
unsigned int last_jet_idx = 0;
for (unsigned int i = 0; i < B0; i++) jets_0_[i] = WolfJet(0, last_jet_idx++);
@@ -2244,7 +2255,7 @@ class FactorAutodiff<FacT, RES, B0, B1, B2, B3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0>
const ProcessorBasePtr& _processor_ptr,
const std::vector<StateBlockPtr>& _state_ptrs,
bool _apply_loss_function,
- FactorStatus _status = FAC_ACTIVE)
+ FactorStatus _status = FAC_ACTIVE)
: FactorBase(_type,
_top,
JAC_AUTO,
@@ -2264,9 +2275,10 @@ class FactorAutodiff<FacT, RES, B0, B1, B2, B3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0>
for (auto i = 0; i < n_blocks; i++)
{
assert(_state_ptrs.at(i) && ("FactorAutodiff: nullptr state block " + toString(i)).c_str());
- assert(state_block_sizes_.at(i) == sizes_template.at(i) && ("FactorAutodiff: inconsistent state block size " + toString(i)).c_str());
+ assert(state_block_sizes_.at(i) == sizes_template.at(i) &&
+ ("FactorAutodiff: inconsistent state block size " + toString(i)).c_str());
}
-
+
// initialize jets
unsigned int last_jet_idx = 0;
for (unsigned int i = 0; i < B0; i++) jets_0_[i] = WolfJet(0, last_jet_idx++);
@@ -2385,7 +2397,7 @@ class FactorAutodiff<FacT, RES, B0, B1, B2, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0>
const ProcessorBasePtr& _processor_ptr,
const std::vector<StateBlockPtr>& _state_ptrs,
bool _apply_loss_function,
- FactorStatus _status = FAC_ACTIVE)
+ FactorStatus _status = FAC_ACTIVE)
: FactorBase(_type,
_top,
JAC_AUTO,
@@ -2405,9 +2417,10 @@ class FactorAutodiff<FacT, RES, B0, B1, B2, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0>
for (auto i = 0; i < n_blocks; i++)
{
assert(_state_ptrs.at(i) && ("FactorAutodiff: nullptr state block " + toString(i)).c_str());
- assert(state_block_sizes_.at(i) == sizes_template.at(i) && ("FactorAutodiff: inconsistent state block size " + toString(i)).c_str());
+ assert(state_block_sizes_.at(i) == sizes_template.at(i) &&
+ ("FactorAutodiff: inconsistent state block size " + toString(i)).c_str());
}
-
+
// initialize jets
unsigned int last_jet_idx = 0;
for (unsigned int i = 0; i < B0; i++) jets_0_[i] = WolfJet(0, last_jet_idx++);
@@ -2521,7 +2534,7 @@ class FactorAutodiff<FacT, RES, B0, B1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0> :
const ProcessorBasePtr& _processor_ptr,
const std::vector<StateBlockPtr>& _state_ptrs,
bool _apply_loss_function,
- FactorStatus _status = FAC_ACTIVE)
+ FactorStatus _status = FAC_ACTIVE)
: FactorBase(_type,
_top,
JAC_AUTO,
@@ -2541,9 +2554,10 @@ class FactorAutodiff<FacT, RES, B0, B1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0> :
for (auto i = 0; i < n_blocks; i++)
{
assert(_state_ptrs.at(i) && ("FactorAutodiff: nullptr state block " + toString(i)).c_str());
- assert(state_block_sizes_.at(i) == sizes_template.at(i) && ("FactorAutodiff: inconsistent state block size " + toString(i)).c_str());
+ assert(state_block_sizes_.at(i) == sizes_template.at(i) &&
+ ("FactorAutodiff: inconsistent state block size " + toString(i)).c_str());
}
-
+
// initialize jets
unsigned int last_jet_idx = 0;
for (unsigned int i = 0; i < B0; i++) jets_0_[i] = WolfJet(0, last_jet_idx++);
@@ -2654,7 +2668,7 @@ class FactorAutodiff<FacT, RES, B0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0> :
const ProcessorBasePtr& _processor_ptr,
const std::vector<StateBlockPtr>& _state_ptrs,
bool _apply_loss_function,
- FactorStatus _status = FAC_ACTIVE)
+ FactorStatus _status = FAC_ACTIVE)
: FactorBase(_type,
_top,
JAC_AUTO,
@@ -2674,9 +2688,10 @@ class FactorAutodiff<FacT, RES, B0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0> :
for (auto i = 0; i < n_blocks; i++)
{
assert(_state_ptrs.at(i) && ("FactorAutodiff: nullptr state block " + toString(i)).c_str());
- assert(state_block_sizes_.at(i) == sizes_template.at(i) && ("FactorAutodiff: inconsistent state block size " + toString(i)).c_str());
+ assert(state_block_sizes_.at(i) == sizes_template.at(i) &&
+ ("FactorAutodiff: inconsistent state block size " + toString(i)).c_str());
}
-
+
// initialize jets
unsigned int last_jet_idx = 0;
for (unsigned int i = 0; i < B0; i++) jets_0_[i] = WolfJet(0, last_jet_idx++);
diff --git a/include/core/factor/factor_base.h b/include/core/factor/factor_base.h
index 11ba8567be861415958afe98d95dde8e42487d5e..af51ee8db33170c8b72b943ecd06b4fae523532b 100644
--- a/include/core/factor/factor_base.h
+++ b/include/core/factor/factor_base.h
@@ -277,7 +277,7 @@ class FactorBase : public NodeBase, public std::enable_shared_from_this<FactorBa
bool _metric,
bool _state_blocks,
std::ostream& _stream = std::cout,
- std::string _tabs = "") const;
+ std::string _tabs = "") const;
virtual CheckLog localCheck(bool _verbose, std::ostream& _stream, std::string _tabs = "") const;
bool check(CheckLog& _log, bool _verbose, std::ostream& _stream, std::string _tabs = "") const;
diff --git a/include/core/factor/factor_block_absolute.h b/include/core/factor/factor_block_absolute.h
index aa3af2b3db9bfe6ede5611bba0882da5369921b0..afc9299a97e51845639505a620069a64ddb96a53 100644
--- a/include/core/factor/factor_block_absolute.h
+++ b/include/core/factor/factor_block_absolute.h
@@ -31,7 +31,6 @@
namespace wolf
{
-
WOLF_PTR_TYPEDEFS(FactorBlockAbsolute);
// class
@@ -91,8 +90,7 @@ class FactorBlockAbsolute : public FactorAnalytic
_node_state_blocks->getStateBlock(_key)->getSize());
assert(sb_constrained_size_ + sb_constrained_start_ <= sb_size_);
assert(sb_constrained_size_ > 0);
- assert(_measurement.size() == sb_constrained_size_ and
- "FactorBlockAbsolute: _measurement wrong size");
+ assert(_measurement.size() == sb_constrained_size_ and "FactorBlockAbsolute: _measurement wrong size");
assert(_measurement_sqrt_information_upper.rows() == sb_constrained_size_ and
_measurement_sqrt_information_upper.cols() == sb_constrained_size_ and
"FactorBlockAbsolute: _measurement_sqrt_information_upper wrong size");
diff --git a/include/core/factor/factor_block_difference.h b/include/core/factor/factor_block_difference.h
index 809c99612eb3bfba4c48b072ebef731453ba2f0a..b5087cb17f4f0321e9608a581b5faddd4a4c8eb8 100644
--- a/include/core/factor/factor_block_difference.h
+++ b/include/core/factor/factor_block_difference.h
@@ -28,7 +28,6 @@
namespace wolf
{
-
WOLF_PTR_TYPEDEFS(FactorBlockDifference);
// class
diff --git a/include/core/factor/factor_diff_drive.h b/include/core/factor/factor_diff_drive.h
index 4ab23b0a0db6b4c095f1e782ed378df5a037a1e2..b7546fdecd58343de3f826b10baf7d48600d4bbb 100644
--- a/include/core/factor/factor_diff_drive.h
+++ b/include/core/factor/factor_diff_drive.h
@@ -29,7 +29,6 @@
namespace
{
-
constexpr std::size_t POSITION_SIZE = 2;
constexpr std::size_t ORIENTATION_SIZE = 1;
constexpr std::size_t INTRINSICS_SIZE = 3;
@@ -39,7 +38,6 @@ constexpr std::size_t RESIDUAL_SIZE = 3;
namespace wolf
{
-
WOLF_PTR_TYPEDEFS(FactorDiffDrive);
class FactorDiffDrive : public FactorAutodiff<FactorDiffDrive,
diff --git a/include/core/factor/factor_distance_3d.h b/include/core/factor/factor_distance_3d.h
index 67816af683860a0c97ff975c8153c8cb36e2baa4..7ed72d0f6e8049a07a4c20cc66e316fb7e3f3d93 100644
--- a/include/core/factor/factor_distance_3d.h
+++ b/include/core/factor/factor_distance_3d.h
@@ -24,7 +24,6 @@
namespace wolf
{
-
WOLF_PTR_TYPEDEFS(FactorDistance3d);
class FactorDistance3d : public FactorAutodiff<FactorDistance3d, 1, 3, 3>
diff --git a/include/core/factor/factor_pose_2d.h b/include/core/factor/factor_pose_2d.h
index 2510db856f9d2b7c0027f399ee9e621d5f2d495a..3df6484ec95519cebcd21be8b718324b3ee91833 100644
--- a/include/core/factor/factor_pose_2d.h
+++ b/include/core/factor/factor_pose_2d.h
@@ -29,7 +29,6 @@
namespace wolf
{
-
WOLF_PTR_TYPEDEFS(FactorPose2d);
// class
diff --git a/include/core/factor/factor_pose_2d_with_extrinsics.h b/include/core/factor/factor_pose_2d_with_extrinsics.h
index 69901d3c93110ab30a49994ddaa9aa2dce90dff3..6b59bab3fd7a1996b3e932c12d25e016d9eb2b47 100644
--- a/include/core/factor/factor_pose_2d_with_extrinsics.h
+++ b/include/core/factor/factor_pose_2d_with_extrinsics.h
@@ -29,7 +29,6 @@
namespace wolf
{
-
WOLF_PTR_TYPEDEFS(FactorPose2dWithExtrinsics);
// class
@@ -43,16 +42,15 @@ class FactorPose2dWithExtrinsics : public FactorAutodiff<FactorPose2dWithExtrins
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function,
FactorStatus _status = FAC_ACTIVE)
- : FactorAutodiff(
- "FactorPose2dWithExtrinsics",
- TOP_ABS,
- _measurement,
- _measurement_sqrt_information_upper,
- {_frame_ptr, _sensor_ptr},
- _processor_ptr,
- {_frame_ptr->getP(), _frame_ptr->getO(), _sensor_ptr->getP(), _sensor_ptr->getO()},
- _apply_loss_function,
- _status)
+ : FactorAutodiff("FactorPose2dWithExtrinsics",
+ TOP_ABS,
+ _measurement,
+ _measurement_sqrt_information_upper,
+ {_frame_ptr, _sensor_ptr},
+ _processor_ptr,
+ {_frame_ptr->getP(), _frame_ptr->getO(), _sensor_ptr->getP(), _sensor_ptr->getO()},
+ _apply_loss_function,
+ _status)
{
assert(_frame_ptr->getP() && "FactorPose2dWithExtrinsics: _frame_ptr state P not found!");
assert(_frame_ptr->getO() && "FactorPose2dWithExtrinsics: _frame_ptr state O not found!");
diff --git a/include/core/factor/factor_pose_3d.h b/include/core/factor/factor_pose_3d.h
index ca967a87e01e59647024ef8acde5976703d3dc39..a3a18e589ed50651bd15a46d503a1985a95d4aa1 100644
--- a/include/core/factor/factor_pose_3d.h
+++ b/include/core/factor/factor_pose_3d.h
@@ -27,7 +27,6 @@
namespace wolf
{
-
WOLF_PTR_TYPEDEFS(FactorPose3d);
// class
diff --git a/include/core/factor/factor_pose_3d_with_extrinsics.h b/include/core/factor/factor_pose_3d_with_extrinsics.h
index 1bcae3a421a725452b6135c12a5cd7f439eade20..aa3e57ebe4b15f63a3e7c5c0eb2d84d461e65f39 100644
--- a/include/core/factor/factor_pose_3d_with_extrinsics.h
+++ b/include/core/factor/factor_pose_3d_with_extrinsics.h
@@ -29,7 +29,6 @@
namespace wolf
{
-
WOLF_PTR_TYPEDEFS(FactorPose3dWithExtrinsics);
// class
@@ -43,16 +42,15 @@ class FactorPose3dWithExtrinsics : public FactorAutodiff<FactorPose3dWithExtrins
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function,
FactorStatus _status = FAC_ACTIVE)
- : FactorAutodiff(
- "FactorPose3dWithExtrinsics",
- TOP_ABS,
- _measurement,
- _measurement_sqrt_information_upper,
- {_frame_ptr, _sensor_ptr},
- _processor_ptr,
- {_frame_ptr->getP(), _frame_ptr->getO(), _sensor_ptr->getP(), _sensor_ptr->getO()},
- _apply_loss_function,
- _status)
+ : FactorAutodiff("FactorPose3dWithExtrinsics",
+ TOP_ABS,
+ _measurement,
+ _measurement_sqrt_information_upper,
+ {_frame_ptr, _sensor_ptr},
+ _processor_ptr,
+ {_frame_ptr->getP(), _frame_ptr->getO(), _sensor_ptr->getP(), _sensor_ptr->getO()},
+ _apply_loss_function,
+ _status)
{
assert(_frame_ptr->getP() && "FactorPose3dWithExtrinsics: _frame_ptr state P not found!");
assert(_frame_ptr->getO() && "FactorPose3dWithExtrinsics: _frame_ptr state O not found!");
diff --git a/include/core/factor/factor_quaternion_absolute.h b/include/core/factor/factor_quaternion_absolute.h
index 3f0fdcb1f23b749c710ccdf8edf5eec8c6053a97..116e6eb8e52e6a9f64d3e429d8f9243ab2d5fb0c 100644
--- a/include/core/factor/factor_quaternion_absolute.h
+++ b/include/core/factor/factor_quaternion_absolute.h
@@ -28,7 +28,6 @@
namespace wolf
{
-
WOLF_PTR_TYPEDEFS(FactorQuaternionAbsolute);
// class
diff --git a/include/core/factor/factor_relative_pose_3d.h b/include/core/factor/factor_relative_pose_3d.h
index df4e20cfaf2508120af48d08e795f128fa0ec43d..43bc6db56a48724b34c4f8aeb52744cbc97b0e9b 100644
--- a/include/core/factor/factor_relative_pose_3d.h
+++ b/include/core/factor/factor_relative_pose_3d.h
@@ -25,7 +25,6 @@
namespace wolf
{
-
WOLF_PTR_TYPEDEFS(FactorRelativePose3d);
// class
diff --git a/include/core/feature/feature_base.h b/include/core/feature/feature_base.h
index 67f0794c54ed716bb9fa16489422bf98b036caaa..8c85bca406686a92800bd7015849996b032a0561 100644
--- a/include/core/feature/feature_base.h
+++ b/include/core/feature/feature_base.h
@@ -35,7 +35,6 @@ class FactorBase;
namespace wolf
{
-
// class FeatureBase
class FeatureBase : public NodeBase, public std::enable_shared_from_this<FeatureBase>
{
@@ -144,15 +143,15 @@ class FeatureBase : public NodeBase, public std::enable_shared_from_this<Feature
template <typename classType, typename... T>
static std::shared_ptr<classType> emplace(CaptureBasePtr _cpt_ptr, T&&... all);
- virtual void printHeader(int depth, //
+ virtual void printHeader(int depth, //
bool factored_by, //
- bool metric, //
+ bool metric, //
bool state_blocks,
std::ostream& stream,
std::string _tabs = "") const;
- void print(int depth, //
+ void print(int depth, //
bool factored_by, //
- bool metric, //
+ bool metric, //
bool state_blocks,
std::ostream& stream = std::cout,
std::string _tabs = "") const;
@@ -177,7 +176,6 @@ class FeatureBase : public NodeBase, public std::enable_shared_from_this<Feature
namespace wolf
{
-
template <typename classType, typename... T>
std::shared_ptr<classType> FeatureBase::emplace(CaptureBasePtr _cpt_ptr, T&&... all)
{
diff --git a/include/core/feature/feature_diff_drive.h b/include/core/feature/feature_diff_drive.h
index 3e8a84ca06f25660a8e8ce1bf2c9796a66b17154..d7314441d34c5c1205679568f9630b581299d706 100644
--- a/include/core/feature/feature_diff_drive.h
+++ b/include/core/feature/feature_diff_drive.h
@@ -20,24 +20,22 @@
#pragma once
-//Wolf includes
+// Wolf includes
#include "core/feature/feature_motion.h"
-namespace wolf {
-
+namespace wolf
+{
WOLF_PTR_TYPEDEFS(FeatureDiffDrive)
class FeatureDiffDrive : public FeatureMotion
{
- public:
-
- FeatureDiffDrive(const Eigen::VectorXd& _delta_preintegrated,
- const Eigen::MatrixXd& _delta_preintegrated_covariance,
- const Eigen::VectorXd& _diff_drive_params,
- const Eigen::MatrixXd& _jacobian_diff_drive_params);
-
- virtual ~FeatureDiffDrive() = default;
+ public:
+ FeatureDiffDrive(const Eigen::VectorXd& _delta_preintegrated,
+ const Eigen::MatrixXd& _delta_preintegrated_covariance,
+ const Eigen::VectorXd& _diff_drive_params,
+ const Eigen::MatrixXd& _jacobian_diff_drive_params);
+ virtual ~FeatureDiffDrive() = default;
};
} /* namespace wolf */
diff --git a/include/core/feature/feature_match.h b/include/core/feature/feature_match.h
index 034cc3ac508f8f2d84b135d46f46c845697a8783..5cc32a60af5da015b2e87f4f5619383e8cd3ce01 100644
--- a/include/core/feature/feature_match.h
+++ b/include/core/feature/feature_match.h
@@ -23,10 +23,10 @@
// Wolf includes
#include "core/common/wolf.h"
-//wolf nampseace
-namespace wolf {
-
-//forward declaration to typedef class pointers
+// wolf nampseace
+namespace wolf
+{
+// forward declaration to typedef class pointers
WOLF_STRUCT_PTR_TYPEDEFS(FeatureMatch);
/** \brief Map of feature matches
@@ -36,8 +36,8 @@ WOLF_STRUCT_PTR_TYPEDEFS(FeatureMatch);
* map<FeatureBasePtr actual_feature, FeatureMatchPtr corresponding_feature_match>
*
*/
-typedef std::map<FeatureBasePtr, FeatureMatchPtr> FeatureMatchMap; //a map is also typedefined
-
+typedef std::map<FeatureBasePtr, FeatureMatchPtr> FeatureMatchMap; // a map is also typedefined
+
/** \brief Match between a feature and a feature
*
* Match between a feature and a feature (feature-feature correspondence)
@@ -45,8 +45,8 @@ typedef std::map<FeatureBasePtr, FeatureMatchPtr> FeatureMatchMap; //a map is al
*/
struct FeatureMatch
{
- FeatureBasePtr feature_ptr_; ///< Corresponding feature
- double normalized_score_; ///< normalized similarity score (0 is bad, 1 is good)
+ FeatureBasePtr feature_ptr_; ///< Corresponding feature
+ double normalized_score_; ///< normalized similarity score (0 is bad, 1 is good)
};
-}//end namespace
+} // namespace wolf
diff --git a/include/core/feature/feature_motion.h b/include/core/feature/feature_motion.h
index 96da75694f82a1c74f0f513c3989f2683cfc9ff7..8675f4c6a332a785312fd8ba384553f9979ca760 100644
--- a/include/core/feature/feature_motion.h
+++ b/include/core/feature/feature_motion.h
@@ -25,26 +25,25 @@
namespace wolf
{
-
WOLF_PTR_TYPEDEFS(FeatureMotion);
class FeatureMotion : public FeatureBase
{
- public:
- FeatureMotion(const std::string& _type,
- const VectorXd& _delta_preint,
- const MatrixXd _delta_preint_cov,
- const VectorXd& _calib_preint,
- const MatrixXd& _jacobian_calib);
- ~FeatureMotion() override;
+ public:
+ FeatureMotion(const std::string& _type,
+ const VectorXd& _delta_preint,
+ const MatrixXd _delta_preint_cov,
+ const VectorXd& _calib_preint,
+ const MatrixXd& _jacobian_calib);
+ ~FeatureMotion() override;
- const Eigen::VectorXd& getDeltaPreint() const; ///< A new name for getMeasurement()
- const Eigen::VectorXd& getCalibrationPreint() const;
- const Eigen::MatrixXd& getJacobianCalibration() const;
+ const Eigen::VectorXd& getDeltaPreint() const; ///< A new name for getMeasurement()
+ const Eigen::VectorXd& getCalibrationPreint() const;
+ const Eigen::MatrixXd& getJacobianCalibration() const;
- private:
- Eigen::VectorXd calib_preint_;
- Eigen::MatrixXd jacobian_calib_;
+ private:
+ Eigen::VectorXd calib_preint_;
+ Eigen::MatrixXd jacobian_calib_;
};
inline const Eigen::VectorXd& FeatureMotion::getDeltaPreint() const
diff --git a/include/core/feature/feature_odom_2d.h b/include/core/feature/feature_odom_2d.h
index 075e82bc7c152b83fd481fa766ec953942b906fa..f647522a22ea06d49376b47008126a2f91072d45 100644
--- a/include/core/feature/feature_odom_2d.h
+++ b/include/core/feature/feature_odom_2d.h
@@ -20,29 +20,28 @@
#pragma once
-//Wolf includes
+// Wolf includes
#include "core/feature/feature_base.h"
-//std includes
-
-namespace wolf {
+// std includes
+namespace wolf
+{
WOLF_PTR_TYPEDEFS(FeatureOdom2d);
-
-//class FeatureOdom2d
+
+// class FeatureOdom2d
class FeatureOdom2d : public FeatureBase
{
- public:
-
- /** \brief Constructor from capture pointer and measure
- *
- * \param _measurement the measurement
- * \param _meas_covariance the noise of the measurement
- *
- */
- FeatureOdom2d(const Eigen::VectorXd& _measurement, const Eigen::MatrixXd& _meas_covariance);
-
- ~FeatureOdom2d() override;
+ public:
+ /** \brief Constructor from capture pointer and measure
+ *
+ * \param _measurement the measurement
+ * \param _meas_covariance the noise of the measurement
+ *
+ */
+ FeatureOdom2d(const Eigen::VectorXd& _measurement, const Eigen::MatrixXd& _meas_covariance);
+
+ ~FeatureOdom2d() override;
};
-} // namespace wolf
+} // namespace wolf
diff --git a/include/core/feature/feature_pose.h b/include/core/feature/feature_pose.h
index 8479a8e146bcc9948ec86172f6b0dba35318f53a..bd9c547ac44c53ebb495fde8c55a48c193e5d5fc 100644
--- a/include/core/feature/feature_pose.h
+++ b/include/core/feature/feature_pose.h
@@ -20,29 +20,28 @@
#pragma once
-//Wolf includes
+// Wolf includes
#include "core/feature/feature_base.h"
-//std includes
+// std includes
//
-namespace wolf {
-
+namespace wolf
+{
WOLF_PTR_TYPEDEFS(FeaturePose);
-//class FeaturePose
+// class FeaturePose
class FeaturePose : public FeatureBase
{
- public:
-
- /** \brief Constructor from capture pointer and measure
- *
- * \param _measurement the measurement
- * \param _meas_covariance the noise of the measurement
- *
- */
- FeaturePose(const Eigen::VectorXd& _measurement, const Eigen::MatrixXd& _meas_covariance);
- ~FeaturePose() override;
+ public:
+ /** \brief Constructor from capture pointer and measure
+ *
+ * \param _measurement the measurement
+ * \param _meas_covariance the noise of the measurement
+ *
+ */
+ FeaturePose(const Eigen::VectorXd& _measurement, const Eigen::MatrixXd& _meas_covariance);
+ ~FeaturePose() override;
};
-} // namespace wolf
+} // namespace wolf
diff --git a/include/core/frame/frame_base.h b/include/core/frame/frame_base.h
index 3472c2be41451f74a930fa07828f332116effb7e..ae9691f84638d1f48c948c70b14264ffd6756a8b 100644
--- a/include/core/frame/frame_base.h
+++ b/include/core/frame/frame_base.h
@@ -159,16 +159,16 @@ class FrameBase : public NodeStateBlocks
CaptureBasePtrList getCapturesOf(SensorBasePtr _sensor_ptr);
// Debug and info -------------------------------------------------------
- virtual void printHeader(int depth, //
+ virtual void printHeader(int depth, //
bool factored_by, //
- bool metric, //
+ bool metric, //
bool state_blocks,
std::ostream& stream,
std::string _tabs = "") const;
- void print(int depth, //
+ void print(int depth, //
bool factored_by, //
- bool metric, //
+ bool metric, //
bool state_blocks,
std::ostream& stream = std::cout,
std::string _tabs = "") const;
diff --git a/include/core/hardware/hardware_base.h b/include/core/hardware/hardware_base.h
index cc9e5516c4203d83e9dfaf78691d4d5f2765d2dd..3bb567ca8380340fcb2fbe2bcf596946b5478406 100644
--- a/include/core/hardware/hardware_base.h
+++ b/include/core/hardware/hardware_base.h
@@ -33,7 +33,6 @@ class SensorBase;
namespace wolf
{
-
// class HardwareBase
class HardwareBase : public NodeBase, public std::enable_shared_from_this<HardwareBase>
{
@@ -56,15 +55,15 @@ class HardwareBase : public NodeBase, public std::enable_shared_from_this<Hardwa
SensorBaseConstPtr getSensor(const std::string& _sensor_name) const;
SensorBasePtr getSensor(const std::string& _sensor_name);
- virtual void printHeader(int depth, //
+ virtual void printHeader(int depth, //
bool factored_by, //
- bool metric, //
+ bool metric, //
bool state_blocks,
std::ostream& stream,
std::string _tabs = "") const;
- void print(int depth, //
+ void print(int depth, //
bool factored_by, //
- bool metric, //
+ bool metric, //
bool state_blocks,
std::ostream& stream = std::cout,
std::string _tabs = "") const;
@@ -82,7 +81,6 @@ class HardwareBase : public NodeBase, public std::enable_shared_from_this<Hardwa
namespace wolf
{
-
inline SensorBaseConstPtrList HardwareBase::getSensorList() const
{
SensorBaseConstPtrList list_const;
diff --git a/include/core/landmark/factory_landmark.h b/include/core/landmark/factory_landmark.h
index b9e14bf698b9a2a5b28c3a11ae2cf8dbf32cba20..cc3c600909aa413c9b03a1cb783be5f1e56fb616 100644
--- a/include/core/landmark/factory_landmark.h
+++ b/include/core/landmark/factory_landmark.h
@@ -28,7 +28,6 @@
namespace wolf
{
-
/** \brief Landmark factory class
*
* This factory can create objects of classes deriving from LandmarkBase.
@@ -56,7 +55,7 @@ namespace wolf
* \code
* static LandmarkBasePtr create(const YAML::Node& _node)
* {
- * return std::make_shared<LandmarkDerived>("LandmarkDerived", _node);
+ * return std::make_shared<LandmarkDerived>("LandmarkDerived", _node);
* }
* \endcode
*
@@ -70,12 +69,13 @@ namespace wolf
* auto corner_ptr = FactoryLandmark::create("LandmarkCorner", _node);
* \endcode
*
- * We RECOMMEND using the macro ````WOLF_LANDMARK_CREATE(LandmarkClass)```` to automatically add the landmark creator, provided in ````landmark_base.h````.
- *
+ * We RECOMMEND using the macro ````WOLF_LANDMARK_CREATE(LandmarkClass)```` to automatically add the landmark creator,
+ * provided in ````landmark_base.h````.
+ *
* To do so, you should implement a constructor with the API:
- *
+ *
* \code
- * LandmarkDerived(const YAML::Node& _node) :
+ * LandmarkDerived(const YAML::Node& _node) :
* LandmarkBase("LandmarkDerived", _node)
* {
* < CODE >
@@ -109,7 +109,7 @@ namespace wolf
* const bool registered_corner = FactoryLandmark::registerCreator("LandmarkCorner", LandmarkCorner::create);
* }
* \endcode
- *
+ *
* Automatic registration is recommended in wolf, and implemented in the classes shipped with it using the macro
* ````WOLF_REGISTER_LANDMARK(LandmarkType)```` in ````landmark_base.h````.
* You are free to comment out these lines and place them wherever you consider it more convenient for your design.
@@ -131,7 +131,7 @@ namespace wolf
* // a YAML node with the info
*
* LandmarkBasePtr corner_1_ptr =
- * FactoryLandmark::create ( "LandmarkCorner",
+ * FactoryLandmark::create ( "LandmarkCorner",
* node );
*
* LandmarkBasePtr corner_2_ptr =
@@ -143,17 +143,19 @@ namespace wolf
* \endcode
*/
-typedef Factory<LandmarkBasePtr,
- const YAML::Node&> FactoryLandmark;
+typedef Factory<LandmarkBasePtr, const YAML::Node&> FactoryLandmark;
-template<>
+template <>
inline std::string FactoryLandmark::getClass() const
{
- return "FactoryLandmark";
+ return "FactoryLandmark";
}
-#define WOLF_REGISTER_LANDMARK(LandmarkType) \
- namespace{ const bool WOLF_UNUSED LandmarkType##Registered = \
- FactoryLandmark::registerCreator(#LandmarkType, LandmarkType::create);} \
+#define WOLF_REGISTER_LANDMARK(LandmarkType) \
+ namespace \
+ { \
+ const bool WOLF_UNUSED LandmarkType##Registered = \
+ FactoryLandmark::registerCreator(#LandmarkType, LandmarkType::create); \
+ }
} /* namespace wolf */
diff --git a/include/core/landmark/landmark_base.h b/include/core/landmark/landmark_base.h
index 2758fa3aad6e68b2beae1fe7ca9650f314ad6617..b11a2ebcf561d1da730c08a080ebe23f3eedc086 100644
--- a/include/core/landmark/landmark_base.h
+++ b/include/core/landmark/landmark_base.h
@@ -54,7 +54,7 @@ namespace wolf
static LandmarkBasePtr create(const YAML::Node& _node) \
{ \
auto lmk = std::make_shared<LandmarkClass>(_node); \
- lmk->applyPriors(SpecStateComposite(_node["states"])); \
+ lmk->emplacePriors(SpecStateComposite(_node["states"])); \
return lmk; \
}
@@ -123,16 +123,16 @@ class LandmarkBase : public NodeStateBlocks
template <typename classType, typename... T>
static std::shared_ptr<classType> emplace(MapBasePtr _map_ptr, T&&... all);
- virtual void printHeader(int depth, //
+ virtual void printHeader(int depth, //
bool factored_by, //
- bool metric, //
+ bool metric, //
bool state_blocks,
std::ostream& stream,
std::string _tabs = "") const;
- void print(int depth, //
+ void print(int depth, //
bool factored_by, //
- bool metric, //
+ bool metric, //
bool state_blocks,
std::ostream& stream = std::cout,
std::string _tabs = "") const;
diff --git a/include/core/landmark/landmark_match.h b/include/core/landmark/landmark_match.h
index 072494273bd2d7979f4d3b970a65cdcea8689e6c..5132e25bf3327b6811ab3c58155ebfa2f06a48c9 100644
--- a/include/core/landmark/landmark_match.h
+++ b/include/core/landmark/landmark_match.h
@@ -23,9 +23,9 @@
// Wolf includes
#include "core/common/wolf.h"
-//wolf nampseace
-namespace wolf {
-
+// wolf nampseace
+namespace wolf
+{
// Map of Feature - Landmark matches
WOLF_STRUCT_PTR_TYPEDEFS(LandmarkMatch);
typedef std::map<FeatureBasePtr, LandmarkMatchPtr> LandmarkMatchMap;
@@ -37,19 +37,14 @@ typedef std::map<FeatureBasePtr, LandmarkMatchPtr> LandmarkMatchMap;
**/
struct LandmarkMatch
{
- LandmarkBasePtr landmark_ptr_; ///< Pointer to the matched landmark
- double normalized_score_; ///< Similarity measure: 0: no match -- 1: perfect match
-
- LandmarkMatch() :
- landmark_ptr_(nullptr), normalized_score_(0)
- {
+ LandmarkBasePtr landmark_ptr_; ///< Pointer to the matched landmark
+ double normalized_score_; ///< Similarity measure: 0: no match -- 1: perfect match
- }
- LandmarkMatch(LandmarkBasePtr _landmark_ptr, double _normalized_score) :
- landmark_ptr_(_landmark_ptr), normalized_score_(_normalized_score)
+ LandmarkMatch() : landmark_ptr_(nullptr), normalized_score_(0) {}
+ LandmarkMatch(LandmarkBasePtr _landmark_ptr, double _normalized_score)
+ : landmark_ptr_(_landmark_ptr), normalized_score_(_normalized_score)
{
-
}
};
-}//end namespace
+} // namespace wolf
diff --git a/include/core/map/factory_map.h b/include/core/map/factory_map.h
index 0b4a347c2cadf32191cd83f4957f683a83bbe514..2aa5616c254b1ef27af84c597434853260c2cbb0 100644
--- a/include/core/map/factory_map.h
+++ b/include/core/map/factory_map.h
@@ -28,7 +28,6 @@
namespace wolf
{
-
/** \brief Map factory class
*
* This factory can create objects of classes deriving from MapBase.
@@ -52,13 +51,13 @@ namespace wolf
* Map creators have the following API:
*
* \code
- * static MapBasePtr create(const YAML::Node& _params_node);
+ * static MapBasePtr create(const YAML::Node& _params_node, const std::vector<std::string> _schema_folders);
* \endcode
*
* They follow the general implementation shown below:
*
* \code
- * static MapBasePtr create(const YAML::Node& _params_node)
+ * static MapBasePtr create(const YAML::Node& _params_node, const std::vector<std::string> _schema_folders)
* {
* // Do create the Map object --- example: MapGrid
* return map_ptr = std::make_shared<MapGrid>(_params_node);
@@ -72,7 +71,7 @@ namespace wolf
* To create e.g. a MapGrid, you type:
*
* \code
- * auto grid_ptr = FactoryMap::create("MapGrid", _params_node);
+ * auto grid_ptr = FactoryMap::create("MapGrid", _params_node, {});
* \endcode
*
* #### Example 2: registering a map creator into the factory
@@ -96,26 +95,38 @@ namespace wolf
* \endcode
*
* - __Automatic registration__: registration is performed at library level.
- * Put the code at the last line of the map_xxx.cpp file,
+ * Put the code at the end of the map_xxx.cpp file inside wolf namespace:
* \code
- * namespace {
- * const bool registered_grid = FactoryMap::registerCreator("MapGrid", MapGrid::create);
- * }
+ * WOLF_REGISTER_MAP(MapGrid);
* \endcode
* Automatic registration is recommended in wolf, and implemented in the classes shipped with it.
* You are free to comment out these lines and place them wherever you consider it more convenient for your design.
*
*/
-typedef Factory<MapBasePtr, const YAML::Node&> FactoryMap;
+typedef Factory<MapBasePtr, const YAML::Node&, const std::vector<std::string>&> FactoryMapNode;
+
+template <>
+inline std::string FactoryMapNode::getClass() const
+{
+ return "FactoryMapNode";
+}
+
+typedef Factory<MapBasePtr, const std::string&, const std::vector<std::string>&> FactoryMapFile;
-template<>
-inline std::string FactoryMap::getClass() const
+template <>
+inline std::string FactoryMapFile::getClass() const
{
- return "FactoryMap";
+ return "FactoryMapFile";
}
-#define WOLF_REGISTER_MAP(MapType) \
- namespace{ const bool WOLF_UNUSED MapType##Registered = \
- FactoryMap::registerCreator(#MapType, MapType::create); } \
+#define WOLF_REGISTER_MAP(MapType) \
+ namespace \
+ { \
+ const bool WOLF_UNUSED MapType##Registered = FactoryMapNode::registerCreator(#MapType, MapType::create); \
+ } \
+ namespace \
+ { \
+ const bool WOLF_UNUSED MapType##RegisteredYaml = FactoryMapFile::registerCreator(#MapType, MapType::create); \
+ }
} /* namespace wolf */
diff --git a/include/core/map/map_base.h b/include/core/map/map_base.h
index 9d9fd4ed2924a1b6764faf7f1a39d5c76762af26..e5533e051f6c059cd68e2a42e8348e48de5b6cae 100644
--- a/include/core/map/map_base.h
+++ b/include/core/map/map_base.h
@@ -30,7 +30,6 @@ class LandmarkBase;
// Wolf includes
#include "core/common/wolf.h"
#include "core/common/node_base.h"
-#include "core/common/params_base.h"
#include "core/map/factory_map.h"
// other includes
@@ -38,7 +37,6 @@ class LandmarkBase;
namespace wolf
{
-
/*
* Macro for defining Autoconf map creator.
*
@@ -48,38 +46,33 @@ namespace wolf
* In order to use this macro, the derived map class, MapClass,
* must have a constructor available with the API:
*
- * MapClass(const ParamsMapClassPtr _params);
+ * MapClass(const YAML::Node& _params);
*
* We recommend writing one of such constructors in your derived maps.
*/
-#define WOLF_MAP_CREATE(MapClass, ParamsMapClass) \
- static MapBasePtr create(const YAML::Node& _node_input) \
+#define WOLF_MAP_CREATE(MapClass) \
+ static MapBasePtr create(const YAML::Node& _input_node, const std::vector<std::string>& _folders_schema = {}) \
{ \
- auto params = std::make_shared<ParamsMapClass>(_node_input); \
- \
- return std::make_shared<MapClass>(params); \
- }
-
-/** \brief base struct for map parameters
- *
- * Derive from this struct to create structs of map parameters.
- */
-struct ParamsMapBase : public ParamsBase
-{
- std::string prefix = "map";
-
- ParamsMapBase(const YAML::Node& _input_node)
- : ParamsBase(_input_node){
-
- };
-
- ~ParamsMapBase() override = default;
-
- std::string print() const override
- {
- return "";
+ if (not _folders_schema.empty()) \
+ { \
+ auto server = yaml_schema_cpp::YamlServer(_folders_schema); \
+ server.setYaml(_input_node); \
+ if (not server.applySchema(#MapClass)) \
+ { \
+ throw std::runtime_error(server.getLog()); \
+ } \
+ } \
+ return std::shared_ptr<MapClass>(new MapClass(_input_node)); \
+ } \
+ static MapBasePtr create(const std::string& _yaml_filepath, const std::vector<std::string>& _folders_schema) \
+ { \
+ auto server = yaml_schema_cpp::YamlServer(_folders_schema, _yaml_filepath); \
+ if (not server.applySchema(#MapClass)) \
+ { \
+ throw std::runtime_error(server.getLog()); \
+ } \
+ return create(server.getNode(), {}); \
}
-};
// class MapBase
class MapBase : public NodeBase, public std::enable_shared_from_this<MapBase>
@@ -91,8 +84,8 @@ class MapBase : public NodeBase, public std::enable_shared_from_this<MapBase>
public:
MapBase();
- MapBase(ParamsMapBasePtr _params, const std::string& _type = "Base");
- WOLF_MAP_CREATE(MapBase, ParamsMapBase);
+ MapBase(const YAML::Node& _params);
+ WOLF_MAP_CREATE(MapBase);
~MapBase() override = default;
bool hasChildren() const override;
@@ -110,15 +103,15 @@ class MapBase : public NodeBase, public std::enable_shared_from_this<MapBase>
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, //
+ virtual void printHeader(int depth, //
bool factored_by, //
- bool metric, //
+ bool metric, //
bool state_blocks,
std::ostream& stream,
std::string _tabs = "") const;
- void print(int depth, //
+ void print(int depth, //
bool factored_by, //
- bool metric, //
+ bool metric, //
bool state_blocks,
std::ostream& stream = std::cout,
std::string _tabs = "") const;
diff --git a/include/core/math/SE2.h b/include/core/math/SE2.h
index 5e14eeaea5b30ee831ab73e7791a0c1d8a01a433..79b6cf27e05b8a9de97d2ebc24ee6d9f903e3837 100644
--- a/include/core/math/SE2.h
+++ b/include/core/math/SE2.h
@@ -31,55 +31,55 @@
/*
* Some of the functions below are based on:
*
- * [1] J. Sola et. al. "A micro Lie theory for state estimation in robotics", tech rep. IRI 2018, https://arxiv.org/pdf/1812.01537.pdf
+ * [1] J. Sola et. al. "A micro Lie theory for state estimation in robotics", tech rep. IRI 2018,
+ * https://arxiv.org/pdf/1812.01537.pdf
*/
namespace wolf
{
-namespace SO2{
-
-template<typename T>
+namespace SO2
+{
+template <typename T>
inline Eigen::Matrix<T, 2, 2> exp(const T theta)
{
return Eigen::Rotation2D<T>(theta).matrix();
}
-} // namespace SO2
-
-
-namespace SE2{
+} // namespace SO2
-template<class T>
+namespace SE2
+{
+template <class T>
inline Eigen::Matrix<T, 2, 2> skew(const T& t)
{
- return (Eigen::Matrix<T, 2, 2>() << (T) 0, -t, t, (T) 0).finished();
+ return (Eigen::Matrix<T, 2, 2>() << (T)0, -t, t, (T)0).finished();
}
/** \brief Compute [1]_x * t = (-t.y ; t.x)
*/
-template<class D>
+template <class D>
inline Eigen::Matrix<typename D::Scalar, 2, 1> skewed(const MatrixBase<D>& t)
{
- return Eigen::Matrix<typename D::Scalar, 2, 1>(-t(1), t(0));
+ return Eigen::Matrix<typename D::Scalar, 2, 1>(-t(1), t(0));
}
-template<typename T>
+template <typename T>
inline Eigen::Matrix2d V_helper(const T theta)
{
- T s; // sin(theta) / theta
- T c_1; // (1-cos(theta)) / theta
+ T s; // sin(theta) / theta
+ T c_1; // (1-cos(theta)) / theta
if (fabs(theta) > T(Constants::EPS))
{
// [1] eq. 158
- s = sin(theta) / theta;
+ s = sin(theta) / theta;
c_1 = (T(1.0) - cos(theta)) / theta;
}
else
{
// approx of [1] eq. 158
- s = T(1.0); // sin(x) ~= x
- c_1 = theta / T(2.0); // cos(x) ~= 1 - x^2/2
+ s = T(1.0); // sin(x) ~= x
+ c_1 = theta / T(2.0); // cos(x) ~= 1 - x^2/2
}
return (Matrix<T, 2, 2>() << s, -c_1, c_1, s).finished();
@@ -94,9 +94,8 @@ inline VectorComposite identity()
return v;
}
-template<typename D1, typename D2>
-inline void inverse(const MatrixBase<D1>& p, const typename D1::Scalar& o,
- MatrixBase<D2>& ip, typename D2::Scalar& io)
+template <typename D1, typename D2>
+inline void inverse(const MatrixBase<D1>& p, const typename D1::Scalar& o, MatrixBase<D2>& ip, typename D2::Scalar& io)
{
MatrixSizeCheck<2, 1>::check(p);
MatrixSizeCheck<2, 1>::check(ip);
@@ -105,9 +104,8 @@ 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)
+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);
@@ -117,20 +115,19 @@ inline void inverse(const MatrixBase<D1>& p, const MatrixBase<D2>& o,
inverse(p, o(0), ip, io(0));
}
-template<typename D1, typename D2>
-inline void inverse(const MatrixBase<D1>& d,
- MatrixBase<D2>& id)
+template <typename D1, typename D2>
+inline void inverse(const MatrixBase<D1>& d, MatrixBase<D2>& id)
{
MatrixSizeCheck<3, 1>::check(d);
MatrixSizeCheck<3, 1>::check(id);
- Map<const Matrix<typename D1::Scalar, 2, 1> > p ( & d( 0 ) );
- Map<Matrix<typename D2::Scalar, 2, 1> > ip ( & id( 0 ) );
+ Map<const Matrix<typename D1::Scalar, 2, 1> > p(&d(0));
+ Map<Matrix<typename D2::Scalar, 2, 1> > ip(&id(0));
inverse(p, d(2), ip, id(2));
}
-template<typename D>
+template <typename D>
inline Matrix<typename D::Scalar, 3, 1> inverse(const MatrixBase<D>& d)
{
Matrix<typename D::Scalar, 3, 1> id;
@@ -150,7 +147,7 @@ inline VectorComposite inverse(const VectorComposite& v)
return c;
}
-template<class D1, class D2>
+template <class D1, class D2>
inline void exp(const Eigen::MatrixBase<D1>& _tau, Eigen::MatrixBase<D2>& _delta)
{
MatrixSizeCheck<3, 1>::check(_tau);
@@ -158,29 +155,29 @@ inline void exp(const Eigen::MatrixBase<D1>& _tau, Eigen::MatrixBase<D2>& _delta
// [1] eq. 156
_delta.head(2) = V_helper(_tau(2)) * _tau.head(2);
- _delta(2) = pi2pi(_tau(2));
+ _delta(2) = pi2pi(_tau(2));
}
-template<class D, typename T>
+template <class D, typename T>
inline Eigen::Matrix<T, 2, 1> J_Vp_theta(const Eigen::MatrixBase<D>& p, const T theta)
{
- MatrixSizeCheck<2, 1>::check (p);
+ MatrixSizeCheck<2, 1>::check(p);
// Jacobian of t = V(theta)*p wrt theta -- developed in Matlab symbolic, and copied here.
- T x = p(0);
- T y = p(1);
+ T x = p(0);
+ T y = p(1);
Matrix<T, 2, 1> J_Vp_th;
if (fabs(theta) > T(Constants::EPS))
{
- T s_th = sin(theta);
- T c_th = cos(theta);
+ T s_th = sin(theta);
+ T c_th = cos(theta);
T theta2 = theta * theta;
J_Vp_th << -(y * c_th - y + x * s_th - theta * x * c_th + theta * y * s_th) / theta2,
- (x * c_th - x - y * s_th + theta * y * c_th + theta * x * s_th) / theta2;
+ (x * c_th - x - y * s_th + theta * y * c_th + theta * x * s_th) / theta2;
}
else
- { // sin(x) ~= x
+ { // sin(x) ~= x
// cos(x) ~= 1 - x^2/2
J_Vp_th << -y / 2.0, x / 2.0;
}
@@ -188,7 +185,7 @@ inline Eigen::Matrix<T, 2, 1> J_Vp_theta(const Eigen::MatrixBase<D>& p, const T
return J_Vp_th;
}
-template<class D1, class D2, class D3>
+template <class D1, class D2, class D3>
inline void exp(const MatrixBase<D1>& _tau, MatrixBase<D2>& _delta, MatrixBase<D3>& _J_delta_tau)
{
MatrixSizeCheck<3, 1>::check(_tau);
@@ -198,10 +195,10 @@ inline void exp(const MatrixBase<D1>& _tau, MatrixBase<D2>& _delta, MatrixBase<D
typedef typename D1::Scalar T;
// [1] eq. 156
- T theta = pi2pi(_tau(2));
- Eigen::Matrix<T, 2, 2> V = V_helper(theta);
- _delta.head(2) = V * _tau.head(2);
- _delta(2) = theta;
+ T theta = pi2pi(_tau(2));
+ Eigen::Matrix<T, 2, 2> V = V_helper(theta);
+ _delta.head(2) = V * _tau.head(2);
+ _delta(2) = theta;
// Jacobian is the composite definition [1] eq. 89, with jacobian blocks:
// J_Vp_p = V: see V_helper, eq. 158
@@ -213,9 +210,9 @@ inline void exp(const MatrixBase<D1>& _tau, MatrixBase<D2>& _delta, MatrixBase<D
inline void exp(const VectorComposite& _tau, VectorComposite& _delta)
{
// [1] eq. 156
- const auto &p = _tau.at('P');
- const auto &theta = pi2pi(_tau.at('O')(0));
- Matrix2d V = V_helper(theta);
+ const auto& p = _tau.at('P');
+ const auto& theta = pi2pi(_tau.at('O')(0));
+ Matrix2d V = V_helper(theta);
_delta['P'] = V * p;
_delta['O'] = Matrix1d(theta);
@@ -233,9 +230,9 @@ inline VectorComposite exp(const VectorComposite& tau)
inline void exp(const VectorComposite& _tau, VectorComposite& _delta, MatrixComposite& _J_delta_tau)
{
// [1] eq. 156
- const auto &p = _tau.at('P');
- const auto &theta = pi2pi(_tau.at('O')(0));
- Matrix2d V = V_helper(theta);
+ const auto& p = _tau.at('P');
+ const auto& theta = pi2pi(_tau.at('O')(0));
+ Matrix2d V = V_helper(theta);
_delta['P'] = V * p;
_delta['O'] = Matrix1d(theta);
@@ -251,32 +248,34 @@ inline void exp(const VectorComposite& _tau, VectorComposite& _delta, MatrixComp
_J_delta_tau.emplace('O', 'O', Matrix1d(1));
}
-template<class D1, class D2, class D3>
-inline void compose(const MatrixBase<D1> &_delta1, const MatrixBase<D2> &_delta2, MatrixBase<D3> &_delta1_compose_delta2)
+template <class D1, class D2, class D3>
+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);
MatrixSizeCheck<3, 1>::check(_delta1_compose_delta2);
- _delta1_compose_delta2.template head<2>() = _delta1.template head<2>()
- + SO2::exp(_delta1(2)) * _delta2.template head<2>();
+ _delta1_compose_delta2.template head<2>() =
+ _delta1.template head<2>() + SO2::exp(_delta1(2)) * _delta2.template head<2>();
_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)
+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;
+ 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>
+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)
+ 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);
@@ -284,7 +283,7 @@ inline void compose(const MatrixBase<D1>& _delta1,
MatrixSizeCheck<3, 3>::check(_J_compose_delta1);
MatrixSizeCheck<3, 3>::check(_J_compose_delta2);
- Matrix2d R1 = SO2::exp(_delta1(2)); // need temporary
+ Matrix2d R1 = SO2::exp(_delta1(2)); // need temporary
// tc = t1 + R1 t2
_delta1_compose_delta2.template head<2>() = _delta1.template head<2>() + R1 * _delta2.template head<2>();
@@ -326,22 +325,20 @@ inline void compose(const MatrixBase<D1>& _delta1,
_J_compose_delta2.template block<2, 2>(0, 0) = R1;
}
-inline void compose(const VectorComposite& _x1,
- const VectorComposite& _x2,
- VectorComposite& _c)
+inline void compose(const VectorComposite& _x1, const VectorComposite& _x2, VectorComposite& _c)
{
const auto& p1 = _x1.at('P');
- const auto& a1 = _x1.at('O')(0); // angle
+ const auto& a1 = _x1.at('O')(0); // angle
const auto& R1 = SO2::exp(a1);
const auto& p2 = _x2.at('P');
- const auto& a2 = _x2.at('O')(0); // angle
+ const auto& a2 = _x2.at('O')(0); // angle
// tc = t1 + R1 t2
_c['P'] = p1 + R1 * p2;
// Rc = R1 R2 --> theta = theta1 + theta2
- _c['O'] = Matrix1d( pi2pi(a1 + a2) ) ;
+ _c['O'] = Matrix1d(pi2pi(a1 + a2));
}
inline VectorComposite compose(const VectorComposite& x1, const VectorComposite& x2)
@@ -353,16 +350,16 @@ 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)
+ VectorComposite& _c,
+ MatrixComposite& _J_c_x1,
+ MatrixComposite& _J_c_x2)
{
const auto& p1 = _x1.at('P');
- const auto& a1 = _x1.at('O')(0); // angle
- Matrix2d R1 = SO2::exp(a1); // need temporary
+ const auto& a1 = _x1.at('O')(0); // angle
+ Matrix2d R1 = SO2::exp(a1); // need temporary
const auto& p2 = _x2.at('P');
- const auto& a2 = _x2.at('O')(0); // angle
+ const auto& a2 = _x2.at('O')(0); // angle
/* Jacobians in composite form [1] see eqs. (89, 125, 129, 130)
*
@@ -393,31 +390,33 @@ inline void compose(const VectorComposite& _x1,
_J_c_x1.clear();
_J_c_x1.emplace('P', 'P', Matrix2d::Identity());
- _J_c_x1.emplace('P', 'O', MatrixXd(R1 * skewed(p2)) );
- _J_c_x1.emplace('O', 'P', RowVector2d(0,0));
+ _J_c_x1.emplace('P', 'O', MatrixXd(R1 * skewed(p2)));
+ _J_c_x1.emplace('O', 'P', RowVector2d(0, 0));
_J_c_x1.emplace('O', 'O', Matrix1d(1));
_J_c_x2.clear();
_J_c_x2.emplace('P', 'P', R1);
- _J_c_x2.emplace('P', 'O', Vector2d(0,0));
- _J_c_x2.emplace('O', 'P', RowVector2d(0,0));
+ _J_c_x2.emplace('P', 'O', Vector2d(0, 0));
+ _J_c_x2.emplace('O', 'P', RowVector2d(0, 0));
_J_c_x2.emplace('O', 'O', Matrix1d(1));
-
- // Actually compose the vectors here. This avoids aliasing in case this is called with 'c' coinciding with 'x1' or 'x2'.
+ // Actually compose the vectors here. This avoids aliasing in case this is called with 'c' coinciding with 'x1' or
+ // 'x2'.
// tc = t1 + R1 t2
_c['P'] = p1 + R1 * p2;
// Rc = R1 R2 --> theta = theta1 + theta2
- _c['O'] = Matrix1d( pi2pi(a1 + a2) ) ;
-
+ _c['O'] = Matrix1d(pi2pi(a1 + a2));
}
-template<typename D1, typename D2, typename D4, typename D5, typename D7, typename D8>
-inline void plus(const MatrixBase<D1>& p1, const MatrixBase<D2>& q1,
- const MatrixBase<D4>& p2, const MatrixBase<D5>& o2,
- MatrixBase<D7>& plus_p, MatrixBase<D8>& plus_q)
+template <typename D1, typename D2, typename D4, typename D5, typename D7, typename D8>
+inline void plus(const MatrixBase<D1>& p1,
+ const MatrixBase<D2>& q1,
+ const MatrixBase<D4>& p2,
+ const MatrixBase<D5>& o2,
+ MatrixBase<D7>& plus_p,
+ MatrixBase<D8>& plus_q)
{
MatrixSizeCheck<2, 1>::check(p1);
MatrixSizeCheck<1, 1>::check(q1);
@@ -430,24 +429,21 @@ inline void plus(const MatrixBase<D1>& p1, const MatrixBase<D2>& q1,
plus_q(0) = pi2pi(q1(0) + o2(0));
}
-template<typename D1, typename D2, typename D3>
-inline void plus(const MatrixBase<D1>& d1,
- const MatrixBase<D2>& d2,
- MatrixBase<D3>& d_plus)
+template <typename D1, typename D2, typename D3>
+inline void plus(const MatrixBase<D1>& d1, const MatrixBase<D2>& d2, MatrixBase<D3>& d_plus)
{
- Map<const Matrix<typename D1::Scalar, 2, 1> > p1 ( & d1(0) );
- Map<const Matrix<typename D1::Scalar, 1, 1> > q1 ( & d1(2) );
- Map<const Matrix<typename D2::Scalar, 2, 1> > p2 ( & d2(0) );
- Map<const Matrix<typename D2::Scalar, 1, 1> > o2 ( & d2(2) );
- Map<Matrix<typename D3::Scalar, 2, 1> > p_p ( & d_plus(0) );
- Map<Matrix<typename D1::Scalar, 1, 1> > q_p ( & d_plus(2) );
+ Map<const Matrix<typename D1::Scalar, 2, 1> > p1(&d1(0));
+ Map<const Matrix<typename D1::Scalar, 1, 1> > q1(&d1(2));
+ Map<const Matrix<typename D2::Scalar, 2, 1> > p2(&d2(0));
+ Map<const Matrix<typename D2::Scalar, 1, 1> > o2(&d2(2));
+ Map<Matrix<typename D3::Scalar, 2, 1> > p_p(&d_plus(0));
+ Map<Matrix<typename D1::Scalar, 1, 1> > q_p(&d_plus(2));
plus(p1, q1, p2, o2, p_p, q_p);
}
-template<typename D1, typename D2>
-inline Matrix<typename D1::Scalar, 3, 1> plus(const MatrixBase<D1>& d1,
- const MatrixBase<D2>& d2)
+template <typename D1, typename D2>
+inline Matrix<typename D1::Scalar, 3, 1> plus(const MatrixBase<D1>& d1, const MatrixBase<D2>& d2)
{
Matrix<typename D1::Scalar, 3, 1> d_plus;
plus(d1, d2, d_plus);
@@ -468,23 +464,24 @@ inline VectorComposite plus(const VectorComposite& x, const VectorComposite& tau
return res;
}
-template<typename D1, typename D2, typename D3>
-inline void between(const MatrixBase<D1>& p1, const typename D1::Scalar& o1,
- const MatrixBase<D2>& p2, const typename D2::Scalar& o2,
- MatrixBase<D3>& p12, typename D3::Scalar& o12)
+template <typename D1, typename D2, typename D3>
+inline void between(const MatrixBase<D1>& p1,
+ const typename D1::Scalar& o1,
+ const MatrixBase<D2>& p2,
+ const typename D2::Scalar& o2,
+ MatrixBase<D3>& p12,
+ typename D3::Scalar& o12)
{
- MatrixSizeCheck<2, 1>::check(p1);
- MatrixSizeCheck<2, 1>::check(p2);
- MatrixSizeCheck<2, 1>::check(p12);
+ MatrixSizeCheck<2, 1>::check(p1);
+ MatrixSizeCheck<2, 1>::check(p2);
+ MatrixSizeCheck<2, 1>::check(p12);
- p12 = SO2::exp(-o1) * ( p2 - p1 );
- o12 = o2 - o1;
+ p12 = SO2::exp(-o1) * (p2 - p1);
+ o12 = o2 - o1;
}
-template<typename D1, typename D2, typename D3>
-inline void between(const MatrixBase<D1>& d1,
- const MatrixBase<D2>& d2,
- MatrixBase<D3>& d12)
+template <typename D1, typename D2, typename D3>
+inline void between(const MatrixBase<D1>& d1, const MatrixBase<D2>& d2, MatrixBase<D3>& d12)
{
MatrixSizeCheck<3, 1>::check(d1);
MatrixSizeCheck<3, 1>::check(d2);
@@ -492,16 +489,15 @@ inline void between(const MatrixBase<D1>& d1,
typedef typename D1::Scalar T;
- Map<const Matrix<T, 2, 1> > p1 ( & d1(0) );
- Map<const Matrix<T, 2, 1> > p2 ( & d2(0) );
- Map<Matrix<T, 2, 1> > p12 ( & d12(0) );
+ Map<const Matrix<T, 2, 1> > p1(&d1(0));
+ Map<const Matrix<T, 2, 1> > p2(&d2(0));
+ Map<Matrix<T, 2, 1> > p12(&d12(0));
between(p1, d1(2), p2, d2(2), p12, d12(2));
}
-template<typename D1, typename D2>
-inline Matrix<typename D1::Scalar, 3, 1> between(const MatrixBase<D1>& d1,
- const MatrixBase<D2>& d2 )
+template <typename D1, typename D2>
+inline Matrix<typename D1::Scalar, 3, 1> between(const MatrixBase<D1>& d1, const MatrixBase<D2>& d2)
{
MatrixSizeCheck<3, 1>::check(d1);
MatrixSizeCheck<3, 1>::check(d2);
@@ -510,5 +506,5 @@ inline Matrix<typename D1::Scalar, 3, 1> between(const MatrixBase<D1>& d1,
return d12;
}
-} // namespace SE2
-} // namespacs wolf
+} // namespace SE2
+} // namespace wolf
diff --git a/include/core/math/SE3.h b/include/core/math/SE3.h
index fe456a9103b420eacdea3461e671b8e68c34ac67..be94f1b8009738597895ac1da98bea6e69c9e543 100644
--- a/include/core/math/SE3.h
+++ b/include/core/math/SE3.h
@@ -50,23 +50,25 @@
*
* Some of the functions below are based on:
*
- * [1] J. Sola et. al. "A micro Lie theory for state estimation in robotics", tech rep. IRI 2018, https://arxiv.org/pdf/1812.01537.pdf
+ * [1] J. Sola et. al. "A micro Lie theory for state estimation in robotics", tech rep. IRI 2018,
+ * https://arxiv.org/pdf/1812.01537.pdf
*/
namespace wolf
{
-namespace SE3 {
+namespace SE3
+{
using namespace Eigen;
-template<typename D1, typename D2>
+template <typename D1, typename D2>
inline void identity(MatrixBase<D1>& p, QuaternionBase<D2>& q)
{
MatrixSizeCheck<3, 1>::check(p);
- p = MatrixBase<D1>::Zero(3,1);
+ p = MatrixBase<D1>::Zero(3, 1);
q = QuaternionBase<D2>::Identity();
}
-template<typename D1, typename D2>
+template <typename D1, typename D2>
inline void identity(MatrixBase<D1>& p, MatrixBase<D2>& q)
{
MatrixSizeCheck<3, 1>::check(p);
@@ -84,42 +86,39 @@ inline void identity(VectorComposite& v)
v['O'] = Quaterniond::Identity().coeffs();
}
-template<typename T = double>
+template <typename T = double>
inline Matrix<T, 7, 1> identity()
{
Matrix<T, 7, 1> ret;
- ret<< T(0), T(0), T(0),
- T(0), T(0), T(0), T(1);
+ ret << T(0), T(0), T(0), T(0), T(0), T(0), T(1);
return ret;
}
-template<typename D1, typename D2, typename D4, typename D5>
-inline void inverse(const MatrixBase<D1>& p, const QuaternionBase<D2>& q,
- MatrixBase<D4>& ip, QuaternionBase<D5>& iq)
+template <typename D1, typename D2, typename D4, typename D5>
+inline void inverse(const MatrixBase<D1>& p, const QuaternionBase<D2>& q, MatrixBase<D4>& ip, QuaternionBase<D5>& iq)
{
MatrixSizeCheck<3, 1>::check(p);
MatrixSizeCheck<3, 1>::check(ip);
- ip = -(q.conjugate() * p) ;
- iq = q.conjugate() ;
+ ip = -(q.conjugate() * p);
+ iq = q.conjugate();
}
-template<typename D1, typename D2>
-inline void inverse(const MatrixBase<D1>& d,
- MatrixBase<D2>& id)
+template <typename D1, typename D2>
+inline void inverse(const MatrixBase<D1>& d, MatrixBase<D2>& id)
{
MatrixSizeCheck<7, 1>::check(d);
MatrixSizeCheck<7, 1>::check(id);
- Map<const Matrix<typename D1::Scalar, 3, 1> > p ( & d( 0 ) );
- Map<const Quaternion<typename D1::Scalar> > q ( & d( 3 ) );
- Map<Matrix<typename D2::Scalar, 3, 1> > ip ( & id( 0 ) );
- Map<Quaternion<typename D2::Scalar> > iq ( & id( 3 ) );
+ Map<const Matrix<typename D1::Scalar, 3, 1>> p(&d(0));
+ Map<const Quaternion<typename D1::Scalar>> q(&d(3));
+ Map<Matrix<typename D2::Scalar, 3, 1>> ip(&id(0));
+ Map<Quaternion<typename D2::Scalar>> iq(&id(3));
inverse(p, q, ip, iq);
}
-template<typename D>
+template <typename D>
inline Matrix<typename D::Scalar, 7, 1> inverse(const MatrixBase<D>& d)
{
Matrix<typename D::Scalar, 7, 1> id;
@@ -129,107 +128,110 @@ inline Matrix<typename D::Scalar, 7, 1> inverse(const MatrixBase<D>& d)
inline void inverse(const VectorComposite& v, VectorComposite& c)
{
- Map<const Quaternion<double> > qv( & v.at('O')(0) );
- Map<Quaternion<double> > qc( & c['O'](0) );
+ Map<const Quaternion<double>> qv(&v.at('O')(0));
+ Map<Quaternion<double>> qc(&c['O'](0));
inverse(v.at('P'), qv, c['P'], qc);
}
inline VectorComposite inverse(const VectorComposite& v)
{
- VectorComposite c("PO", Vector7d::Zero(), {3,4});
+ VectorComposite c("PO", Vector7d::Zero(), {3, 4});
inverse(v, c);
return c;
}
-
-template<typename D1, typename D2, typename D4, typename D5, typename D7, typename D8>
-inline void compose(const MatrixBase<D1>& p1, const QuaternionBase<D2>& q1,
- const MatrixBase<D4>& p2, const QuaternionBase<D5>& q2,
- MatrixBase<D7>& pc, QuaternionBase<D8>& qc )
+template <typename D1, typename D2, typename D4, typename D5, typename D7, typename D8>
+inline void compose(const MatrixBase<D1>& p1,
+ const QuaternionBase<D2>& q1,
+ const MatrixBase<D4>& p2,
+ const QuaternionBase<D5>& q2,
+ MatrixBase<D7>& pc,
+ QuaternionBase<D8>& qc)
{
- MatrixSizeCheck<3, 1>::check(p1);
- MatrixSizeCheck<3, 1>::check(p2);
- MatrixSizeCheck<3, 1>::check(pc);
+ MatrixSizeCheck<3, 1>::check(p1);
+ MatrixSizeCheck<3, 1>::check(p2);
+ MatrixSizeCheck<3, 1>::check(pc);
- pc = p1 + q1 * p2;
- qc = q1 * q2; // q here to avoid possible aliasing between d1 and sum
+ pc = p1 + q1 * p2;
+ qc = q1 * q2; // q here to avoid possible aliasing between d1 and sum
}
-template<typename D1, typename D2, typename D4, typename D5, typename D7, typename D8>
-inline void compose(const MatrixBase<D1>& p1, const MatrixBase<D2>& q1,
- const MatrixBase<D4>& p2, const MatrixBase<D5>& q2,
- MatrixBase<D7>& pc, MatrixBase<D8>& qc )
+template <typename D1, typename D2, typename D4, typename D5, typename D7, typename D8>
+inline void compose(const MatrixBase<D1>& p1,
+ const MatrixBase<D2>& q1,
+ const MatrixBase<D4>& p2,
+ const MatrixBase<D5>& q2,
+ MatrixBase<D7>& pc,
+ MatrixBase<D8>& qc)
{
- MatrixSizeCheck<3, 1>::check(p1);
- MatrixSizeCheck<4, 1>::check(q1);
- MatrixSizeCheck<3, 1>::check(p2);
- MatrixSizeCheck<4, 1>::check(q2);
- MatrixSizeCheck<3, 1>::check(pc);
- MatrixSizeCheck<4, 1>::check(qc);
+ MatrixSizeCheck<3, 1>::check(p1);
+ MatrixSizeCheck<4, 1>::check(q1);
+ MatrixSizeCheck<3, 1>::check(p2);
+ MatrixSizeCheck<4, 1>::check(q2);
+ MatrixSizeCheck<3, 1>::check(pc);
+ MatrixSizeCheck<4, 1>::check(qc);
- Map<const Quaternion<typename D2::Scalar> > mq1 ( & q1( 0 ) );
- Map<const Quaternion<typename D5::Scalar> > mq2 ( & q2( 0 ) );
- Map< Quaternion<typename D8::Scalar> > mqc ( & qc( 0 ) );
+ Map<const Quaternion<typename D2::Scalar>> mq1(&q1(0));
+ Map<const Quaternion<typename D5::Scalar>> mq2(&q2(0));
+ Map<Quaternion<typename D8::Scalar>> mqc(&qc(0));
- compose(p1, mq1, p2, mq2, pc, mqc);
+ compose(p1, mq1, p2, mq2, pc, mqc);
}
-template<typename D1, typename D2, typename D3>
-inline void compose(const MatrixBase<D1>& d1,
- const MatrixBase<D2>& d2,
- MatrixBase<D3>& sum)
+template <typename D1, typename D2, typename D3>
+inline void compose(const MatrixBase<D1>& d1, const MatrixBase<D2>& d2, MatrixBase<D3>& sum)
{
MatrixSizeCheck<7, 1>::check(d1);
MatrixSizeCheck<7, 1>::check(d2);
MatrixSizeCheck<7, 1>::check(sum);
- Map<const Matrix<typename D1::Scalar, 3, 1> > p1 ( & d1( 0 ) );
- Map<const Quaternion<typename D1::Scalar> > q1 ( & d1( 3 ) );
- Map<const Matrix<typename D2::Scalar, 3, 1> > p2 ( & d2( 0 ) );
- Map<const Quaternion<typename D2::Scalar> > q2 ( & d2( 3 ) );
- Map<Matrix<typename D3::Scalar, 3, 1> > pc ( & sum( 0 ) );
- Map<Quaternion<typename D3::Scalar> > qc ( & sum( 3 ) );
+ Map<const Matrix<typename D1::Scalar, 3, 1>> p1(&d1(0));
+ Map<const Quaternion<typename D1::Scalar>> q1(&d1(3));
+ Map<const Matrix<typename D2::Scalar, 3, 1>> p2(&d2(0));
+ Map<const Quaternion<typename D2::Scalar>> q2(&d2(3));
+ Map<Matrix<typename D3::Scalar, 3, 1>> pc(&sum(0));
+ Map<Quaternion<typename D3::Scalar>> qc(&sum(3));
compose(p1, q1, p2, q2, pc, qc);
}
-template<typename D1, typename D2>
-inline Matrix<typename D1::Scalar, 7, 1> compose(const MatrixBase<D1>& d1,
- const MatrixBase<D2>& d2 )
+template <typename D1, typename D2>
+inline Matrix<typename D1::Scalar, 7, 1> compose(const MatrixBase<D1>& d1, const MatrixBase<D2>& d2)
{
- Matrix<typename D1::Scalar, 7, 1> ret;
+ Matrix<typename D1::Scalar, 7, 1> ret;
compose(d1, d2, ret);
return ret;
}
-template<typename D1, typename D2, typename D3, typename D4, typename D5>
+template <typename D1, typename D2, typename D3, typename D4, typename D5>
inline void compose(const MatrixBase<D1>& d1,
const MatrixBase<D2>& d2,
- MatrixBase<D3>& sum,
- MatrixBase<D4>& J_sum_d1,
- MatrixBase<D5>& J_sum_d2)
+ MatrixBase<D3>& sum,
+ MatrixBase<D4>& J_sum_d1,
+ MatrixBase<D5>& J_sum_d2)
{
MatrixSizeCheck<7, 1>::check(d1);
MatrixSizeCheck<7, 1>::check(d2);
MatrixSizeCheck<7, 1>::check(sum);
- MatrixSizeCheck< 6, 6>::check(J_sum_d1);
- MatrixSizeCheck< 6, 6>::check(J_sum_d2);
+ MatrixSizeCheck<6, 6>::check(J_sum_d1);
+ MatrixSizeCheck<6, 6>::check(J_sum_d2);
// Some useful temporaries
- Matrix<typename D1::Scalar, 3, 3> dR1 = q2R(d1.segment(3,4)); //q1.matrix(); // First Delta, DR
- Matrix<typename D2::Scalar, 3, 3> dR2 = q2R(d2.segment(3,4)); //q2.matrix(); // Second delta, dR
+ Matrix<typename D1::Scalar, 3, 3> dR1 = q2R(d1.segment(3, 4)); // q1.matrix(); // First Delta, DR
+ Matrix<typename D2::Scalar, 3, 3> dR2 = q2R(d2.segment(3, 4)); // q2.matrix(); // Second delta, dR
// Jac wrt first delta
- J_sum_d1.setIdentity(); // dDp'/dDp = dDv'/dDv = I
- J_sum_d1.block(0,3,3,3).noalias() = - dR1 * skew(d2.head(3)) ; // dDp'/dDo
- J_sum_d1.block(3,3,3,3) = dR2.transpose(); // dDo'/dDo
+ J_sum_d1.setIdentity(); // dDp'/dDp = dDv'/dDv = I
+ J_sum_d1.block(0, 3, 3, 3).noalias() = -dR1 * skew(d2.head(3)); // dDp'/dDo
+ J_sum_d1.block(3, 3, 3, 3) = dR2.transpose(); // dDo'/dDo
// Jac wrt second delta
- J_sum_d2.setIdentity(); //
- J_sum_d2.block(0,0,3,3) = dR1; // dDp'/p
+ J_sum_d2.setIdentity(); //
+ J_sum_d2.block(0, 0, 3, 3) = dR1; // dDp'/p
// J_sum_d2.block(3,3,3,3) = Matrix3d::Identity(); // dDo'/ddo = I
- // compose deltas -- done here to avoid aliasing when calling with input `d1` and result `sum` referencing the same variable
+ // compose deltas -- done here to avoid aliasing when calling with input `d1` and result `sum` referencing the same
+ // variable
compose(d1, d2, sum);
}
@@ -240,73 +242,73 @@ inline void compose(const VectorComposite& x1, const VectorComposite& x2, Vector
inline VectorComposite compose(const VectorComposite& x1, const VectorComposite& x2)
{
- VectorComposite c("PO", Vector7d::Zero(), {3,4});
+ VectorComposite c("PO", Vector7d::Zero(), {3, 4});
compose(x1.at('P'), x1.at('O'), x2.at('P'), x2.at('O'), c['P'], c['O']);
return c;
}
inline void compose(const VectorComposite& x1,
const VectorComposite& x2,
- VectorComposite& c,
- MatrixComposite& J_c_x1,
- MatrixComposite& J_c_x2)
+ VectorComposite& c,
+ MatrixComposite& J_c_x1,
+ MatrixComposite& J_c_x2)
{
-
// Some useful temporaries
- const auto R1 = q2R(x1.at('O')); //q1.matrix(); // make temporary
- const auto& R2 = q2R(x2.at('O')); //q2.matrix(); // do not make temporary, only reference
+ const auto R1 = q2R(x1.at('O')); // q1.matrix(); // make temporary
+ const auto& R2 = q2R(x2.at('O')); // q2.matrix(); // do not make temporary, only reference
// Jac wrt first delta
- J_c_x1.emplace('P', 'P', Matrix3d::Identity() ) ;
- J_c_x1.emplace('P', 'O', - R1 * skew(x2.at('P')) ) ;
- J_c_x1.emplace('O', 'P', Matrix3d::Zero() ) ;
- J_c_x1.emplace('O', 'O', R2.transpose() ) ;
+ J_c_x1.emplace('P', 'P', Matrix3d::Identity());
+ J_c_x1.emplace('P', 'O', -R1 * skew(x2.at('P')));
+ J_c_x1.emplace('O', 'P', Matrix3d::Zero());
+ J_c_x1.emplace('O', 'O', R2.transpose());
// Jac wrt second delta
- J_c_x2.emplace('P', 'P', R1 );
- J_c_x2.emplace('P', 'O', Matrix3d::Zero() );
- J_c_x2.emplace('O', 'P', Matrix3d::Zero() );
- J_c_x2.emplace('O', 'O', Matrix3d::Identity() );
+ J_c_x2.emplace('P', 'P', R1);
+ J_c_x2.emplace('P', 'O', Matrix3d::Zero());
+ J_c_x2.emplace('O', 'P', Matrix3d::Zero());
+ J_c_x2.emplace('O', 'O', Matrix3d::Identity());
- // compose deltas -- done here to avoid aliasing when calling with input `x1` and result `c` referencing the same variable
+ // compose deltas -- done here to avoid aliasing when calling with input `x1` and result `c` referencing the same
+ // variable
compose(x1, x2, c);
}
-template<typename D1, typename D2, typename D4, typename D5, typename D7, typename D8>
-inline void between(const MatrixBase<D1>& p1, const QuaternionBase<D2>& q1,
- const MatrixBase<D4>& p2, const QuaternionBase<D5>& q2,
- MatrixBase<D7>& p12, QuaternionBase<D8>& q12)
+template <typename D1, typename D2, typename D4, typename D5, typename D7, typename D8>
+inline void between(const MatrixBase<D1>& p1,
+ const QuaternionBase<D2>& q1,
+ const MatrixBase<D4>& p2,
+ const QuaternionBase<D5>& q2,
+ MatrixBase<D7>& p12,
+ QuaternionBase<D8>& q12)
{
- MatrixSizeCheck<3, 1>::check(p1);
- MatrixSizeCheck<3, 1>::check(p2);
- MatrixSizeCheck<3, 1>::check(p12);
+ MatrixSizeCheck<3, 1>::check(p1);
+ MatrixSizeCheck<3, 1>::check(p2);
+ MatrixSizeCheck<3, 1>::check(p12);
- p12 = q1.conjugate() * ( p2 - p1 );
- q12 = q1.conjugate() * q2;
+ p12 = q1.conjugate() * (p2 - p1);
+ q12 = q1.conjugate() * q2;
}
-template<typename D1, typename D2, typename D3>
-inline void between(const MatrixBase<D1>& d1,
- const MatrixBase<D2>& d2,
- MatrixBase<D3>& d12)
+template <typename D1, typename D2, typename D3>
+inline void between(const MatrixBase<D1>& d1, const MatrixBase<D2>& d2, MatrixBase<D3>& d12)
{
MatrixSizeCheck<7, 1>::check(d1);
MatrixSizeCheck<7, 1>::check(d2);
MatrixSizeCheck<7, 1>::check(d12);
- Map<const Matrix<typename D1::Scalar, 3, 1> > p1 ( & d1(0) );
- Map<const Quaternion<typename D1::Scalar> > q1 ( & d1(3) );
- Map<const Matrix<typename D2::Scalar, 3, 1> > p2 ( & d2(0) );
- Map<const Quaternion<typename D2::Scalar> > q2 ( & d2(3) );
- Map<Matrix<typename D3::Scalar, 3, 1> > p12 ( & d12(0) );
- Map<Quaternion<typename D3::Scalar> > q12 ( & d12(3) );
+ Map<const Matrix<typename D1::Scalar, 3, 1>> p1(&d1(0));
+ Map<const Quaternion<typename D1::Scalar>> q1(&d1(3));
+ Map<const Matrix<typename D2::Scalar, 3, 1>> p2(&d2(0));
+ Map<const Quaternion<typename D2::Scalar>> q2(&d2(3));
+ Map<Matrix<typename D3::Scalar, 3, 1>> p12(&d12(0));
+ Map<Quaternion<typename D3::Scalar>> q12(&d12(3));
between(p1, q1, p2, q2, p12, q12);
}
-template<typename D1, typename D2>
-inline Matrix<typename D1::Scalar, 7, 1> between(const MatrixBase<D1>& d1,
- const MatrixBase<D2>& d2 )
+template <typename D1, typename D2>
+inline Matrix<typename D1::Scalar, 7, 1> between(const MatrixBase<D1>& d1, const MatrixBase<D2>& d2)
{
MatrixSizeCheck<7, 1>::check(d1);
MatrixSizeCheck<7, 1>::check(d2);
@@ -315,28 +317,28 @@ inline Matrix<typename D1::Scalar, 7, 1> between(const MatrixBase<D1>& d1,
return d12;
}
-template<typename Derived>
+template <typename Derived>
inline Matrix<typename Derived::Scalar, 6, 1> log(const MatrixBase<Derived>& delta_in)
{
MatrixSizeCheck<7, 1>::check(delta_in);
Matrix<typename Derived::Scalar, 6, 1> ret;
- Map<const Matrix<typename Derived::Scalar, 3, 1> > p_in ( & delta_in(0) );
- Map<const Quaternion<typename Derived::Scalar> > q_in ( & delta_in(3) );
- Map<Matrix<typename Derived::Scalar, 3, 1> > p_ret ( & ret(0) );
- Map<Matrix<typename Derived::Scalar, 3, 1> > do_ret ( & ret(3) );
+ Map<const Matrix<typename Derived::Scalar, 3, 1>> p_in(&delta_in(0));
+ Map<const Quaternion<typename Derived::Scalar>> q_in(&delta_in(3));
+ Map<Matrix<typename Derived::Scalar, 3, 1>> p_ret(&ret(0));
+ Map<Matrix<typename Derived::Scalar, 3, 1>> do_ret(&ret(3));
Matrix<typename Derived::Scalar, 3, 3> V_inv;
- do_ret = log_q(q_in);
- V_inv = jac_SO3_left_inv(do_ret);
+ do_ret = log_q(q_in);
+ V_inv = jac_SO3_left_inv(do_ret);
p_ret = V_inv * p_in;
return ret;
}
-template<typename Derived>
+template <typename Derived>
inline Matrix<typename Derived::Scalar, 7, 1> exp(const MatrixBase<Derived>& d_in)
{
MatrixSizeCheck<6, 1>::check(d_in);
@@ -347,10 +349,10 @@ inline Matrix<typename Derived::Scalar, 7, 1> exp(const MatrixBase<Derived>& d_i
V = jac_SO3_left(d_in.template tail<3>());
- Map<const Matrix<typename Derived::Scalar, 3, 1> > p_in ( & d_in(0) );
- Map<const Matrix<typename Derived::Scalar, 3, 1> > o_in ( & d_in(3) );
- Map<Matrix<typename Derived::Scalar, 3, 1> > p ( & ret(0) );
- Map<Quaternion<typename Derived::Scalar> > q ( & ret(3) );
+ Map<const Matrix<typename Derived::Scalar, 3, 1>> p_in(&d_in(0));
+ Map<const Matrix<typename Derived::Scalar, 3, 1>> o_in(&d_in(3));
+ Map<Matrix<typename Derived::Scalar, 3, 1>> p(&ret(0));
+ Map<Quaternion<typename Derived::Scalar>> q(&ret(3));
p = V * p_in;
q = exp_q(o_in);
@@ -363,20 +365,23 @@ inline VectorComposite exp(const VectorComposite& tau)
const auto& p = tau.at('P');
const auto& theta = tau.at('O');
- Matrix3d V = jac_SO3_left(theta); // see [1] eqs. (174) and (145)
+ Matrix3d V = jac_SO3_left(theta); // see [1] eqs. (174) and (145)
- VectorComposite res;
+ VectorComposite res;
- res['P'] = V * p ;
- res['O'] = exp_q(theta).coeffs() ;
+ res['P'] = V * p;
+ res['O'] = exp_q(theta).coeffs();
return res;
}
-template<typename D1, typename D2, typename D4, typename D5, typename D7, typename D8>
-inline void plus(const MatrixBase<D1>& p1, const QuaternionBase<D2>& q1,
- const MatrixBase<D4>& p2, const MatrixBase<D5>& o2,
- MatrixBase<D7>& plus_p, QuaternionBase<D8>& plus_q)
+template <typename D1, typename D2, typename D4, typename D5, typename D7, typename D8>
+inline void plus(const MatrixBase<D1>& p1,
+ const QuaternionBase<D2>& q1,
+ const MatrixBase<D4>& p2,
+ const MatrixBase<D5>& o2,
+ MatrixBase<D7>& plus_p,
+ QuaternionBase<D8>& plus_q)
{
MatrixSizeCheck<3, 1>::check(p1);
MatrixSizeCheck<3, 1>::check(p2);
@@ -387,38 +392,38 @@ inline void plus(const MatrixBase<D1>& p1, const QuaternionBase<D2>& q1,
plus_q = q1 * exp_q(o2);
}
-template<typename D1, typename D2, typename D4, typename D5, typename D7, typename D8>
-inline void plus(const MatrixBase<D1>& p1, const MatrixBase<D2>& q1,
- const MatrixBase<D4>& p2, const MatrixBase<D5>& o2,
- MatrixBase<D7>& plus_p, MatrixBase<D8>& plus_q)
+template <typename D1, typename D2, typename D4, typename D5, typename D7, typename D8>
+inline void plus(const MatrixBase<D1>& p1,
+ const MatrixBase<D2>& q1,
+ const MatrixBase<D4>& p2,
+ const MatrixBase<D5>& o2,
+ MatrixBase<D7>& plus_p,
+ MatrixBase<D8>& plus_q)
{
MatrixSizeCheck<4, 1>::check(q1);
MatrixSizeCheck<4, 1>::check(plus_q);
- Map<const Quaterniond> q1m ( & q1 (0) );
- Map<Quaterniond> plus_qm ( & plus_q(0) );
+ Map<const Quaterniond> q1m(&q1(0));
+ Map<Quaterniond> plus_qm(&plus_q(0));
plus(p1, q1m, p2, o2, plus_p, plus_qm);
}
-template<typename D1, typename D2, typename D3>
-inline void plus(const MatrixBase<D1>& d1,
- const MatrixBase<D2>& d2,
- MatrixBase<D3>& d_plus)
+template <typename D1, typename D2, typename D3>
+inline void plus(const MatrixBase<D1>& d1, const MatrixBase<D2>& d2, MatrixBase<D3>& d_plus)
{
- Map<const Matrix<typename D1::Scalar, 3, 1> > p1 ( & d1(0) );
- Map<const Quaternion<typename D1::Scalar> > q1 ( & d1(3) );
- Map<const Matrix<typename D2::Scalar, 3, 1> > p2 ( & d2(0) );
- Map<const Matrix<typename D2::Scalar, 3, 1> > o2 ( & d2(3) );
- Map<Matrix<typename D3::Scalar, 3, 1> > p_p ( & d_plus(0) );
- Map<Quaternion<typename D3::Scalar> > q_p ( & d_plus(3) );
+ Map<const Matrix<typename D1::Scalar, 3, 1>> p1(&d1(0));
+ Map<const Quaternion<typename D1::Scalar>> q1(&d1(3));
+ Map<const Matrix<typename D2::Scalar, 3, 1>> p2(&d2(0));
+ Map<const Matrix<typename D2::Scalar, 3, 1>> o2(&d2(3));
+ Map<Matrix<typename D3::Scalar, 3, 1>> p_p(&d_plus(0));
+ Map<Quaternion<typename D3::Scalar>> q_p(&d_plus(3));
plus(p1, q1, p2, o2, p_p, q_p);
}
-template<typename D1, typename D2>
-inline Matrix<typename D1::Scalar, 7, 1> plus(const MatrixBase<D1>& d1,
- const MatrixBase<D2>& d2)
+template <typename D1, typename D2>
+inline Matrix<typename D1::Scalar, 7, 1> plus(const MatrixBase<D1>& d1, const MatrixBase<D2>& d2)
{
Matrix<typename D1::Scalar, 7, 1> d_plus;
plus(d1, d2, d_plus);
@@ -442,55 +447,60 @@ inline VectorComposite plus(const VectorComposite& x, const VectorComposite& tau
return res;
}
-template<typename D1, typename D2, typename D4, typename D5, typename D7, typename D8>
-inline void minus(const MatrixBase<D1>& p1, const QuaternionBase<D2>& q1,
- const MatrixBase<D4>& p2, const QuaternionBase<D5>& q2,
- MatrixBase<D7>& pd, MatrixBase<D8>& od )
+template <typename D1, typename D2, typename D4, typename D5, typename D7, typename D8>
+inline void minus(const MatrixBase<D1>& p1,
+ const QuaternionBase<D2>& q1,
+ const MatrixBase<D4>& p2,
+ const QuaternionBase<D5>& q2,
+ MatrixBase<D7>& pd,
+ MatrixBase<D8>& od)
{
pd = p2 - p1;
od = log_q(q1.conjugate() * q2);
}
-template<typename D1, typename D2, typename D4, typename D5, typename D6, typename D7, typename D8, typename D9>
-inline void minus(const MatrixBase<D1>& p1, const QuaternionBase<D2>& q1,
- const MatrixBase<D4>& p2, const QuaternionBase<D5>& q2,
- MatrixBase<D6>& pd, MatrixBase<D7>& od,
- MatrixBase<D8>& J_do_q1, MatrixBase<D9>& J_do_q2)
+template <typename D1, typename D2, typename D4, typename D5, typename D6, typename D7, typename D8, typename D9>
+inline void minus(const MatrixBase<D1>& p1,
+ const QuaternionBase<D2>& q1,
+ const MatrixBase<D4>& p2,
+ const QuaternionBase<D5>& q2,
+ MatrixBase<D6>& pd,
+ MatrixBase<D7>& od,
+ MatrixBase<D8>& J_do_q1,
+ MatrixBase<D9>& J_do_q2)
{
minus(p1, q1, p2, q2, pd, od);
- J_do_q1 = - jac_SO3_left_inv(od);
- J_do_q2 = jac_SO3_right_inv(od);
+ J_do_q1 = -jac_SO3_left_inv(od);
+ J_do_q2 = jac_SO3_right_inv(od);
}
-template<typename D1, typename D2, typename D3>
-inline void minus(const MatrixBase<D1>& d1,
- const MatrixBase<D2>& d2,
- MatrixBase<D3>& err)
+template <typename D1, typename D2, typename D3>
+inline void minus(const MatrixBase<D1>& d1, const MatrixBase<D2>& d2, MatrixBase<D3>& err)
{
- Map<const Matrix<typename D1::Scalar, 3, 1> > p1 ( & d1(0) );
- Map<const Quaternion<typename D1::Scalar> > q1 ( & d1(3) );
- Map<const Matrix<typename D2::Scalar, 3, 1> > p2 ( & d2(0) );
- Map<const Quaternion<typename D2::Scalar> > q2 ( & d2(3) );
- Map<Matrix<typename D3::Scalar, 3, 1> > pd ( & err(0) );
- Map<Matrix<typename D3::Scalar, 3, 1> > od ( & err(3) );
+ Map<const Matrix<typename D1::Scalar, 3, 1>> p1(&d1(0));
+ Map<const Quaternion<typename D1::Scalar>> q1(&d1(3));
+ Map<const Matrix<typename D2::Scalar, 3, 1>> p2(&d2(0));
+ Map<const Quaternion<typename D2::Scalar>> q2(&d2(3));
+ Map<Matrix<typename D3::Scalar, 3, 1>> pd(&err(0));
+ Map<Matrix<typename D3::Scalar, 3, 1>> od(&err(3));
minus(p1, q1, p2, q2, pd, od);
}
-template<typename D1, typename D2, typename D3, typename D4, typename D5>
+template <typename D1, typename D2, typename D3, typename D4, typename D5>
inline void minus(const MatrixBase<D1>& d1,
const MatrixBase<D2>& d2,
- MatrixBase<D3>& dif,
- MatrixBase<D4>& J_diff_d1,
- MatrixBase<D5>& J_diff_d2)
+ MatrixBase<D3>& dif,
+ MatrixBase<D4>& J_diff_d1,
+ MatrixBase<D5>& J_diff_d2)
{
- Map<const Matrix<typename D1::Scalar, 3, 1> > p1 ( & d1(0) );
- Map<const Quaternion<typename D1::Scalar> > q1 ( & d1(3) );
- Map<const Matrix<typename D2::Scalar, 3, 1> > p2 ( & d2(0) );
- Map<const Quaternion<typename D2::Scalar> > q2 ( & d2(3) );
- Map<Matrix<typename D3::Scalar, 3, 1> > pd ( & dif(0) );
- Map<Matrix<typename D3::Scalar, 3, 1> > od ( & dif(3) );
+ Map<const Matrix<typename D1::Scalar, 3, 1>> p1(&d1(0));
+ Map<const Quaternion<typename D1::Scalar>> q1(&d1(3));
+ Map<const Matrix<typename D2::Scalar, 3, 1>> p2(&d2(0));
+ Map<const Quaternion<typename D2::Scalar>> q2(&d2(3));
+ Map<Matrix<typename D3::Scalar, 3, 1>> pd(&dif(0));
+ Map<Matrix<typename D3::Scalar, 3, 1>> od(&dif(3));
Matrix<typename D4::Scalar, 3, 3> J_do_q1, J_do_q2;
@@ -504,27 +514,26 @@ inline void minus(const MatrixBase<D1>& d1,
* J_do_q1 = - J_l_inv(theta)
* J_do_q2 = J_r_inv(theta)
*/
- J_diff_d1 = - Matrix<typename D4::Scalar, 6, 6>::Identity();// d(p2 - p1) / d(p1) = - Identity
- J_diff_d1.block(3,3,3,3) = J_do_q1; // d(R1.tr*R2) / d(R1) = - J_l_inv(theta)
+ J_diff_d1 = -Matrix<typename D4::Scalar, 6, 6>::Identity(); // d(p2 - p1) / d(p1) = - Identity
+ J_diff_d1.block(3, 3, 3, 3) = J_do_q1; // d(R1.tr*R2) / d(R1) = - J_l_inv(theta)
- J_diff_d2.setIdentity(6,6); // d(R1.tr*R2) / d(R2) = Identity
- J_diff_d2.block(3,3,3,3) = J_do_q2; // d(R1.tr*R2) / d(R1) = J_r_inv(theta)
+ J_diff_d2.setIdentity(6, 6); // d(R1.tr*R2) / d(R2) = Identity
+ J_diff_d2.block(3, 3, 3, 3) = J_do_q2; // d(R1.tr*R2) / d(R1) = J_r_inv(theta)
}
-template<typename D1, typename D2>
-inline Matrix<typename D1::Scalar, 6, 1> minus(const MatrixBase<D1>& d1,
- const MatrixBase<D2>& d2)
+template <typename D1, typename D2>
+inline Matrix<typename D1::Scalar, 6, 1> minus(const MatrixBase<D1>& d1, const MatrixBase<D2>& d2)
{
Matrix<typename D1::Scalar, 6, 1> ret;
minus(d1, d2, ret);
return ret;
}
-template<typename D1, typename D2, typename D3>
-inline void interpolate(const MatrixBase<D1>& d1,
- const MatrixBase<D2>& d2,
+template <typename D1, typename D2, typename D3>
+inline void interpolate(const MatrixBase<D1>& d1,
+ const MatrixBase<D2>& d2,
const typename D1::Scalar& t,
- MatrixBase<D3>& ret)
+ MatrixBase<D3>& ret)
{
Matrix<typename D1::Scalar, 7, 1> dd = between(d1, d2);
@@ -533,87 +542,87 @@ inline void interpolate(const MatrixBase<D1>& d1,
ret = compose(d1, exp(tau));
}
-template<typename D1, typename D2>
-inline void toSE3(const MatrixBase<D1>& pose,
- MatrixBase<D2>& SE3)
+template <typename D1, typename D2>
+inline void toSE3(const MatrixBase<D1>& pose, MatrixBase<D2>& SE3)
{
- MatrixSizeCheck<4,4>::check(SE3);
+ MatrixSizeCheck<4, 4>::check(SE3);
typedef typename D1::Scalar T;
- SE3.template block<3,1>(0,3) = pose.template head<3>();
- SE3.template block<3,3>(0,0) = q2R(pose.template tail<4>());
- SE3.template block<1,3>(3,0).setZero();
- SE3(3,3) = (T)1.0;
+ SE3.template block<3, 1>(0, 3) = pose.template head<3>();
+ SE3.template block<3, 3>(0, 0) = q2R(pose.template tail<4>());
+ SE3.template block<1, 3>(3, 0).setZero();
+ SE3(3, 3) = (T)1.0;
}
-template<typename D1, typename D2>
+template <typename D1, typename D2>
inline Matrix<typename D1::Scalar, 6, 6> Q_helper(const MatrixBase<D1>& v, const MatrixBase<D1>& w)
{
typedef typename D1::Scalar T;
- Matrix<T, 3, 3> V = skew(v);
- Matrix<T, 3, 3> W = skew(w);
- Matrix<T, 3, 3> VW = V * W;
- Matrix<T, 3, 3> WV = VW.transpose(); // Note on this change wrt. Barfoot: it happens that V*W = (W*V).transpose() !!!
+ Matrix<T, 3, 3> V = skew(v);
+ Matrix<T, 3, 3> W = skew(w);
+ Matrix<T, 3, 3> VW = V * W;
+ Matrix<T, 3, 3> WV =
+ VW.transpose(); // Note on this change wrt. Barfoot: it happens that V*W = (W*V).transpose() !!!
Matrix<T, 3, 3> WVW = WV * W;
Matrix<T, 3, 3> VWW = VW * W;
- T th_2 = w.squaredNorm();
+ T th_2 = w.squaredNorm();
T A(T(0.5)), B, C, D;
// Small angle approximation
if (th_2 <= T(1e-8))
{
- B = double(1./6.) + double(1./120.) * th_2;
- C = -double(1./24.) + double(1./720.) * th_2;
- D = -double(1./60.);
+ B = double(1. / 6.) + double(1. / 120.) * th_2;
+ C = -double(1. / 24.) + double(1. / 720.) * th_2;
+ D = -double(1. / 60.);
}
else
{
- T th = sqrt(th_2);
- T th_3 = th_2*th;
- T th_4 = th_2*th_2;
- T th_5 = th_3*th_2;
- T sin_th = sin(th);
- T cos_th = cos(th);
- B = (th-sin_th)/th_3;
- C = (T(1.0) - th_2/T(2.0) - cos_th)/th_4;
- D = (th - sin_th - th_3/T(6.0))/th_5;
+ T th = sqrt(th_2);
+ T th_3 = th_2 * th;
+ T th_4 = th_2 * th_2;
+ T th_5 = th_3 * th_2;
+ T sin_th = sin(th);
+ T cos_th = cos(th);
+ B = (th - sin_th) / th_3;
+ C = (T(1.0) - th_2 / T(2.0) - cos_th) / th_4;
+ D = (th - sin_th - th_3 / T(6.0)) / th_5;
}
Matrix<T, 3, 3> Q;
Q.noalias() =
- + A * V
- + B * (WV + VW + WVW)
- - C * (VWW - VWW.transpose() - double(3) * WVW) // Note on this change wrt. Barfoot: it happens that V*W*W = -(W*W*V).transpose() !!!
- - D * WVW * W; // Note on this change wrt. Barfoot: it happens that W*V*W*W = W*W*V*W !!!
+ +A * V + B * (WV + VW + WVW) -
+ C * (VWW - VWW.transpose() -
+ double(3) * WVW) // Note on this change wrt. Barfoot: it happens that V*W*W = -(W*W*V).transpose() !!!
+ - D * WVW * W; // Note on this change wrt. Barfoot: it happens that W*V*W*W = W*W*V*W !!!
return Q;
}
-template<typename D1>
+template <typename D1>
inline Matrix<typename D1::Scalar, 6, 6> jac_SE3_left(const MatrixBase<D1>& tangent)
{
typedef typename D1::Scalar T;
- Map<Matrix<T, 3, 1>> v(tangent.data() + 0); // linear
- Map<Matrix<T, 3, 1>> w(tangent.data() + 3); // angular
+ Map<Matrix<T, 3, 1>> v(tangent.data() + 0); // linear
+ Map<Matrix<T, 3, 1>> w(tangent.data() + 3); // angular
Matrix<T, 3, 3> Jl(jac_SO3_left(w));
- Matrix<T, 3, 3> Q = Q_helper(v,w);
+ Matrix<T, 3, 3> Q = Q_helper(v, w);
Matrix<T, 6, 6> Jl_SE3;
- Jl_SE3.topLeftCorner(3,3) = Jl;
- Jl_SE3.bottomRightCorner(3,3) = Jl;
- Jl_SE3.topRightCorner(3,3) = Q;
- Jl_SE3.bottomLeftCorner(3,3) .setZero();
+ Jl_SE3.topLeftCorner(3, 3) = Jl;
+ Jl_SE3.bottomRightCorner(3, 3) = Jl;
+ Jl_SE3.topRightCorner(3, 3) = Q;
+ Jl_SE3.bottomLeftCorner(3, 3).setZero();
}
-template<typename D1>
+template <typename D1>
inline Matrix<typename D1::Scalar, 6, 6> jac_SE3_right(const MatrixBase<D1>& tangent)
{
return jac_SE3_left(-tangent);
}
-} // namespace three_d
-} // namespace wolf
+} // namespace SE3
+} // namespace wolf
diff --git a/include/core/math/covariance.h b/include/core/math/covariance.h
index e083cb4c75d2f367a9892668790b3c61dc9b0ac1..729549c276f1ab8f151bba8fcae8c1c03e551610 100644
--- a/include/core/math/covariance.h
+++ b/include/core/math/covariance.h
@@ -22,49 +22,45 @@
#include <Eigen/Dense>
-namespace wolf{
-
+namespace wolf
+{
template <typename T, int N, int RC>
-inline bool isSymmetric(const Eigen::Matrix<T, N, N, RC>& M,
- const T eps = wolf::Constants::EPS)
+inline bool isSymmetric(const Eigen::Matrix<T, N, N, RC>& M, const T eps = wolf::Constants::EPS)
{
- if (M.cols() != M.rows())
- return false;
- return M.isApprox(M.transpose(), eps);
+ if (M.cols() != M.rows()) return false;
+ return M.isApprox(M.transpose(), eps);
}
template <typename T, int N, int RC>
inline bool isPositiveSemiDefinite(const Eigen::Matrix<T, N, N, RC>& M, const T& eps = Constants::EPS)
{
- Eigen::SelfAdjointEigenSolver<Eigen::Matrix<T, N, N, RC> > eigensolver(M, Eigen::EigenvaluesOnly);
+ Eigen::SelfAdjointEigenSolver<Eigen::Matrix<T, N, N, RC> > eigensolver(M, Eigen::EigenvaluesOnly);
- if (eigensolver.info() == Eigen::Success)
- {
- // All eigenvalues must be >= 0:
- return (eigensolver.eigenvalues().array() >= eps).all();
- }
- else
- std::cout << "eigen decomposition failed" << std::endl;
+ if (eigensolver.info() == Eigen::Success)
+ {
+ // All eigenvalues must be >= 0:
+ return (eigensolver.eigenvalues().array() >= eps).all();
+ }
+ else
+ std::cout << "eigen decomposition failed" << std::endl;
- return false;
+ return false;
}
template <typename T, int N, int RC>
inline bool isCovariance(const Eigen::Matrix<T, N, N, RC>& M, const T& eps = Constants::EPS)
{
- return isSymmetric(M) && isPositiveSemiDefinite(M, eps);
+ return isSymmetric(M) && isPositiveSemiDefinite(M, eps);
}
-#define WOLF_ASSERT_COVARIANCE_MATRIX(x) \
- assert(isCovariance(x, Constants::EPS_SMALL) && "Not a covariance");
+#define WOLF_ASSERT_COVARIANCE_MATRIX(x) assert(isCovariance(x, Constants::EPS_SMALL) && "Not a covariance");
-#define WOLF_ASSERT_INFORMATION_MATRIX(x) \
- assert(isCovariance(x, double(0.0)) && "Not an information matrix");
+#define WOLF_ASSERT_INFORMATION_MATRIX(x) assert(isCovariance(x, double(0.0)) && "Not an information matrix");
template <typename T, int N, int RC>
-inline bool makePosDef(Eigen::Matrix<T,N,N,RC>& M, const T& eps = Constants::EPS)
+inline bool makePosDef(Eigen::Matrix<T, N, N, RC>& M, const T& eps = Constants::EPS)
{
- Eigen::SelfAdjointEigenSolver<Eigen::Matrix<T,N,N,RC> > eigensolver(M);
+ Eigen::SelfAdjointEigenSolver<Eigen::Matrix<T, N, N, RC> > eigensolver(M);
if (eigensolver.info() == Eigen::Success)
{
@@ -72,17 +68,16 @@ inline bool makePosDef(Eigen::Matrix<T,N,N,RC>& M, const T& eps = Constants::EPS
double epsilon = eps;
while ((eigensolver.eigenvalues().array() < eps).any())
{
- //std::cout << "----- any negative eigenvalue or too close to zero\n";
- //std::cout << "previous eigenvalues: " << eigensolver.eigenvalues().transpose() << std::endl;
- //std::cout << "previous determinant: " << M.determinant() << std::endl;
- M = eigensolver.eigenvectors() *
- eigensolver.eigenvalues().cwiseMax(epsilon).asDiagonal() *
+ // std::cout << "----- any negative eigenvalue or too close to zero\n";
+ // std::cout << "previous eigenvalues: " << eigensolver.eigenvalues().transpose() << std::endl;
+ // std::cout << "previous determinant: " << M.determinant() << std::endl;
+ M = eigensolver.eigenvectors() * eigensolver.eigenvalues().cwiseMax(epsilon).asDiagonal() *
eigensolver.eigenvectors().transpose();
eigensolver.compute(M);
- //std::cout << "epsilon used: " << epsilon << std::endl;
- //std::cout << "posterior eigenvalues: " << eigensolver.eigenvalues().transpose() << std::endl;
- //std::cout << "posterior determinant: " << M.determinant() << std::endl;
- epsilon *=10;
+ // std::cout << "epsilon used: " << epsilon << std::endl;
+ // std::cout << "posterior eigenvalues: " << eigensolver.eigenvalues().transpose() << std::endl;
+ // std::cout << "posterior determinant: " << M.determinant() << std::endl;
+ epsilon *= 10;
}
WOLF_ASSERT_COVARIANCE_MATRIX(M);
@@ -94,26 +89,25 @@ inline bool makePosDef(Eigen::Matrix<T,N,N,RC>& M, const T& eps = Constants::EPS
return false;
}
-inline Eigen::MatrixXd computeSqrtUpper(const Eigen::MatrixXd & _info)
+inline Eigen::MatrixXd computeSqrtUpper(const Eigen::MatrixXd& _info)
{
// impose symmetry
Eigen::MatrixXd info = _info.selfadjointView<Eigen::Upper>();
// Normal Cholesky factorization
Eigen::LLT<Eigen::MatrixXd> llt_of_info(info);
- Eigen::MatrixXd R = llt_of_info.matrixU();
+ Eigen::MatrixXd R = llt_of_info.matrixU();
// Good factorization
- if (info.isApprox(R.transpose() * R, Constants::EPS))
- return R;
+ if (info.isApprox(R.transpose() * R, Constants::EPS)) return R;
// Not good factorization: SelfAdjointEigenSolver
Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> es(info);
- Eigen::VectorXd eval = es.eigenvalues().real().cwiseMax(Constants::EPS);
+ Eigen::VectorXd eval = es.eigenvalues().real().cwiseMax(Constants::EPS);
R = eval.cwiseSqrt().asDiagonal() * es.eigenvectors().real().transpose();
return R;
}
-}
+} // namespace wolf
diff --git a/include/core/math/rotations.h b/include/core/math/rotations.h
index 651bd5900b4bb85138bf85c272d31f63ea8cc93f..2823f74dfa3b36675c840c25063835c022e83170 100644
--- a/include/core/math/rotations.h
+++ b/include/core/math/rotations.h
@@ -24,18 +24,18 @@
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)
+ * 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>
+template <typename T>
inline T pi2pi(const T& _angle)
{
- return remainder(_angle,2*M_PI);
+ return remainder(_angle, 2 * M_PI);
}
/** \brief Conversion to radians
@@ -43,9 +43,8 @@ inline T pi2pi(const T& _angle)
* @param deg angle in degrees
* @return angle in radians
*/
-template<typename T>
-inline T
-toRad(const T deg)
+template <typename T>
+inline T toRad(const T deg)
{
return (T)M_TORAD * deg;
}
@@ -55,9 +54,8 @@ toRad(const T deg)
* @param rad angle in radians
* @return angle in degrees
*/
-template<typename T>
-inline T
-toDeg(const T rad)
+template <typename T>
+inline T toDeg(const T rad)
{
return (T)M_TODEG * rad;
}
@@ -70,19 +68,16 @@ toDeg(const T rad)
* @param _v a 3d vector
* @return the skew-symmetric matrix V so that V*u = _v.cross(u), for u in R^3
*/
-template<typename Derived>
-inline Eigen::Matrix<typename Derived::Scalar, 3, 3>
-skew(const Eigen::MatrixBase<Derived>& _v)
+template <typename Derived>
+inline Eigen::Matrix<typename Derived::Scalar, 3, 3> skew(const Eigen::MatrixBase<Derived>& _v)
{
- MatrixSizeCheck<3,1>::check(_v);
+ MatrixSizeCheck<3, 1>::check(_v);
typedef typename Derived::Scalar T;
Eigen::Matrix<T, 3, 3> sk;
- sk << (T)0.0 , -_v(2), +_v(1),
- +_v(2), (T)0.0, -_v(0),
- -_v(1), +_v(0), (T)0.0;
+ sk << (T)0.0, -_v(2), +_v(1), +_v(2), (T)0.0, -_v(0), -_v(1), +_v(0), (T)0.0;
return sk;
}
@@ -92,11 +87,10 @@ skew(const Eigen::MatrixBase<Derived>& _v)
* @param _m A skew-symmetric matrix
* @return a 3-vector v such that skew(v) = _m
*/
-template<typename Derived>
-inline Eigen::Matrix<typename Derived::Scalar, 3, 1>
-vee(const Eigen::MatrixBase<Derived>& _m)
+template <typename Derived>
+inline Eigen::Matrix<typename Derived::Scalar, 3, 1> vee(const Eigen::MatrixBase<Derived>& _m)
{
- MatrixSizeCheck<3,3>::check(_m);
+ MatrixSizeCheck<3, 3>::check(_m);
typedef typename Derived::Scalar T;
@@ -113,9 +107,8 @@ vee(const Eigen::MatrixBase<Derived>& _m)
* @param _q a right-handed unit quaternion
* @return the equivalent rotation matrix
*/
-template<typename Derived>
-inline Eigen::Matrix<typename Derived::Scalar, 3, 3>
-q2R(const Eigen::QuaternionBase<Derived>& _q)
+template <typename Derived>
+inline Eigen::Matrix<typename Derived::Scalar, 3, 3> q2R(const Eigen::QuaternionBase<Derived>& _q)
{
return _q.matrix();
}
@@ -125,13 +118,12 @@ q2R(const Eigen::QuaternionBase<Derived>& _q)
* @param _q a right-handed unit quaternion
* @return the equivalent rotation matrix
*/
-template<typename Derived>
-inline Eigen::Matrix<typename Derived::Scalar, 3, 3>
-q2R(const Eigen::MatrixBase<Derived>& _q)
+template <typename Derived>
+inline Eigen::Matrix<typename Derived::Scalar, 3, 3> q2R(const Eigen::MatrixBase<Derived>& _q)
{
- MatrixSizeCheck<4,1>::check(_q);
- Eigen::Quaternion<typename Derived::Scalar> q(_q(3),_q(0),_q(1),_q(2));
- return q2R( q );
+ MatrixSizeCheck<4, 1>::check(_q);
+ Eigen::Quaternion<typename Derived::Scalar> q(_q(3), _q(0), _q(1), _q(2));
+ return q2R(q);
}
/** \brief rotation matrix to quaternion conversion
@@ -139,11 +131,10 @@ q2R(const Eigen::MatrixBase<Derived>& _q)
* @param _R a rotation matrix
* @return the equivalent right-handed unit quaternion
*/
-template<typename Derived>
-inline Eigen::Quaternion<typename Derived::Scalar>
-R2q(const Eigen::MatrixBase<Derived>& _R)
+template <typename Derived>
+inline Eigen::Quaternion<typename Derived::Scalar> R2q(const Eigen::MatrixBase<Derived>& _R)
{
- MatrixSizeCheck<3,3>::check(_R);
+ MatrixSizeCheck<3, 3>::check(_R);
return Eigen::Quaternion<typename Derived::Scalar>(_R);
}
@@ -152,11 +143,10 @@ R2q(const Eigen::MatrixBase<Derived>& _R)
* \param _euler = (roll pitch yaw) in ZYX convention
* \return equivalent quaternion
*/
-template<typename D>
-inline Eigen::Quaternion<typename D::Scalar>
-e2q(const Eigen::MatrixBase<D>& _euler)
+template <typename D>
+inline Eigen::Quaternion<typename D::Scalar> e2q(const Eigen::MatrixBase<D>& _euler)
{
- MatrixSizeCheck<3,1>::check(_euler);
+ MatrixSizeCheck<3, 1>::check(_euler);
typedef typename D::Scalar T;
@@ -171,9 +161,8 @@ e2q(const Eigen::MatrixBase<D>& _euler)
* \param _e = (roll pitch yaw) in ZYX convention
* \return equivalent rotation matrix
*/
-template<typename Derived>
-inline Eigen::Matrix<typename Derived::Scalar, 3, 3>
-e2R(const Eigen::MatrixBase<Derived>& _e)
+template <typename Derived>
+inline Eigen::Matrix<typename Derived::Scalar, 3, 3> e2R(const Eigen::MatrixBase<Derived>& _e)
{
MatrixSizeCheck<3, 1>::check(_e);
@@ -181,66 +170,62 @@ e2R(const Eigen::MatrixBase<Derived>& _e)
}
template <typename Derived>
-inline typename Eigen::MatrixBase<Derived>::Scalar
-getYaw(const Eigen::MatrixBase<Derived>& R)
+inline typename Eigen::MatrixBase<Derived>::Scalar getYaw(const Eigen::MatrixBase<Derived>& R)
{
MatrixSizeCheck<3, 3>::check(R);
using std::atan2;
- return atan2( R(1, 0), R(0, 0) );
+ return atan2(R(1, 0), R(0, 0));
}
-template<typename Derived>
-inline typename Eigen::Matrix<typename Derived::Scalar, 3, 1>
-R2e(const Eigen::MatrixBase<Derived>& _R)
+template <typename Derived>
+inline typename Eigen::Matrix<typename Derived::Scalar, 3, 1> R2e(const Eigen::MatrixBase<Derived>& _R)
{
Eigen::Matrix<typename Derived::Scalar, 3, 1> e;
- e(0) = atan2( _R(2,1), _R(2,2));
- e(1) = atan2(-_R(2,0), sqrt(_R(0,0)*_R(0,0) + _R(1,0)*_R(1,0)));
- e(2) = atan2( _R(1,0), _R(0,0));
+ e(0) = atan2(_R(2, 1), _R(2, 2));
+ e(1) = atan2(-_R(2, 0), sqrt(_R(0, 0) * _R(0, 0) + _R(1, 0) * _R(1, 0)));
+ e(2) = atan2(_R(1, 0), _R(0, 0));
return e;
}
-template<typename Derived>
-inline typename Eigen::Matrix<typename Derived::Scalar, 3, 1>
-q2e(const Eigen::MatrixBase<Derived>& _q)
+template <typename Derived>
+inline typename Eigen::Matrix<typename Derived::Scalar, 3, 1> q2e(const Eigen::MatrixBase<Derived>& _q)
{
typedef typename Derived::Scalar T;
- Eigen::Matrix<T, 3, 1> e;
-
- double w = _q(3);
- double x = _q(0);
- double y = _q(1);
- double z = _q(2);
-
- double xx = x*x;
- double xy = x*y;
- double xz = x*z;
- double yy = y*y;
- double yz = y*z;
- double zz = z*z;
- double wx = w*x;
- double wy = w*y;
- double wz = w*z;
-
- double r00 = T(1) - T(2) * ( yy + zz );
- double r10 = T(2) * ( xy + wz );
- double r20 = T(2) * ( xz - wy );
- double r21 = T(2) * ( yz + wx );
- double r22 = T(1) - T(2) * ( xx + yy );
-
- e(0) = atan2( r21, r22);
- e(1) = atan2(-r20, sqrt(r00*r00 + r10*r10));
- e(2) = atan2( r10, r00);
+ Eigen::Matrix<T, 3, 1> e;
+
+ double w = _q(3);
+ double x = _q(0);
+ double y = _q(1);
+ double z = _q(2);
+
+ double xx = x * x;
+ double xy = x * y;
+ double xz = x * z;
+ double yy = y * y;
+ double yz = y * z;
+ double zz = z * z;
+ double wx = w * x;
+ double wy = w * y;
+ double wz = w * z;
+
+ double r00 = T(1) - T(2) * (yy + zz);
+ double r10 = T(2) * (xy + wz);
+ double r20 = T(2) * (xz - wy);
+ double r21 = T(2) * (yz + wx);
+ double r22 = T(1) - T(2) * (xx + yy);
+
+ e(0) = atan2(r21, r22);
+ e(1) = atan2(-r20, sqrt(r00 * r00 + r10 * r10));
+ e(2) = atan2(r10, r00);
return e;
}
-template<typename Derived>
-inline typename Eigen::Matrix<typename Derived::Scalar, 3, 1>
-q2e(const Eigen::QuaternionBase<Derived>& _q)
+template <typename Derived>
+inline typename Eigen::Matrix<typename Derived::Scalar, 3, 1> q2e(const Eigen::QuaternionBase<Derived>& _q)
{
return q2e(_q.coeffs());
}
@@ -253,28 +238,27 @@ q2e(const Eigen::QuaternionBase<Derived>& _q)
* @param _v a rotation vector with _v.norm() the rotated angle and _v.normalized() the rotation axis.
* @return the right-handed unit quaternion performing the rotation encoded by _v
*/
-template<typename Derived>
-inline Eigen::Quaternion<typename Derived::Scalar>
-exp_q(const Eigen::MatrixBase<Derived>& _v)
+template <typename Derived>
+inline Eigen::Quaternion<typename Derived::Scalar> exp_q(const Eigen::MatrixBase<Derived>& _v)
{
- using std::sqrt;
using std::cos;
using std::sin;
+ using std::sqrt;
- MatrixSizeCheck<3,1>::check(_v);
+ MatrixSizeCheck<3, 1>::check(_v);
typedef typename Derived::Scalar T;
T angle_squared = _v.squaredNorm();
- T angle = sqrt(angle_squared); // Allow ceres::Jet to use its own sqrt() version.
+ T angle = sqrt(angle_squared); // Allow ceres::Jet to use its own sqrt() version.
if (angle > (T)(wolf::Constants::EPS_SMALL))
{
- return Eigen::Quaternion<T> ( Eigen::AngleAxis<T>(angle, _v.normalized()) );
+ return Eigen::Quaternion<T>(Eigen::AngleAxis<T>(angle, _v.normalized()));
}
else
{
- return Eigen::Quaternion<T> ( (T)1.0 , _v(0,0)/(T)2 , _v(1,0)/(T)2 , _v(2,0)/(T)2 );
+ return Eigen::Quaternion<T>((T)1.0, _v(0, 0) / (T)2, _v(1, 0) / (T)2, _v(2, 0) / (T)2);
}
}
@@ -283,11 +267,9 @@ exp_q(const Eigen::MatrixBase<Derived>& _v)
* @param _q a unit right-handed quaternion
* @return a rotation vector v such that _q = exp_q(v)
*/
-template<typename Derived>
-inline Eigen::Matrix<typename Derived::Scalar, 3, 1>
-log_q(const Eigen::QuaternionBase<Derived>& _q)
+template <typename Derived>
+inline Eigen::Matrix<typename Derived::Scalar, 3, 1> log_q(const Eigen::QuaternionBase<Derived>& _q)
{
-
// Will try this implementation once Eigen accepts it!
// see https://forum.kde.org/viewtopic.php?f=74&t=143269&p=385299#p385265
// typedef typename Derived::Scalar T;
@@ -298,11 +280,11 @@ log_q(const Eigen::QuaternionBase<Derived>& _q)
typedef typename Derived::Scalar T;
- Eigen::Matrix<T, 3, 1> vec = _q.vec();
- const T sin_angle_squared = vec.squaredNorm();
+ Eigen::Matrix<T, 3, 1> vec = _q.vec();
+ const T sin_angle_squared = vec.squaredNorm();
if (sin_angle_squared > (T)wolf::Constants::EPS_SMALL)
{
- const T sin_angle = sqrt(sin_angle_squared); // Allow ceres::Jet to use its own sqrt() version.
+ const T sin_angle = sqrt(sin_angle_squared); // Allow ceres::Jet to use its own sqrt() version.
const T& cos_angle = _q.w();
/* If (cos_angle < 0) then angle >= pi/2 , means : angle for angle_axis vector >= pi (== 2*angle)
@@ -314,7 +296,8 @@ log_q(const Eigen::QuaternionBase<Derived>& _q)
angle - pi = atan(sin(angle - pi), cos(angle - pi))
= atan(-sin(angle), -cos(angle))
*/
- const T two_angle = T(2.0) * ((cos_angle < T(0.0)) ? atan2(-sin_angle, -cos_angle) : atan2(sin_angle, cos_angle));
+ const T two_angle =
+ T(2.0) * ((cos_angle < T(0.0)) ? atan2(-sin_angle, -cos_angle) : atan2(sin_angle, cos_angle));
const T k = two_angle / sin_angle;
return vec * k;
}
@@ -330,9 +313,8 @@ log_q(const Eigen::QuaternionBase<Derived>& _q)
* @param _v a rotation vector
* @return the rotation matrix performing the rotation encoded by _v
*/
-template<typename Derived>
-inline Eigen::Matrix<typename Derived::Scalar, 3, 3>
-exp_R(const Eigen::MatrixBase<Derived>& _v)
+template <typename Derived>
+inline Eigen::Matrix<typename Derived::Scalar, 3, 3> exp_R(const Eigen::MatrixBase<Derived>& _v)
{
using std::sqrt;
@@ -341,7 +323,7 @@ exp_R(const Eigen::MatrixBase<Derived>& _v)
typedef typename Derived::Scalar T;
T angle_squared = _v.squaredNorm();
- T angle = sqrt(angle_squared); // Allow ceres::Jet to use its own sqrt() version.
+ T angle = sqrt(angle_squared); // Allow ceres::Jet to use its own sqrt() version.
if (angle > wolf::Constants::EPS_SMALL)
return Eigen::AngleAxis<T>(angle, _v.normalized()).toRotationMatrix();
@@ -354,9 +336,8 @@ exp_R(const Eigen::MatrixBase<Derived>& _v)
* @param _R a 3d rotation matrix
* @return the rotation vector v such that _R = exp_R(v)
*/
-template<typename Derived>
-inline Eigen::Matrix<typename Derived::Scalar, 3, 1>
-log_R(const Eigen::MatrixBase<Derived>& _R)
+template <typename Derived>
+inline Eigen::Matrix<typename Derived::Scalar, 3, 1> log_R(const Eigen::MatrixBase<Derived>& _R)
{
return log_q(R2q(_R));
}
@@ -366,9 +347,8 @@ log_R(const Eigen::MatrixBase<Derived>& _R)
* @param _v a rotation vector
* @return the equivalent right-handed unit quaternion
*/
-template<typename Derived>
-inline Eigen::Quaternion<typename Derived::Scalar>
-v2q(const Eigen::MatrixBase<Derived>& _v)
+template <typename Derived>
+inline Eigen::Quaternion<typename Derived::Scalar> v2q(const Eigen::MatrixBase<Derived>& _v)
{
return exp_q(_v);
}
@@ -378,9 +358,8 @@ v2q(const Eigen::MatrixBase<Derived>& _v)
* @param _q a right-handed unit quaternion
* @return the equivalent rotation vector
*/
-template<typename Derived>
-inline Eigen::Matrix<typename Derived::Scalar, 3, 1>
-q2v(const Eigen::QuaternionBase<Derived>& _q)
+template <typename Derived>
+inline Eigen::Matrix<typename Derived::Scalar, 3, 1> q2v(const Eigen::QuaternionBase<Derived>& _q)
{
return log_q(_q);
}
@@ -390,9 +369,8 @@ q2v(const Eigen::QuaternionBase<Derived>& _q)
* @param _v a rotation vector
* @return the equivalent rotation matrix
*/
-template<typename Derived>
-inline Eigen::Matrix<typename Derived::Scalar, 3, 3>
-v2R(const Eigen::MatrixBase<Derived>& _v)
+template <typename Derived>
+inline Eigen::Matrix<typename Derived::Scalar, 3, 3> v2R(const Eigen::MatrixBase<Derived>& _v)
{
return exp_R(_v);
}
@@ -402,9 +380,8 @@ v2R(const Eigen::MatrixBase<Derived>& _v)
* @param _R a rotation matrix
* @return the equivalent rotation vector
*/
-template<typename Derived>
-inline Eigen::Matrix<typename Derived::Scalar, 3, 1>
-R2v(const Eigen::MatrixBase<Derived>& _R)
+template <typename Derived>
+inline Eigen::Matrix<typename Derived::Scalar, 3, 1> R2v(const Eigen::MatrixBase<Derived>& _R)
{
return log_R(_R);
}
@@ -426,23 +403,22 @@ R2v(const Eigen::MatrixBase<Derived>& _R)
* exp(theta+d_theta) = exp(theta)*exp(Jr(theta)*d_theta)
*/
-template<typename Derived>
-inline Eigen::Matrix<typename Derived::Scalar, 3, 3>
-jac_SO3_right(const Eigen::MatrixBase<Derived>& _theta)
+template <typename Derived>
+inline Eigen::Matrix<typename Derived::Scalar, 3, 3> jac_SO3_right(const Eigen::MatrixBase<Derived>& _theta)
{
- using std::sqrt;
using std::cos;
using std::sin;
+ using std::sqrt;
MatrixSizeCheck<3, 1>::check(_theta);
typedef typename Derived::Scalar T;
- T theta2 = _theta.squaredNorm();
+ T theta2 = _theta.squaredNorm();
Eigen::Matrix<T, 3, 3> W(skew(_theta));
if (theta2 <= Constants::EPS_SMALL)
- return Eigen::Matrix<T, 3, 3>::Identity() - (T)0.5 * W; // Small angle approximation
- T theta = sqrt(theta2); // rotation angle
+ return Eigen::Matrix<T, 3, 3>::Identity() - (T)0.5 * W; // Small angle approximation
+ T theta = sqrt(theta2); // rotation angle
Eigen::Matrix<T, 3, 3> M1, M2;
M1.noalias() = ((T)1 - cos(theta)) / theta2 * W;
M2.noalias() = (theta - sin(theta)) / (theta2 * theta) * (W * W);
@@ -458,7 +434,8 @@ jac_SO3_right(const Eigen::MatrixBase<Derived>& _theta)
*
* where Jrinv = jac_SO3_right_inv(theta);
*
- * This maps a perturbation on the manifold (expmap(theta)) to a perturbation in the tangent space (Jrinv * theta) so that
+ * This maps a perturbation on the manifold (expmap(theta)) to a perturbation in the tangent space (Jrinv * theta) so
+ * that
*
* log( exp(theta) * exp(d_theta) ) = theta + Jrinv(theta) * d_theta
*
@@ -466,26 +443,25 @@ jac_SO3_right(const Eigen::MatrixBase<Derived>& _theta)
*
* log( R * exp(d_theta) ) = log(R) + Jrinv(theta) * d_theta
*/
-template<typename Derived>
-inline Eigen::Matrix<typename Derived::Scalar, 3, 3>
-jac_SO3_right_inv(const Eigen::MatrixBase<Derived>& _theta)
+template <typename Derived>
+inline Eigen::Matrix<typename Derived::Scalar, 3, 3> jac_SO3_right_inv(const Eigen::MatrixBase<Derived>& _theta)
{
- using std::sqrt;
using std::cos;
using std::sin;
+ using std::sqrt;
MatrixSizeCheck<3, 1>::check(_theta);
typedef typename Derived::Scalar T;
- T theta2 = _theta.squaredNorm();
+ T theta2 = _theta.squaredNorm();
Eigen::Matrix<T, 3, 3> W(skew(_theta));
if (theta2 <= Constants::EPS_SMALL)
- return Eigen::Matrix<T, 3, 3>::Identity() + (T)0.5 * W; // Small angle approximation
- T theta = sqrt(theta2); // rotation angle
+ return Eigen::Matrix<T, 3, 3>::Identity() + (T)0.5 * W; // Small angle approximation
+ T theta = sqrt(theta2); // rotation angle
Eigen::Matrix<T, 3, 3> M;
M.noalias() = ((T)1.0 / theta2 - ((T)1.0 + cos(theta)) / ((T)2.0 * theta * sin(theta))) * (W * W);
- return Eigen::Matrix<T, 3, 3>::Identity() + (T)0.5 * W + M; //is this really more optimized?
+ return Eigen::Matrix<T, 3, 3>::Identity() + (T)0.5 * W + M; // is this really more optimized?
}
/** \brief Compute Jl (Left Jacobian)
@@ -501,23 +477,22 @@ jac_SO3_right_inv(const Eigen::MatrixBase<Derived>& _theta)
*
* exp(theta+d_theta) = exp(Jl(theta)*d_theta)*exp(theta)
*/
-template<typename Derived>
-inline Eigen::Matrix<typename Derived::Scalar, 3, 3>
-jac_SO3_left(const Eigen::MatrixBase<Derived>& _theta)
+template <typename Derived>
+inline Eigen::Matrix<typename Derived::Scalar, 3, 3> jac_SO3_left(const Eigen::MatrixBase<Derived>& _theta)
{
- using std::sqrt;
using std::cos;
using std::sin;
+ using std::sqrt;
MatrixSizeCheck<3, 1>::check(_theta);
typedef typename Derived::Scalar T;
- T theta2 = _theta.squaredNorm();
+ T theta2 = _theta.squaredNorm();
Eigen::Matrix<T, 3, 3> W(skew(_theta));
if (theta2 <= Constants::EPS_SMALL)
- return Eigen::Matrix<T, 3, 3>::Identity() - (T)0.5 * W; // Small angle approximation
- T theta = sqrt(theta2); // rotation angle
+ return Eigen::Matrix<T, 3, 3>::Identity() - (T)0.5 * W; // Small angle approximation
+ T theta = sqrt(theta2); // rotation angle
Eigen::Matrix<T, 3, 3> M1, M2;
M1.noalias() = ((T)1 - cos(theta)) / theta2 * W;
M2.noalias() = (theta - sin(theta)) / (theta2 * theta) * (W * W);
@@ -532,7 +507,8 @@ jac_SO3_left(const Eigen::MatrixBase<Derived>& _theta)
*
* where Jlinv = jac_SO3_left_inv(theta);
*
- * This maps a perturbation on the manifold (expmap(theta)) to a perturbation in the tangent space (Jlinv * theta) so that
+ * This maps a perturbation on the manifold (expmap(theta)) to a perturbation in the tangent space (Jlinv * theta) so
+ * that
*
* log( exp(d_theta) * exp(theta) ) = theta + Jlinv(theta) * d_theta
*
@@ -540,111 +516,107 @@ jac_SO3_left(const Eigen::MatrixBase<Derived>& _theta)
*
* log( exp(d_theta) * R ) = log(R) + Jlinv(theta) * d_theta
*/
-template<typename Derived>
-inline Eigen::Matrix<typename Derived::Scalar, 3, 3>
-jac_SO3_left_inv(const Eigen::MatrixBase<Derived>& _theta)
+template <typename Derived>
+inline Eigen::Matrix<typename Derived::Scalar, 3, 3> jac_SO3_left_inv(const Eigen::MatrixBase<Derived>& _theta)
{
- using std::sqrt;
using std::cos;
using std::sin;
+ using std::sqrt;
MatrixSizeCheck<3, 1>::check(_theta);
typedef typename Derived::Scalar T;
- T theta2 = _theta.squaredNorm();
+ T theta2 = _theta.squaredNorm();
Eigen::Matrix<T, 3, 3> W(skew(_theta));
if (theta2 <= Constants::EPS_SMALL)
- return Eigen::Matrix<T, 3, 3>::Identity() + (T)0.5 * W; // Small angle approximation
- T theta = sqrt(theta2); // rotation angle
+ return Eigen::Matrix<T, 3, 3>::Identity() + (T)0.5 * W; // Small angle approximation
+ T theta = sqrt(theta2); // rotation angle
Eigen::Matrix<T, 3, 3> M;
M.noalias() = ((T)1.0 / theta2 - ((T)1.0 + cos(theta)) / ((T)2.0 * theta * sin(theta))) * (W * W);
- return Eigen::Matrix<T, 3, 3>::Identity() - (T)0.5 * W + M; //is this really more optimized?
+ return Eigen::Matrix<T, 3, 3>::Identity() - (T)0.5 * W + M; // is this really more optimized?
}
-template<typename D1, typename D2, typename D3, typename D4, typename D5>
-inline void
-compose(const Eigen::QuaternionBase<D1>& _q1,
- const Eigen::QuaternionBase<D2>& _q2,
- Eigen::QuaternionBase<D3>& _q_comp,
- Eigen::MatrixBase<D4>& _J_comp_q1,
- Eigen::MatrixBase<D5>& _J_comp_q2)
+template <typename D1, typename D2, typename D3, typename D4, typename D5>
+inline void compose(const Eigen::QuaternionBase<D1>& _q1,
+ const Eigen::QuaternionBase<D2>& _q2,
+ Eigen::QuaternionBase<D3>& _q_comp,
+ Eigen::MatrixBase<D4>& _J_comp_q1,
+ Eigen::MatrixBase<D5>& _J_comp_q2)
{
MatrixSizeCheck<3, 3>::check(_J_comp_q1);
MatrixSizeCheck<3, 3>::check(_J_comp_q2);
_q_comp = _q1 * _q2;
- _J_comp_q1 = q2R(_q2.conjugate()); // R2.tr
- _J_comp_q2 . setIdentity();
+ _J_comp_q1 = q2R(_q2.conjugate()); // R2.tr
+ _J_comp_q2.setIdentity();
}
-template<typename D1, typename D2, typename D3, typename D4, typename D5>
-inline void
-between(const Eigen::QuaternionBase<D1>& _q1,
- const Eigen::QuaternionBase<D2>& _q2,
- Eigen::QuaternionBase<D3>& _q_between,
- Eigen::MatrixBase<D4>& _J_between_q1,
- Eigen::MatrixBase<D5>& _J_between_q2)
+template <typename D1, typename D2, typename D3, typename D4, typename D5>
+inline void between(const Eigen::QuaternionBase<D1>& _q1,
+ const Eigen::QuaternionBase<D2>& _q2,
+ Eigen::QuaternionBase<D3>& _q_between,
+ Eigen::MatrixBase<D4>& _J_between_q1,
+ Eigen::MatrixBase<D5>& _J_between_q2)
{
MatrixSizeCheck<3, 3>::check(_J_between_q1);
MatrixSizeCheck<3, 3>::check(_J_between_q2);
_q_between = _q1.conjugate() * _q2;
- _J_between_q1 = -q2R(_q2.conjugate()*_q1); // - R2.tr * R1
- _J_between_q2 . setIdentity();
+ _J_between_q1 = -q2R(_q2.conjugate() * _q1); // - R2.tr * R1
+ _J_between_q2.setIdentity();
}
-template<typename D1, typename D2>
-inline Eigen::Quaternion<typename D1::Scalar>
-plus_right(const Eigen::QuaternionBase<D1>& q, const Eigen::MatrixBase<D2>& v)
+template <typename D1, typename D2>
+inline Eigen::Quaternion<typename D1::Scalar> plus_right(const Eigen::QuaternionBase<D1>& q,
+ const Eigen::MatrixBase<D2>& v)
{
- MatrixSizeCheck<3,1>::check(v);
+ MatrixSizeCheck<3, 1>::check(v);
return q * exp_q(v);
}
-template<typename D1, typename D2>
-inline Eigen::Matrix<typename D2::Scalar, 3, 1>
-minus_right(const Eigen::QuaternionBase<D1>& q1, const Eigen::QuaternionBase<D2>& q2)
+template <typename D1, typename D2>
+inline Eigen::Matrix<typename D2::Scalar, 3, 1> minus_right(const Eigen::QuaternionBase<D1>& q1,
+ const Eigen::QuaternionBase<D2>& q2)
{
return log_q(q1.conjugate() * q2);
}
-template<typename D1, typename D2>
-inline Eigen::Quaternion<typename D1::Scalar>
-plus_left(const Eigen::MatrixBase<D2>& v, const Eigen::QuaternionBase<D1>& q)
+template <typename D1, typename D2>
+inline Eigen::Quaternion<typename D1::Scalar> plus_left(const Eigen::MatrixBase<D2>& v,
+ const Eigen::QuaternionBase<D1>& q)
{
- MatrixSizeCheck<3,1>::check(v);
+ MatrixSizeCheck<3, 1>::check(v);
return exp_q(v) * q;
}
-template<typename D1, typename D2>
-inline Eigen::Matrix<typename D2::Scalar, 3, 1>
-minus_left(const Eigen::QuaternionBase<D1>& q1, const Eigen::QuaternionBase<D2>& q2)
+template <typename D1, typename D2>
+inline Eigen::Matrix<typename D2::Scalar, 3, 1> minus_left(const Eigen::QuaternionBase<D1>& q1,
+ const Eigen::QuaternionBase<D2>& q2)
{
return log_q(q2 * q1.conjugate());
}
-template<typename D1, typename D2>
-inline Eigen::Quaternion<typename D1::Scalar>
-plus(const Eigen::QuaternionBase<D1>& q, const Eigen::MatrixBase<D2>& v)
+template <typename D1, typename D2>
+inline Eigen::Quaternion<typename D1::Scalar> plus(const Eigen::QuaternionBase<D1>& q, const Eigen::MatrixBase<D2>& v)
{
return plus_right(q, v);
}
-template<typename D1, typename D2>
-inline Eigen::Matrix<typename D2::Scalar, 3, 1>
-minus(const Eigen::QuaternionBase<D1>& q1, const Eigen::QuaternionBase<D2>& q2)
+template <typename D1, typename D2>
+inline Eigen::Matrix<typename D2::Scalar, 3, 1> minus(const Eigen::QuaternionBase<D1>& q1,
+ const Eigen::QuaternionBase<D2>& q2)
{
return minus_right(q1, q2);
}
-template<typename D1, typename D2>
-inline Eigen::Matrix<typename D2::Scalar, 3, 1>
-diff(const Eigen::QuaternionBase<D1>& q1, const Eigen::QuaternionBase<D2>& q2)
+template <typename D1, typename D2>
+inline Eigen::Matrix<typename D2::Scalar, 3, 1> diff(const Eigen::QuaternionBase<D1>& q1,
+ const Eigen::QuaternionBase<D2>& q2)
{
return minus(q1, q2);
}
-} // namespace wolf
+} // namespace wolf
diff --git a/include/core/problem/problem.h b/include/core/problem/problem.h
index b06d1aae69c0e7a66c5302074538a0206a59102b..07391d09f41ab7845fecadb38c30a86ac5b21581 100644
--- a/include/core/problem/problem.h
+++ b/include/core/problem/problem.h
@@ -134,8 +134,9 @@ class Problem : public std::enable_shared_from_this<Problem>
/** \brief Factory method to install (create and add) sensors only from a YAML node -- Helper method loading
* parameters from file
* \param _sensor_node YAML node containing all necessary information to call the factory and create the sensor.
+ * \param _folders_schema a vector of paths where the schema files (to validate the YAML node) are placed, if empty the node is not validated.
*/
- SensorBasePtr installSensor(const YAML::Node& _sensor_node);
+ SensorBasePtr installSensor(const YAML::Node& _sensor_node, const std::vector<std::string>& _folders_schema = {});
/** \brief Factory method to install (create and add) sensors only from its properties -- Helper method loading
* parameters from file \param _sen_type type of sensor
* \param _params_yaml_filename the name of a file containing the parameters in a format compatible with the intrinsics creator registered in FactoryParamsSensor under the key _sen_type.
@@ -148,8 +149,10 @@ class Problem : public std::enable_shared_from_this<Problem>
/** \brief Factory method to install (create, and add to sensor) processors only from a YAML node -- Helper method
* loading parameters from file
* \param _processor_node YAML node containing all necessary information to call the factory and create the processor.
+ * \param _folders_schema a vector of paths where the schema files (to validate the YAML node) are placed, if empty the node is not validated.
*/
- ProcessorBasePtr installProcessor(const YAML::Node& _processor_node);
+ ProcessorBasePtr installProcessor(const YAML::Node& _processor_node,
+ const std::vector<std::string>& _folders_schema = {});
/** \brief Factory method to install (create, and add to sensor) processors only from its properties
*
@@ -177,7 +180,7 @@ class Problem : public std::enable_shared_from_this<Problem>
* \param _folders_schema a vector of paths where the schema files (to validate the YAML file) are placed
*/
ProcessorBasePtr installProcessor(const std::string& _prc_type,
- SensorBasePtr& _corresponding_sensor,
+ SensorBasePtr _corresponding_sensor,
const std::string& _params_yaml_filename,
const std::vector<std::string>& _folders_schema);
@@ -300,7 +303,7 @@ class Problem : public std::enable_shared_from_this<Problem>
* - Therefore, if you want to permanently configure a processor for not creating key frames, see
* Processor::voting_active_ and its accessors.
*/
- bool permitKeyFrame(ProcessorBasePtr _processor_ptr) const;
+ bool permitKeyFrame(ProcessorBaseConstPtr _processor_ptr) const;
/** \brief New key frame callback
*
@@ -406,7 +409,7 @@ class Problem : public std::enable_shared_from_this<Problem>
* \param state_blocks : show state blocks
*/
void print(int depth = 4, //
- bool factored_by = false, //
+ bool factored_by = false, //
bool metric = true, //
bool state_blocks = false,
std::ostream& stream = std::cout) const;
diff --git a/include/core/processor/factory_processor.h b/include/core/processor/factory_processor.h
index d1563a43fc6bde36d01b9699a39a0ab878dcea02..985152c4a3e9239ecfb31031ddb8274a7b87f076 100644
--- a/include/core/processor/factory_processor.h
+++ b/include/core/processor/factory_processor.h
@@ -55,8 +55,8 @@ namespace wolf
*
* \code
* static ProcessorBasePtr create(const YAML::Node& _server);
- * static ProcessorBasePtr create(const std::string& _schema,
- * const std::string& _yaml_filepath,
+ * static ProcessorBasePtr create(const std::string& _schema,
+ * const std::string& _yaml_filepath,
* std::vector<std::string>& _folders_schema);
* \endcode
*
@@ -67,26 +67,26 @@ namespace wolf
* {
* // Do create the Processor Parameters --- example: ParamsProcessorOdom3d
* auto params = std::make_shared<ParamsProcessorOdom3d>(_server);
- *
+ *
* // Do create the Processor object --- example: ProcessorOdom3d
* return std::make_shared<ProcessorOdom3d>(params);
* }
- * static ProcessorBasePtr create(const std::string& _schema,
- * const std::string& _yaml_filepath,
+ * static ProcessorBasePtr create(const std::string& _schema,
+ * const std::string& _yaml_filepath,
* std::vector<std::string>& _folders_schema)
* {
* // parse the yaml file
* auto server = yaml_schema_cpp::YamlServer(_folders_schema, _yaml_filepath);
- *
+ *
* // Check that the yaml has all necessary inforamtion
* if (not server.applySchema(_schema))
* {
* throw std::runtime_error(server.getLog());
* }
- *
+ *
* // Do create the Processor Parameters --- example: ParamsProcessorOdom3d
* auto params = std::make_shared<ParamsProcessorOdom3d>(server.getNode());
- *
+ *
* // Do create the Processor object --- example: ProcessorOdom3d
* return std::make_shared<ProcessorOdom3d>(params);
* }
@@ -99,40 +99,44 @@ namespace wolf
* To create a ProcessorOdom2d, you type:
*
* \code
- * auto camera_ptr = FactoryProcessor::create("ProcessorOdom2d", yaml_node);
+ * auto camera_ptr = FactoryProcessorNode::create("ProcessorOdom2d", yaml_node);
* \endcode
*
* or:
*
* \code
- * auto camera_ptr = FactoryProcessorYaml::create("ProcessorOdom2d", "path_of_params_file.yaml", schema_folders_vector);
- * \endcode
- *
- * We RECOMMEND using the macro ````WOLF_PROCESSOR_CREATE(ProcessorClass, ParamsProcessorClass)```` to automatically
+ * auto camera_ptr = FactoryProcessorYaml::create("ProcessorOdom2d", "path_of_params_file.yaml",
+ * schema_folders_vector); \endcode
+ *
+ * We RECOMMEND using the macro ````WOLF_PROCESSOR_CREATE(ProcessorClass, ParamsProcessorClass)```` to automatically
* generate the processor creators, provided in 'processor_base.h'.
*
*/
-typedef Factory<ProcessorBasePtr,
- const YAML::Node&> FactoryProcessor;
-template<>
-inline std::string FactoryProcessor::getClass() const
+typedef Factory<ProcessorBasePtr, const YAML::Node&, const std::vector<std::string>&> FactoryProcessorNode;
+template <>
+inline std::string FactoryProcessorNode::getClass() const
{
return "FactoryProcessor";
}
-typedef Factory<std::shared_ptr<ProcessorBase>,
- const std::string&,
- const std::vector<std::string>&> FactoryProcessorYaml;
-template<>
+typedef Factory<std::shared_ptr<ProcessorBase>, const std::string&, const std::vector<std::string>&>
+ FactoryProcessorYaml;
+template <>
inline std::string FactoryProcessorYaml::getClass() const
{
return "FactoryProcessorYaml";
}
-#define WOLF_REGISTER_PROCESSOR(ProcessorType) \
- namespace{ const bool WOLF_UNUSED ProcessorType##Registered = \
- wolf::FactoryProcessor::registerCreator(#ProcessorType, ProcessorType::create); } \
- namespace{ const bool WOLF_UNUSED ProcessorType##YamlRegistered = \
- wolf::FactoryProcessorYaml::registerCreator(#ProcessorType, ProcessorType::create); } \
+#define WOLF_REGISTER_PROCESSOR(ProcessorType) \
+ namespace \
+ { \
+ const bool WOLF_UNUSED ProcessorType##NodeRegistered = \
+ wolf::FactoryProcessorNode::registerCreator(#ProcessorType, ProcessorType::create); \
+ } \
+ namespace \
+ { \
+ const bool WOLF_UNUSED ProcessorType##YamlRegistered = \
+ wolf::FactoryProcessorYaml::registerCreator(#ProcessorType, ProcessorType::create); \
+ }
} /* namespace wolf */
diff --git a/include/core/processor/motion_buffer.h b/include/core/processor/motion_buffer.h
index e192871d18f5158622c546a4d269afc960599b5c..c2109c9b6bc163dccf4f7fe340f1d7ec47434460 100644
--- a/include/core/processor/motion_buffer.h
+++ b/include/core/processor/motion_buffer.h
@@ -26,42 +26,47 @@
#include <list>
#include <algorithm>
-namespace wolf {
-
+namespace wolf
+{
using namespace Eigen;
struct Motion
{
- public:
- SizeEigen data_size_, delta_size_, cov_size_, calib_size_;
- TimeStamp ts_; ///< Time stamp
- Eigen::VectorXd data_; ///< instantaneous motion data
- Eigen::MatrixXd data_cov_; ///< covariance of the instantaneous data
- Eigen::VectorXd delta_; ///< instantaneous motion delta
- Eigen::MatrixXd delta_cov_; ///< covariance of the instantaneous delta
- Eigen::VectorXd delta_integr_; ///< the integrated motion or delta-integral
- Eigen::MatrixXd delta_integr_cov_; ///< covariance of the integrated delta
- Eigen::MatrixXd jacobian_delta_; ///< Jacobian of the integration wrt delta_
- Eigen::MatrixXd jacobian_delta_integr_; ///< Jacobian of the integration wrt delta_integr_
- Eigen::MatrixXd jacobian_calib_; ///< Jacobian of delta_integr wrt extra states (TBD by the derived processors)
- public:
- Motion() = delete; // completely delete unpredictable stuff like this
- Motion(const TimeStamp& _ts, SizeEigen _data_size, SizeEigen _delta_size, SizeEigen _cov_size, SizeEigen _calib_size);
- Motion(const TimeStamp& _ts,
- const VectorXd& _data,
- const MatrixXd& _data_cov,
- const VectorXd& _delta,
- const MatrixXd& _delta_cov,
- const VectorXd& _delta_int,
- const MatrixXd& _delta_integr_cov,
- const MatrixXd& _jac_delta,
- const MatrixXd& _jac_delta_int,
- const MatrixXd& _jacobian_calib);
- ~Motion();
- private:
- void resize(SizeEigen _data_s, SizeEigen _delta_s, SizeEigen _delta_cov_s, SizeEigen _calib_s);
+ public:
+ SizeEigen data_size_, delta_size_, cov_size_, calib_size_;
+ TimeStamp ts_; ///< Time stamp
+ Eigen::VectorXd data_; ///< instantaneous motion data
+ Eigen::MatrixXd data_cov_; ///< covariance of the instantaneous data
+ Eigen::VectorXd delta_; ///< instantaneous motion delta
+ Eigen::MatrixXd delta_cov_; ///< covariance of the instantaneous delta
+ Eigen::VectorXd delta_integr_; ///< the integrated motion or delta-integral
+ Eigen::MatrixXd delta_integr_cov_; ///< covariance of the integrated delta
+ Eigen::MatrixXd jacobian_delta_; ///< Jacobian of the integration wrt delta_
+ Eigen::MatrixXd jacobian_delta_integr_; ///< Jacobian of the integration wrt delta_integr_
+ Eigen::MatrixXd jacobian_calib_; ///< Jacobian of delta_integr wrt extra states (TBD by the derived processors)
+ public:
+ Motion() = delete; // completely delete unpredictable stuff like this
+ Motion(const TimeStamp& _ts,
+ SizeEigen _data_size,
+ SizeEigen _delta_size,
+ SizeEigen _cov_size,
+ SizeEigen _calib_size);
+ Motion(const TimeStamp& _ts,
+ const VectorXd& _data,
+ const MatrixXd& _data_cov,
+ const VectorXd& _delta,
+ const MatrixXd& _delta_cov,
+ const VectorXd& _delta_int,
+ const MatrixXd& _delta_integr_cov,
+ const MatrixXd& _jac_delta,
+ const MatrixXd& _jac_delta_int,
+ const MatrixXd& _jacobian_calib);
+ ~Motion();
+
+ private:
+ void resize(SizeEigen _data_s, SizeEigen _delta_s, SizeEigen _delta_cov_s, SizeEigen _calib_s);
-}; ///< One instance of the buffered data, corresponding to a particular time stamp.
+}; ///< One instance of the buffered data, corresponding to a particular time stamp.
/** \brief class for motion buffers.
*
@@ -86,12 +91,16 @@ struct Motion
*/
class MotionBuffer : public std::list<Motion>
{
- public:
- MotionBuffer() ;
- const Motion& getMotion(const TimeStamp& _ts) const;
- void getMotion(const TimeStamp& _ts, Motion& _motion) const;
- void split(const TimeStamp& _ts, MotionBuffer& _oldest_buffer);
- void print(bool show_data = 0, bool show_delta = 0, bool show_delta_int = 0, bool show_jacs = 0, bool show_covs = 0);
+ public:
+ MotionBuffer();
+ const Motion& getMotion(const TimeStamp& _ts) const;
+ void getMotion(const TimeStamp& _ts, Motion& _motion) const;
+ void split(const TimeStamp& _ts, MotionBuffer& _oldest_buffer);
+ void print(bool show_data = 0,
+ bool show_delta = 0,
+ bool show_delta_int = 0,
+ bool show_jacs = 0,
+ bool show_covs = 0);
};
-} // namespace wolf
+} // namespace wolf
diff --git a/include/core/processor/motion_provider.h b/include/core/processor/motion_provider.h
index ed7aba865e2a04be26ac7e46d5c26007ef2e2e7f..49d1651b3578155b90d0fe60f7fd50ad6d4d0468 100644
--- a/include/core/processor/motion_provider.h
+++ b/include/core/processor/motion_provider.h
@@ -23,102 +23,86 @@
#include "core/common/wolf.h"
#include "core/composite/vector_composite.h"
#include "core/composite/type_composite.h"
-#include "core/common/params_base.h" // for toString
#include "yaml-cpp/yaml.h"
namespace wolf
{
-
-WOLF_STRUCT_PTR_TYPEDEFS(ParamsMotionProvider);
-
-struct ParamsMotionProvider
-{
- bool state_provider = true;
- int state_provider_order = 1;
-
- ParamsMotionProvider() = default;
- ParamsMotionProvider(const YAML::Node& _n)
- {
- state_provider = _n["state_provider"].as<bool>();
- if (state_provider)
- state_provider_order = _n["state_provider_order"].as<double>();
- }
- std::string print() const
- {
- return "state_provider: " + toString(state_provider) + "\n"
- + "state_provider_order: " + toString(state_provider_order) + "\n";
- }
-
-};
class TimeStamp;
WOLF_PTR_TYPEDEFS(MotionProvider);
class MotionProvider
{
- public:
-
- MotionProvider(const TypeComposite& _state_types, ParamsMotionProviderPtr _params);
- virtual ~MotionProvider();
+ public:
+ MotionProvider(const TypeComposite& _state_types, const YAML::Node& _params);
+ virtual ~MotionProvider();
- // Queries to the processor:
- virtual TimeStamp getTimeStamp() const = 0;
- virtual VectorComposite getState(const StateKeys& _structure = "") const = 0;
- virtual VectorComposite getState(const TimeStamp& _ts, const StateKeys& _structure = "") const = 0;
+ // Queries to the processor:
+ virtual TimeStamp getTimeStamp() const = 0;
+ virtual VectorComposite getState(const StateKeys& _structure = "") const = 0;
+ virtual VectorComposite getState(const TimeStamp& _ts, const StateKeys& _structure = "") const = 0;
- VectorComposite getOdometry ( ) const;
- void setOdometry(const VectorComposite&);
+ VectorComposite getOdometry() const;
+ void setOdometry(const VectorComposite&);
- bool isStateGetter() const;
- int getOrder() const;
- void setOrder(int);
+ bool isStateGetter() const;
+ int getOrder() const;
+ void setOrder(int);
- public:
- TypeComposite getStateTypes ( ) const { return state_types_; };
- StateKeys getStateKeys ( ) const { return state_types_.getKeys(); };
- void setStateTypes(const TypeComposite& _state_structure) { state_types_ = _state_structure; };
- void addToProblem(ProblemPtr _prb_ptr, MotionProviderPtr _motion_ptr);
-
- protected:
- TypeComposite state_types_;
- ParamsMotionProviderPtr params_motion_provider_;
+ public:
+ TypeComposite getStateTypes() const
+ {
+ return state_types_;
+ };
+ StateKeys getStateKeys() const
+ {
+ return state_types_.getKeys();
+ };
+ void setStateTypes(const TypeComposite& _state_structure)
+ {
+ state_types_ = _state_structure;
+ };
+ void addToProblem(ProblemPtr _prb_ptr, MotionProviderPtr _motion_ptr);
- private:
- VectorComposite odometry_;
- mutable std::mutex mut_odometry_;
+ protected:
+ TypeComposite state_types_;
+ YAML::Node params_motion_provider_;
+ private:
+ VectorComposite odometry_;
+ mutable std::mutex mut_odometry_;
};
-// inline MotionProvider::MotionProvider(const StateKeys& _structure, ParamsMotionProviderPtr _params) :
-inline MotionProvider::MotionProvider(const TypeComposite& _state_types, ParamsMotionProviderPtr _params) :
- state_types_(_state_types),
- params_motion_provider_(_params)
+inline MotionProvider::MotionProvider(const TypeComposite& _state_types, const YAML::Node& _params)
+ : state_types_(_state_types), params_motion_provider_(Clone(_params))
{
checkTypeComposite(_state_types);
}
-}
+} // namespace wolf
///// IMPLEMENTATION ///////
-namespace wolf{
-
-inline MotionProvider::~MotionProvider()
+namespace wolf
{
-}
+inline MotionProvider::~MotionProvider() {}
inline bool MotionProvider::isStateGetter() const
{
- return params_motion_provider_->state_provider;
+ return params_motion_provider_["state_provider"].as<bool>();
}
inline int MotionProvider::getOrder() const
{
- return params_motion_provider_->state_provider_order;
+ if (not isStateGetter())
+ throw std::runtime_error("MotionProvider::getOrder: motion provider not state getter, doesn't have order");
+ return params_motion_provider_["state_provider_order"].as<int>();
}
inline void MotionProvider::setOrder(int _order)
{
- params_motion_provider_->state_provider_order = _order;
+ if (not isStateGetter())
+ throw std::runtime_error("MotionProvider::getOrder: motion provider not state getter, cannot have order");
+ params_motion_provider_["state_provider_order"] = _order;
}
} /* namespace wolf */
diff --git a/include/core/processor/processor_base.h b/include/core/processor/processor_base.h
index 7a3d1c97ef63241a1ce2658e498c650c5c077eb5..44c6bbccc6d3e807b9fb94c9de09922221127e26 100644
--- a/include/core/processor/processor_base.h
+++ b/include/core/processor/processor_base.h
@@ -34,7 +34,6 @@ class SensorBase;
#include "core/sensor/sensor_base.h"
#include "core/frame/frame_base.h"
#include "core/common/time_stamp.h"
-#include "core/common/params_base.h"
// std
#include <memory>
@@ -46,7 +45,6 @@ class SensorBase;
namespace wolf
{
-
/*
* Macro for defining Autoconf processor creator for WOLF's high level API.
*
@@ -56,17 +54,25 @@ namespace wolf
* In order to use this macro, the derived processor class, ProcessorClass,
* must have a constructor available with the API:
*
- * ProcessorClass(const ParamsProcessorClassPtr _params);
+ * ProcessorClass(const YAML::Node& _params);
*
* Also, there should be the schema file 'SensorClass.schema' containing the specifications
* of the user input yaml file.
*/
-#define WOLF_PROCESSOR_CREATE(ProcessorClass, ParamsProcessorClass) \
- static ProcessorBasePtr create(const YAML::Node& _input_node) \
+#define WOLF_PROCESSOR_CREATE(ProcessorClass) \
+ static ProcessorBasePtr create(const YAML::Node& _input_node, \
+ const std::vector<std::string>& _folders_schema = {}) \
{ \
- auto params = std::make_shared<ParamsProcessorClass>(_input_node); \
- \
- return std::make_shared<ProcessorClass>(params); \
+ if (not _folders_schema.empty()) \
+ { \
+ auto server = yaml_schema_cpp::YamlServer(_folders_schema); \
+ server.setYaml(_input_node); \
+ if (not server.applySchema(#ProcessorClass)) \
+ { \
+ throw std::runtime_error(server.getLog()); \
+ } \
+ } \
+ return std::make_shared<ProcessorClass>(_input_node); \
} \
static ProcessorBasePtr create(const std::string& _yaml_filepath, \
const std::vector<std::string>& _folders_schema) \
@@ -78,9 +84,33 @@ namespace wolf
WOLF_ERROR(server.getLog()); \
return nullptr; \
} \
- auto params = std::make_shared<ParamsProcessorClass>(server.getNode()); \
- \
- return std::make_shared<ProcessorClass>(params); \
+ return create(server.getNode(), {}); \
+ }
+
+#define WOLF_PROCESSOR_TEMPLATE_DIM_CREATE(ProcessorClass, Dim) \
+ static ProcessorBasePtr create(const YAML::Node& _input_node, \
+ const std::vector<std::string>& _folders_schema = {}) \
+ { \
+ if (not _folders_schema.empty()) \
+ { \
+ auto server = yaml_schema_cpp::YamlServer(_folders_schema); \
+ server.setYaml(_input_node); \
+ if (not server.applySchema(#ProcessorClass + toString(Dim) + "d")) \
+ { \
+ throw std::runtime_error(server.getLog()); \
+ } \
+ } \
+ return std::make_shared<ProcessorClass<Dim>>(_input_node); \
+ } \
+ static ProcessorBasePtr create(const std::string& _yaml_filepath, \
+ const std::vector<std::string>& _folders_schema) \
+ { \
+ auto server = yaml_schema_cpp::YamlServer(_folders_schema, _yaml_filepath); \
+ if (not server.applySchema(#ProcessorClass + toString(Dim) + "d")) \
+ { \
+ throw std::runtime_error(server.getLog()); \
+ } \
+ return create(server.getNode(), {}); \
}
/** \brief Buffer for arbitrary type objects
@@ -196,65 +226,28 @@ class BufferCapture : public Buffer<CaptureBasePtr>
{
};
-/** \brief base struct for processor parameters
- *
- * Derive from this struct to create structs of processor parameters.
- */
-struct ParamsProcessorBase : public ParamsBase
-{
- ParamsProcessorBase() = default;
- ParamsProcessorBase(const YAML::Node& _n) : ParamsBase(_n)
- {
- name = _n["name"].as<std::string>();
- time_tolerance = _n["time_tolerance"].as<double>();
- voting_active = _n["keyframe_vote"]["voting_active"].as<bool>();
- apply_loss_function = _n["apply_loss_function"].as<bool>();
- }
-
- ~ParamsProcessorBase() override = default;
-
- /** maximum time difference between a Keyframe time stamp and
- * a particular Capture of this processor to allow assigning
- * this Capture to the Keyframe.
- */
- std::string name;
- double time_tolerance;
- bool voting_active; ///< Whether this processor is allowed to vote for a Key Frame or not
- bool apply_loss_function; ///< Whether this processor emplaces factors with loss function or not
-
- std::string print() const override
- {
- return "name: " + name + "\n" + "voting_active: " + toString(voting_active) + "\n" +
- "time_tolerance: " + toString(time_tolerance) + "\n" +
- "apply_loss_function: " + toString(apply_loss_function) + "\n";
- }
-};
-
// class ProcessorBase
class ProcessorBase : public NodeBase, public std::enable_shared_from_this<ProcessorBase>
{
friend SensorBase;
protected:
- unsigned int processor_id_;
- ParamsProcessorBasePtr params_;
- BufferFrame buffer_frame_;
- BufferCapture buffer_capture_;
- int dim_;
+ unsigned int processor_id_;
+ YAML::Node params_;
+ BufferFrame buffer_frame_;
+ BufferCapture buffer_capture_;
+ int dim_;
private:
SensorBaseWPtr sensor_ptr_;
static unsigned int processor_id_count_;
public:
- ProcessorBase(const std::string& _type, int _dim, ParamsProcessorBasePtr _params);
+ ProcessorBase(const std::string& _type, int _dim, const YAML::Node& _params);
~ProcessorBase() override;
virtual void configure(SensorBasePtr _sensor) = 0;
virtual void remove();
- bool hasChildren() const override
- {
- return false;
- };
+ bool hasChildren() const override;
unsigned int id() const;
@@ -321,7 +314,7 @@ class ProcessorBase : public NodeBase, public std::enable_shared_from_this<Proce
*/
virtual bool voteForKeyFrame() const = 0;
- virtual bool permittedKeyFrame() final;
+ virtual bool permittedKeyFrame() const final;
void setProblem(ProblemPtr) override;
@@ -349,35 +342,38 @@ class ProcessorBase : public NodeBase, public std::enable_shared_from_this<Proce
}
public:
- bool isMotionProvider() const;
-
- bool isVotingActive() const;
-
- void setVotingActive(bool _voting_active = true);
-
- int getDim() const;
-
- void setTimeTolerance(double _time_tolerance);
- double getTimeTolerance() const;
- 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;
+ YAML::Node getParams() const;
+ bool isMotionProvider() const;
+ int getDim() const;
+ bool applyLossFunction() const;
+ bool isVotingActive() const;
+ double getTimeTolerance() const;
+
+ void setApplyLossFunction(bool _apply_loss_function);
+ void setVotingActive(bool _voting_active);
+ void setTimeTolerance(double _time_tolerance);
+
+ 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);
- template <typename classType, typename... T>
- static std::shared_ptr<classType> emplace(SensorBasePtr _sen_ptr, T&&... all);
+ template <typename classType>
+ static std::shared_ptr<classType> emplace(SensorBasePtr _sen_ptr,
+ const YAML::Node& _params,
+ const std::vector<std::string> _folders_schema = {});
- virtual void printHeader(int depth, //
+ virtual void printHeader(int depth, //
bool factored_by, //
- bool metric, //
+ bool metric, //
bool state_blocks,
std::ostream& stream,
std::string _tabs = "") const;
- void print(int depth, //
+ void print(int depth, //
bool factored_by, //
- bool metric, //
+ bool metric, //
bool state_blocks,
std::ostream& stream = std::cout,
std::string _tabs = "") const;
@@ -386,6 +382,11 @@ class ProcessorBase : public NodeBase, public std::enable_shared_from_this<Proce
bool check(CheckLog& _log, bool _verbose, std::ostream& _stream, std::string _tabs = "") const;
};
+inline bool ProcessorBase::hasChildren() const
+{
+ return false;
+};
+
inline void ProcessorBase::startCaptureProfiling()
{
start_capture_ = std::chrono::high_resolution_clock::now();
@@ -414,14 +415,29 @@ inline void ProcessorBase::stopKFProfiling()
acc_duration_kf_ += duration_kf;
}
+inline YAML::Node ProcessorBase::getParams() const
+{
+ return Clone(params_);
+}
+
+inline bool ProcessorBase::applyLossFunction() const
+{
+ return params_["apply_loss_function"].as<bool>();
+}
+
+inline void ProcessorBase::setApplyLossFunction(bool _apply_loss_function)
+{
+ params_["apply_loss_function"] = _apply_loss_function;
+}
+
inline bool ProcessorBase::isVotingActive() const
{
- return params_->voting_active;
+ return params_["keyframe_vote"]["voting_active"].as<bool>();
}
inline void ProcessorBase::setVotingActive(bool _voting_active)
{
- params_->voting_active = _voting_active;
+ params_["keyframe_vote"]["voting_active"] = _voting_active;
}
inline bool ProcessorBase::isMotionProvider() const
@@ -452,17 +468,19 @@ inline int ProcessorBase::getDim() const
inline void ProcessorBase::setTimeTolerance(double _time_tolerance)
{
- params_->time_tolerance = _time_tolerance;
+ params_["time_tolerance"] = _time_tolerance;
}
inline double ProcessorBase::getTimeTolerance() const
{
- return params_->time_tolerance;
+ return params_["time_tolerance"].as<double>();
}
-template <typename classType, typename... T>
-std::shared_ptr<classType> ProcessorBase::emplace(SensorBasePtr _sen_ptr, T&&... all)
+template <typename classType>
+std::shared_ptr<classType> ProcessorBase::emplace(SensorBasePtr _sen_ptr,
+ const YAML::Node& _params,
+ const std::vector<std::string> _folders_schema)
{
- std::shared_ptr<classType> prc = std::make_shared<classType>(std::forward<T>(all)...);
+ std::shared_ptr<classType> prc = std::static_pointer_cast<classType>(classType::create(_params, _folders_schema));
prc->configure(_sen_ptr);
prc->link(_sen_ptr);
return prc;
diff --git a/include/core/processor/processor_diff_drive.h b/include/core/processor/processor_diff_drive.h
index 1ef1bcfccf1dea5336abddceb4899a7c3a103f42..e7fd6fd01eab8bf8d07bc87557d5de0adc4a6bc7 100644
--- a/include/core/processor/processor_diff_drive.h
+++ b/include/core/processor/processor_diff_drive.h
@@ -24,64 +24,46 @@
namespace wolf
{
-
-WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorDiffDrive);
-
-struct ParamsProcessorDiffDrive : public ParamsProcessorOdom2d
-{
- ParamsProcessorDiffDrive() = default;
- ParamsProcessorDiffDrive(const YAML::Node& _n) :
- ParamsProcessorOdom2d(_n)
- {
- }
- std::string print() const override
- {
- return ParamsProcessorOdom2d::print();
- }
-};
-
WOLF_PTR_TYPEDEFS(ProcessorDiffDrive);
class ProcessorDiffDrive : public ProcessorOdom2d
{
- public:
- ProcessorDiffDrive(ParamsProcessorDiffDrivePtr _params_motion);
- ~ProcessorDiffDrive() override;
- void configure(SensorBasePtr _sensor) override;
-
- // Factory method for high level API
- WOLF_PROCESSOR_CREATE(ProcessorDiffDrive, ParamsProcessorDiffDrive);
-
- protected:
- // Motion integration
- void computeCurrentDelta(const Eigen::VectorXd& _data,
- const Eigen::MatrixXd& _data_cov,
- const Eigen::VectorXd& _calib,
- const double _dt,
- Eigen::VectorXd& _delta,
- Eigen::MatrixXd& _delta_cov,
- Eigen::MatrixXd& _jacobian_calib) const override;
-
- CaptureMotionPtr emplaceCapture(const FrameBasePtr& _frame_own,
- const SensorBasePtr& _sensor,
- const TimeStamp& _ts,
- const VectorXd& _data,
- const MatrixXd& _data_cov,
- const VectorXd& _calib,
- const VectorXd& _calib_preint,
- const CaptureBasePtr& _capture_origin) override;
- void emplaceFeaturesAndFactors(CaptureBasePtr _capture_origin, CaptureMotionPtr _capture_own) override;
-
- VectorXd getCalibration(const CaptureBaseConstPtr _capture = nullptr) const override;
- void setCalibration(const CaptureBasePtr _capture, const VectorXd& _calibration) override;
-
- protected:
- ParamsProcessorDiffDrivePtr params_prc_diff_drv_selfcal_;
- double radians_per_tick_;
-
+ public:
+ ProcessorDiffDrive(const YAML::Node& _params);
+ ~ProcessorDiffDrive() override;
+ void configure(SensorBasePtr _sensor) override;
+
+ // Factory method for high level API
+ WOLF_PROCESSOR_CREATE(ProcessorDiffDrive);
+
+ protected:
+ // Motion integration
+ void computeCurrentDelta(const Eigen::VectorXd& _data,
+ const Eigen::MatrixXd& _data_cov,
+ const Eigen::VectorXd& _calib,
+ const double _dt,
+ Eigen::VectorXd& _delta,
+ Eigen::MatrixXd& _delta_cov,
+ Eigen::MatrixXd& _jacobian_calib) const override;
+
+ CaptureMotionPtr emplaceCapture(const FrameBasePtr& _frame_own,
+ const SensorBasePtr& _sensor,
+ const TimeStamp& _ts,
+ const VectorXd& _data,
+ const MatrixXd& _data_cov,
+ const VectorXd& _calib,
+ const VectorXd& _calib_preint,
+ const CaptureBasePtr& _capture_origin) override;
+ void emplaceFeaturesAndFactors(CaptureBasePtr _capture_origin, CaptureMotionPtr _capture_own) override;
+
+ VectorXd getCalibration(const CaptureBaseConstPtr _capture = nullptr) const override;
+ void setCalibration(const CaptureBasePtr _capture, const VectorXd& _calibration) override;
+
+ protected:
+ double radians_per_tick_;
};
-inline Eigen::VectorXd ProcessorDiffDrive::getCalibration (const CaptureBaseConstPtr _capture) const
+inline Eigen::VectorXd ProcessorDiffDrive::getCalibration(const CaptureBaseConstPtr _capture) const
{
if (_capture)
return _capture->getStateBlock('I')->getState();
@@ -89,10 +71,9 @@ inline Eigen::VectorXd ProcessorDiffDrive::getCalibration (const CaptureBaseCons
return getSensor()->getStateBlockDynamic('I')->getState();
}
-inline void ProcessorDiffDrive::setCalibration (const CaptureBasePtr _capture, const VectorXd& _calibration)
+inline void ProcessorDiffDrive::setCalibration(const CaptureBasePtr _capture, const VectorXd& _calibration)
{
_capture->getStateBlock('I')->setState(_calibration);
}
-
-}
+} // namespace wolf
diff --git a/include/core/processor/processor_fixed_wing_model.h b/include/core/processor/processor_fixed_wing_model.h
index c299b756af336719191d078343436ceec621e42a..2fa506a34adc673cba0575b4418d968cbd695857 100644
--- a/include/core/processor/processor_fixed_wing_model.h
+++ b/include/core/processor/processor_fixed_wing_model.h
@@ -24,79 +24,67 @@
namespace wolf
{
-
-WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorFixedWingModel);
-
-struct ParamsProcessorFixedWingModel : public ParamsProcessorBase
-{
- Eigen::Vector3d velocity_local;
- double angle_stdev;
- double min_vel_norm;
-
- ParamsProcessorFixedWingModel() = default;
- ParamsProcessorFixedWingModel(const YAML::Node & _n) :
- ParamsProcessorBase(_n)
- {
- velocity_local = _n["velocity_local"].as<Eigen::Vector3d>();
- angle_stdev = _n["angle_stdev"].as<double>();
- min_vel_norm = _n["min_vel_norm"].as<double>();
-
- assert(std::abs(velocity_local.norm() - 1.0) < wolf::Constants::EPS && "ParamsProcessorFixedWingModel: 'velocity_local' must be normalized");
- }
- std::string print() const override
- {
- return ParamsProcessorBase::print() + "\n"
- + "velocity_local: "+ toString(velocity_local) + "\n"
- + "angle_stdev: " + toString(angle_stdev) + "\n"
- + "min_vel_norm: " + toString(min_vel_norm) + "\n";
- }
-};
-
WOLF_PTR_TYPEDEFS(ProcessorFixedWingModel);
class ProcessorFixedWingModel : public ProcessorBase
{
- public:
- ProcessorFixedWingModel(ParamsProcessorFixedWingModelPtr _params);
-
- // Factory method for high level API
- WOLF_PROCESSOR_CREATE(ProcessorFixedWingModel, ParamsProcessorFixedWingModel);
-
- virtual ~ProcessorFixedWingModel() override;
- void configure(SensorBasePtr _sensor) override;
-
- protected:
-
- /** \brief process an incoming capture NEVER CALLED
- */
- virtual void processCapture(CaptureBasePtr) override {};
-
- /** \brief process an incoming key-frame: applies the motion model between consecutive keyframes
- */
- virtual void processKeyFrame(FrameBasePtr _keyframe_ptr) override;
-
- /** \brief trigger in capture
- */
- virtual bool triggerInCapture(CaptureBasePtr) const override {return false;};
-
- /** \brief trigger in key-frame
- */
- virtual bool triggerInKeyFrame(FrameBasePtr _keyframe_ptr) const override {return true;};
-
- /** \brief store key frame
- */
- virtual bool storeKeyFrame(FrameBasePtr) override {return false;};
-
- /** \brief store capture
- */
- virtual bool storeCapture(CaptureBasePtr) override {return false;};
-
- /** \brief Vote for KeyFrame generation
- */
- virtual bool voteForKeyFrame() const override {return false;};
-
- // ATTRIBUTES
- ParamsProcessorFixedWingModelPtr params_processor_;
+ public:
+ ProcessorFixedWingModel(const YAML::Node& _params);
+
+ // Factory method for high level API
+ WOLF_PROCESSOR_CREATE(ProcessorFixedWingModel);
+
+ virtual ~ProcessorFixedWingModel() override;
+ void configure(SensorBasePtr _sensor) override;
+
+ protected:
+ /** \brief process an incoming capture NEVER CALLED
+ */
+ virtual void processCapture(CaptureBasePtr) override{};
+
+ /** \brief process an incoming key-frame: applies the motion model between consecutive keyframes
+ */
+ virtual void processKeyFrame(FrameBasePtr _keyframe_ptr) override;
+
+ /** \brief trigger in capture
+ */
+ virtual bool triggerInCapture(CaptureBasePtr) const override
+ {
+ return false;
+ };
+
+ /** \brief trigger in key-frame
+ */
+ virtual bool triggerInKeyFrame(FrameBasePtr _keyframe_ptr) const override
+ {
+ return true;
+ };
+
+ /** \brief store key frame
+ */
+ virtual bool storeKeyFrame(FrameBasePtr) override
+ {
+ return false;
+ };
+
+ /** \brief store capture
+ */
+ virtual bool storeCapture(CaptureBasePtr) override
+ {
+ return false;
+ };
+
+ /** \brief Vote for KeyFrame generation
+ */
+ virtual bool voteForKeyFrame() const override
+ {
+ return false;
+ };
+
+ // Params
+ Eigen::Vector3d velocity_local_;
+ double angle_stdev_;
+ double min_vel_norm_;
};
} /* namespace wolf */
diff --git a/include/core/processor/processor_landmark_external.h b/include/core/processor/processor_landmark_external.h
index 8bc016fa73a37ee87ab12911b9083d55b102febe..1702ec0a300ef16f91494be12e42187142026e20 100644
--- a/include/core/processor/processor_landmark_external.h
+++ b/include/core/processor/processor_landmark_external.h
@@ -26,138 +26,117 @@
namespace wolf
{
-
-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)
-
- ParamsProcessorLandmarkExternal() = default;
- ParamsProcessorLandmarkExternal(const YAML::Node& _n):
- ParamsProcessorTracker(_n)
- {
- use_orientation = _n["use_orientation"].as<bool>();
- filter_quality_th = _n["filter"]["quality_th"].as<double>();
- filter_track_length_th = _n["filter"]["track_length_th"].as<unsigned int>();
- match_dist_th = _n["match_dist_th"].as<double>();
- time_span = _n["keyframe_vote"]["time_span"].as<double>();
- new_features_for_keyframe = _n["keyframe_vote"]["new_features_for_keyframe"].as<unsigned int>();
- }
-
- std::string print() const override
- {
- return ParamsProcessorTracker::print() + "\n"
- + "use_orientation" + toString(use_orientation) + "\n"
- + "filter/quality_th" + toString(filter_quality_th) + "\n"
- + "filter/track_length_th" + toString(filter_track_length_th) + "\n"
- + "match_dist_th" + toString(match_dist_th) + "\n"
- + "keyframe_vote/time_span" + toString(time_span) + "\n"
- + "keyframe_vote/new_features_for_keyframe" + toString(new_features_for_keyframe) + "\n";
- }
-};
-
WOLF_PTR_TYPEDEFS(ProcessorLandmarkExternal);
-//Class
+// Class
class ProcessorLandmarkExternal : public ProcessorTracker
{
- public:
- ProcessorLandmarkExternal(ParamsProcessorLandmarkExternalPtr _params_tracker_feature);
- ~ProcessorLandmarkExternal() override = default;
-
- // 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(const YAML::Node& _params);
+ ~ProcessorLandmarkExternal() override = default;
+
+ // Factory method for high level API
+ WOLF_PROCESSOR_CREATE(ProcessorLandmarkExternal);
+
+ void configure(SensorBasePtr _sensor) override{};
+
+ protected:
+ 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;
+
+ // Params
+ 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)
};
-inline ProcessorLandmarkExternal::ProcessorLandmarkExternal(ParamsProcessorLandmarkExternalPtr _params_tfle) :
- ProcessorTracker("ProcessorLandmarkExternal", "PO", 0, _params_tfle),
- params_tfle_(_params_tfle),
- lmks_ids_origin_()
+inline ProcessorLandmarkExternal::ProcessorLandmarkExternal(const YAML::Node& _params)
+ : ProcessorTracker("ProcessorLandmarkExternal", "PO", 0, _params),
+ lmks_ids_origin_(),
+ use_orientation_(_params["use_orientation"].as<bool>()),
+ match_dist_th_(_params["match_dist_th"].as<double>()),
+ new_features_for_keyframe_(_params["keyframe_vote"]["new_features_for_keyframe"].as<unsigned int>()),
+ filter_quality_th_(_params["filter"]["quality_th"].as<double>()),
+ filter_track_length_th_(_params["filter"]["track_length_th"].as<unsigned int>()),
+ time_span_(_params["keyframe_vote"]["time_span"].as<double>())
{
//
}
-} // namespace wolf
+} // namespace wolf
diff --git a/include/core/processor/processor_loop_closure.h b/include/core/processor/processor_loop_closure.h
index ff9402092d2b54d22f04561cde96846d145974cf..335161845b0e6ae53b237f4919644a78ea18bdf0 100644
--- a/include/core/processor/processor_loop_closure.h
+++ b/include/core/processor/processor_loop_closure.h
@@ -23,28 +23,8 @@
// Wolf related headers
#include "core/processor/processor_base.h"
-namespace wolf{
-
-WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorLoopClosure);
-
-struct ParamsProcessorLoopClosure : public ParamsProcessorBase
+namespace wolf
{
- int max_loops=-1;
-
- ParamsProcessorLoopClosure() = default;
- ParamsProcessorLoopClosure(const YAML::Node& _n):
- ParamsProcessorBase(_n)
- {
- max_loops = _n["max_loops"].as<int>();
- }
-
- std::string print() const override
- {
- return "\n" + ParamsProcessorBase::print()
- + "max_loops: " + toString(max_loops) + "\n";
- }
-};
-
WOLF_STRUCT_PTR_TYPEDEFS(MatchLoopClosure);
/** \brief Match between a capture and a capture
@@ -54,9 +34,9 @@ WOLF_STRUCT_PTR_TYPEDEFS(MatchLoopClosure);
*/
struct MatchLoopClosure
{
- CaptureBasePtr capture_reference; ///< Capture reference
- CaptureBasePtr capture_target; ///< Capture target
- double normalized_score; ///< normalized similarity score (0 is bad, 1 is good)
+ CaptureBasePtr capture_reference; ///< Capture reference
+ CaptureBasePtr capture_target; ///< Capture target
+ double normalized_score; ///< normalized similarity score (0 is bad, 1 is good)
};
/** \brief General loop closure processor
@@ -73,55 +53,77 @@ struct MatchLoopClosure
*/
class ProcessorLoopClosure : public ProcessorBase
{
- protected:
-
- ParamsProcessorLoopClosurePtr params_loop_closure_;
+ public:
+ ProcessorLoopClosure(const std::string& _type, int _dim, const YAML::Node& _params);
- public:
+ ~ProcessorLoopClosure() override = default;
+ void configure(SensorBasePtr _sensor) override{};
- ProcessorLoopClosure(const std::string& _type, int _dim, ParamsProcessorLoopClosurePtr _params_loop_closure);
+ int getMaxLoops() const;
+ void setMaxLoops(int _max_loops);
- ~ProcessorLoopClosure() override = default;
- void configure(SensorBasePtr _sensor) override { };
+ protected:
+ /** \brief Process a capture (linked to a frame)
+ * If voteFindLoopClosures() returns true, findLoopClosures() is called.
+ * emplaceFactors() is called for pairs of current capture and each capture returned by findLoopClosures()
+ */
+ virtual void process(CaptureBasePtr);
- protected:
+ /** \brief Returns if findLoopClosures() has to be called for the given capture
+ */
+ virtual bool voteFindLoopClosures(CaptureBasePtr cap) = 0;
- /** \brief Process a capture (linked to a frame)
- * If voteFindLoopClosures() returns true, findLoopClosures() is called.
- * emplaceFactors() is called for pairs of current capture and each capture returned by findLoopClosures()
- */
- virtual void process(CaptureBasePtr);
+ /** \brief detects and emplaces all features of the given capture
+ */
+ virtual void emplaceFeatures(CaptureBasePtr cap) = 0;
- /** \brief Returns if findLoopClosures() has to be called for the given capture
- */
- virtual bool voteFindLoopClosures(CaptureBasePtr cap) = 0;
+ /** \brief Find captures that correspond to loop closures with the given capture
+ */
+ virtual std::map<double, MatchLoopClosurePtr> findLoopClosures(CaptureBasePtr _capture) = 0;
- /** \brief detects and emplaces all features of the given capture
- */
- virtual void emplaceFeatures(CaptureBasePtr cap) = 0;
+ /** \brief validates a loop closure
+ */
+ virtual bool validateLoopClosure(MatchLoopClosurePtr) = 0;
- /** \brief Find captures that correspond to loop closures with the given capture
- */
- virtual std::map<double,MatchLoopClosurePtr> findLoopClosures(CaptureBasePtr _capture) = 0;
+ /** \brief emplaces the factor(s) corresponding to a Loop Closure between two captures
+ */
+ virtual void emplaceFactors(MatchLoopClosurePtr) = 0;
- /** \brief validates a loop closure
- */
- virtual bool validateLoopClosure(MatchLoopClosurePtr) = 0;
+ void processCapture(CaptureBasePtr) override;
+ void processKeyFrame(FrameBasePtr _frm) override;
- /** \brief emplaces the factor(s) corresponding to a Loop Closure between two captures
- */
- virtual void emplaceFactors(MatchLoopClosurePtr) = 0;
+ bool triggerInCapture(CaptureBasePtr _cap) const override
+ {
+ return true;
+ };
+ bool triggerInKeyFrame(FrameBasePtr _frm) const override
+ {
+ return true;
+ };
- void processCapture(CaptureBasePtr) override;
- void processKeyFrame(FrameBasePtr _frm) override;
+ bool storeKeyFrame(FrameBasePtr _frm) override
+ {
+ return false;
+ };
+ bool storeCapture(CaptureBasePtr _cap) override
+ {
+ return false;
+ };
- bool triggerInCapture(CaptureBasePtr _cap) const override { return true;};
- bool triggerInKeyFrame(FrameBasePtr _frm) const override { return true;};
+ bool voteForKeyFrame() const override
+ {
+ return false;
+ };
+};
- bool storeKeyFrame(FrameBasePtr _frm) override { return false;};
- bool storeCapture(CaptureBasePtr _cap) override { return false;};
+inline int ProcessorLoopClosure::getMaxLoops() const
+{
+ return params_["max_loops"].as<int>();
+}
- bool voteForKeyFrame() const override { return false;};
-};
+inline void ProcessorLoopClosure::setMaxLoops(int _max_loops)
+{
+ params_["max_loops"] = _max_loops;
+}
-} // namespace wolf
+} // namespace wolf
diff --git a/include/core/processor/processor_motion.h b/include/core/processor/processor_motion.h
index f312b57a4f4208f1a6de657592daa6532a03595b..503a9357a14007afc17a438516c6b2612828800a 100644
--- a/include/core/processor/processor_motion.h
+++ b/include/core/processor/processor_motion.h
@@ -31,41 +31,6 @@
namespace wolf
{
-
-WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorMotion);
-
-struct ParamsProcessorMotion : public ParamsProcessorBase, public ParamsMotionProvider
-{
- double max_time_span = 0.5;
- unsigned int max_buff_length = 10;
- double dist_traveled = 5;
- double angle_turned = 0.5;
- double unmeasured_perturbation_std = 1e-4;
-
- ParamsProcessorMotion() = default;
- ParamsProcessorMotion(const YAML::Node& _n):
- ParamsProcessorBase(_n),
- ParamsMotionProvider(_n)
- {
- max_time_span = _n["keyframe_vote"]["max_time_span"].as<double>();
- max_buff_length = _n["keyframe_vote"]["max_buff_length"].as<unsigned int>();
- dist_traveled = _n["keyframe_vote"]["dist_traveled"].as<double>();
- angle_turned = _n["keyframe_vote"]["angle_turned"].as<double>();
- unmeasured_perturbation_std = _n["unmeasured_perturbation_std"].as<double>();
- }
- std::string print() const override
- {
- return ParamsProcessorBase::print() + "\n" +
- ParamsMotionProvider::print() + "\n"
- + "max_time_span: " + toString(max_time_span) + "\n"
- + "max_buff_length: " + toString(max_buff_length) + "\n"
- + "dist_traveled: " + toString(dist_traveled) + "\n"
- + "angle_turned: " + toString(angle_turned) + "\n"
- + "unmeasured_perturbation_std: " + toString(unmeasured_perturbation_std) + "\n";
- }
-
-};
-
/** \brief class for Motion processors
*
* This processor integrates motion data into vehicle states.
@@ -93,7 +58,7 @@ struct ParamsProcessorMotion : public ParamsProcessorBase, public ParamsMotionPr
*
* which are called at the beginning and at the end of process(). See the doc of these functions for more info.
*/
- /* // TODO: JS: review these instructions from here onwards:
+/* // TODO: JS: review these instructions from here onwards:
*
* while the integrated motion refers to the robot frame.
*
@@ -145,415 +110,427 @@ struct ParamsProcessorMotion : public ParamsProcessorBase, public ParamsMotionPr
*/
class ProcessorMotion : public ProcessorBase, public MotionProvider
{
- public:
- typedef enum {
- FIRST_TIME_WITHOUT_KF,
- FIRST_TIME_WITH_KF_BEFORE_INCOMING,
- FIRST_TIME_WITH_KF_ON_INCOMING,
- FIRST_TIME_WITH_KF_AFTER_INCOMING,
- RUNNING_WITHOUT_KF,
- RUNNING_WITH_KF_BEFORE_ORIGIN,
- RUNNING_WITH_KF_ON_ORIGIN,
- RUNNING_WITH_KF_AFTER_ORIGIN
- } ProcessingStep ;
-
- protected:
- ParamsProcessorMotionPtr params_motion_;
- ProcessingStep processing_step_; ///< State machine controlling the processing step
- bool bootstrapping_; ///< processor is bootstrapping
-
- // This is the main public interface
- public:
- ProcessorMotion(const std::string& _type,
- TypeComposite _state_types,
- int _dim,
- SizeEigen _state_size,
- SizeEigen _delta_size,
- SizeEigen _delta_cov_size,
- SizeEigen _data_size,
- SizeEigen _calib_size,
- ParamsProcessorMotionPtr _params_motion);
- ~ProcessorMotion() override;
-
- // Instructions to the processor:
- virtual void resetDerived();
-
- // Queries to the processor:
- TimeStamp getTimeStamp() const override;
- VectorComposite getState(const StateKeys& _structure = "") const override;
- VectorComposite getState(const TimeStamp& _ts, const StateKeys& _structure = "") const override;
-
- /** \brief Gets the delta preintegrated covariance from all integrations so far
- * \return the delta preintegrated covariance matrix
- */
- const Eigen::MatrixXd getCurrentDeltaPreintCov() const;
-
- /** \brief Provide the motion integrated so far
- * \return the integrated motion
- */
- Motion getMotion() const;
-
- /** \brief Provide the motion integrated until a given timestamp
- * \return the integrated motion
- */
- Motion getMotion(const TimeStamp& _ts) const;
-
- /** \brief Finds the capture that contains the closest previous motion of _ts
- * \return a pointer to the capture (if it exists) or a nullptr (otherwise)
- */
- CaptureMotionConstPtr findCaptureContainingTimeStamp(const TimeStamp& _ts) const;
- CaptureMotionPtr findCaptureContainingTimeStamp(const TimeStamp& _ts);
-
- /** Set the origin of all motion for this processor
- * \param _origin_frame the keyframe to be the origin
- */
- void setOrigin(FrameBasePtr _origin_frame);
-
- /** Set the origin of all motion for this processor
- * \param _x_origin the state at the origin
- * \param _ts_origin origin timestamp.
- */
- FrameBasePtr setOrigin(const VectorComposite& _x_origin, const TimeStamp& _ts_origin);
-
- // Helper functions:
- MotionBuffer& getBuffer();
- const MotionBuffer& getBuffer() const;
-
- protected:
-
- /** \brief process an incoming capture
- *
- * Each derived processor should implement this function. It will be called if:
- * - A new capture arrived and triggerInCapture() returned true.
- */
- void processCapture(CaptureBasePtr) override;
-
- /** \brief process an incoming key-frame
- *
- * The ProcessorMotion only processes incoming captures (it is not called).
- */
- void processKeyFrame(FrameBasePtr _keyframe_ptr) override {};
-
- /** \brief trigger in capture
- *
- * Returns true if processCapture() should be called after the provided capture arrived.
- */
- bool triggerInCapture(CaptureBasePtr) const override {return true;}
-
- /** \brief trigger in key-frame
- *
- * The ProcessorMotion only processes incoming captures, then it returns false.
- */
- bool triggerInKeyFrame(FrameBasePtr _keyframe_ptr) const override {return false;}
-
- /** \brief store key frame
- *
- * Returns true if the key frame should be stored
- */
- bool storeKeyFrame(FrameBasePtr) override { return true;}
-
- /** \brief store capture
- *
- * Returns true if the capture should be stored
- */
- bool storeCapture(CaptureBasePtr) override { return false;}
-
- bool voteForKeyFrame() const override;
-
- double updateDt();
- /** \brief Make one step of motion integration
- *
- * Integrate motiondata in incoming_ptr_ and store all results in the MotionBuffer in last_ptr_
- */
- void integrateOneStep();
- void reintegrateBuffer(CaptureMotionPtr _capture_ptr) const;
- void splitBuffer(const wolf::CaptureMotionPtr& capture_source,
- const TimeStamp ts_split,
- const wolf::CaptureMotionPtr& capture_target) const;
-
- /** Pre-process incoming Capture
- *
- * This is called by process() just after assigning incoming_ptr_ to a valid Capture.
- *
- * Overload this function to prepare stuff on derived classes.
- *
- * Typical uses of prePrecess() are:
- * - casting base types to derived types
- * - initializing counters, flags, or any derived variables
- * - initializing algorithms needed for processing the derived data
- */
- virtual void preProcess(){ };
-
- /**
- * @brief Bootstrap the processor with some initialization steps
- */
- virtual void bootstrap(){};
-
- /** Post-process
- *
- * This is called by process() after finishing the processing algorithm.
- *
- * Overload this function to post-process stuff on derived classes.
- *
- * Typical uses of postPrecess() are:
- * - resetting and/or clearing variables and/or algorithms at the end of processing
- * - drawing / printing / logging the results of the processing
- */
- virtual void postProcess(){ };
-
- FrameBasePtr computeProcessingStep();
-
- void assertsCaptureMotion(CaptureMotionPtr _capture, std::string error_prefix) const;
-
- // These are the pure virtual functions doing the mathematics
- public:
-
- /** \brief convert raw CaptureMotion data to the delta-state format
- *
- * This function accepts raw data and time step dt,
- * and computes the value of the delta-state and its covariance. Note that these values are
- * held by the members delta_ and delta_cov_.
- *
- * @param _data measured motion data
- * @param _data_cov covariance
- * @param _dt time step
- * @param _delta computed delta
- * @param _delta_cov covariance
- * @param _calib current state of the calibrated parameters
- * @param _jacobian_calib Jacobian of the delta wrt calib
- *
- * Rationale:
- *
- * The delta-state format must be compatible for integration using
- * the composition functions doing the math in this class: statePlusDelta() and deltaPlusDelta().
- * See the class documentation for some Eigen::VectorXd suggestions.
- *
- * The data format is generally not the same as the delta format,
- * because it is the format of the raw data provided by the Capture,
- * which is unaware of the needs of this processor.
- *
- * Additionally, sometimes the data format is in the form of a
- * velocity, or a higher derivative, while the delta is in the form of an increment.
- * In such cases, converting from data to delta-state needs integrating
- * the data over the period dt.
- *
- * Two trivial implementations would establish:
- * - If `_data` is an increment:
- *
- * delta_ = _data;
- * delta_cov_ = _data_cov
- * - If `_data` is a velocity:
- *
- * delta_ = _data * _dt
- * delta_cov_ = _data_cov * _dt.
- *
- * However, other more complicated relations are possible.
- * In general, we'll have a nonlinear
- * function relating `delta_` to `_data` and `_dt`, as follows:
- *
- * delta_ = f ( _data, _dt)
- *
- * The delta covariance is obtained by classical uncertainty propagation of the data covariance,
- * that is, through the Jacobians of `f()`,
- *
- * delta_cov_ = F_data * _data_cov * F_data.transpose
- *
- * where `F_data = d_f/d_data` is the Jacobian of `f()`.
- */
- virtual void computeCurrentDelta(const Eigen::VectorXd& _data,
- const Eigen::MatrixXd& _data_cov,
- const Eigen::VectorXd& _calib,
- const double _dt,
- Eigen::VectorXd& _delta,
- Eigen::MatrixXd& _delta_cov,
- Eigen::MatrixXd& _jacobian_calib) const = 0;
-
- /** \brief composes a delta-state on top of another delta-state
- * \param _delta1 the first delta-state
- * \param _delta2 the second delta-state
- * \param _dt2 the second delta-state's time delta
- * \param _delta1_plus_delta2 the delta2 composed on top of delta1. It has the format of delta-state.
- *
- * This function implements the composition (+) so that _delta1_plus_delta2 = _delta1 (+) _delta2.
- */
- virtual void deltaPlusDelta(const Eigen::VectorXd& _delta1,
- const Eigen::VectorXd& _delta2,
- const double _dt2,
- Eigen::VectorXd& _delta1_plus_delta2) const = 0;
-
- /** \brief composes a delta-state on top of another delta-state, and computes the Jacobians
- * \param _delta1 the first delta-state
- * \param _delta2 the second delta-state
- * \param _dt2 the second delta-state's time delta
- * \param _delta1_plus_delta2 the delta2 composed on top of delta1. It has the format of delta-state.
- * \param _jacobian1 the jacobian of the composition w.r.t. _delta1.
- * \param _jacobian2 the jacobian of the composition w.r.t. _delta2.
- *
- * This function implements the composition (+) so that _delta1_plus_delta2 = _delta1 (+) _delta2 and its jacobians.
- */
- virtual void deltaPlusDelta(const Eigen::VectorXd& _delta1,
- const Eigen::VectorXd& _delta2,
- const double _dt2,
- Eigen::VectorXd& _delta1_plus_delta2,
- Eigen::MatrixXd& _jacobian1,
- Eigen::MatrixXd& _jacobian2) const = 0;
-
- /** \brief composes a delta-state on top of a state
- * \param _x the initial state
- * \param _delta the delta-state
- * \param _x_plus_delta the updated state. It has the same format as the initial state.
- * \param _dt the time interval spanned by the Delta
- *
- * This function implements the composition (+) so that _x2 = _x1 (+) _delta.
- */
- virtual void statePlusDelta(const VectorComposite& _x,
- const Eigen::VectorXd& _delta,
- const double _dt,
- VectorComposite& _x_plus_delta) const = 0;
-
- /** \brief Delta zero
- * \return a delta state equivalent to the null motion.
- *
- * Examples (see documentation of the the class for info on Eigen::VectorXd):
- * - 2d odometry: a 3-vector with all zeros, e.g. VectorXd::Zero(3)
- * - 3d odometry: delta type is a PQ vector: 7-vector with [0,0,0, 0,0,0,1]
- * - IMU: PQVBB 10-vector with [0,0,0, 0,0,0,1, 0,0,0] // No biases in the delta !!
- */
- virtual Eigen::VectorXd deltaZero() const = 0;
-
-
- /** \brief correct the preintegrated delta
- * This produces a delta correction according to the appropriate manifold.
- * @param delta_preint : the preintegrated delta to correct
- * @param delta_step : an increment in the tangent space of the manifold
- *
- * We want to do
- *
- * delta_corr = delta_preint (+) d_step
- *
- * where the operator (+) is the right-plus operator on the delta's manifold.
- *
- * Note: usually in motion pre-integration we have
- *
- * delta_step = Jac_delta_calib * (calib - calib_pre)
- *
- */
- virtual Eigen::VectorXd correctDelta(const Eigen::VectorXd& delta_preint,
- const Eigen::VectorXd& delta_step) const = 0;
-
- /** \brief merge two consecutive capture motions into the second one
- * Merge two consecutive capture motions into the second one.
- * The first capture motion is not removed.
- * If the second capture has feature and factor emplaced, they are replaced by a new ones.
- * @param cap_prev : the first capture motion to be merged (input)
- * @param cap_target : the second capture motion (modified)
- */
- void mergeCaptures(CaptureMotionPtr cap_prev,
- CaptureMotionPtr cap_target);
-
- protected:
- /** \brief emplace a CaptureMotion
- * \param _frame_own frame owning the Capture to create
- * \param _sensor Sensor that produced the Capture
- * \param _ts time stamp
- * \param _data a vector of motion data
- * \param _data_cov covariances matrix of the motion data data
- * \param _calib calibration vector
- * \param _calib_preint calibration vector used during pre-integration
- * \param _capture_origin_ptr capture acting as the origin of motions for the MorionBuffer of the created MotionCapture
- */
- virtual CaptureMotionPtr emplaceCapture(const FrameBasePtr& _frame_own,
- const SensorBasePtr& _sensor,
- const TimeStamp& _ts,
- const VectorXd& _data,
- const MatrixXd& _data_cov,
- const VectorXd& _calib,
- const VectorXd& _calib_preint,
- const CaptureBasePtr& _capture_origin_ptr) = 0;
-
-
- /** \brief emplace the features and factors corresponding to given capture and link them to the capture
- * \param _capture_own: the parent capture
- * \param _capture_origin: the capture constrained by this motion factor
- *
- * Typical factors to add for a ProcessorMotionDerived can be:
- * - A preintegrated motion factor -- this is the main factor
- * - A calibration drift factor -- only for dynamic sensor calibration parameters
- * - An instantaneous factor for observing non-typical states of the Frame -- useful for complex dynamic robot models
- */
- virtual void emplaceFeaturesAndFactors(CaptureBasePtr _capture_origin, CaptureMotionPtr _capture_own) = 0;
-
- virtual void setCalibration(const CaptureBasePtr _capture, const VectorXd& _calibration) = 0;
-
- Motion motionZero(const TimeStamp& _ts) const;
-
- public:
-
- virtual VectorXd getCalibration (const CaptureBaseConstPtr _capture = nullptr) const = 0;
- bool hasCalibration() const {return calib_size_ > 0;}
-
- //getters
- CaptureMotionConstPtr getOrigin() const;
- CaptureMotionConstPtr getLast() const;
- CaptureMotionConstPtr getIncoming() const;
- CaptureMotionPtr getOrigin();
- CaptureMotionPtr getLast();
- CaptureMotionPtr getIncoming();
-
- double getMaxTimeSpan() const;
- double getMaxBuffLength() const;
- double getDistTraveled() const;
- double getAngleTurned() const;
-
- void setMaxTimeSpan(const double& _max_time_span);
- void setMaxBuffLength(const double& _max_buff_length);
- void setDistTraveled(const double& _dist_traveled);
- void setAngleTurned(const double& _angle_turned);
-
- void printHeader(int depth, //
- bool factored_by, //
- bool metric, //
- bool state_blocks,
- std::ostream& stream ,
- std::string _tabs = "") const override;
-
- protected:
- // Attributes
- SizeEigen x_size_; ///< The size of the state vector
- SizeEigen data_size_; ///< the size of the incoming data
- SizeEigen delta_size_; ///< the size of the deltas
- SizeEigen delta_cov_size_; ///< the size of the delta covariances matrix
- SizeEigen calib_size_; ///< the size of the calibration parameters (TBD in derived classes)
- CaptureMotionPtr origin_ptr_;
- CaptureMotionPtr last_ptr_;
- CaptureMotionPtr incoming_ptr_;
-
- FrameBasePtr last_frame_ptr_;
-
- protected:
- // helpers to avoid allocation
- mutable double dt_; ///< Time step
- mutable Eigen::VectorXd x_; ///< current state
- mutable Eigen::VectorXd delta_; ///< current delta
- mutable Eigen::MatrixXd delta_cov_; ///< current delta covariance
- mutable Eigen::VectorXd delta_integrated_; ///< integrated delta
- mutable Eigen::MatrixXd delta_integrated_cov_; ///< integrated delta covariance
- mutable Eigen::VectorXd calib_preint_; ///< calibration vector used during pre-integration
- mutable Eigen::MatrixXd jacobian_delta_preint_; ///< jacobian of delta composition w.r.t previous delta integrated
- mutable Eigen::MatrixXd jacobian_delta_; ///< jacobian of delta composition w.r.t current delta
- mutable Eigen::MatrixXd jacobian_calib_; ///< jacobian of delta preintegration wrt calibration params
- mutable Eigen::MatrixXd jacobian_delta_calib_; ///< jacobian of delta wrt calib params
-
- Eigen::MatrixXd unmeasured_perturbation_cov_; ///< Covariance of unmeasured DoF to avoid singularity
+ public:
+ typedef enum
+ {
+ FIRST_TIME_WITHOUT_KF,
+ FIRST_TIME_WITH_KF_BEFORE_INCOMING,
+ FIRST_TIME_WITH_KF_ON_INCOMING,
+ FIRST_TIME_WITH_KF_AFTER_INCOMING,
+ RUNNING_WITHOUT_KF,
+ RUNNING_WITH_KF_BEFORE_ORIGIN,
+ RUNNING_WITH_KF_ON_ORIGIN,
+ RUNNING_WITH_KF_AFTER_ORIGIN
+ } ProcessingStep;
+
+ protected:
+ ProcessingStep processing_step_; ///< State machine controlling the processing step
+ bool bootstrapping_; ///< processor is bootstrapping
+
+ // This is the main public interface
+ public:
+ ProcessorMotion(const std::string& _type,
+ TypeComposite _state_types,
+ int _dim,
+ SizeEigen _state_size,
+ SizeEigen _delta_size,
+ SizeEigen _delta_cov_size,
+ SizeEigen _data_size,
+ SizeEigen _calib_size,
+ const YAML::Node& _params);
+ ~ProcessorMotion() override;
+
+ // Instructions to the processor:
+ virtual void resetDerived();
+
+ // Queries to the processor:
+ TimeStamp getTimeStamp() const override;
+ VectorComposite getState(const StateKeys& _structure = "") const override;
+ VectorComposite getState(const TimeStamp& _ts, const StateKeys& _structure = "") const override;
+
+ /** \brief Gets the delta preintegrated covariance from all integrations so far
+ * \return the delta preintegrated covariance matrix
+ */
+ const Eigen::MatrixXd getCurrentDeltaPreintCov() const;
+
+ /** \brief Provide the motion integrated so far
+ * \return the integrated motion
+ */
+ Motion getMotion() const;
+
+ /** \brief Provide the motion integrated until a given timestamp
+ * \return the integrated motion
+ */
+ Motion getMotion(const TimeStamp& _ts) const;
+
+ /** \brief Finds the capture that contains the closest previous motion of _ts
+ * \return a pointer to the capture (if it exists) or a nullptr (otherwise)
+ */
+ CaptureMotionConstPtr findCaptureContainingTimeStamp(const TimeStamp& _ts) const;
+ CaptureMotionPtr findCaptureContainingTimeStamp(const TimeStamp& _ts);
+
+ /** Set the origin of all motion for this processor
+ * \param _origin_frame the keyframe to be the origin
+ */
+ void setOrigin(FrameBasePtr _origin_frame);
+
+ /** Set the origin of all motion for this processor
+ * \param _x_origin the state at the origin
+ * \param _ts_origin origin timestamp.
+ */
+ FrameBasePtr setOrigin(const VectorComposite& _x_origin, const TimeStamp& _ts_origin);
+
+ // Helper functions:
+ MotionBuffer& getBuffer();
+ const MotionBuffer& getBuffer() const;
+
+ protected:
+ /** \brief process an incoming capture
+ *
+ * Each derived processor should implement this function. It will be called if:
+ * - A new capture arrived and triggerInCapture() returned true.
+ */
+ void processCapture(CaptureBasePtr) override;
+
+ /** \brief process an incoming key-frame
+ *
+ * The ProcessorMotion only processes incoming captures (it is not called).
+ */
+ void processKeyFrame(FrameBasePtr _keyframe_ptr) override{};
+
+ /** \brief trigger in capture
+ *
+ * Returns true if processCapture() should be called after the provided capture arrived.
+ */
+ bool triggerInCapture(CaptureBasePtr) const override
+ {
+ return true;
+ }
+
+ /** \brief trigger in key-frame
+ *
+ * The ProcessorMotion only processes incoming captures, then it returns false.
+ */
+ bool triggerInKeyFrame(FrameBasePtr _keyframe_ptr) const override
+ {
+ return false;
+ }
+
+ /** \brief store key frame
+ *
+ * Returns true if the key frame should be stored
+ */
+ bool storeKeyFrame(FrameBasePtr) override
+ {
+ return true;
+ }
+
+ /** \brief store capture
+ *
+ * Returns true if the capture should be stored
+ */
+ bool storeCapture(CaptureBasePtr) override
+ {
+ return false;
+ }
+
+ bool voteForKeyFrame() const override;
+
+ double updateDt();
+ /** \brief Make one step of motion integration
+ *
+ * Integrate motiondata in incoming_ptr_ and store all results in the MotionBuffer in last_ptr_
+ */
+ void integrateOneStep();
+ void reintegrateBuffer(CaptureMotionPtr _capture_ptr) const;
+ void splitBuffer(const wolf::CaptureMotionPtr& capture_source,
+ const TimeStamp ts_split,
+ const wolf::CaptureMotionPtr& capture_target) const;
+
+ /** Pre-process incoming Capture
+ *
+ * This is called by process() just after assigning incoming_ptr_ to a valid Capture.
+ *
+ * Overload this function to prepare stuff on derived classes.
+ *
+ * Typical uses of prePrecess() are:
+ * - casting base types to derived types
+ * - initializing counters, flags, or any derived variables
+ * - initializing algorithms needed for processing the derived data
+ */
+ virtual void preProcess(){};
+
+ /**
+ * @brief Bootstrap the processor with some initialization steps
+ */
+ virtual void bootstrap(){};
+
+ /** Post-process
+ *
+ * This is called by process() after finishing the processing algorithm.
+ *
+ * Overload this function to post-process stuff on derived classes.
+ *
+ * Typical uses of postPrecess() are:
+ * - resetting and/or clearing variables and/or algorithms at the end of processing
+ * - drawing / printing / logging the results of the processing
+ */
+ virtual void postProcess(){};
+
+ FrameBasePtr computeProcessingStep();
+
+ void assertsCaptureMotion(CaptureMotionPtr _capture, std::string error_prefix) const;
+
+ // These are the pure virtual functions doing the mathematics
+ public:
+ /** \brief convert raw CaptureMotion data to the delta-state format
+ *
+ * This function accepts raw data and time step dt,
+ * and computes the value of the delta-state and its covariance. Note that these values are
+ * held by the members delta_ and delta_cov_.
+ *
+ * @param _data measured motion data
+ * @param _data_cov covariance
+ * @param _dt time step
+ * @param _delta computed delta
+ * @param _delta_cov covariance
+ * @param _calib current state of the calibrated parameters
+ * @param _jacobian_calib Jacobian of the delta wrt calib
+ *
+ * Rationale:
+ *
+ * The delta-state format must be compatible for integration using
+ * the composition functions doing the math in this class: statePlusDelta() and deltaPlusDelta().
+ * See the class documentation for some Eigen::VectorXd suggestions.
+ *
+ * The data format is generally not the same as the delta format,
+ * because it is the format of the raw data provided by the Capture,
+ * which is unaware of the needs of this processor.
+ *
+ * Additionally, sometimes the data format is in the form of a
+ * velocity, or a higher derivative, while the delta is in the form of an increment.
+ * In such cases, converting from data to delta-state needs integrating
+ * the data over the period dt.
+ *
+ * Two trivial implementations would establish:
+ * - If `_data` is an increment:
+ *
+ * delta_ = _data;
+ * delta_cov_ = _data_cov
+ * - If `_data` is a velocity:
+ *
+ * delta_ = _data * _dt
+ * delta_cov_ = _data_cov * _dt.
+ *
+ * However, other more complicated relations are possible.
+ * In general, we'll have a nonlinear
+ * function relating `delta_` to `_data` and `_dt`, as follows:
+ *
+ * delta_ = f ( _data, _dt)
+ *
+ * The delta covariance is obtained by classical uncertainty propagation of the data covariance,
+ * that is, through the Jacobians of `f()`,
+ *
+ * delta_cov_ = F_data * _data_cov * F_data.transpose
+ *
+ * where `F_data = d_f/d_data` is the Jacobian of `f()`.
+ */
+ virtual void computeCurrentDelta(const Eigen::VectorXd& _data,
+ const Eigen::MatrixXd& _data_cov,
+ const Eigen::VectorXd& _calib,
+ const double _dt,
+ Eigen::VectorXd& _delta,
+ Eigen::MatrixXd& _delta_cov,
+ Eigen::MatrixXd& _jacobian_calib) const = 0;
+
+ /** \brief composes a delta-state on top of another delta-state
+ * \param _delta1 the first delta-state
+ * \param _delta2 the second delta-state
+ * \param _dt2 the second delta-state's time delta
+ * \param _delta1_plus_delta2 the delta2 composed on top of delta1. It has the format of delta-state.
+ *
+ * This function implements the composition (+) so that _delta1_plus_delta2 = _delta1 (+) _delta2.
+ */
+ virtual void deltaPlusDelta(const Eigen::VectorXd& _delta1,
+ const Eigen::VectorXd& _delta2,
+ const double _dt2,
+ Eigen::VectorXd& _delta1_plus_delta2) const = 0;
+
+ /** \brief composes a delta-state on top of another delta-state, and computes the Jacobians
+ * \param _delta1 the first delta-state
+ * \param _delta2 the second delta-state
+ * \param _dt2 the second delta-state's time delta
+ * \param _delta1_plus_delta2 the delta2 composed on top of delta1. It has the format of delta-state.
+ * \param _jacobian1 the jacobian of the composition w.r.t. _delta1.
+ * \param _jacobian2 the jacobian of the composition w.r.t. _delta2.
+ *
+ * This function implements the composition (+) so that _delta1_plus_delta2 = _delta1 (+) _delta2 and its
+ * jacobians.
+ */
+ virtual void deltaPlusDelta(const Eigen::VectorXd& _delta1,
+ const Eigen::VectorXd& _delta2,
+ const double _dt2,
+ Eigen::VectorXd& _delta1_plus_delta2,
+ Eigen::MatrixXd& _jacobian1,
+ Eigen::MatrixXd& _jacobian2) const = 0;
+
+ /** \brief composes a delta-state on top of a state
+ * \param _x the initial state
+ * \param _delta the delta-state
+ * \param _x_plus_delta the updated state. It has the same format as the initial state.
+ * \param _dt the time interval spanned by the Delta
+ *
+ * This function implements the composition (+) so that _x2 = _x1 (+) _delta.
+ */
+ virtual void statePlusDelta(const VectorComposite& _x,
+ const Eigen::VectorXd& _delta,
+ const double _dt,
+ VectorComposite& _x_plus_delta) const = 0;
+
+ /** \brief Delta zero
+ * \return a delta state equivalent to the null motion.
+ *
+ * Examples (see documentation of the the class for info on Eigen::VectorXd):
+ * - 2d odometry: a 3-vector with all zeros, e.g. VectorXd::Zero(3)
+ * - 3d odometry: delta type is a PQ vector: 7-vector with [0,0,0, 0,0,0,1]
+ * - IMU: PQVBB 10-vector with [0,0,0, 0,0,0,1, 0,0,0] // No biases in the delta !!
+ */
+ virtual Eigen::VectorXd deltaZero() const = 0;
+
+ /** \brief correct the preintegrated delta
+ * This produces a delta correction according to the appropriate manifold.
+ * @param delta_preint : the preintegrated delta to correct
+ * @param delta_step : an increment in the tangent space of the manifold
+ *
+ * We want to do
+ *
+ * delta_corr = delta_preint (+) d_step
+ *
+ * where the operator (+) is the right-plus operator on the delta's manifold.
+ *
+ * Note: usually in motion pre-integration we have
+ *
+ * delta_step = Jac_delta_calib * (calib - calib_pre)
+ *
+ */
+ virtual Eigen::VectorXd correctDelta(const Eigen::VectorXd& delta_preint,
+ const Eigen::VectorXd& delta_step) const = 0;
+
+ /** \brief merge two consecutive capture motions into the second one
+ * Merge two consecutive capture motions into the second one.
+ * The first capture motion is not removed.
+ * If the second capture has feature and factor emplaced, they are replaced by a new ones.
+ * @param cap_prev : the first capture motion to be merged (input)
+ * @param cap_target : the second capture motion (modified)
+ */
+ void mergeCaptures(CaptureMotionPtr cap_prev, CaptureMotionPtr cap_target);
+
+ protected:
+ /** \brief emplace a CaptureMotion
+ * \param _frame_own frame owning the Capture to create
+ * \param _sensor Sensor that produced the Capture
+ * \param _ts time stamp
+ * \param _data a vector of motion data
+ * \param _data_cov covariances matrix of the motion data data
+ * \param _calib calibration vector
+ * \param _calib_preint calibration vector used during pre-integration
+ * \param _capture_origin_ptr capture acting as the origin of motions for the MorionBuffer of the created MotionCapture
+ */
+ virtual CaptureMotionPtr emplaceCapture(const FrameBasePtr& _frame_own,
+ const SensorBasePtr& _sensor,
+ const TimeStamp& _ts,
+ const VectorXd& _data,
+ const MatrixXd& _data_cov,
+ const VectorXd& _calib,
+ const VectorXd& _calib_preint,
+ const CaptureBasePtr& _capture_origin_ptr) = 0;
+
+ /** \brief emplace the features and factors corresponding to given capture and link them to the capture
+ * \param _capture_own: the parent capture
+ * \param _capture_origin: the capture constrained by this motion factor
+ *
+ * Typical factors to add for a ProcessorMotionDerived can be:
+ * - A preintegrated motion factor -- this is the main factor
+ * - A calibration drift factor -- only for dynamic sensor calibration parameters
+ * - An instantaneous factor for observing non-typical states of the Frame -- useful for complex dynamic robot
+ * models
+ */
+ virtual void emplaceFeaturesAndFactors(CaptureBasePtr _capture_origin, CaptureMotionPtr _capture_own) = 0;
+
+ virtual void setCalibration(const CaptureBasePtr _capture, const VectorXd& _calibration) = 0;
+
+ Motion motionZero(const TimeStamp& _ts) const;
+
+ public:
+ virtual VectorXd getCalibration(const CaptureBaseConstPtr _capture = nullptr) const = 0;
+ bool hasCalibration() const
+ {
+ return calib_size_ > 0;
+ }
+
+ // getters
+ CaptureMotionConstPtr getOrigin() const;
+ CaptureMotionConstPtr getLast() const;
+ CaptureMotionConstPtr getIncoming() const;
+ CaptureMotionPtr getOrigin();
+ CaptureMotionPtr getLast();
+ CaptureMotionPtr getIncoming();
+
+ Eigen::MatrixXd getUnmeasuredPerturbationCov() const;
+
+ double getMaxTimeSpan() const;
+ double getMaxBuffLength() const;
+ double getMaxDistTraveled() const;
+ double getMaxAngleTurned() const;
+
+ void setMaxTimeSpan(const double& _max_time_span);
+ void setMaxBuffLength(const double& _max_buff_length);
+ void setMaxDistTraveled(const double& _max_dist_traveled);
+ void setMaxAngleTurned(const double& _max_angle_turned);
+
+ void printHeader(int depth, //
+ bool factored_by, //
+ bool metric, //
+ bool state_blocks,
+ std::ostream& stream,
+ std::string _tabs = "") const override;
+
+ protected:
+ // Attributes
+ SizeEigen x_size_; ///< The size of the state vector
+ SizeEigen data_size_; ///< the size of the incoming data
+ SizeEigen delta_size_; ///< the size of the deltas
+ SizeEigen delta_cov_size_; ///< the size of the delta covariances matrix
+ SizeEigen calib_size_; ///< the size of the calibration parameters (TBD in derived classes)
+ CaptureMotionPtr origin_ptr_;
+ CaptureMotionPtr last_ptr_;
+ CaptureMotionPtr incoming_ptr_;
+
+ FrameBasePtr last_frame_ptr_;
+
+ protected:
+ // helpers to avoid allocation
+ mutable double dt_; ///< Time step
+ mutable Eigen::VectorXd x_; ///< current state
+ mutable Eigen::VectorXd delta_; ///< current delta
+ mutable Eigen::MatrixXd delta_cov_; ///< current delta covariance
+ mutable Eigen::VectorXd delta_integrated_; ///< integrated delta
+ mutable Eigen::MatrixXd delta_integrated_cov_; ///< integrated delta covariance
+ mutable Eigen::VectorXd calib_preint_; ///< calibration vector used during pre-integration
+ mutable Eigen::MatrixXd jacobian_delta_preint_; ///< jacobian of delta composition w.r.t previous delta integrated
+ mutable Eigen::MatrixXd jacobian_delta_; ///< jacobian of delta composition w.r.t current delta
+ mutable Eigen::MatrixXd jacobian_calib_; ///< jacobian of delta preintegration wrt calibration params
+ mutable Eigen::MatrixXd jacobian_delta_calib_; ///< jacobian of delta wrt calib params
+
+ Eigen::MatrixXd unmeasured_perturbation_cov_; ///< Covariance of unmeasured DoF to avoid singularity
};
-
-}
+} // namespace wolf
#include "core/frame/frame_base.h"
-namespace wolf{
-
+namespace wolf
+{
inline void ProcessorMotion::resetDerived()
{
// Blank function, to be implemented in derived classes
@@ -602,15 +579,15 @@ inline MotionBuffer& ProcessorMotion::getBuffer()
inline Motion ProcessorMotion::motionZero(const TimeStamp& _ts) const
{
return Motion(_ts,
- VectorXd::Zero(data_size_), // data
- Eigen::MatrixXd::Zero(data_size_, data_size_), // Cov data
+ VectorXd::Zero(data_size_), // data
+ Eigen::MatrixXd::Zero(data_size_, data_size_), // Cov data
deltaZero(),
- Eigen::MatrixXd::Zero(delta_cov_size_, delta_cov_size_), // Cov delta
+ Eigen::MatrixXd::Zero(delta_cov_size_, delta_cov_size_), // Cov delta
deltaZero(),
- Eigen::MatrixXd::Zero(delta_cov_size_, delta_cov_size_), // Cov delta_integr
- Eigen::MatrixXd::Zero(delta_cov_size_, delta_cov_size_), // Jac delta
- Eigen::MatrixXd::Zero(delta_cov_size_, delta_cov_size_), // Jac delta_integr
- Eigen::MatrixXd::Zero(delta_cov_size_, calib_size_) // Jac calib
+ Eigen::MatrixXd::Zero(delta_cov_size_, delta_cov_size_), // Cov delta_integr
+ Eigen::MatrixXd::Zero(delta_cov_size_, delta_cov_size_), // Jac delta
+ Eigen::MatrixXd::Zero(delta_cov_size_, delta_cov_size_), // Jac delta_integr
+ Eigen::MatrixXd::Zero(delta_cov_size_, calib_size_) // Jac calib
);
}
@@ -644,41 +621,49 @@ inline CaptureMotionPtr ProcessorMotion::getIncoming()
return incoming_ptr_;
}
+inline Eigen::MatrixXd ProcessorMotion::getUnmeasuredPerturbationCov() const
+{
+ return unmeasured_perturbation_cov_;
+}
+
inline double ProcessorMotion::getMaxTimeSpan() const
{
- return params_motion_->max_time_span;
+ return params_["keyframe_vote"]["max_time_span"].as<double>();
}
inline double ProcessorMotion::getMaxBuffLength() const
{
- return params_motion_->max_buff_length;
+ return params_["keyframe_vote"]["max_buff_length"].as<unsigned int>();
}
-inline double ProcessorMotion::getDistTraveled() const
+inline double ProcessorMotion::getMaxDistTraveled() const
{
- return params_motion_->dist_traveled;
+ return params_["keyframe_vote"]["max_dist_traveled"].as<double>();
}
-inline double ProcessorMotion::getAngleTurned() const
+inline double ProcessorMotion::getMaxAngleTurned() const
{
- return params_motion_->angle_turned;
+ return params_["keyframe_vote"]["max_angle_turned"].as<double>();
}
inline void ProcessorMotion::setMaxTimeSpan(const double& _max_time_span)
{
- params_motion_->max_time_span = _max_time_span;
+ params_["keyframe_vote"]["max_time_span"] = _max_time_span;
}
+
inline void ProcessorMotion::setMaxBuffLength(const double& _max_buff_length)
{
- params_motion_->max_buff_length = _max_buff_length;
+ params_["keyframe_vote"]["max_buff_length"] = _max_buff_length;
}
-inline void ProcessorMotion::setDistTraveled(const double& _dist_traveled)
+
+inline void ProcessorMotion::setMaxDistTraveled(const double& _max_dist_traveled)
{
- params_motion_->dist_traveled = _dist_traveled;
+ params_["keyframe_vote"]["max_dist_traveled"] = _max_dist_traveled;
}
-inline void ProcessorMotion::setAngleTurned(const double& _angle_turned)
+
+inline void ProcessorMotion::setMaxAngleTurned(const double& _max_angle_turned)
{
- params_motion_->angle_turned = _angle_turned;
+ params_["keyframe_vote"]["max_angle_turned"] = _max_angle_turned;
}
-} // namespace wolf
+} // namespace wolf
diff --git a/include/core/processor/processor_odom_2d.h b/include/core/processor/processor_odom_2d.h
index 940b7eb1f63eb8eea1bb91896c306c7e51ef9298..0c3268f447a9bf126ee007df99672a860c0def3c 100644
--- a/include/core/processor/processor_odom_2d.h
+++ b/include/core/processor/processor_odom_2d.h
@@ -25,82 +25,60 @@
#include "core/math/rotations.h"
#include "core/math/SE2.h"
-namespace wolf {
-
-WOLF_PTR_TYPEDEFS(ProcessorOdom2d);
-WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorOdom2d);
-
-struct ParamsProcessorOdom2d : public ParamsProcessorMotion
+namespace wolf
{
- double cov_det = 1.0;
-
- ParamsProcessorOdom2d() = default;
- ParamsProcessorOdom2d(const YAML::Node& _n) :
- ParamsProcessorMotion(_n)
- {
- cov_det = _n["keyframe_vote"]["cov_det"].as<double>();
- }
-
- std::string print() const override
- {
- return ParamsProcessorMotion::print() + "\n"
- + "keyframe_vote/cov_det: " + toString(cov_det) + "\n";
- }
-};
+WOLF_PTR_TYPEDEFS(ProcessorOdom2d);
class ProcessorOdom2d : public ProcessorMotion
{
- public:
- ProcessorOdom2d(ParamsProcessorOdom2dPtr _params);
- ~ProcessorOdom2d() override;
- void configure(SensorBasePtr _sensor) override { };
-
- // Factory method for high level API
- WOLF_PROCESSOR_CREATE(ProcessorOdom2d, ParamsProcessorOdom2d);
-
- bool voteForKeyFrame() const override;
-
- protected:
- void computeCurrentDelta(const Eigen::VectorXd& _data,
- const Eigen::MatrixXd& _data_cov,
- const Eigen::VectorXd& _calib,
- const double _dt,
- Eigen::VectorXd& _delta,
- Eigen::MatrixXd& _delta_cov,
- Eigen::MatrixXd& _jacobian_calib) const override;
- void deltaPlusDelta(const Eigen::VectorXd& _delta1,
- const Eigen::VectorXd& _delta2,
- const double _Dt2,
- Eigen::VectorXd& _delta1_plus_delta2) const override;
- void deltaPlusDelta(const Eigen::VectorXd& _delta1,
- const Eigen::VectorXd& _delta2,
- const double _Dt2,
- Eigen::VectorXd& _delta1_plus_delta2,
- Eigen::MatrixXd& _jacobian1,
- Eigen::MatrixXd& _jacobian2) const override;
- void statePlusDelta(const VectorComposite& _x,
- const Eigen::VectorXd& _delta,
- const double _Dt,
- VectorComposite& _x_plus_delta) const override;
- Eigen::VectorXd deltaZero() const override;
- Eigen::VectorXd correctDelta(const Eigen::VectorXd& delta_preint,
- const Eigen::VectorXd& delta_step) const override;
-
- CaptureMotionPtr emplaceCapture(const FrameBasePtr& _frame_own,
- const SensorBasePtr& _sensor,
- const TimeStamp& _ts,
- const VectorXd& _data,
- const MatrixXd& _data_cov,
- const VectorXd& _calib,
- const VectorXd& _calib_preint,
- const CaptureBasePtr& _capture_origin_ptr) override;
- void emplaceFeaturesAndFactors(CaptureBasePtr _capture_origin, CaptureMotionPtr _capture_own) override;
- VectorXd getCalibration(const CaptureBaseConstPtr _capture) const override;
- void setCalibration(const CaptureBasePtr _capture, const VectorXd& _calibration) override;
-
- protected:
- ParamsProcessorOdom2dPtr params_odom_2d_;
-
+ public:
+ ProcessorOdom2d(const YAML::Node& _params);
+ ~ProcessorOdom2d() override;
+ void configure(SensorBasePtr _sensor) override{};
+
+ // Factory method for high level API
+ WOLF_PROCESSOR_CREATE(ProcessorOdom2d);
+
+ double getCovDet() const;
+ bool voteForKeyFrame() const override;
+
+ protected:
+ void computeCurrentDelta(const Eigen::VectorXd& _data,
+ const Eigen::MatrixXd& _data_cov,
+ const Eigen::VectorXd& _calib,
+ const double _dt,
+ Eigen::VectorXd& _delta,
+ Eigen::MatrixXd& _delta_cov,
+ Eigen::MatrixXd& _jacobian_calib) const override;
+ void deltaPlusDelta(const Eigen::VectorXd& _delta1,
+ const Eigen::VectorXd& _delta2,
+ const double _Dt2,
+ Eigen::VectorXd& _delta1_plus_delta2) const override;
+ void deltaPlusDelta(const Eigen::VectorXd& _delta1,
+ const Eigen::VectorXd& _delta2,
+ const double _Dt2,
+ Eigen::VectorXd& _delta1_plus_delta2,
+ Eigen::MatrixXd& _jacobian1,
+ Eigen::MatrixXd& _jacobian2) const override;
+ void statePlusDelta(const VectorComposite& _x,
+ const Eigen::VectorXd& _delta,
+ const double _Dt,
+ VectorComposite& _x_plus_delta) const override;
+ Eigen::VectorXd deltaZero() const override;
+ Eigen::VectorXd correctDelta(const Eigen::VectorXd& delta_preint,
+ const Eigen::VectorXd& delta_step) const override;
+
+ CaptureMotionPtr emplaceCapture(const FrameBasePtr& _frame_own,
+ const SensorBasePtr& _sensor,
+ const TimeStamp& _ts,
+ const VectorXd& _data,
+ const MatrixXd& _data_cov,
+ const VectorXd& _calib,
+ const VectorXd& _calib_preint,
+ const CaptureBasePtr& _capture_origin_ptr) override;
+ void emplaceFeaturesAndFactors(CaptureBasePtr _capture_origin, CaptureMotionPtr _capture_own) override;
+ VectorXd getCalibration(const CaptureBaseConstPtr _capture) const override;
+ void setCalibration(const CaptureBasePtr _capture, const VectorXd& _calibration) override;
};
inline Eigen::VectorXd ProcessorOdom2d::deltaZero() const
@@ -108,21 +86,24 @@ inline Eigen::VectorXd ProcessorOdom2d::deltaZero() const
return Eigen::VectorXd::Zero(delta_size_);
}
-inline Eigen::VectorXd ProcessorOdom2d::correctDelta (const Eigen::VectorXd& delta_preint,
- const Eigen::VectorXd& delta_step) const
+inline Eigen::VectorXd ProcessorOdom2d::correctDelta(const Eigen::VectorXd& delta_preint,
+ const Eigen::VectorXd& delta_step) const
{
VectorXd delta_corrected(3);
SE2::plus(delta_preint, delta_step, delta_corrected);
return delta_corrected;
}
-inline VectorXd ProcessorOdom2d::getCalibration (const CaptureBaseConstPtr _capture) const
+inline VectorXd ProcessorOdom2d::getCalibration(const CaptureBaseConstPtr _capture) const
{
return VectorXd::Zero(0);
}
-inline void ProcessorOdom2d::setCalibration (const CaptureBasePtr _capture, const VectorXd& _calibration)
+inline void ProcessorOdom2d::setCalibration(const CaptureBasePtr _capture, const VectorXd& _calibration) {}
+
+inline double ProcessorOdom2d::getCovDet() const
{
+ return params_["keyframe_vote"]["cov_det"].as<double>();
}
-} // namespace wolf
+} // namespace wolf
diff --git a/include/core/processor/processor_odom_3d.h b/include/core/processor/processor_odom_3d.h
index f959b98ec4d38a13e4bf9c2bdf6e5ae04c1e56b3..ea9855622162affaf61cc616b4936bde8276d208 100644
--- a/include/core/processor/processor_odom_3d.h
+++ b/include/core/processor/processor_odom_3d.h
@@ -26,24 +26,8 @@
#include "core/factor/factor_relative_pose_3d.h"
#include <cmath>
-namespace wolf {
-
-WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorOdom3d);
-
-struct ParamsProcessorOdom3d : public ParamsProcessorMotion
+namespace wolf
{
- ParamsProcessorOdom3d() = default;
- ParamsProcessorOdom3d(const YAML::Node& _n):
- ParamsProcessorMotion(_n)
- {
- //
- }
- std::string print() const override
- {
- return ParamsProcessorMotion::print();
- }
-};
-
WOLF_PTR_TYPEDEFS(ProcessorOdom3d);
/** \brief Processor for 3d odometry integration.
@@ -53,7 +37,8 @@ WOLF_PTR_TYPEDEFS(ProcessorOdom3d);
* The odometry data is extracted from Captures of the type CaptureOdometry3d.
* This data comes in the form of a 6-vector, or a 7-vector, containing the following components:
* - a 3d position increment in the local frame of the robot (dx, dy, dz).
- * - a 3d orientation increment in the local frame of the robot (droll, dpitch, dyaw), or quaternion (dqx, dqy, dqz, dqw).
+ * - a 3d orientation increment in the local frame of the robot (droll, dpitch, dyaw), or quaternion (dqx, dqy, dqz,
+ * dqw).
*
* The produced integrated deltas are in the form of 7-vectors with the following components:
* - a 3d position increment in the local frame of the robot (Dx, Dy, Dz)
@@ -70,64 +55,60 @@ WOLF_PTR_TYPEDEFS(ProcessorOdom3d);
*/
class ProcessorOdom3d : public ProcessorMotion
{
- public:
- ProcessorOdom3d(ParamsProcessorOdom3dPtr _params);
- ~ProcessorOdom3d() override;
- void configure(SensorBasePtr _sensor) override;
-
- // Factory method for high level API
- WOLF_PROCESSOR_CREATE(ProcessorOdom3d, ParamsProcessorOdom3d);
-
- public:
- // Motion integration
- void computeCurrentDelta(const Eigen::VectorXd& _data,
- const Eigen::MatrixXd& _data_cov,
- const Eigen::VectorXd& _calib,
- const double _dt,
- Eigen::VectorXd& _delta,
- Eigen::MatrixXd& _delta_cov,
- Eigen::MatrixXd& _jacobian_calib) const override;
- void deltaPlusDelta(const Eigen::VectorXd& _delta1,
- const Eigen::VectorXd& _delta2,
- const double _Dt2,
- Eigen::VectorXd& _delta1_plus_delta2) const override;
- void deltaPlusDelta(const Eigen::VectorXd& _delta1,
- const Eigen::VectorXd& _delta2,
- const double _Dt2,
- Eigen::VectorXd& _delta1_plus_delta2,
- Eigen::MatrixXd& _jacobian1,
- Eigen::MatrixXd& _jacobian2) const override;
- void statePlusDelta(const VectorComposite& _x,
- const Eigen::VectorXd& _delta,
- const double _Dt,
- VectorComposite& _x_plus_delta) const override;
- Eigen::VectorXd deltaZero() const override;
- Eigen::VectorXd correctDelta(const Eigen::VectorXd& delta_preint,
- const Eigen::VectorXd& delta_step) const override;
+ public:
+ ProcessorOdom3d(const YAML::Node& _params);
+ ~ProcessorOdom3d() override;
+ void configure(SensorBasePtr _sensor) override;
- bool voteForKeyFrame() const override;
+ // Factory method for high level API
+ WOLF_PROCESSOR_CREATE(ProcessorOdom3d);
- CaptureMotionPtr emplaceCapture(const FrameBasePtr& _frame_own,
- const SensorBasePtr& _sensor,
- const TimeStamp& _ts,
- const VectorXd& _data,
- const MatrixXd& _data_cov,
- const VectorXd& _calib,
- const VectorXd& _calib_preint,
- const CaptureBasePtr& _capture_origin) override;
- void emplaceFeaturesAndFactors(CaptureBasePtr _capture_origin, CaptureMotionPtr _capture_own) override;
+ public:
+ // Motion integration
+ void computeCurrentDelta(const Eigen::VectorXd& _data,
+ const Eigen::MatrixXd& _data_cov,
+ const Eigen::VectorXd& _calib,
+ const double _dt,
+ Eigen::VectorXd& _delta,
+ Eigen::MatrixXd& _delta_cov,
+ Eigen::MatrixXd& _jacobian_calib) const override;
+ void deltaPlusDelta(const Eigen::VectorXd& _delta1,
+ const Eigen::VectorXd& _delta2,
+ const double _Dt2,
+ Eigen::VectorXd& _delta1_plus_delta2) const override;
+ void deltaPlusDelta(const Eigen::VectorXd& _delta1,
+ const Eigen::VectorXd& _delta2,
+ const double _Dt2,
+ Eigen::VectorXd& _delta1_plus_delta2,
+ Eigen::MatrixXd& _jacobian1,
+ Eigen::MatrixXd& _jacobian2) const override;
+ void statePlusDelta(const VectorComposite& _x,
+ const Eigen::VectorXd& _delta,
+ const double _Dt,
+ VectorComposite& _x_plus_delta) const override;
+ Eigen::VectorXd deltaZero() const override;
+ Eigen::VectorXd correctDelta(const Eigen::VectorXd& delta_preint,
+ const Eigen::VectorXd& delta_step) const override;
- VectorXd getCalibration(const CaptureBaseConstPtr _capture) const override;
- void setCalibration(const CaptureBasePtr _capture, const VectorXd& _calibration) override;
+ bool voteForKeyFrame() const override;
- protected:
- ParamsProcessorOdom3dPtr params_odom_3d_;
+ CaptureMotionPtr emplaceCapture(const FrameBasePtr& _frame_own,
+ const SensorBasePtr& _sensor,
+ const TimeStamp& _ts,
+ const VectorXd& _data,
+ const MatrixXd& _data_cov,
+ const VectorXd& _calib,
+ const VectorXd& _calib_preint,
+ const CaptureBasePtr& _capture_origin) override;
+ void emplaceFeaturesAndFactors(CaptureBasePtr _capture_origin, CaptureMotionPtr _capture_own) override;
+ VectorXd getCalibration(const CaptureBaseConstPtr _capture) const override;
+ void setCalibration(const CaptureBasePtr _capture, const VectorXd& _calibration) override;
};
inline Eigen::VectorXd ProcessorOdom3d::deltaZero() const
{
- return (Eigen::VectorXd(7) << 0,0,0, 0,0,0,1).finished(); // p, q
+ return (Eigen::VectorXd(7) << 0, 0, 0, 0, 0, 0, 1).finished(); // p, q
}
-} // namespace wolf
+} // namespace wolf
diff --git a/include/core/processor/processor_pose.h b/include/core/processor/processor_pose.h
index 29f1dc43a14be4f5121008a646fd2049ca0550a8..03aae6627321ebf2e233bcd5f9f449ea989654ac 100644
--- a/include/core/processor/processor_pose.h
+++ b/include/core/processor/processor_pose.h
@@ -25,27 +25,14 @@
namespace wolf
{
-WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorPose);
-
-struct ParamsProcessorPose : public ParamsProcessorBase
-{
- ParamsProcessorPose() = default;
- ParamsProcessorPose(const YAML::Node& _n) : ParamsProcessorBase(_n) {}
- ~ParamsProcessorPose() override = default;
- std::string print() const override
- {
- return ParamsProcessorBase::print();
- }
-};
-
// class
template <unsigned int DIM>
class ProcessorPose : public ProcessorBase
{
public:
- ProcessorPose(ParamsProcessorPosePtr _params_pose);
+ ProcessorPose(const YAML::Node& _params);
~ProcessorPose() override = default;
- WOLF_PROCESSOR_CREATE(ProcessorPose, ParamsProcessorPose);
+ WOLF_PROCESSOR_TEMPLATE_DIM_CREATE(ProcessorPose, DIM);
void configure(SensorBasePtr _sensor) override;
@@ -72,9 +59,6 @@ class ProcessorPose : public ProcessorBase
return false;
};
void createFactorIfNecessary();
-
- protected:
- ParamsProcessorPosePtr params_pose_;
};
typedef ProcessorPose<2> ProcessorPose2d;
@@ -95,10 +79,8 @@ WOLF_DECLARED_PTR_TYPEDEFS(ProcessorPose3d);
namespace wolf
{
-
template <unsigned int DIM>
-inline ProcessorPose<DIM>::ProcessorPose(ParamsProcessorPosePtr _params_pose)
- : ProcessorBase("ProcessorPose", DIM, _params_pose), params_pose_(_params_pose)
+inline ProcessorPose<DIM>::ProcessorPose(const YAML::Node& _params) : ProcessorBase("ProcessorPose", DIM, _params)
{
static_assert(DIM == 2 or DIM == 3);
}
@@ -140,7 +122,7 @@ void ProcessorPose<DIM>::createFactorIfNecessary()
kf_it->second,
cap->getSensor(),
shared_from_this(),
- params_pose_->apply_loss_function);
+ applyLossFunction());
}
else
{
@@ -150,7 +132,7 @@ void ProcessorPose<DIM>::createFactorIfNecessary()
kf_it->second,
cap->getSensor(),
shared_from_this(),
- params_pose_->apply_loss_function);
+ applyLossFunction());
}
// erase removes range [first, last): it does not removes last
// so increment the iterator so that it points to the next element in the container
diff --git a/include/core/processor/processor_tracker.h b/include/core/processor/processor_tracker.h
index 5eca366a5004181e434478929dbf4c0dc6cea27d..9653707c768f4cddb2837e5c8037d15efd46d3ab 100644
--- a/include/core/processor/processor_tracker.h
+++ b/include/core/processor/processor_tracker.h
@@ -23,31 +23,8 @@
#include "core/processor/processor_base.h"
#include "core/capture/capture_motion.h"
-namespace wolf {
-
-WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorTracker);
-
-struct ParamsProcessorTracker : public ParamsProcessorBase
+namespace wolf
{
- unsigned int min_features_for_keyframe; ///< minimum nbr. of features to vote for keyframe
- int max_new_features; ///< maximum nbr. of new features to be processed when adding a keyframe (-1: unlimited. 0: none.)
-
- ParamsProcessorTracker() = default;
- ParamsProcessorTracker(const YAML::Node& _n):
- ParamsProcessorBase(_n)
- {
- min_features_for_keyframe = _n["keyframe_vote"]["min_features_for_keyframe"].as<unsigned int>();
- max_new_features = _n["max_new_features"].as<int>();
- }
- std::string print() const override
- {
- return ParamsProcessorBase::print() + "\n"
- + "min_features_for_keyframe: " + toString(min_features_for_keyframe) + "\n"
- + "max_new_features: " + toString(max_new_features) + "\n";
- }
-
-};
-
WOLF_PTR_TYPEDEFS(ProcessorTracker);
/** \brief General tracker processor
@@ -97,195 +74,206 @@ WOLF_PTR_TYPEDEFS(ProcessorTracker);
*/
class ProcessorTracker : public ProcessorBase
{
- public:
- typedef enum {
- FIRST_TIME_WITH_KEYFRAME,
- FIRST_TIME_WITHOUT_KEYFRAME,
- SECOND_TIME_WITH_KEYFRAME,
- SECOND_TIME_WITHOUT_KEYFRAME,
- RUNNING_WITH_KEYFRAME,
- RUNNING_WITHOUT_KEYFRAME
- } ProcessingStep ;
-
- protected:
- ParamsProcessorTrackerPtr params_tracker_; ///< parameters for the tracker
- ProcessingStep processing_step_; ///< State machine controlling the processing step
- CaptureBasePtr origin_ptr_; ///< Pointer to the origin of the tracker.
- CaptureBasePtr last_ptr_; ///< Pointer to the last tracked capture.
- CaptureBasePtr incoming_ptr_; ///< Pointer to the incoming capture being processed.
- FrameBasePtr last_frame_ptr_;
- FeatureBasePtrList new_features_last_; ///< List of new features in \b last for landmark initialization and new key-frame creation.
- FeatureBasePtrList new_features_incoming_; ///< list of the new features of \b last successfully tracked in \b incoming
- FeatureBasePtrList known_features_last_; ///< list of the known features in previous captures successfully tracked in \b last
- FeatureBasePtrList known_features_incoming_; ///< list of the known features in \b last successfully tracked in \b incoming
- StateKeys state_keys_; ///< Keys of frames created or used by this processor
-
- public:
- ProcessorTracker(const std::string& _type,
- const StateKeys& _structure,
- int _dim,
- ParamsProcessorTrackerPtr _params_tracker);
- ~ProcessorTracker() override;
-
- const StateKeys& getStateKeys() const;
-
- virtual CaptureBaseConstPtr getOrigin() const;
- virtual CaptureBasePtr getOrigin();
- virtual CaptureBaseConstPtr getLast() const;
- virtual CaptureBasePtr getLast();
- virtual CaptureBaseConstPtr getIncoming() const;
- virtual CaptureBasePtr getIncoming();
-
- protected:
- /** \brief process an incoming capture
- *
- * Each derived processor should implement this function. It will be called if:
- * - A new capture arrived and triggerInCapture() returned true.
- */
- void processCapture(CaptureBasePtr) override;
-
- /** \brief process an incoming key-frame
- *
- * The ProcessorTracker only processes incoming captures (it is not called).
- */
- void processKeyFrame(FrameBasePtr _keyframe_ptr) override {}
-
- /** \brief trigger in capture
- *
- * Returns true if processCapture() should be called after the provided capture arrived.
- */
- bool triggerInCapture(CaptureBasePtr) const override { return true;}
-
- /** \brief trigger in key-frame
- *
- * The ProcessorTracker only processes incoming captures, then it returns false.
- */
- bool triggerInKeyFrame(FrameBasePtr _keyframe_ptr) const override {return false;}
-
- /** \brief store key frame
- *
- * Returns true if the key frame should be stored
- */
- bool storeKeyFrame(FrameBasePtr) override { return true;}
-
- /** \brief store capture
- *
- * Returns true if the capture should be stored
- */
- bool storeCapture(CaptureBasePtr) override { return false;}
-
- /** Pre-process incoming Capture
- *
- * This is called by process() just after assigning incoming_ptr_ to a valid Capture.
- *
- * Overload this function to prepare stuff on derived classes.
- *
- * Typical uses of prePrecess() are:
- * - casting base types to derived types
- * - initializing counters, flags, or any derived variables
- * - initializing algorithms needed for processing the derived data
- */
- virtual void preProcess() { };
-
- /** Post-process
- *
- * This is called by process() after finishing the processing algorithm.
- *
- * Overload this function to post-process stuff on derived classes.
- *
- * Typical uses of postPrecess() are:
- * - resetting and/or clearing variables and/or algorithms at the end of processing
- * - drawing / printing / logging the results of the processing
- */
- virtual void postProcess() { };
-
- /** \brief Tracker function
- * \return The number of successful tracks.
- *
- * This is the tracker function to be implemented in derived classes.
- * It operates on the \b incoming capture pointed by incoming_ptr_.
- *
- * This should do one of the following, depending on the design of the tracker -- see use_landmarks_:
- * - Track Features against other Features in the \b origin Capture. Tips:
- * - An intermediary step of matching against Features in the \b last Capture makes tracking easier.
- * - Once tracked against last, then the link to Features in \b origin is provided by the Features' Factors in \b last.
- * - If required, correct the drift by re-comparing against the Features in origin.
- * - The Factors in \b last need to be transferred to \b incoming (moved, not copied).
- * - Track Features against Landmarks in the Map. Tips:
- * - The links to the Landmarks are in the Features' Factors in last.
- * - The Factors in \b last need to be transferred to \b incoming (moved, not copied).
- *
- * The function must generate the necessary Features in the \b incoming Capture,
- * of the correct type, derived from FeatureBase.
- *
- * It must also generate the factors, of the correct type, derived from FactorBase
- * (through FactorAnalytic or FactorSparse).
- */
- virtual unsigned int processKnown() = 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 = 0;
-
- /** \brief Advance the incoming Capture to become the last.
- *
- * Call this when the tracking and keyframe policy work is done and
- * we need to get ready to accept a new incoming Capture.
- */
- virtual void advanceDerived() = 0;
-
- /**\brief Process new Features or Landmarks
- *
- */
- virtual unsigned int processNew(const int& _max_features) = 0;
-
- /**\brief Creates and adds factors from last_ to origin_
- *
- */
- virtual void establishFactors() = 0;
-
- /** \brief Reset the tracker using the \b last Capture as the new \b origin.
- */
- virtual void resetDerived() = 0;
-
- public:
-
- FeatureBaseConstPtrList getNewFeaturesListLast() const;
- FeatureBasePtrList getNewFeaturesListLast();
-
- std::string print() const {
- return this->params_tracker_->print();
- }
-
- void printHeader(int depth, //
- bool factored_by, //
- bool metric, //
- bool state_blocks,
- std::ostream& stream ,
- std::string _tabs = "") const override;
-
- protected:
-
- void computeProcessingStep();
-
- void addNewFeatureLast(FeatureBasePtr _feature_ptr);
-
- FeatureBasePtrList& getNewFeaturesListIncoming();
-
- void addNewFeatureIncoming(FeatureBasePtr _feature_ptr);
+ public:
+ typedef enum
+ {
+ FIRST_TIME_WITH_KEYFRAME,
+ FIRST_TIME_WITHOUT_KEYFRAME,
+ SECOND_TIME_WITH_KEYFRAME,
+ SECOND_TIME_WITHOUT_KEYFRAME,
+ RUNNING_WITH_KEYFRAME,
+ RUNNING_WITHOUT_KEYFRAME
+ } ProcessingStep;
+
+ protected:
+ ProcessingStep processing_step_; ///< State machine controlling the processing step
+ CaptureBasePtr origin_ptr_; ///< Pointer to the origin of the tracker.
+ CaptureBasePtr last_ptr_; ///< Pointer to the last tracked capture.
+ CaptureBasePtr incoming_ptr_; ///< Pointer to the incoming capture being processed.
+ FrameBasePtr last_frame_ptr_;
+
+ FeatureBasePtrList new_features_last_; ///< List of new features in \b last for landmark initialization and new
+ ///< key-frame creation.
+ FeatureBasePtrList
+ new_features_incoming_; ///< list of the new features of \b last successfully tracked in \b incoming
+ FeatureBasePtrList
+ known_features_last_; ///< list of the known features in previous captures successfully tracked in \b last
+ FeatureBasePtrList
+ known_features_incoming_; ///< list of the known features in \b last successfully tracked in \b incoming
+ StateKeys state_keys_; ///< Keys of frames created or used by this processor
+
+ unsigned int min_features_for_keyframe_; ///< minimum nbr. of features to vote for keyframe
+ int max_new_features_; ///< maximum nbr. of new features to be processed when adding a keyframe (-1: unlimited. 0:
+ ///< none.)
+
+ public:
+ ProcessorTracker(const std::string& _type, const StateKeys& _structure, int _dim, const YAML::Node& _params);
+ ~ProcessorTracker() override;
+
+ const StateKeys& getStateKeys() const;
+
+ virtual CaptureBaseConstPtr getOrigin() const;
+ virtual CaptureBasePtr getOrigin();
+ virtual CaptureBaseConstPtr getLast() const;
+ virtual CaptureBasePtr getLast();
+ virtual CaptureBaseConstPtr getIncoming() const;
+ virtual CaptureBasePtr getIncoming();
+
+ protected:
+ /** \brief process an incoming capture
+ *
+ * Each derived processor should implement this function. It will be called if:
+ * - A new capture arrived and triggerInCapture() returned true.
+ */
+ void processCapture(CaptureBasePtr) override;
+
+ /** \brief process an incoming key-frame
+ *
+ * The ProcessorTracker only processes incoming captures (it is not called).
+ */
+ void processKeyFrame(FrameBasePtr _keyframe_ptr) override {}
+
+ /** \brief trigger in capture
+ *
+ * Returns true if processCapture() should be called after the provided capture arrived.
+ */
+ bool triggerInCapture(CaptureBasePtr) const override
+ {
+ return true;
+ }
+
+ /** \brief trigger in key-frame
+ *
+ * The ProcessorTracker only processes incoming captures, then it returns false.
+ */
+ bool triggerInKeyFrame(FrameBasePtr _keyframe_ptr) const override
+ {
+ return false;
+ }
+
+ /** \brief store key frame
+ *
+ * Returns true if the key frame should be stored
+ */
+ bool storeKeyFrame(FrameBasePtr) override
+ {
+ return true;
+ }
+
+ /** \brief store capture
+ *
+ * Returns true if the capture should be stored
+ */
+ bool storeCapture(CaptureBasePtr) override
+ {
+ return false;
+ }
+ /** Pre-process incoming Capture
+ *
+ * This is called by process() just after assigning incoming_ptr_ to a valid Capture.
+ *
+ * Overload this function to prepare stuff on derived classes.
+ *
+ * Typical uses of prePrecess() are:
+ * - casting base types to derived types
+ * - initializing counters, flags, or any derived variables
+ * - initializing algorithms needed for processing the derived data
+ */
+ virtual void preProcess(){};
+
+ /** Post-process
+ *
+ * This is called by process() after finishing the processing algorithm.
+ *
+ * Overload this function to post-process stuff on derived classes.
+ *
+ * Typical uses of postPrecess() are:
+ * - resetting and/or clearing variables and/or algorithms at the end of processing
+ * - drawing / printing / logging the results of the processing
+ */
+ virtual void postProcess(){};
+
+ /** \brief Tracker function
+ * \return The number of successful tracks.
+ *
+ * This is the tracker function to be implemented in derived classes.
+ * It operates on the \b incoming capture pointed by incoming_ptr_.
+ *
+ * This should do one of the following, depending on the design of the tracker -- see use_landmarks_:
+ * - Track Features against other Features in the \b origin Capture. Tips:
+ * - An intermediary step of matching against Features in the \b last Capture makes tracking easier.
+ * - Once tracked against last, then the link to Features in \b origin is provided by the Features' Factors in
+ * \b last.
+ * - If required, correct the drift by re-comparing against the Features in origin.
+ * - The Factors in \b last need to be transferred to \b incoming (moved, not copied).
+ * - Track Features against Landmarks in the Map. Tips:
+ * - The links to the Landmarks are in the Features' Factors in last.
+ * - The Factors in \b last need to be transferred to \b incoming (moved, not copied).
+ *
+ * The function must generate the necessary Features in the \b incoming Capture,
+ * of the correct type, derived from FeatureBase.
+ *
+ * It must also generate the factors, of the correct type, derived from FactorBase
+ * (through FactorAnalytic or FactorSparse).
+ */
+ virtual unsigned int processKnown() = 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 = 0;
+
+ /** \brief Advance the incoming Capture to become the last.
+ *
+ * Call this when the tracking and keyframe policy work is done and
+ * we need to get ready to accept a new incoming Capture.
+ */
+ virtual void advanceDerived() = 0;
+
+ /**\brief Process new Features or Landmarks
+ *
+ */
+ virtual unsigned int processNew(const int& _max_features) = 0;
+
+ /**\brief Creates and adds factors from last_ to origin_
+ *
+ */
+ virtual void establishFactors() = 0;
+
+ /** \brief Reset the tracker using the \b last Capture as the new \b origin.
+ */
+ virtual void resetDerived() = 0;
+
+ public:
+ FeatureBaseConstPtrList getNewFeaturesListLast() const;
+ FeatureBasePtrList getNewFeaturesListLast();
+
+ void printHeader(int depth, //
+ bool factored_by, //
+ bool metric, //
+ bool state_blocks,
+ std::ostream& stream,
+ std::string _tabs = "") const override;
+
+ protected:
+ void computeProcessingStep();
+
+ void addNewFeatureLast(FeatureBasePtr _feature_ptr);
+
+ FeatureBasePtrList& getNewFeaturesListIncoming();
+
+ void addNewFeatureIncoming(FeatureBasePtr _feature_ptr);
};
inline FeatureBaseConstPtrList ProcessorTracker::getNewFeaturesListLast() const
{
FeatureBaseConstPtrList list_const;
- for (auto&& obj_ptr : new_features_last_)
- list_const.push_back(obj_ptr);
+ for (auto&& obj_ptr : new_features_last_) list_const.push_back(obj_ptr);
return list_const;
}
@@ -304,7 +292,7 @@ inline FeatureBasePtrList& ProcessorTracker::getNewFeaturesListIncoming()
return new_features_incoming_;
}
-inline const StateKeys& ProcessorTracker::getStateKeys ( ) const
+inline const StateKeys& ProcessorTracker::getStateKeys() const
{
return state_keys_;
}
@@ -344,4 +332,4 @@ inline CaptureBasePtr ProcessorTracker::getIncoming()
return incoming_ptr_;
}
-} // namespace wolf
+} // namespace wolf
diff --git a/include/core/processor/processor_tracker_feature.h b/include/core/processor/processor_tracker_feature.h
index 51ec817d570d12c83e0962c4ad51ee029f24b5b8..522d60f020016a873473a97efc399e378bd6b1dd 100644
--- a/include/core/processor/processor_tracker_feature.h
+++ b/include/core/processor/processor_tracker_feature.h
@@ -20,7 +20,7 @@
#pragma once
-//wolf includes
+// wolf includes
#include "core/processor/processor_tracker.h"
#include "core/capture/capture_base.h"
#include "core/feature/feature_match.h"
@@ -29,14 +29,6 @@
namespace wolf
{
-
-WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorTrackerFeature);
-
-struct ParamsProcessorTrackerFeature : public ParamsProcessorTracker
-{
- using ParamsProcessorTracker::ParamsProcessorTracker;
-};
-
WOLF_PTR_TYPEDEFS(ProcessorTrackerFeature);
/** \brief Feature tracker processor
@@ -59,7 +51,8 @@ WOLF_PTR_TYPEDEFS(ProcessorTrackerFeature);
*
* This tracker builds on top of the ProcessorTracker by implementing some of its pure virtual functions.
* As a reminder, we sketch here the pipeline of the parent ProcessorTracker process() function.
- * We highlight the functions implemented here with a sign '<--- IMPLEMENTED', and the ones to be implemented by derived classes with '<=== IMPLEMENT'
+ * We highlight the functions implemented here with a sign '<--- IMPLEMENTED', and the ones to be implemented by
+ * derived classes with '<=== IMPLEMENT'
*
* - On each incoming Capture,
* - Track known features in the \b incoming Capture: processKnown() <--- IMPLEMENTED
@@ -92,118 +85,118 @@ WOLF_PTR_TYPEDEFS(ProcessorTrackerFeature);
*/
class ProcessorTrackerFeature : public ProcessorTracker
{
- public:
-
- /** \brief Constructor with type
- */
- ProcessorTrackerFeature(const std::string& _type,
- const StateKeys& _structure,
- int _dim,
- ParamsProcessorTrackerFeaturePtr _params_tracker_feature);
- ~ProcessorTrackerFeature() override;
-
- protected:
- ParamsProcessorTrackerFeaturePtr params_tracker_feature_;
- TrackMatrix track_matrix_;
-
- FeatureMatchMap matches_last_from_incoming_;
-
- /** \brief Process known Features
- * \return The number of successful matches.
- *
- * This function operates on the \b incoming capture pointed by incoming_ptr_.
- *
- * This function does:
- * - Track Features against other Features in the \b origin Capture. Tips:
- * - An intermediary step of matching against Features in the \b last Capture makes tracking easier.
- * - Once tracked against last, then the link to Features in \b origin is provided by the Features' Factors in \b last.
- * - If required, correct the drift by re-comparing against the Features in origin.
- * - The Factors in \b last need to be transferred to \b incoming (moved, not copied).
- * - Create the necessary Features in the \b incoming Capture,
- * of the correct type, derived from FeatureBase.
- * - Create the factors, of the correct type, derived from FactorBase
- * (through FactorAnalytic or FactorSparse).
- */
- unsigned int processKnown() override;
-
- /** \brief Track provided features in \b _capture
- * \param _features_in input list of features in \b last to track
- * \param _capture the capture in which the _features_in should be searched
- * \param _features_out returned list of features found in \b _capture
- * \param _feature_correspondences returned map of correspondences: _feature_correspondences[feature_out_ptr] = feature_in_ptr
- *
- * IMPORTANT: The features in _features_out should be emplaced. Don't use `make_shared`, use `FeatureBase::emplace` instead.
- * Then, they will be already linked to the _capture.
- * If you detect all the features at once in preprocess(), you should create them (`make_shared()`) and only link the
- * features that are returned in _features_out (`FeatureBase::link(_capture)`).
- *
- * \return the number of features tracked
- */
- virtual unsigned int trackFeatures(const FeatureBasePtrList& _features_in,
+ public:
+ /** \brief Constructor with type
+ */
+ ProcessorTrackerFeature(const std::string& _type,
+ const StateKeys& _structure,
+ int _dim,
+ const YAML::Node& _params);
+ ~ProcessorTrackerFeature() override;
+
+ protected:
+ TrackMatrix track_matrix_;
+
+ FeatureMatchMap matches_last_from_incoming_;
+
+ /** \brief Process known Features
+ * \return The number of successful matches.
+ *
+ * This function operates on the \b incoming capture pointed by incoming_ptr_.
+ *
+ * This function does:
+ * - Track Features against other Features in the \b origin Capture. Tips:
+ * - An intermediary step of matching against Features in the \b last Capture makes tracking easier.
+ * - Once tracked against last, then the link to Features in \b origin is provided by the Features' Factors in
+ * \b last.
+ * - If required, correct the drift by re-comparing against the Features in origin.
+ * - The Factors in \b last need to be transferred to \b incoming (moved, not copied).
+ * - Create the necessary Features in the \b incoming Capture,
+ * of the correct type, derived from FeatureBase.
+ * - Create the factors, of the correct type, derived from FactorBase
+ * (through FactorAnalytic or FactorSparse).
+ */
+ unsigned int processKnown() override;
+
+ /** \brief Track provided features in \b _capture
+ * \param _features_in input list of features in \b last to track
+ * \param _capture the capture in which the _features_in should be searched
+ * \param _features_out returned list of features found in \b _capture
+ * \param _feature_correspondences returned map of correspondences: _feature_correspondences[feature_out_ptr] = feature_in_ptr
+ *
+ * IMPORTANT: The features in _features_out should be emplaced. Don't use `make_shared`, use `FeatureBase::emplace`
+ * instead. Then, they will be already linked to the _capture. If you detect all the features at once in
+ * preprocess(), you should create them (`make_shared()`) and only link the features that are returned in
+ * _features_out (`FeatureBase::link(_capture)`).
+ *
+ * \return the number of features tracked
+ */
+ virtual unsigned int trackFeatures(const FeatureBasePtrList& _features_in,
+ const CaptureBasePtr& _capture,
+ FeatureBasePtrList& _features_out,
+ FeatureMatchMap& _feature_correspondences) = 0;
+
+ /** \brief Correct the drift in incoming feature by re-comparing against the corresponding feature in origin.
+ * \param _origin_feature input feature in origin capture tracked
+ * \param _incoming_feature input/output feature in incoming capture to be corrected
+ * \return false if the the process discards the correspondence with origin's feature
+ */
+ virtual bool correctFeatureDrift(const FeatureBasePtr _origin_feature,
+ const FeatureBasePtr _last_feature,
+ FeatureBasePtr _incoming_feature) = 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 = 0;
+
+ // We overload the advance and reset functions to update the lists of matches
+ void advanceDerived() override;
+ void resetDerived() override;
+
+ /**\brief Process new Features
+ *
+ */
+ unsigned int processNew(const int& _max_features) override;
+
+ /** \brief Detect new Features
+ * \param _max_features maximum number of features detected (-1: unlimited. 0: none)
+ * \param _capture The capture in which the new features should be detected.
+ * \param _features_out The list of detected Features in _capture.
+ * \return The number of detected Features.
+ *
+ * This function detects Features that do not correspond to known Features/Landmarks in the system.
+ *
+ * IMPORTANT: The features in _features_out should be emplaced. Don't use `make_shared`, use `FeatureBase::emplace`
+ * instead. Then, they will be already linked to the _capture. If you detect all the features at once in
+ * preprocess(), you should create them (`make_shared()`) and only link the features that are returned in
+ * _features_out (`FeatureBase::link(_capture)`).
+ *
+ * The function is called in ProcessorTrackerFeature::processNew() to set the member new_features_last_,
+ * the list of newly detected features of the capture last_ptr_.
+ */
+ virtual unsigned int detectNewFeatures(const int& _max_new_features,
const CaptureBasePtr& _capture,
- FeatureBasePtrList& _features_out,
- FeatureMatchMap& _feature_correspondences) = 0;
-
- /** \brief Correct the drift in incoming feature by re-comparing against the corresponding feature in origin.
- * \param _origin_feature input feature in origin capture tracked
- * \param _incoming_feature input/output feature in incoming capture to be corrected
- * \return false if the the process discards the correspondence with origin's feature
- */
- virtual bool correctFeatureDrift(const FeatureBasePtr _origin_feature,
- const FeatureBasePtr _last_feature,
- FeatureBasePtr _incoming_feature) = 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 = 0;
-
- // We overload the advance and reset functions to update the lists of matches
- void advanceDerived() override;
- void resetDerived() override;
-
- /**\brief Process new Features
- *
- */
- unsigned int processNew(const int& _max_features) override;
-
- /** \brief Detect new Features
- * \param _max_features maximum number of features detected (-1: unlimited. 0: none)
- * \param _capture The capture in which the new features should be detected.
- * \param _features_out The list of detected Features in _capture.
- * \return The number of detected Features.
- *
- * This function detects Features that do not correspond to known Features/Landmarks in the system.
- *
- * IMPORTANT: The features in _features_out should be emplaced. Don't use `make_shared`, use `FeatureBase::emplace` instead.
- * Then, they will be already linked to the _capture.
- * If you detect all the features at once in preprocess(), you should create them (`make_shared()`) and only link the
- * features that are returned in _features_out (`FeatureBase::link(_capture)`).
- *
- * The function is called in ProcessorTrackerFeature::processNew() to set the member new_features_last_,
- * the list of newly detected features of the capture last_ptr_.
- */
- virtual unsigned int detectNewFeatures(const int& _max_new_features,
- const CaptureBasePtr& _capture,
- FeatureBasePtrList& _features_out) = 0;
-
- /** \brief Emplaces a new factor
- * \param _feature_ptr pointer to the parent Feature
- * \param _feature_other_ptr pointer to the other feature constrained.
- *
- * Implement this method in derived classes.
- *
- * This function emplaces a factor of the appropriate type for the derived processor.
- */
- virtual FactorBasePtr emplaceFactor(FeatureBasePtr _feature_ptr, FeatureBasePtr _feature_other_ptr) = 0;
-
- /** \brief Emplaces a new factor for each correspondence between a feature in Capture \b last and a feature in Capture \b origin
- */
- void establishFactors() override;
+ FeatureBasePtrList& _features_out) = 0;
+
+ /** \brief Emplaces a new factor
+ * \param _feature_ptr pointer to the parent Feature
+ * \param _feature_other_ptr pointer to the other feature constrained.
+ *
+ * Implement this method in derived classes.
+ *
+ * This function emplaces a factor of the appropriate type for the derived processor.
+ */
+ virtual FactorBasePtr emplaceFactor(FeatureBasePtr _feature_ptr, FeatureBasePtr _feature_other_ptr) = 0;
+
+ /** \brief Emplaces a new factor for each correspondence between a feature in Capture \b last and a feature in
+ * Capture \b origin
+ */
+ void establishFactors() override;
};
-} // namespace wolf
+} // namespace wolf
diff --git a/include/core/processor/processor_tracker_landmark.h b/include/core/processor/processor_tracker_landmark.h
index ca97f7fb4df61f59c3f6e20697f8f0e95b3a56f9..d024f4132daf257806ac8a42e64f5a572a6093eb 100644
--- a/include/core/processor/processor_tracker_landmark.h
+++ b/include/core/processor/processor_tracker_landmark.h
@@ -20,24 +20,15 @@
#pragma once
-//wolf includes
+// wolf includes
#include "core/processor/processor_tracker.h"
#include "core/capture/capture_base.h"
#include "core/landmark/landmark_match.h"
namespace wolf
{
-
-WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorTrackerLandmark);
-
-struct ParamsProcessorTrackerLandmark : public ParamsProcessorTracker
-{
- using ParamsProcessorTracker::ParamsProcessorTracker;
- //
-};
-
WOLF_PTR_TYPEDEFS(ProcessorTrackerLandmark);
-
+
/** \brief Landmark tracker processor
*
* This is an abstract class.
@@ -58,7 +49,8 @@ WOLF_PTR_TYPEDEFS(ProcessorTrackerLandmark);
*
* This tracker builds on top of the ProcessorTracker by implementing some of its pure virtual functions.
* As a reminder, we sketch here the pipeline of the parent ProcessorTracker process() function.
- * We highlight the functions implemented here with a sign '<--- IMPLEMENTED', and the ones to be implemented by derived classes with '<=== IMPLEMENT'
+ * We highlight the functions implemented here with a sign '<--- IMPLEMENTED', and the ones to be implemented by
+ * derived classes with '<=== IMPLEMENT'
*
* - On each incoming Capture,
* - processKnown() : Track known features in the \b incoming Capture <--- IMPLEMENTED
@@ -91,109 +83,107 @@ WOLF_PTR_TYPEDEFS(ProcessorTrackerLandmark);
*/
class ProcessorTrackerLandmark : public ProcessorTracker
{
- public:
- ProcessorTrackerLandmark(const std::string& _type,
- const StateKeys& _structure,
- ParamsProcessorTrackerLandmarkPtr _params_tracker_landmark);
- ~ProcessorTrackerLandmark() override;
-
- protected:
-
- ParamsProcessorTrackerLandmarkPtr params_tracker_landmark_;
- LandmarkBasePtrList new_landmarks_; ///< List of new detected landmarks
- LandmarkMatchMap matches_landmark_from_incoming_;
- LandmarkMatchMap matches_landmark_from_last_;
-
- /** \brief Tracker function
- * \return The number of successful tracks.
- *
- * Find Features in \b incoming Capture corresponding to known landmarks in the \b Map.
- *
- * This is the tracker function to be implemented in derived classes.
- * It operates on the \b incoming capture pointed by incoming_ptr_.
- *
- * The function must populate the list of Features in the \b incoming Capture.
- * Features need to be of the correct type, derived from FeatureBase.
- */
- unsigned int processKnown() override;
-
- /** \brief Find provided landmarks as features in the provided capture
- * \param _landmarks_in input list of landmarks to be found
- * \param _capture the capture in which the _landmarks_in should be searched
- * \param _features_out returned list of features found in \b _capture corresponding to a landmark of _landmarks_in
- * \param _feature_landmark_correspondences returned map of landmark correspondences: _feature_landmark_correspondences[_feature_out_ptr] = landmark_in_ptr
- *
- * IMPORTANT: The features in _features_out should be emplaced. Don't use `make_shared`, use `FeatureBase::emplace` instead.
- * Then, they will be already linked to the _capture.
- * If you detect all the features at once in preprocess(), you should create them (`make_shared()`) and only link the
- * features that are returned in _features_out (`FeatureBase::link(_capture)`).
- *
- * \return the number of landmarks found
- */
- virtual unsigned int findLandmarks(const LandmarkBasePtrList& _landmarks_in,
+ public:
+ ProcessorTrackerLandmark(const std::string& _type,
+ const StateKeys& _structure,
+ const YAML::Node& _params_tracker_landmark);
+ ~ProcessorTrackerLandmark() override;
+
+ protected:
+ LandmarkBasePtrList new_landmarks_; ///< List of new detected landmarks
+ LandmarkMatchMap matches_landmark_from_incoming_;
+ LandmarkMatchMap matches_landmark_from_last_;
+
+ /** \brief Tracker function
+ * \return The number of successful tracks.
+ *
+ * Find Features in \b incoming Capture corresponding to known landmarks in the \b Map.
+ *
+ * This is the tracker function to be implemented in derived classes.
+ * It operates on the \b incoming capture pointed by incoming_ptr_.
+ *
+ * The function must populate the list of Features in the \b incoming Capture.
+ * Features need to be of the correct type, derived from FeatureBase.
+ */
+ unsigned int processKnown() override;
+
+ /** \brief Find provided landmarks as features in the provided capture
+ * \param _landmarks_in input list of landmarks to be found
+ * \param _capture the capture in which the _landmarks_in should be searched
+ * \param _features_out returned list of features found in \b _capture corresponding to a landmark of _landmarks_in
+ * \param _feature_landmark_correspondences returned map of landmark correspondences: _feature_landmark_correspondences[_feature_out_ptr] = landmark_in_ptr
+ *
+ * IMPORTANT: The features in _features_out should be emplaced. Don't use `make_shared`, use `FeatureBase::emplace`
+ * instead. Then, they will be already linked to the _capture. If you detect all the features at once in
+ * preprocess(), you should create them (`make_shared()`) and only link the features that are returned in
+ * _features_out (`FeatureBase::link(_capture)`).
+ *
+ * \return the number of landmarks found
+ */
+ virtual unsigned int findLandmarks(const LandmarkBasePtrList& _landmarks_in,
+ const CaptureBasePtr& _capture,
+ FeatureBasePtrList& _features_out,
+ LandmarkMatchMap& _feature_landmark_correspondences) = 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 = 0;
+
+ // We overload the advance and reset functions to update the lists of matches
+ void advanceDerived() override;
+ void resetDerived() override;
+
+ /** \brief Process new Features
+ *
+ */
+ unsigned int processNew(const int& _max_features) override;
+
+ /** \brief Detect new Features
+ * \param _max_features maximum number of features detected (-1: unlimited. 0: none)
+ * \param _capture The capture in which the new features should be detected.
+ * \param _features_out The list of detected Features in _capture.
+ * \return The number of detected Features.
+ *
+ * This function detects Features that do not correspond to known Features/Landmarks in the system.
+ *
+ * IMPORTANT: The features in _features_out should be emplaced. Don't use `make_shared`, use `FeatureBase::emplace`
+ * instead. Then, they will be already linked to the _capture. If you detect all the features at once in
+ * preprocess(), you should create them (`make_shared()`) and only link the features that are returned in
+ * _features_out (`FeatureBase::link(_capture)`).
+ *
+ * The function is called in ProcessorTrackerLandmark::processNew() to set the member new_features_last_,
+ * the list of newly detected features of the capture last_ptr_.
+ */
+ virtual unsigned int detectNewFeatures(const int& _max_new_features,
const CaptureBasePtr& _capture,
- FeatureBasePtrList& _features_out,
- LandmarkMatchMap& _feature_landmark_correspondences) = 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 = 0;
-
- // We overload the advance and reset functions to update the lists of matches
- void advanceDerived() override;
- void resetDerived() override;
-
- /** \brief Process new Features
- *
- */
- unsigned int processNew(const int& _max_features) override;
-
- /** \brief Detect new Features
- * \param _max_features maximum number of features detected (-1: unlimited. 0: none)
- * \param _capture The capture in which the new features should be detected.
- * \param _features_out The list of detected Features in _capture.
- * \return The number of detected Features.
- *
- * This function detects Features that do not correspond to known Features/Landmarks in the system.
- *
- * IMPORTANT: The features in _features_out should be emplaced. Don't use `make_shared`, use `FeatureBase::emplace` instead.
- * Then, they will be already linked to the _capture.
- * If you detect all the features at once in preprocess(), you should create them (`make_shared()`) and only link the
- * features that are returned in _features_out (`FeatureBase::link(_capture)`).
- *
- * The function is called in ProcessorTrackerLandmark::processNew() to set the member new_features_last_,
- * the list of newly detected features of the capture last_ptr_.
- */
- virtual unsigned int detectNewFeatures(const int& _max_new_features,
- const CaptureBasePtr& _capture,
- FeatureBasePtrList& _features_out) = 0;
-
- /** \brief Emplaces a landmark for each new feature of new_features_last_
- **/
- virtual void emplaceNewLandmarks();
-
- /** \brief Emplaces one landmark
- *
- * Implement in derived classes to build the type of landmark you need for this tracker.
- */
- virtual LandmarkBasePtr emplaceLandmark(FeatureBasePtr _feature_ptr) = 0;
-
- /** \brief Emplaces a new factor
- * \param _feature_ptr pointer to the Feature to constrain
- * \param _landmark_ptr LandmarkBase pointer to the Landmark constrained.
- *
- * Implement this method in derived classes.
- */
- virtual FactorBasePtr emplaceFactor(FeatureBasePtr _feature_ptr, LandmarkBasePtr _landmark_ptr) = 0;
-
- /** \brief Emplaces a new factor for each correspondence between a feature in Capture \b last and a landmark
- */
- void establishFactors() override;
+ FeatureBasePtrList& _features_out) = 0;
+
+ /** \brief Emplaces a landmark for each new feature of new_features_last_
+ **/
+ virtual void emplaceNewLandmarks();
+
+ /** \brief Emplaces one landmark
+ *
+ * Implement in derived classes to build the type of landmark you need for this tracker.
+ */
+ virtual LandmarkBasePtr emplaceLandmark(FeatureBasePtr _feature_ptr) = 0;
+
+ /** \brief Emplaces a new factor
+ * \param _feature_ptr pointer to the Feature to constrain
+ * \param _landmark_ptr LandmarkBase pointer to the Landmark constrained.
+ *
+ * Implement this method in derived classes.
+ */
+ virtual FactorBasePtr emplaceFactor(FeatureBasePtr _feature_ptr, LandmarkBasePtr _landmark_ptr) = 0;
+
+ /** \brief Emplaces a new factor for each correspondence between a feature in Capture \b last and a landmark
+ */
+ void establishFactors() override;
};
-}// namespace wolf
+} // namespace wolf
diff --git a/include/core/processor/track_matrix.h b/include/core/processor/track_matrix.h
index 6daa3880da9fc97782be6bdd86fc91616b9ce458..3e13668e041ae12579a347f5610b89a2d1156c71 100644
--- a/include/core/processor/track_matrix.h
+++ b/include/core/processor/track_matrix.h
@@ -30,18 +30,19 @@
namespace wolf
{
-using std::map;
-using std::vector;
using std::list;
+using std::map;
using std::pair;
using std::shared_ptr;
+using std::vector;
-typedef map<TimeStamp, FeatureBasePtr> Track;
-typedef map<TimeStamp, FeatureBaseConstPtr> TrackConst;
-typedef map<SizeStd, FeatureBasePtr > Snapshot;
-typedef map<SizeStd, FeatureBaseConstPtr > SnapshotConst;
-typedef map<SizeStd, pair<FeatureBasePtr, FeatureBasePtr> > TrackMatches; // matched feature pairs indexed by track_id
-typedef map<SizeStd, pair<FeatureBaseConstPtr, FeatureBaseConstPtr> > TrackMatchesConst; // matched feature pairs indexed by track_id
+typedef map<TimeStamp, FeatureBasePtr> Track;
+typedef map<TimeStamp, FeatureBaseConstPtr> TrackConst;
+typedef map<SizeStd, FeatureBasePtr> Snapshot;
+typedef map<SizeStd, FeatureBaseConstPtr> SnapshotConst;
+typedef map<SizeStd, pair<FeatureBasePtr, FeatureBasePtr> > TrackMatches; // matched feature pairs indexed by track_id
+typedef map<SizeStd, pair<FeatureBaseConstPtr, FeatureBaseConstPtr> >
+ TrackMatchesConst; // matched feature pairs indexed by track_id
/** \brief Matrix of tracked features, by track and by snapshot (Captures or time stamps)
* This class implements the following data structure:
@@ -71,7 +72,8 @@ typedef map<SizeStd, pair<FeatureBaseConstPtr, FeatureBaseConstPtr> > TrackMatc
*
* The class makes sure that both accesses are consistent each time some addition or removal of features is performed.
*
- * The storage is implemented as two maps of maps, so each addition and removal of single features is accomplished in logarithmic time:
+ * The storage is implemented as two maps of maps, so each addition and removal of single features is accomplished in
+ * logarithmic time:
*
* Tracks are identified with the track ID in f->trackId()
* Snapshots are identified with the Capture pointer in f->getCapture()
@@ -89,54 +91,54 @@ typedef map<SizeStd, pair<FeatureBaseConstPtr, FeatureBaseConstPtr> > TrackMatc
class TrackMatrix
{
- public:
- TrackMatrix();
- virtual ~TrackMatrix();
-
- void newTrack (FeatureBasePtr _ftr);
- void add (const SizeStd& _track_id, const FeatureBasePtr& _ftr);
- void add (const FeatureBasePtr& _ftr_existing, const FeatureBasePtr& _ftr_new);
- void remove (FeatureBasePtr _ftr);
- void remove (const SizeStd& _track_id);
- void remove (CaptureBasePtr _cap);
-
- SizeStd numTracks () const;
- SizeStd trackSize (const SizeStd& _track_id) const;
- TrackConst track (const SizeStd& _track_id) const;
- Track track (const SizeStd& _track_id);
- SnapshotConst snapshot (CaptureBaseConstPtr _capture) const;
- Snapshot snapshot (CaptureBasePtr _capture);
- vector<FeatureBaseConstPtr> trackAsVector(const SizeStd& _track_id) const;
- vector<FeatureBasePtr> trackAsVector(const SizeStd& _track_id);
- FeatureBaseConstPtrList snapshotAsList(CaptureBaseConstPtr _cap) const;
- FeatureBasePtrList snapshotAsList(CaptureBasePtr _cap);
- TrackMatchesConst matches (CaptureBaseConstPtr _cap_1, CaptureBaseConstPtr _cap_2) const;
- TrackMatches matches (CaptureBasePtr _cap_1, CaptureBasePtr _cap_2);
-
- FeatureBaseConstPtr firstFeature(const SizeStd& _track_id) const;
- FeatureBasePtr firstFeature(const SizeStd& _track_id);
- FeatureBaseConstPtr lastFeature (const SizeStd& _track_id) const;
- FeatureBasePtr lastFeature (const SizeStd& _track_id);
- FeatureBaseConstPtr feature (const SizeStd& _track_id, CaptureBaseConstPtr _cap) const;
- FeatureBasePtr feature (const SizeStd& _track_id, CaptureBasePtr _cap);
- CaptureBaseConstPtr firstCapture(const SizeStd& _track_id) const;
- CaptureBasePtr firstCapture(const SizeStd& _track_id);
-
- list<SizeStd> trackIds(CaptureBaseConstPtr _capture = nullptr) const;
-
- // tracks across captures that belong to keyframe
- TrackConst trackAtKeyframes(const SizeStd& _track_id) const;
- Track trackAtKeyframes(const SizeStd& _track_id);
-
- private:
-
- static SizeStd track_id_count_;
-
- // tracks across all Captures
- map<SizeStd, Track > tracks_; // map indexed by track_Id of ( maps indexed by TimeStamp of ( features ) )
-
- // Across track: maps of Feature pointers indexed by track_Id.
- map<CaptureBasePtr, Snapshot > snapshots_; // map indexed by capture_ptr of ( maps indexed by track_Id of ( features ) )
+ public:
+ TrackMatrix();
+ virtual ~TrackMatrix();
+
+ void newTrack(FeatureBasePtr _ftr);
+ void add(const SizeStd& _track_id, const FeatureBasePtr& _ftr);
+ void add(const FeatureBasePtr& _ftr_existing, const FeatureBasePtr& _ftr_new);
+ void remove(FeatureBasePtr _ftr);
+ void remove(const SizeStd& _track_id);
+ void remove(CaptureBasePtr _cap);
+
+ SizeStd numTracks() const;
+ SizeStd trackSize(const SizeStd& _track_id) const;
+ TrackConst track(const SizeStd& _track_id) const;
+ Track track(const SizeStd& _track_id);
+ SnapshotConst snapshot(CaptureBaseConstPtr _capture) const;
+ Snapshot snapshot(CaptureBasePtr _capture);
+ vector<FeatureBaseConstPtr> trackAsVector(const SizeStd& _track_id) const;
+ vector<FeatureBasePtr> trackAsVector(const SizeStd& _track_id);
+ FeatureBaseConstPtrList snapshotAsList(CaptureBaseConstPtr _cap) const;
+ FeatureBasePtrList snapshotAsList(CaptureBasePtr _cap);
+ TrackMatchesConst matches(CaptureBaseConstPtr _cap_1, CaptureBaseConstPtr _cap_2) const;
+ TrackMatches matches(CaptureBasePtr _cap_1, CaptureBasePtr _cap_2);
+
+ FeatureBaseConstPtr firstFeature(const SizeStd& _track_id) const;
+ FeatureBasePtr firstFeature(const SizeStd& _track_id);
+ FeatureBaseConstPtr lastFeature(const SizeStd& _track_id) const;
+ FeatureBasePtr lastFeature(const SizeStd& _track_id);
+ FeatureBaseConstPtr feature(const SizeStd& _track_id, CaptureBaseConstPtr _cap) const;
+ FeatureBasePtr feature(const SizeStd& _track_id, CaptureBasePtr _cap);
+ CaptureBaseConstPtr firstCapture(const SizeStd& _track_id) const;
+ CaptureBasePtr firstCapture(const SizeStd& _track_id);
+
+ list<SizeStd> trackIds(CaptureBaseConstPtr _capture = nullptr) const;
+
+ // tracks across captures that belong to keyframe
+ TrackConst trackAtKeyframes(const SizeStd& _track_id) const;
+ Track trackAtKeyframes(const SizeStd& _track_id);
+
+ private:
+ static SizeStd track_id_count_;
+
+ // tracks across all Captures
+ map<SizeStd, Track> tracks_; // map indexed by track_Id of ( maps indexed by TimeStamp of ( features ) )
+
+ // Across track: maps of Feature pointers indexed by track_Id.
+ map<CaptureBasePtr, Snapshot>
+ snapshots_; // map indexed by capture_ptr of ( maps indexed by track_Id of ( features ) )
};
} /* namespace wolf */
diff --git a/include/core/sensor/factory_sensor.h b/include/core/sensor/factory_sensor.h
index 4367f9e6eeabbb7edf3284eb67b6d6e4117a4512..073e6c68a58c5ae2f8c139eaca42a1ce0ceb2e3c 100644
--- a/include/core/sensor/factory_sensor.h
+++ b/include/core/sensor/factory_sensor.h
@@ -25,7 +25,8 @@ namespace wolf
class SensorBase;
struct ParamsSensorBase;
class SpecStateSensor;
-template <typename T> class Composite;
+template <typename T>
+class Composite;
} // namespace wolf
#include <unordered_map>
@@ -38,7 +39,6 @@ template <typename T> class Composite;
namespace wolf
{
-
/** \brief Sensor factory class
*
* This factory can create objects of classes deriving from SensorBase.
@@ -66,7 +66,7 @@ namespace wolf
* Sensor creators have the following API:
*
* \code
- * static SensorBasePtr create(const YAML::Node& _server);
+ * static SensorBasePtr create(const YAML::Node& _server, std::vector<std::string>& _folders_schema);
* static SensorBasePtr create(const std::string& _schema,
* const std::string& _yaml_filepath,
* std::vector<std::string>& _folders_schema);
@@ -75,19 +75,25 @@ namespace wolf
* They follow the general implementations shown below:
*
* \code
- * static SensorBasePtr create(const YAML::Node& _server)
+ * static SensorBasePtr create(const YAML::Node& _input_node, std::vector<std::string>& _folders_schema={})
* {
- * // Do create the Sensor Parameters --- example: ParamsSensorCamera
- * auto params = std::make_shared<ParamsSensorCamera>(_server);
- *
- * // Do create the Sensor States priors
- * auto priors = SpecStateSensorComposite(_server["states"]);
+ * // validate the _input_node if provided folders where to find the schema file
+ * if (not _folders_schema.empty())
+ * {
+ * auto server = yaml_schema_cpp::YamlServer(_folders_schema);
+ * server.setYaml(_input_node);
+ * if (not server.applySchema(#SensorClass))
+ * {
+ * throw std::runtime_error(server.getLog());
+ * }
+ * }
*
* // Do create the Sensor object --- example: SensorCamera
* auto sensor = std::make_shared<SensorCamera>(params, priors);
*
* return sensor;
* }
+ *
* static SensorBasePtr create(const std::string& _schema,
* const std::string& _yaml_filepath,
* std::vector<std::string>& _folders_schema)
@@ -95,29 +101,20 @@ namespace wolf
* // parse the yaml file
* auto server = yaml_schema_cpp::YamlServer(_folders_schema, _yaml_filepath);
*
- *
* // Check that the yaml has all necessary inforamtion
* if (not server.applySchema(_schema))
* {
* throw std::runtime_error(server.getLog());
* }
*
- * // Do create the Sensor Parameters --- example: ParamsSensorCamera
- * auto params = std::make_shared<ParamsSensorCamera>(server.getNode());
- *
- * // Do create the Sensor States priors
- * auto priors = SpecStateSensorComposite(_server["states"]);
- *
- * // Do create the Sensor object --- example: SensorCamera
- * auto sensor = std::make_shared<SensorCamera>(params, priors);
+ * // Do create the Sensor object (calling the previous creator)
+ * auto sensor = create(server.getNode(), {});
*
* return sensor;
* }
* \endcode
*
*
- *
- *
* #### Creating sensors
* Note: Prior to invoking the creation of a sensor of a particular type,
* you must register the creator for this type into the factory.
@@ -142,10 +139,7 @@ namespace wolf
* To do so, you should implement a constructor with the API:
*
* \code
- * SensorDerived(const std::string& _unique_name,
- * SizeEigen _dim,
- * ParamsSensorDummyPtr _params,
- * const ParamsServer& _server)
+ * SensorDerived(const YAML::Node& _params)
* \endcode
*
* #### Registering sensor creator into the factory
@@ -167,7 +161,7 @@ namespace wolf
* \code
* main () {
* FactorySensorNode::registerCreator("SensorCamera", SensorCamera::create);
- * FactorySensorNode::registerCreatorYaml("SensorCamera", SensorCamera::create);
+ * FactorySensorFile::registerCreator("SensorCamera", SensorCamera::create);
* ...
* }
* \endcode
@@ -180,8 +174,8 @@ namespace wolf
* const bool registered_camera_yaml = FactorySensorFile::registerCreator("SensorCamera", SensorCamera::create);
* }
* \endcode
- * Automatic registration is recommended in wolf, and implemented in the classes shipped with it using the macros
- * WOLF_REGISTER_SENSOR(SensorType) and WOLF_REGISTER_SENSOR_YAML(SensorType).
+ * Automatic registration is recommended in wolf, and implemented in the classes shipped with it using the macro
+ * WOLF_REGISTER_SENSOR(SensorType).
* You are free to comment out these lines and place them wherever you consider it more convenient for your design.
*
* #### Example 2: creating sensors
@@ -200,11 +194,11 @@ namespace wolf
* // a type = "SensorCamera",
* // a name = "Front-left camera",
* // the problem dimension dim = 3, and
- * // the parameter server or a yaml file
+ * // the yaml node or a yaml file
*
* SensorBasePtr camera_1_ptr =
* FactorySensorNode::create ( "SensorCamera",
- * parameter_server );
+ * parameter_node );
*
* // A second camera... with a different name specified in the yaml file or the parameters!
*
@@ -221,22 +215,10 @@ namespace wolf
* - Problem::installSensor() : to install sensors in WOLF Problem.
*/
-typedef Factory<std::shared_ptr<SensorBase>, std::shared_ptr<ParamsSensorBase>, const Composite<SpecStateSensor>&>
- FactorySensorParams;
+typedef Factory<std::shared_ptr<SensorBase>, const YAML::Node&, const std::vector<std::string>&> FactorySensorNode;
-typedef Factory<std::shared_ptr<SensorBase>, const YAML::Node&> FactorySensorNode;
+typedef Factory<std::shared_ptr<SensorBase>, const std::string&, const std::vector<std::string>&> FactorySensorFile;
-typedef Factory<std::shared_ptr<SensorBase>,
- // const std::string&,
- const std::string&,
- const std::vector<std::string>&>
- FactorySensorFile;
-
-template <>
-inline std::string FactorySensorParams::getClass() const
-{
- return "FactorySensorParams";
-}
template <>
inline std::string FactorySensorNode::getClass() const
{
@@ -251,8 +233,6 @@ inline std::string FactorySensorFile::getClass() const
#define WOLF_REGISTER_SENSOR(SensorType) \
namespace \
{ \
- const bool WOLF_UNUSED SensorType##ParamsRegistered = \
- FactorySensorParams::registerCreator(#SensorType, SensorType::create); \
const bool WOLF_UNUSED SensorType##NodeRegistered = \
FactorySensorNode::registerCreator(#SensorType, SensorType::create); \
const bool WOLF_UNUSED SensorType##FileRegistered = \
diff --git a/include/core/sensor/sensor_base.h b/include/core/sensor/sensor_base.h
index bd8dbeb43901da1bba6ef0a3728b45ecc6cc8dbc..2ec64bcb723c6aefe60f8586603134bdf7933059 100644
--- a/include/core/sensor/sensor_base.h
+++ b/include/core/sensor/sensor_base.h
@@ -34,7 +34,7 @@ class StateBlock;
#include "core/composite/spec_state_sensor_composite.h"
#include "core/common/node_state_blocks.h"
#include "core/common/time_stamp.h"
-#include "core/common/params_base.h"
+#include "yaml-cpp/yaml.h"
#include "yaml-schema-cpp/yaml_schema.hpp"
// std includes
@@ -50,27 +50,28 @@ namespace wolf
* In order to use this macro, the derived sensor class, SensorClass,
* must have two constructors available with the API:
*
- * SensorClass(ParamsSensorClassPtr _params,
- * const SpecStateSensorComposite& _priors)
+ * SensorClass(const YAML::Node& _params);
*
* Also, there should be the schema file 'SensorClass.schema' containing the specifications
* of the user input yaml file.
*/
-// We use `new` and not `make_shared` since the Problem constructor may (should) be private
-#define WOLF_SENSOR_CREATE(SensorClass, ParamsSensorClass) \
- static SensorBasePtr create(ParamsSensorBasePtr params, const SpecStateSensorComposite& priors) \
+// We use `new` and not `make_shared` since the constructor may (should) be private/protected
+#define WOLF_SENSOR_CREATE(SensorClass) \
+ static SensorBasePtr create(const YAML::Node& _input_node, const std::vector<std::string>& _folders_schema = {}) \
{ \
- auto params_derived = std::static_pointer_cast<ParamsSensorClass>(params); \
- std::shared_ptr<SensorClass> sen(new SensorClass(params_derived, priors(params->getStatesKeys()))); \
- sen->applyPriors(priors(params->getStatesKeys()).cast<SpecStateComposite>()); \
+ if (not _folders_schema.empty()) \
+ { \
+ auto server = yaml_schema_cpp::YamlServer(_folders_schema); \
+ server.setYaml(_input_node); \
+ if (not server.applySchema(#SensorClass)) \
+ { \
+ throw std::runtime_error(server.getLog()); \
+ } \
+ } \
+ std::shared_ptr<SensorClass> sen(new SensorClass(_input_node)); \
+ sen->emplacePriors(SpecStateComposite(_input_node["states"])); \
return sen->shared_from_this_sensor(); \
} \
- static SensorBasePtr create(const YAML::Node& _input_node) \
- { \
- auto params = std::make_shared<ParamsSensorClass>(_input_node); \
- auto priors = SpecStateSensorComposite(_input_node["states"], params->getStatesKeys()); \
- return create(params, priors); \
- } \
static SensorBasePtr create(const std::string& _yaml_filepath, const std::vector<std::string>& _folders_schema) \
{ \
auto server = yaml_schema_cpp::YamlServer(_folders_schema, _yaml_filepath); \
@@ -78,65 +79,35 @@ namespace wolf
{ \
throw std::runtime_error(server.getLog()); \
} \
- return create(server.getNode()); \
+ return create(server.getNode(), {}); \
}
-#define WOLF_SENSOR_TEMPLATE_DIM_CREATE(SensorClass, Dim, ParamsSensorClass) \
- static SensorBasePtr create(ParamsSensorBasePtr params, const SpecStateSensorComposite& priors) \
+#define WOLF_SENSOR_TEMPLATE_DIM_CREATE(SensorClass, Dim) \
+ static SensorBasePtr create(const YAML::Node& _input_node, const std::vector<std::string>& _folders_schema = {}) \
{ \
- auto params_derived = std::static_pointer_cast<ParamsSensorClass>(params); \
- std::shared_ptr<SensorClass> sen(new SensorClass<Dim>(params_derived, priors(params->getStatesKeys()))); \
- sen->applyPriors(priors(params->getStatesKeys()).cast<SpecStateComposite>()); \
+ if (not _folders_schema.empty()) \
+ { \
+ auto server = yaml_schema_cpp::YamlServer(_folders_schema); \
+ server.setYaml(_input_node); \
+ if (not server.applySchema(#SensorClass + toString(Dim) + "d")) \
+ { \
+ throw std::runtime_error(server.getLog()); \
+ } \
+ } \
+ std::shared_ptr<SensorClass> sen(new SensorClass<Dim>(_input_node)); \
+ sen->emplacePriors(SpecStateComposite(_input_node["states"])); \
return sen->shared_from_this_sensor(); \
} \
- static SensorBasePtr create(const YAML::Node& _input_node) \
- { \
- auto params = std::make_shared<ParamsSensorClass>(_input_node); \
- auto priors = SpecStateSensorComposite(_input_node["states"], params->getStatesKeys()); \
- return create(params, priors); \
- } \
static SensorBasePtr create(const std::string& _yaml_filepath, const std::vector<std::string>& _folders_schema) \
{ \
auto server = yaml_schema_cpp::YamlServer(_folders_schema, _yaml_filepath); \
- if (not server.applySchema(#SensorClass + std::to_string(Dim) + "d")) \
+ if (not server.applySchema(#SensorClass + toString(Dim) + "d")) \
{ \
throw std::runtime_error(server.getLog()); \
} \
- return create(server.getNode()); \
+ return create(server.getNode(), {}); \
}
-/** \brief base struct for sensor parameters
- *
- * Derive from this struct to create structs of sensor parameters.
- *
- * The constructors for derived struct should take only a YAML::Node.
- *
- * The method 'getStatesKeys()' has to be implemented in the derived struct.
- * It should return a string containing the keys of the states contained in the sensor.
- *
- * The method print is recommended to ve overriten and call print() of inherited classes.
- */
-WOLF_STRUCT_PTR_TYPEDEFS(ParamsSensorBase);
-struct ParamsSensorBase : public ParamsBase
-{
- std::string name;
-
- ParamsSensorBase() = default;
- ParamsSensorBase(const YAML::Node& _input_node)
- {
- name = _input_node["name"].as<std::string>();
- }
- ~ParamsSensorBase() override = default;
-
- virtual std::string getStatesKeys() const = 0;
-
- std::string print() const override
- {
- return "name: " + name + "\n" + //
- "states_keys: " + getStatesKeys() + "\n"; //
- }
-};
-
WOLF_PTR_TYPEDEFS(SensorBase);
class SensorBase : public NodeStateBlocks
{
@@ -151,8 +122,6 @@ class SensorBase : public NodeStateBlocks
unsigned int sensor_id_; // sensor ID
- ParamsSensorBasePtr params_; ///< Params struct
-
std::map<char, bool> state_block_dynamic_; // mark dynamic state blocks
CaptureBasePtr last_capture_; // last capture of the sensor (in the WOLF tree)
@@ -160,6 +129,8 @@ class SensorBase : public NodeStateBlocks
protected:
std::map<char, Eigen::MatrixXd> drift_covs_; // covariance of the drift of dynamic state blocks [unit²/s]
+ YAML::Node params_; ///< Params yaml node
+
int dim_compatible_; ///< Dimension compatibility of the sensor: 2: 2D, 3: 3D, -1: both
void setProblem(ProblemPtr _problem) override final;
@@ -167,16 +138,13 @@ class SensorBase : public NodeStateBlocks
/** \brief Constructor with Prior and Params
*
* Constructor with parameter vector
- * \param _type Type of the sensor (types defined at wolf.h)
- * \param _dim_compatible Which dimension is the sensor compatible (2: 2D, 3: 3D, -1: both)
- * \param _params params struct
- * \param _priors priors of the sensor state blocks
+ * \param _type TO BE HARDCODED IN THE DERIVED CLASS CONSTRUCTOR: type name
+ * \param _dim_compatible TO BE HARDCODED IN THE DERIVED CLASS CONSTRUCTOR:
+ * Which dimension is the sensor compatible (2: 2D, 3: 3D, -1: both)
+ * \param _params params yaml node
*
**/
- SensorBase(const std::string& _type,
- const int& _dim_compatible,
- ParamsSensorBasePtr _params,
- const SpecStateSensorComposite& _priors);
+ SensorBase(const std::string& _type, const int& _dim_compatible, const YAML::Node& _params);
public:
~SensorBase() override;
@@ -187,6 +155,8 @@ class SensorBase : public NodeStateBlocks
HardwareBaseConstPtr getHardware() const;
HardwareBasePtr getHardware();
+ YAML::Node getParams() const;
+
private:
void setHardware(const HardwareBasePtr _hw_ptr);
ProcessorBasePtr addProcessor(ProcessorBasePtr _proc_ptr);
@@ -274,8 +244,10 @@ class SensorBase : public NodeStateBlocks
bool check(CheckLog& _log, bool _verbose, std::ostream& _stream, std::string _tabs = "") const;
void link(HardwareBasePtr);
- template <typename classType, typename... T>
- static std::shared_ptr<classType> emplace(HardwareBasePtr _hwd_ptr, T&&... all);
+ template <typename classType>
+ static std::shared_ptr<classType> emplace(HardwareBasePtr _hwd_ptr,
+ const YAML::Node& _params,
+ const std::vector<std::string>& _folders_schema = {});
SensorBasePtr shared_from_this_sensor()
{
@@ -295,10 +267,12 @@ class SensorBase : public NodeStateBlocks
namespace wolf
{
-template <typename classType, typename... T>
-std::shared_ptr<classType> SensorBase::emplace(HardwareBasePtr _hwd_ptr, T&&... all)
+template <typename classType>
+std::shared_ptr<classType> SensorBase::emplace(HardwareBasePtr _hwd_ptr,
+ const YAML::Node& _params,
+ const std::vector<std::string>& _folders_schema)
{
- std::shared_ptr<classType> sen = std::static_pointer_cast<classType>(classType::create(std::forward<T>(all)...));
+ std::shared_ptr<classType> sen = std::static_pointer_cast<classType>(classType::create(_params, _folders_schema));
sen->link(_hwd_ptr);
return sen;
}
@@ -325,6 +299,11 @@ inline ProcessorBasePtrList SensorBase::getProcessorList()
return processor_list_;
}
+inline YAML::Node SensorBase::getParams() const
+{
+ return Clone(params_);
+}
+
inline CaptureBaseConstPtr SensorBase::getLastCapture() const
{
return last_capture_;
diff --git a/include/core/sensor/sensor_diff_drive.h b/include/core/sensor/sensor_diff_drive.h
index 2c7f6f710083fe106ca611231ca026bfcb80e994..087ea3b8237351bf309da17eec8723933bfb351f 100644
--- a/include/core/sensor/sensor_diff_drive.h
+++ b/include/core/sensor/sensor_diff_drive.h
@@ -24,69 +24,42 @@
namespace wolf
{
-WOLF_STRUCT_PTR_TYPEDEFS(ParamsSensorDiffDrive);
-
-struct ParamsSensorDiffDrive : public ParamsSensorBase
-{
- double ticks_per_wheel_revolution;
- double ticks_std_factor;
-
- ParamsSensorDiffDrive() = default;
- ParamsSensorDiffDrive(const YAML::Node& _input_node) : ParamsSensorBase(_input_node)
- {
- ticks_per_wheel_revolution = _input_node["ticks_per_wheel_revolution"].as<double>();
- ticks_std_factor = _input_node["ticks_std_factor"].as<double>();
- }
- ~ParamsSensorDiffDrive() = default;
-
- std::string getStatesKeys() const override
- {
- return "POI";
- }
-
- std::string print() const override
- {
- return ParamsSensorBase::print() + "\n" //
- + "ticks_per_wheel_revolution: " + toString(ticks_per_wheel_revolution) + "\n" //
- + "ticks_std_factor: " + toString(ticks_std_factor) + "\n"; //
- }
-};
-
WOLF_PTR_TYPEDEFS(SensorDiffDrive);
class SensorDiffDrive : public SensorBase
{
- SensorDiffDrive(ParamsSensorDiffDrivePtr _params, const SpecStateSensorComposite& _priors);
-
+ SensorDiffDrive(const YAML::Node& _params);
+
public:
- WOLF_SENSOR_CREATE(SensorDiffDrive, ParamsSensorDiffDrive);
+ WOLF_SENSOR_CREATE(SensorDiffDrive);
~SensorDiffDrive() override = default;
- ParamsSensorDiffDriveConstPtr getParams() const;
- ParamsSensorDiffDrivePtr getParams();
-
Eigen::MatrixXd computeNoiseCov(const Eigen::VectorXd& _data) const override;
+ double getTicksPerWheelRevolution() const;
+ double getTicksStdFactor() const;
double getRadiansPerTick() const;
protected:
- ParamsSensorDiffDrivePtr params_diff_drive_;
+ // params
+ double ticks_per_wheel_revolution_;
+ double ticks_std_factor_;
};
-inline ParamsSensorDiffDriveConstPtr SensorDiffDrive::getParams() const
+inline double SensorDiffDrive::getTicksPerWheelRevolution() const
{
- return params_diff_drive_;
+ return ticks_per_wheel_revolution_;
}
-inline ParamsSensorDiffDrivePtr SensorDiffDrive::getParams()
+inline double SensorDiffDrive::getTicksStdFactor() const
{
- return params_diff_drive_;
+ return ticks_std_factor_;
}
inline double SensorDiffDrive::getRadiansPerTick() const
{
- return 2.0 * M_PI / params_diff_drive_->ticks_per_wheel_revolution;
+ return 2.0 * M_PI / ticks_per_wheel_revolution_;
}
} /* namespace wolf */
diff --git a/include/core/sensor/sensor_motion_model.h b/include/core/sensor/sensor_motion_model.h
index 28683fea19dc671c408fa2ea283c82f36f1f6981..c4ed862cc81f0b8b34e9d7b01625ccce198aae1c 100644
--- a/include/core/sensor/sensor_motion_model.h
+++ b/include/core/sensor/sensor_motion_model.h
@@ -25,34 +25,14 @@
namespace wolf
{
-
-WOLF_STRUCT_PTR_TYPEDEFS(ParamsSensorMotionModel);
-
-struct ParamsSensorMotionModel : public ParamsSensorBase
-{
- ParamsSensorMotionModel() = default;
- ParamsSensorMotionModel(const YAML::Node& _input_node) : ParamsSensorBase(_input_node) {}
- ~ParamsSensorMotionModel() = default;
-
- std::string getStatesKeys() const override
- {
- return "";
- }
-
- std::string print() const override
- {
- return ParamsSensorBase::print();
- }
-};
-
WOLF_PTR_TYPEDEFS(SensorMotionModel);
class SensorMotionModel : public SensorBase
{
- SensorMotionModel(ParamsSensorMotionModelPtr _params, const SpecStateSensorComposite& _priors);
+ SensorMotionModel(const YAML::Node& _params);
public:
- WOLF_SENSOR_CREATE(SensorMotionModel, ParamsSensorMotionModel);
+ WOLF_SENSOR_CREATE(SensorMotionModel);
~SensorMotionModel() override;
diff --git a/include/core/sensor/sensor_odom.h b/include/core/sensor/sensor_odom.h
index 3cb74ab60b571ae6cb12ed72a0204c4d55b146b2..89eb373a1021254585d6c9c5111e184c25eb3ad3 100644
--- a/include/core/sensor/sensor_odom.h
+++ b/include/core/sensor/sensor_odom.h
@@ -26,56 +26,17 @@
namespace wolf
{
-WOLF_STRUCT_PTR_TYPEDEFS(ParamsSensorOdom);
-
-struct ParamsSensorOdom : public ParamsSensorBase
-{
- double k_disp_to_disp; ///< ratio of displacement variance to displacement, for odometry noise calculation
- double k_disp_to_rot; ///< ratio of displacement variance to rotation, for odometry noise calculation
- double k_rot_to_rot; ///< ratio of rotation variance to rotation, for odometry noise calculation
- double min_disp_var;
- double min_rot_var;
-
- ParamsSensorOdom() = default;
- ParamsSensorOdom(const YAML::Node& _input_node) : ParamsSensorBase(_input_node)
- {
- k_disp_to_disp = _input_node["k_disp_to_disp"].as<double>();
- k_disp_to_rot = _input_node["k_disp_to_rot"].as<double>();
- k_rot_to_rot = _input_node["k_rot_to_rot"].as<double>();
- min_disp_var = _input_node["min_disp_var"].as<double>();
- min_rot_var = _input_node["min_rot_var"].as<double>();
- }
- ~ParamsSensorOdom() override = default;
-
- std::string getStatesKeys() const override
- {
- return "PO";
- }
- std::string print() const override
- {
- return ParamsSensorBase::print() + "\n" //
- + "k_disp_to_disp: " + toString(k_disp_to_disp) + "\n" //
- + "k_disp_to_rot: " + toString(k_disp_to_rot) + "\n" //
- + "k_rot_to_rot: " + toString(k_rot_to_rot) + "\n" //
- + "min_disp_var: " + toString(min_disp_var) + "\n" //
- + "min_rot_var: " + toString(min_rot_var) + "\n"; //
- }
-};
-
template <unsigned int DIM>
class SensorOdom : public SensorBase
{
protected:
- ParamsSensorOdomPtr params_odom_;
-
- SensorOdom(ParamsSensorOdomPtr _params, const SpecStateSensorComposite& _priors)
- : SensorBase("SensorOdom" + toString(DIM) + "d", DIM, _params, _priors), params_odom_(_params)
+ SensorOdom(const YAML::Node& _params) : SensorBase("SensorOdom" + toString(DIM) + "d", DIM, _params)
{
static_assert(DIM == 2 or DIM == 3);
}
public:
- WOLF_SENSOR_TEMPLATE_DIM_CREATE(SensorOdom, DIM, ParamsSensorOdom);
+ WOLF_SENSOR_TEMPLATE_DIM_CREATE(SensorOdom, DIM);
~SensorOdom() override = default;
@@ -91,31 +52,31 @@ class SensorOdom : public SensorBase
template <unsigned int DIM>
inline double SensorOdom<DIM>::getDispVarToDispNoiseFactor() const
{
- return params_odom_->k_disp_to_disp;
+ return params_["k_disp_to_disp"].as<double>();
}
template <unsigned int DIM>
inline double SensorOdom<DIM>::getDispVarToRotNoiseFactor() const
{
- return params_odom_->k_disp_to_rot;
+ return params_["k_disp_to_rot"].as<double>();
}
template <unsigned int DIM>
inline double SensorOdom<DIM>::getRotVarToRotNoiseFactor() const
{
- return params_odom_->k_rot_to_rot;
+ return params_["k_rot_to_rot"].as<double>();
}
template <unsigned int DIM>
inline double SensorOdom<DIM>::getMinDispVar() const
{
- return params_odom_->min_disp_var;
+ return params_["min_disp_var"].as<double>();
}
template <unsigned int DIM>
inline double SensorOdom<DIM>::getMinRotVar() const
{
- return params_odom_->min_rot_var;
+ return params_["min_rot_var"].as<double>();
}
template <unsigned int DIM>
@@ -154,9 +115,9 @@ inline Eigen::MatrixXd SensorOdom<DIM>::computeNoiseCov(const Eigen::VectorXd& _
}
// variances
- double dvar = std::max(params_odom_->min_disp_var, params_odom_->k_disp_to_disp * d);
+ double dvar = std::max(this->getMinDispVar(), this->getDispVarToDispNoiseFactor() * d);
double rvar =
- std::max(params_odom_->min_rot_var, params_odom_->k_disp_to_rot * d + params_odom_->k_rot_to_rot * r);
+ std::max(this->getMinRotVar(), this->getDispVarToRotNoiseFactor() * d + this->getRotVarToRotNoiseFactor() * r);
// return
if (DIM == 2)
diff --git a/include/core/sensor/sensor_pose.h b/include/core/sensor/sensor_pose.h
index 3e18a06d247dcfb3459ac40228b894c0fd73d4dd..57fd3c571e64920ab0241268058496032285e44f 100644
--- a/include/core/sensor/sensor_pose.h
+++ b/include/core/sensor/sensor_pose.h
@@ -26,60 +26,35 @@
namespace wolf
{
-WOLF_STRUCT_PTR_TYPEDEFS(ParamsSensorPose);
-
-struct ParamsSensorPose : public ParamsSensorBase
-{
- Eigen::VectorXd std_noise;
-
- ParamsSensorPose() = default;
- ParamsSensorPose(const YAML::Node& _input_node) : ParamsSensorBase(_input_node)
- {
- std_noise = _input_node["std_noise"].as<Eigen::VectorXd>();
- }
- ~ParamsSensorPose() override = default;
-
- std::string getStatesKeys() const override
- {
- return "PO";
- }
- std::string print() const override
- {
- return ParamsSensorBase::print() + "\n" //
- + "std_noise: " + toString(std_noise) + "\n"; //
- }
-};
-
template <unsigned int DIM>
class SensorPose : public SensorBase
{
protected:
- ParamsSensorPosePtr params_pose_;
-
- SensorPose(ParamsSensorPosePtr _params, const SpecStateSensorComposite& _priors)
- : SensorBase("SensorPose" + toString(DIM) + "d", DIM, _params, _priors), params_pose_(_params)
+ SensorPose(const YAML::Node& _params) : SensorBase("SensorPose" + toString(DIM) + "d", DIM, _params)
{
static_assert(DIM == 2 or DIM == 3);
- if (DIM == 2 and params_pose_->std_noise.size() != 3)
- throw std::runtime_error("SensorBase: wrong std_noise size for 2D: " +
- toString(params_pose_->std_noise.size()) + "(should be 3)");
- if (DIM == 3 and params_pose_->std_noise.size() != 6)
- throw std::runtime_error("SensorBase: wrong std_noise size for 3D: " +
- toString(params_pose_->std_noise.size()) + "(should be 6)");
}
public:
- WOLF_SENSOR_TEMPLATE_DIM_CREATE(SensorPose, DIM, ParamsSensorPose);
+ WOLF_SENSOR_TEMPLATE_DIM_CREATE(SensorPose, DIM);
~SensorPose() override = default;
+ Eigen::VectorXd getStdNoise() const;
+
Eigen::MatrixXd computeNoiseCov(const Eigen::VectorXd& _data) const override;
};
+template <unsigned int DIM>
+inline Eigen::VectorXd SensorPose<DIM>::getStdNoise() const
+{
+ return params_["std_noise"].as<Eigen::VectorXd>();
+}
+
template <unsigned int DIM>
inline Eigen::MatrixXd SensorPose<DIM>::computeNoiseCov(const Eigen::VectorXd& _data) const
{
- return params_pose_->std_noise.cwiseAbs2().asDiagonal();
+ return getStdNoise().cwiseAbs2().asDiagonal();
}
typedef SensorPose<2> SensorPose2d;
diff --git a/include/core/solver/factory_solver.h b/include/core/solver/factory_solver.h
index f31fa027350998efe133f5828adb398c046bdb31..7be084b310a86f2eae80562fcad9f0ee76ffff62 100644
--- a/include/core/solver/factory_solver.h
+++ b/include/core/solver/factory_solver.h
@@ -24,7 +24,7 @@ namespace wolf
{
class SensorBase;
struct ParamsSensorBase;
-}
+} // namespace wolf
// wolf
#include "core/common/factory.h"
@@ -34,36 +34,38 @@ struct ParamsSensorBase;
namespace wolf
{
-
/** \brief Solver factory class
*
*/
-typedef Factory<std::shared_ptr<SolverManager>,
- const ProblemPtr&,
- const YAML::Node&> FactorySolver;
+typedef Factory<std::shared_ptr<SolverManager>, const ProblemPtr&, const YAML::Node&, const std::vector<std::string>&>
+ FactorySolverNode;
-template<>
-inline std::string FactorySolver::getClass() const
+template <>
+inline std::string FactorySolverNode::getClass() const
{
- return "FactorySolver";
+ return "FactorySolverNode";
}
-typedef Factory<std::shared_ptr<SolverManager>,
- const ProblemPtr&,
- const std::string&,
- const std::vector<std::string>&> FactorySolverYaml;
+typedef Factory<std::shared_ptr<SolverManager>, const ProblemPtr&, const std::string&, const std::vector<std::string>&>
+ FactorySolverFile;
-template<>
-inline std::string FactorySolverYaml::getClass() const
+template <>
+inline std::string FactorySolverFile::getClass() const
{
- return "FactorySolverYaml";
+ return "FactorySolverFile";
}
-#define WOLF_REGISTER_SOLVER(SolverType) \
- namespace{ const bool WOLF_UNUSED SolverType##Registered = \
- wolf::FactorySolver::registerCreator(#SolverType, SolverType::create); } \
- namespace{ const bool WOLF_UNUSED SolverType##RegisteredYaml = \
- wolf::FactorySolverYaml::registerCreator(#SolverType, SolverType::create); } \
+#define WOLF_REGISTER_SOLVER(SolverType) \
+ namespace \
+ { \
+ const bool WOLF_UNUSED SolverType##Registered = \
+ wolf::FactorySolverNode::registerCreator(#SolverType, SolverType::create); \
+ } \
+ namespace \
+ { \
+ const bool WOLF_UNUSED SolverType##RegisteredYaml = \
+ wolf::FactorySolverFile::registerCreator(#SolverType, SolverType::create); \
+ }
} /* namespace wolf */
diff --git a/include/core/solver/solver_manager.h b/include/core/solver/solver_manager.h
index c4e02ede2faa274ddb2a4a3fa4d8314ccd381fa1..1c84626ec1ae8b9671ddff2f6dcadd16345cbb05 100644
--- a/include/core/solver/solver_manager.h
+++ b/include/core/solver/solver_manager.h
@@ -20,17 +20,15 @@
#pragma once
-//wolf includes
+// wolf includes
#include "core/common/wolf.h"
#include "core/state_block/state_block.h"
#include "core/factor/factor_base.h"
-#include "core/common/params_base.h"
#include "core/solver/factory_solver.h"
-namespace wolf {
-
+namespace wolf
+{
WOLF_PTR_TYPEDEFS(SolverManager)
-WOLF_STRUCT_PTR_TYPEDEFS(ParamsSolver)
/*
* Macro for defining Autoconf solver creator for WOLF's high level API.
@@ -44,234 +42,209 @@ WOLF_STRUCT_PTR_TYPEDEFS(ParamsSolver)
* SolverManagerClass(const ProblemPtr& wolf_problem,
* const ParamsSolverClassPtr _params);
*/
-#define WOLF_SOLVER_CREATE(SolverClass, ParamsSolverClass) \
-static SolverManagerPtr create(const ProblemPtr& _problem, \
- const YAML::Node& _input_node) \
-{ \
- auto params = std::make_shared<ParamsSolverClass>(_input_node); \
- return std::make_shared<SolverClass>(_problem, params); \
-} \
-static SolverManagerPtr create(const ProblemPtr& _problem, \
- const std::string& _yaml_filepath, \
- const std::vector<std::string>& _folders_schema) \
-{ \
- auto server = yaml_schema_cpp::YamlServer(_folders_schema, _yaml_filepath); \
- if (not server.applySchema(#SolverClass)) \
- { \
- WOLF_ERROR(server.getLog()); \
- return nullptr; \
- } \
- auto params = std::make_shared<ParamsSolverClass>(server.getNode()); \
- return std::make_shared<SolverClass>(_problem, params); \
-} \
-
-struct ParamsSolver;
+#define WOLF_SOLVER_CREATE(SolverClass) \
+ static SolverManagerPtr create(const ProblemPtr& _problem, \
+ const YAML::Node& _input_node, \
+ const std::vector<std::string>& _folders_schema = {}) \
+ { \
+ if (not _folders_schema.empty()) \
+ { \
+ auto server = yaml_schema_cpp::YamlServer(_folders_schema); \
+ server.setYaml(_input_node); \
+ if (not server.applySchema(#SolverClass)) \
+ { \
+ throw std::runtime_error(server.getLog()); \
+ } \
+ } \
+ return std::make_shared<SolverClass>(_problem, _input_node); \
+ } \
+ static SolverManagerPtr create(const ProblemPtr& _problem, \
+ const std::string& _yaml_filepath, \
+ const std::vector<std::string>& _folders_schema) \
+ { \
+ auto server = yaml_schema_cpp::YamlServer(_folders_schema, _yaml_filepath); \
+ if (not server.applySchema(#SolverClass)) \
+ { \
+ WOLF_ERROR(server.getLog()); \
+ return nullptr; \
+ } \
+ return create(_problem, server.getNode(), {}); \
+ }
/**
* \brief Solver manager for WOLF
*/
class SolverManager
{
- public:
-
- /** \brief Enumeration of covariance blocks to be computed
- *
- * Enumeration of covariance blocks to be computed
- *
- */
- enum class CovarianceBlocksToBeComputed : std::size_t
- {
- ALL = 0, ///< All blocks and all cross-covariances
- ALL_MARGINALS = 1, ///< All marginals
- ROBOT_LANDMARKS = 2, ///< marginals of landmarks and current robot pose plus cross covariances of current robot and all landmarks
- GAUSS = 3, ///< GAUSS: last frame P and V
- GAUSS_TUCAN = 4, ///< GAUSS: last frame P, O, V and T
- GAUSS_TUCAN_ONLY_POSITION = 5 ///< GAUSS: last frame P and T
- };
-
- /**
- * \brief Enumeration for the verbosity of the solver report.
- */
- enum class ReportVerbosity : std::size_t
- {
- QUIET = 0,
- BRIEF = 1,
- FULL = 2
- };
-
- // PROFILING
- unsigned int n_solve_, n_cov_;
- std::chrono::microseconds acc_duration_total_;
- std::chrono::microseconds acc_duration_solver_;
- std::chrono::microseconds acc_duration_update_;
- std::chrono::microseconds acc_duration_state_;
- std::chrono::microseconds acc_duration_cov_;
-
- std::chrono::microseconds max_duration_total_;
- std::chrono::microseconds max_duration_solver_;
- std::chrono::microseconds max_duration_update_;
- std::chrono::microseconds max_duration_state_;
- std::chrono::microseconds max_duration_cov_;
-
- void printProfiling(std::ostream& stream = std::cout) const;
-
- protected:
-
- ProblemPtr wolf_problem_;
- ParamsSolverPtr params_;
-
- public:
- /**
- * \brief Constructor with default params_
- */
- SolverManager(const ProblemPtr& _problem);
- /**
- * \brief Constructor with given params_
- */
- SolverManager(const ProblemPtr& _problem,
- const ParamsSolverPtr& _params);
-
- virtual ~SolverManager();
-
- /**
- * \brief Solves with the verbosity defined in params_
- */
- std::string solve();
- /**
- * \brief Solves with a given verbosity
- */
- std::string solve(const ReportVerbosity report_level);
-
- virtual bool computeCovariances() final;
-
- virtual bool computeCovariances(const CovarianceBlocksToBeComputed blocks) final;
-
- virtual bool computeCovariances(const std::vector<StateBlockPtr>& st_list) final;
-
-
- /**
- * \brief Updates solver's problem according to the wolf_problem
- */
- virtual void update();
-
- ProblemPtr getProblem();
+ public:
+ /** \brief Enumeration of covariance blocks to be computed
+ *
+ * Enumeration of covariance blocks to be computed
+ *
+ */
+ enum class CovarianceBlocksToBeComputed : std::size_t
+ {
+ ALL = 0, ///< All blocks and all cross-covariances
+ ALL_MARGINALS = 1, ///< All marginals
+ ROBOT_LANDMARKS = 2, ///< marginals of landmarks and current robot pose plus cross covariances of current
+ ///< robot and all landmarks
+ GAUSS = 3, ///< GAUSS: last frame P and V
+ GAUSS_TUCAN = 4, ///< GAUSS: last frame P, O, V and T
+ GAUSS_TUCAN_ONLY_POSITION = 5 ///< GAUSS: last frame P and T
+ };
+
+ /**
+ * \brief Enumeration for the verbosity of the solver report.
+ */
+ enum class ReportVerbosity : std::size_t
+ {
+ QUIET = 0,
+ BRIEF = 1,
+ FULL = 2
+ };
+
+ // PROFILING
+ unsigned int n_solve_, n_cov_;
+ std::chrono::microseconds acc_duration_total_;
+ std::chrono::microseconds acc_duration_solver_;
+ std::chrono::microseconds acc_duration_update_;
+ std::chrono::microseconds acc_duration_state_;
+ std::chrono::microseconds acc_duration_cov_;
+
+ std::chrono::microseconds max_duration_total_;
+ std::chrono::microseconds max_duration_solver_;
+ std::chrono::microseconds max_duration_update_;
+ std::chrono::microseconds max_duration_state_;
+ std::chrono::microseconds max_duration_cov_;
+
+ void printProfiling(std::ostream& stream = std::cout) const;
+
+ protected:
+ ProblemPtr wolf_problem_;
+ YAML::Node params_;
+
+ public:
+ /**
+ * \brief Constructor with default params_
+ */
+ SolverManager(const ProblemPtr& _problem);
+ /**
+ * \brief Constructor with given params_
+ */
+ SolverManager(const ProblemPtr& _problem, const YAML::Node& _params);
+
+ virtual ~SolverManager();
+
+ /**
+ * \brief Solves with the verbosity defined in params_
+ */
+ std::string solve();
+ /**
+ * \brief Solves with a given verbosity
+ */
+ std::string solve(const ReportVerbosity report_level);
+
+ virtual bool computeCovariances() final;
+
+ virtual bool computeCovariances(const CovarianceBlocksToBeComputed blocks) final;
+
+ virtual bool computeCovariances(const std::vector<StateBlockPtr>& st_list) final;
+
+ /**
+ * \brief Updates solver's problem according to the wolf_problem
+ */
+ virtual void update();
+
+ ProblemPtr getProblem();
+
+ YAML::Node getParams() const;
+
+ double getPeriod() const;
+
+ double getCovPeriod() const;
+
+ ReportVerbosity getVerbosity() const;
+
+ virtual bool isStateBlockRegistered(const StateBlockPtr& state_ptr) const final;
+
+ virtual bool isStateBlockFloating(const StateBlockPtr& state_ptr) const final;
+
+ virtual bool isStateBlockFixed(const StateBlockPtr& st) final;
+
+ virtual bool isFactorRegistered(const FactorBasePtr& fac_ptr) const final;
+
+ virtual bool hasThisLocalParametrization(const StateBlockPtr& st,
+ const LocalParametrizationBasePtr& local_param) final;
+
+ virtual bool hasLocalParametrization(const StateBlockPtr& st) const final;
+
+ virtual int numFactors() const final;
+ virtual int numStateBlocks() const final;
+ virtual int numStateBlocksFloating() const final;
+
+ virtual int numFactorsDerived() const = 0;
+ virtual int numStateBlocksDerived() const = 0;
- virtual ParamsSolverPtr getParams() const;
+ virtual bool check(std::string prefix = "") const final;
- double getPeriod() const;
+ protected:
+ std::map<StateBlockPtr, Eigen::VectorXd> state_blocks_;
+ std::map<StateBlockPtr, FactorBasePtrList> state_blocks_2_factors_;
+ std::set<FactorBasePtr> factors_;
+ std::set<StateBlockPtr> floating_state_blocks_;
- double getCovPeriod() const;
+ virtual Eigen::VectorXd& getAssociatedMemBlock(const StateBlockPtr& state_ptr);
+ const double* getAssociatedMemBlockPtr(const StateBlockPtr& state_ptr) const;
+ double* getAssociatedMemBlockPtr(const StateBlockPtr& state_ptr);
- ReportVerbosity getVerbosity() const;
-
- virtual bool isStateBlockRegistered(const StateBlockPtr& state_ptr) const final;
-
- virtual bool isStateBlockFloating(const StateBlockPtr& state_ptr) const final;
-
- virtual bool isStateBlockFixed(const StateBlockPtr& st) final;
-
- virtual bool isFactorRegistered(const FactorBasePtr& fac_ptr) const final;
-
- virtual bool hasThisLocalParametrization(const StateBlockPtr& st,
- const LocalParametrizationBasePtr& local_param) final;
-
- virtual bool hasLocalParametrization(const StateBlockPtr& st) const final;
-
- virtual int numFactors() const final;
- virtual int numStateBlocks() const final;
- virtual int numStateBlocksFloating() const final;
-
- virtual int numFactorsDerived() const = 0;
- virtual int numStateBlocksDerived() const = 0;
-
- virtual bool check(std::string prefix="") const final;
-
- protected:
-
- std::map<StateBlockPtr, Eigen::VectorXd> state_blocks_;
- std::map<StateBlockPtr, FactorBasePtrList> state_blocks_2_factors_;
- std::set<FactorBasePtr> factors_;
- std::set<StateBlockPtr> floating_state_blocks_;
-
- virtual Eigen::VectorXd& getAssociatedMemBlock(const StateBlockPtr& state_ptr);
- const double* getAssociatedMemBlockPtr(const StateBlockPtr& state_ptr) const;
- double* getAssociatedMemBlockPtr(const StateBlockPtr& state_ptr);
-
- private:
- // SolverManager functions
- virtual void addFactor(const FactorBasePtr& fac_ptr) final;
- virtual void removeFactor(const FactorBasePtr& fac_ptr) final;
- virtual void addStateBlock(const StateBlockPtr& state_ptr) final;
- virtual void removeStateBlock(const StateBlockPtr& state_ptr) final;
- virtual void updateStateBlockState(const StateBlockPtr& state_ptr) final;
- virtual void updateStateBlockStatus(const StateBlockPtr& state_ptr) final;
- virtual void updateStateBlockLocalParametrization(const StateBlockPtr& state_ptr) final;
+ private:
+ // SolverManager functions
+ virtual void addFactor(const FactorBasePtr& fac_ptr) final;
+ virtual void removeFactor(const FactorBasePtr& fac_ptr) final;
+ virtual void addStateBlock(const StateBlockPtr& state_ptr) final;
+ virtual void removeStateBlock(const StateBlockPtr& state_ptr) final;
+ virtual void updateStateBlockState(const StateBlockPtr& state_ptr) final;
+ virtual void updateStateBlockStatus(const StateBlockPtr& state_ptr) final;
+ virtual void updateStateBlockLocalParametrization(const StateBlockPtr& state_ptr) final;
// Derived virtual functions
- protected:
- virtual std::string solveDerived(const ReportVerbosity report_level) = 0;
- virtual bool computeCovariancesDerived(const CovarianceBlocksToBeComputed blocks) = 0;
- virtual bool computeCovariancesDerived(const std::vector<StateBlockPtr>& st_list) = 0;
-
- virtual void addFactorDerived(const FactorBasePtr& fac_ptr) = 0;
- virtual void removeFactorDerived(const FactorBasePtr& fac_ptr) = 0;
- virtual void addStateBlockDerived(const StateBlockPtr& state_ptr) = 0;
- virtual void removeStateBlockDerived(const StateBlockPtr& state_ptr) = 0;
- virtual void updateStateBlockStatusDerived(const StateBlockPtr& state_ptr) = 0;
- virtual void updateStateBlockLocalParametrizationDerived(const StateBlockPtr& state_ptr) = 0;
-
- virtual bool isStateBlockRegisteredDerived(const StateBlockPtr& state_ptr) const = 0;
- virtual bool isFactorRegisteredDerived(const FactorBasePtr& fac_ptr) const = 0;
- virtual bool isStateBlockFixedDerived(const StateBlockPtr& st) = 0;
- virtual bool hasLocalParametrizationDerived(const StateBlockPtr& st) const = 0;
- virtual bool hasThisLocalParametrizationDerived(const StateBlockPtr& st,
- const LocalParametrizationBasePtr& local_param) = 0;
-
- virtual void printProfilingDerived(std::ostream& stream = std::cout) const = 0;
- virtual bool checkDerived(std::string prefix="") const = 0;
-
- public:
- virtual bool hasConverged() = 0;
- virtual bool wasStopped() = 0;
- virtual unsigned int iterations() = 0;
- virtual double initialCost() = 0;
- virtual double finalCost() = 0;
- virtual double totalTime() = 0;
-
-};
-
-// Params (here becaure it needs of declaration of SolverManager::ReportVerbosity)
-struct ParamsSolver: public ParamsBase
-{
- double period = 0.0;
- SolverManager::ReportVerbosity verbose = SolverManager::ReportVerbosity::QUIET;
- bool compute_cov = false;
- SolverManager::CovarianceBlocksToBeComputed cov_enum = SolverManager::CovarianceBlocksToBeComputed::ROBOT_LANDMARKS;
- double cov_period = 1.0;
-
- ParamsSolver() = default;
- ParamsSolver(const YAML::Node& _input_node):
- ParamsBase(_input_node)
- {
- period = _input_node["period"].as<double>();
- verbose = (SolverManager::ReportVerbosity)_input_node["verbose"].as<int>();
- compute_cov = _input_node["compute_cov"].as<bool>();
- if (compute_cov)
- {
- cov_enum = (SolverManager::CovarianceBlocksToBeComputed)_input_node["cov_enum"].as<int>();
- cov_period = _input_node["cov_period"].as<double>();
- }
- }
- std::string print() const override
- {
- return "period: " + toString(period) + "\n" +
- "verbose: " + toString((int)verbose) + "\n" +
- "compute_cov: " + toString(compute_cov) + "\n" +
- "cov_enum: " + toString((int)cov_enum) + "\n" +
- "cov_period: " + toString(cov_period) + "\n";
- }
-
- ~ParamsSolver() override = default;
+ protected:
+ virtual std::string solveDerived(const ReportVerbosity report_level) = 0;
+ virtual bool computeCovariancesDerived(const CovarianceBlocksToBeComputed blocks) = 0;
+ virtual bool computeCovariancesDerived(const std::vector<StateBlockPtr>& st_list) = 0;
+
+ virtual void addFactorDerived(const FactorBasePtr& fac_ptr) = 0;
+ virtual void removeFactorDerived(const FactorBasePtr& fac_ptr) = 0;
+ virtual void addStateBlockDerived(const StateBlockPtr& state_ptr) = 0;
+ virtual void removeStateBlockDerived(const StateBlockPtr& state_ptr) = 0;
+ virtual void updateStateBlockStatusDerived(const StateBlockPtr& state_ptr) = 0;
+ virtual void updateStateBlockLocalParametrizationDerived(const StateBlockPtr& state_ptr) = 0;
+
+ virtual bool isStateBlockRegisteredDerived(const StateBlockPtr& state_ptr) const = 0;
+ virtual bool isFactorRegisteredDerived(const FactorBasePtr& fac_ptr) const = 0;
+ virtual bool isStateBlockFixedDerived(const StateBlockPtr& st) = 0;
+ virtual bool hasLocalParametrizationDerived(const StateBlockPtr& st) const = 0;
+ virtual bool hasThisLocalParametrizationDerived(const StateBlockPtr& st,
+ const LocalParametrizationBasePtr& local_param) = 0;
+
+ virtual void printProfilingDerived(std::ostream& stream = std::cout) const = 0;
+ virtual bool checkDerived(std::string prefix = "") const = 0;
+
+ public:
+ virtual bool hasConverged() = 0;
+ virtual bool wasStopped() = 0;
+ virtual unsigned int iterations() = 0;
+ virtual double initialCost() = 0;
+ virtual double finalCost() = 0;
+ virtual double totalTime() = 0;
+
+ protected:
+ // PARAMS
+ double period_;
+ SolverManager::ReportVerbosity verbose_;
+ bool compute_cov_;
+ SolverManager::CovarianceBlocksToBeComputed cov_enum_;
+ double cov_period_;
};
-} // namespace wolf
+} // namespace wolf
diff --git a/include/core/solver_suitesparse/ccolamd_ordering.h b/include/core/solver_suitesparse/ccolamd_ordering.h
index 12bb7e698d5dfef5ee11c934344e1cb613d86f5c..7272b3a1b898afe2eeb02661a8c81ac308ff8383 100644
--- a/include/core/solver_suitesparse/ccolamd_ordering.h
+++ b/include/core/solver_suitesparse/ccolamd_ordering.h
@@ -20,7 +20,7 @@
#pragma once
-//std includes
+// std includes
#include <iostream>
// Eigen includes
@@ -31,60 +31,63 @@
// ccolamd
#include "ccolamd.h"
-namespace Eigen{
-
-template<typename Index>
+namespace Eigen
+{
+template <typename Index>
class CCOLAMDOrdering
{
- public:
- typedef PermutationMatrix<Dynamic, Dynamic, Index> PermutationType;
- typedef Matrix<Index, Dynamic, 1> IndexVector;
+ public:
+ typedef PermutationMatrix<Dynamic, Dynamic, Index> PermutationType;
+ typedef Matrix<Index, Dynamic, 1> IndexVector;
- template<typename MatrixType>
- void operator()(const MatrixType& mat, PermutationType& perm, Index* cmember = nullptr)
- {
- Index m = mat.rows();
- Index n = mat.cols();
- Index nnz = mat.nonZeros();
+ template <typename MatrixType>
+ void operator()(const MatrixType& mat, PermutationType& perm, Index* cmember = nullptr)
+ {
+ Index m = mat.rows();
+ Index n = mat.cols();
+ Index nnz = mat.nonZeros();
- // Get the recommended value of Alen to be used by colamd
- Index Alen = ccolamd_recommended(nnz, m, n);
- // Set the default parameters
- double knobs[CCOLAMD_KNOBS];
- Index stats[CCOLAMD_STATS];
- ccolamd_set_defaults(knobs);
+ // Get the recommended value of Alen to be used by colamd
+ Index Alen = ccolamd_recommended(nnz, m, n);
+ // Set the default parameters
+ double knobs[CCOLAMD_KNOBS];
+ Index stats[CCOLAMD_STATS];
+ ccolamd_set_defaults(knobs);
- IndexVector p(n + 1), A(Alen);
- for (Index i = 0; i <= n; i++)
- p(i) = mat.outerIndex()[i];
- for (Index i = 0; i < nnz; i++)
- A(i) = mat.innerIndex()[i];
-// std::cout << "p = " << std::endl << p.transpose() << std::endl;
-// std::cout << "A = " << std::endl << A.head(nnz).transpose() << std::endl;
+ IndexVector p(n + 1), A(Alen);
+ for (Index i = 0; i <= n; i++) p(i) = mat.outerIndex()[i];
+ for (Index i = 0; i < nnz; i++) A(i) = mat.innerIndex()[i];
+ // std::cout << "p = " << std::endl << p.transpose() << std::endl;
+ // std::cout << "A = " << std::endl << A.head(nnz).transpose() << std::endl;
- // Call CColamd routine to compute the ordering
- Index info = compute_ccolamd(m, n, Alen, A.data(), p.data(), knobs, stats, cmember);
- if (!info)
- assert(info && "CCOLAMD failed ");
+ // Call CColamd routine to compute the ordering
+ Index info = compute_ccolamd(m, n, Alen, A.data(), p.data(), knobs, stats, cmember);
+ if (!info) assert(info && "CCOLAMD failed ");
- perm.resize(n);
- for (Index i = 0; i < n; i++)
- perm.indices()(p(i)) = i;
- }
+ perm.resize(n);
+ for (Index i = 0; i < n; i++) perm.indices()(p(i)) = i;
+ }
- private:
- int compute_ccolamd(int &m, int &n, int &Alen, int* A, int* p, double* knobs, int* stats, int* cmember)
- {
- int info = ccolamd(m, n, Alen, A, p, knobs, stats, cmember);
- //ccolamd_report (stats) ;
- return info;
- }
+ private:
+ int compute_ccolamd(int& m, int& n, int& Alen, int* A, int* p, double* knobs, int* stats, int* cmember)
+ {
+ int info = ccolamd(m, n, Alen, A, p, knobs, stats, cmember);
+ // ccolamd_report (stats) ;
+ return info;
+ }
- long int compute_ccolamd(long int &m, long int &n, long int &Alen, long int* A, long int* p, double* knobs, long int* stats, long int* cmember)
- {
- long int info = ccolamd_l(m, n, Alen, A, p, knobs, stats, cmember);
- //ccolamd_l_report (stats) ;
- return info;
- }
+ long int compute_ccolamd(long int& m,
+ long int& n,
+ long int& Alen,
+ long int* A,
+ long int* p,
+ double* knobs,
+ long int* stats,
+ long int* cmember)
+ {
+ long int info = ccolamd_l(m, n, Alen, A, p, knobs, stats, cmember);
+ // ccolamd_l_report (stats) ;
+ return info;
+ }
};
-}
+} // namespace Eigen
diff --git a/include/core/solver_suitesparse/cost_function_base.h b/include/core/solver_suitesparse/cost_function_base.h
index 3668a5bf738311545fd14cc27fed85d8ec341524..75e62aba983ec2384183dd8d09fb3bc2e9458ccd 100644
--- a/include/core/solver_suitesparse/cost_function_base.h
+++ b/include/core/solver_suitesparse/cost_function_base.h
@@ -25,83 +25,88 @@
class CostFunctionBase
{
- protected:
- unsigned int n_blocks_;
- Eigen::MatrixXd J_0_;
- Eigen::MatrixXd J_1_;
- Eigen::MatrixXd J_2_;
- Eigen::MatrixXd J_3_;
- Eigen::MatrixXd J_4_;
- Eigen::MatrixXd J_5_;
- Eigen::MatrixXd J_6_;
- Eigen::MatrixXd J_7_;
- Eigen::MatrixXd J_8_;
- Eigen::MatrixXd J_9_;
- Eigen::VectorXd residual_;
- std::vector<Eigen::MatrixXd*> jacobians_;
- std::vector<unsigned int> block_sizes_;
+ protected:
+ unsigned int n_blocks_;
+ Eigen::MatrixXd J_0_;
+ Eigen::MatrixXd J_1_;
+ Eigen::MatrixXd J_2_;
+ Eigen::MatrixXd J_3_;
+ Eigen::MatrixXd J_4_;
+ Eigen::MatrixXd J_5_;
+ Eigen::MatrixXd J_6_;
+ Eigen::MatrixXd J_7_;
+ Eigen::MatrixXd J_8_;
+ Eigen::MatrixXd J_9_;
+ Eigen::VectorXd residual_;
+ std::vector<Eigen::MatrixXd *> jacobians_;
+ std::vector<unsigned int> block_sizes_;
- public:
- CostFunctionBase(const unsigned int &_measurement_size,
- const unsigned int &_block_0_size,
- const unsigned int &_block_1_size,
- const unsigned int &_block_2_size,
- const unsigned int &_block_3_size,
- const unsigned int &_block_4_size,
- const unsigned int &_block_5_size,
- const unsigned int &_block_6_size,
- const unsigned int &_block_7_size,
- const unsigned int &_block_8_size,
- const unsigned int &_block_9_size) :
- n_blocks_(10),
- J_0_(_measurement_size, _block_0_size),
- J_1_(_measurement_size, _block_1_size),
- J_2_(_measurement_size, _block_2_size),
- J_3_(_measurement_size, _block_3_size),
- J_4_(_measurement_size, _block_4_size),
- J_5_(_measurement_size, _block_5_size),
- J_6_(_measurement_size, _block_6_size),
- J_7_(_measurement_size, _block_7_size),
- J_8_(_measurement_size, _block_8_size),
- J_9_(_measurement_size, _block_9_size),
- residual_(_measurement_size),
- jacobians_({&J_0_,&J_1_,&J_2_,&J_3_,&J_4_,&J_5_,&J_6_,&J_7_,&J_8_,&J_9_}),
- block_sizes_({_block_0_size, _block_1_size, _block_2_size, _block_3_size, _block_4_size, _block_5_size, _block_6_size, _block_7_size, _block_8_size, _block_9_size})
+ public:
+ CostFunctionBase(const unsigned int &_measurement_size,
+ const unsigned int &_block_0_size,
+ const unsigned int &_block_1_size,
+ const unsigned int &_block_2_size,
+ const unsigned int &_block_3_size,
+ const unsigned int &_block_4_size,
+ const unsigned int &_block_5_size,
+ const unsigned int &_block_6_size,
+ const unsigned int &_block_7_size,
+ const unsigned int &_block_8_size,
+ const unsigned int &_block_9_size)
+ : n_blocks_(10),
+ J_0_(_measurement_size, _block_0_size),
+ J_1_(_measurement_size, _block_1_size),
+ J_2_(_measurement_size, _block_2_size),
+ J_3_(_measurement_size, _block_3_size),
+ J_4_(_measurement_size, _block_4_size),
+ J_5_(_measurement_size, _block_5_size),
+ J_6_(_measurement_size, _block_6_size),
+ J_7_(_measurement_size, _block_7_size),
+ J_8_(_measurement_size, _block_8_size),
+ J_9_(_measurement_size, _block_9_size),
+ residual_(_measurement_size),
+ jacobians_({&J_0_, &J_1_, &J_2_, &J_3_, &J_4_, &J_5_, &J_6_, &J_7_, &J_8_, &J_9_}),
+ block_sizes_({_block_0_size,
+ _block_1_size,
+ _block_2_size,
+ _block_3_size,
+ _block_4_size,
+ _block_5_size,
+ _block_6_size,
+ _block_7_size,
+ _block_8_size,
+ _block_9_size})
+ {
+ for (unsigned int i = 1; i < n_blocks_; i++)
+ {
+ if (block_sizes_.at(i) == 0)
{
- for (unsigned int i = 1; i<n_blocks_; i++)
- {
- if (block_sizes_.at(i) == 0)
- {
- n_blocks_ = i;
- jacobians_.resize(n_blocks_);
- block_sizes_.resize(n_blocks_);
- break;
- }
- }
+ n_blocks_ = i;
+ jacobians_.resize(n_blocks_);
+ block_sizes_.resize(n_blocks_);
+ break;
}
-
- virtual ~CostFunctionBase()
- {
-
}
+ }
- virtual void evaluateResidualJacobians() = 0;
+ virtual ~CostFunctionBase() {}
- void getResidual(Eigen::VectorXd &residual)
- {
- residual.resize(residual_.size());
- residual = residual_;
- }
+ virtual void evaluateResidualJacobians() = 0;
- std::vector<Eigen::MatrixXd*> getJacobians()
- {
- return jacobians_;
- }
+ void getResidual(Eigen::VectorXd &residual)
+ {
+ residual.resize(residual_.size());
+ residual = residual_;
+ }
- void getJacobians(std::vector<Eigen::MatrixXd>& jacobians)
- {
- jacobians.resize(n_blocks_);
- for (unsigned int i = 0; i<n_blocks_; i++)
- jacobians.at(i) = (*jacobians_.at(i));
- }
+ std::vector<Eigen::MatrixXd *> getJacobians()
+ {
+ return jacobians_;
+ }
+
+ void getJacobians(std::vector<Eigen::MatrixXd> &jacobians)
+ {
+ jacobians.resize(n_blocks_);
+ for (unsigned int i = 0; i < n_blocks_; i++) jacobians.at(i) = (*jacobians_.at(i));
+ }
};
diff --git a/include/core/solver_suitesparse/cost_function_sparse.h b/include/core/solver_suitesparse/cost_function_sparse.h
index 52d94c4c15016a1b20c66fa7ecd13cc4bc9af187..1c1c77f95d14c21d00d49e12cf531f45a7aab8b4 100644
--- a/include/core/solver_suitesparse/cost_function_sparse.h
+++ b/include/core/solver_suitesparse/cost_function_sparse.h
@@ -20,7 +20,7 @@
#pragma once
-//wolf includes
+// wolf includes
#include "core/common/wolf.h"
#include "cost_function_sparse_base.h"
@@ -29,100 +29,133 @@
namespace wolf
{
-
template <class FactorT,
const unsigned int MEASUREMENT_SIZE,
- unsigned int BLOCK_0_SIZE,
- unsigned int BLOCK_1_SIZE,
- unsigned int BLOCK_2_SIZE,
- unsigned int BLOCK_3_SIZE,
- unsigned int BLOCK_4_SIZE,
- unsigned int BLOCK_5_SIZE,
- unsigned int BLOCK_6_SIZE,
- unsigned int BLOCK_7_SIZE,
- unsigned int BLOCK_8_SIZE,
- unsigned int BLOCK_9_SIZE>
+ unsigned int BLOCK_0_SIZE,
+ unsigned int BLOCK_1_SIZE,
+ unsigned int BLOCK_2_SIZE,
+ unsigned int BLOCK_3_SIZE,
+ unsigned int BLOCK_4_SIZE,
+ unsigned int BLOCK_5_SIZE,
+ unsigned int BLOCK_6_SIZE,
+ unsigned int BLOCK_7_SIZE,
+ unsigned int BLOCK_8_SIZE,
+ unsigned int BLOCK_9_SIZE>
class CostFunctionSparse : CostFunctionSparseBase<FactorT,
- MEASUREMENT_SIZE,
- BLOCK_0_SIZE,
- BLOCK_1_SIZE,
- BLOCK_2_SIZE,
- BLOCK_3_SIZE,
- BLOCK_4_SIZE,
- BLOCK_5_SIZE,
- BLOCK_6_SIZE,
- BLOCK_7_SIZE,
- BLOCK_8_SIZE,
- BLOCK_9_SIZE>
+ MEASUREMENT_SIZE,
+ BLOCK_0_SIZE,
+ BLOCK_1_SIZE,
+ BLOCK_2_SIZE,
+ BLOCK_3_SIZE,
+ BLOCK_4_SIZE,
+ BLOCK_5_SIZE,
+ BLOCK_6_SIZE,
+ BLOCK_7_SIZE,
+ BLOCK_8_SIZE,
+ BLOCK_9_SIZE>
{
- public:
- CostFunctionSparse(FactorT* _factor_ptr) :
- CostFunctionSparse<FactorT,
- MEASUREMENT_SIZE,
- BLOCK_0_SIZE,
- BLOCK_1_SIZE,
- BLOCK_2_SIZE,
- BLOCK_3_SIZE,
- BLOCK_4_SIZE,
- BLOCK_5_SIZE,
- BLOCK_6_SIZE,
- BLOCK_7_SIZE,
- BLOCK_8_SIZE,
- BLOCK_9_SIZE>(_factor_ptr)
- {
-
- }
-
- void callFunctor()
- {
-// if (BLOCK_0_SIZE > 0 && BLOCK_1_SIZE > 0 && BLOCK_2_SIZE > 0 && BLOCK_3_SIZE > 0 && BLOCK_4_SIZE > 0 &&
-// BLOCK_5_SIZE > 0 && BLOCK_6_SIZE > 0 && BLOCK_7_SIZE > 0 && BLOCK_8_SIZE > 0 && BLOCK_9_SIZE > 0)
- (*this->factor_ptr_)(this->jets_0_, this->jets_1_, this->jets_2_, this->jets_3_, this->jets_4_, this->jets_5_, this->jets_6_, this->jets_7_, this->jets_8_ , this->jets_9_ ,this->residuals_jet_);
-// else if (BLOCK_0_SIZE > 0 && BLOCK_1_SIZE > 0 && BLOCK_2_SIZE > 0 && BLOCK_3_SIZE > 0 && BLOCK_4_SIZE > 0 &&
-// BLOCK_5_SIZE > 0 && BLOCK_6_SIZE > 0 && BLOCK_7_SIZE > 0 && BLOCK_8_SIZE > 0 && BLOCK_9_SIZE == 0)
-// (*this->factor_ptr_)(this->jets_0_, this->jets_1_, this->jets_2_, this->jets_3_, this->jets_4_, this->jets_5_, this->jets_6_, this->jets_7_, this->jets_8_ ,this->residuals_jet_);
-// else if (BLOCK_0_SIZE > 0 && BLOCK_1_SIZE > 0 && BLOCK_2_SIZE > 0 && BLOCK_3_SIZE > 0 && BLOCK_4_SIZE > 0 &&
-// BLOCK_5_SIZE > 0 && BLOCK_6_SIZE > 0 && BLOCK_7_SIZE > 0 && BLOCK_8_SIZE ==0 && BLOCK_9_SIZE == 0)
-// (*this->factor_ptr_)(this->jets_0_, this->jets_1_, this->jets_2_, this->jets_3_, this->jets_4_, this->jets_5_, this->jets_6_, this->jets_7_,this->residuals_jet_);
-// else if (BLOCK_0_SIZE > 0 && BLOCK_1_SIZE > 0 && BLOCK_2_SIZE > 0 && BLOCK_3_SIZE > 0 && BLOCK_4_SIZE > 0 &&
-// BLOCK_5_SIZE > 0 && BLOCK_6_SIZE > 0 && BLOCK_7_SIZE ==0 && BLOCK_8_SIZE ==0 && BLOCK_9_SIZE == 0)
-// (*this->factor_ptr_)(this->jets_0_, this->jets_1_, this->jets_2_, this->jets_3_, this->jets_4_, this->jets_5_, this->jets_6_,this->residuals_jet_);
-// else if (BLOCK_0_SIZE > 0 && BLOCK_1_SIZE > 0 && BLOCK_2_SIZE > 0 && BLOCK_3_SIZE > 0 && BLOCK_4_SIZE > 0 &&
-// BLOCK_5_SIZE > 0 && BLOCK_6_SIZE ==0 && BLOCK_7_SIZE ==0 && BLOCK_8_SIZE ==0 && BLOCK_9_SIZE == 0)
-// (*this->factor_ptr_)(this->jets_0_, this->jets_1_, this->jets_2_, this->jets_3_, this->jets_4_, this->jets_5_,this->residuals_jet_);
-// else if (BLOCK_0_SIZE > 0 && BLOCK_1_SIZE > 0 && BLOCK_2_SIZE > 0 && BLOCK_3_SIZE > 0 && BLOCK_4_SIZE > 0 &&
-// BLOCK_5_SIZE ==0 && BLOCK_6_SIZE ==0 && BLOCK_7_SIZE ==0 && BLOCK_8_SIZE ==0 && BLOCK_9_SIZE == 0)
-// (*this->factor_ptr_)(this->jets_0_, this->jets_1_, this->jets_2_, this->jets_3_, this->jets_4_, this->residuals_jet_);
-// else if (BLOCK_0_SIZE > 0 && BLOCK_1_SIZE > 0 && BLOCK_2_SIZE > 0 && BLOCK_3_SIZE > 0 && BLOCK_4_SIZE ==0 &&
-// BLOCK_5_SIZE ==0 && BLOCK_6_SIZE ==0 && BLOCK_7_SIZE ==0 && BLOCK_8_SIZE ==0 && BLOCK_9_SIZE == 0)
-// (*this->factor_ptr_)(this->jets_0_, this->jets_1_, this->jets_2_, this->jets_3_, this->residuals_jet_);
-// else if (BLOCK_0_SIZE > 0 && BLOCK_1_SIZE > 0 && BLOCK_2_SIZE > 0 && BLOCK_3_SIZE ==0 && BLOCK_4_SIZE ==0 &&
-// BLOCK_5_SIZE ==0 && BLOCK_6_SIZE ==0 && BLOCK_7_SIZE ==0 && BLOCK_8_SIZE ==0 && BLOCK_9_SIZE == 0)
-// (*this->factor_ptr_)(this->jets_0_, this->jets_1_, this->jets_2_, this->residuals_jet_);
-// else if (BLOCK_0_SIZE > 0 && BLOCK_1_SIZE > 0 && BLOCK_2_SIZE ==0 && BLOCK_3_SIZE ==0 && BLOCK_4_SIZE ==0 &&
-// BLOCK_5_SIZE ==0 && BLOCK_6_SIZE ==0 && BLOCK_7_SIZE ==0 && BLOCK_8_SIZE ==0 && BLOCK_9_SIZE == 0)
-// (*this->factor_ptr_)(this->jets_0_, this->jets_1_, this->residuals_jet_);
-// else if (BLOCK_0_SIZE > 0 && BLOCK_1_SIZE ==0 && BLOCK_2_SIZE ==0 && BLOCK_3_SIZE ==0 && BLOCK_4_SIZE ==0 &&
-// BLOCK_5_SIZE ==0 && BLOCK_6_SIZE ==0 && BLOCK_7_SIZE ==0 && BLOCK_8_SIZE ==0 && BLOCK_9_SIZE == 0)
-// (*this->factor_ptr_)(this->jets_0_, this->residuals_jet_);
-// else
-// assert("Wrong combination of zero sized blocks");
-
- }
-
+ public:
+ CostFunctionSparse(FactorT* _factor_ptr)
+ : CostFunctionSparse<FactorT,
+ MEASUREMENT_SIZE,
+ BLOCK_0_SIZE,
+ BLOCK_1_SIZE,
+ BLOCK_2_SIZE,
+ BLOCK_3_SIZE,
+ BLOCK_4_SIZE,
+ BLOCK_5_SIZE,
+ BLOCK_6_SIZE,
+ BLOCK_7_SIZE,
+ BLOCK_8_SIZE,
+ BLOCK_9_SIZE>(_factor_ptr)
+ {
+ }
+
+ void callFunctor()
+ {
+ // if (BLOCK_0_SIZE > 0 && BLOCK_1_SIZE > 0 && BLOCK_2_SIZE > 0 && BLOCK_3_SIZE > 0 && BLOCK_4_SIZE
+ // > 0 &&
+ // BLOCK_5_SIZE > 0 && BLOCK_6_SIZE > 0 && BLOCK_7_SIZE > 0 && BLOCK_8_SIZE > 0 && BLOCK_9_SIZE
+ // > 0)
+ (*this->factor_ptr_)(this->jets_0_,
+ this->jets_1_,
+ this->jets_2_,
+ this->jets_3_,
+ this->jets_4_,
+ this->jets_5_,
+ this->jets_6_,
+ this->jets_7_,
+ this->jets_8_,
+ this->jets_9_,
+ this->residuals_jet_);
+ // else if (BLOCK_0_SIZE > 0 && BLOCK_1_SIZE > 0 && BLOCK_2_SIZE > 0 && BLOCK_3_SIZE > 0 &&
+ // BLOCK_4_SIZE > 0 &&
+ // BLOCK_5_SIZE > 0 && BLOCK_6_SIZE > 0 && BLOCK_7_SIZE > 0 && BLOCK_8_SIZE > 0 &&
+ // BLOCK_9_SIZE == 0)
+ // (*this->factor_ptr_)(this->jets_0_, this->jets_1_, this->jets_2_, this->jets_3_,
+ // this->jets_4_, this->jets_5_, this->jets_6_, this->jets_7_, this->jets_8_
+ // ,this->residuals_jet_);
+ // else if (BLOCK_0_SIZE > 0 && BLOCK_1_SIZE > 0 && BLOCK_2_SIZE > 0 && BLOCK_3_SIZE > 0 &&
+ // BLOCK_4_SIZE > 0 &&
+ // BLOCK_5_SIZE > 0 && BLOCK_6_SIZE > 0 && BLOCK_7_SIZE > 0 && BLOCK_8_SIZE ==0 &&
+ // BLOCK_9_SIZE == 0)
+ // (*this->factor_ptr_)(this->jets_0_, this->jets_1_, this->jets_2_, this->jets_3_,
+ // this->jets_4_, this->jets_5_, this->jets_6_, this->jets_7_,this->residuals_jet_);
+ // else if (BLOCK_0_SIZE > 0 && BLOCK_1_SIZE > 0 && BLOCK_2_SIZE > 0 && BLOCK_3_SIZE > 0 &&
+ // BLOCK_4_SIZE > 0 &&
+ // BLOCK_5_SIZE > 0 && BLOCK_6_SIZE > 0 && BLOCK_7_SIZE ==0 && BLOCK_8_SIZE ==0 &&
+ // BLOCK_9_SIZE == 0)
+ // (*this->factor_ptr_)(this->jets_0_, this->jets_1_, this->jets_2_, this->jets_3_,
+ // this->jets_4_, this->jets_5_, this->jets_6_,this->residuals_jet_);
+ // else if (BLOCK_0_SIZE > 0 && BLOCK_1_SIZE > 0 && BLOCK_2_SIZE > 0 && BLOCK_3_SIZE > 0 &&
+ // BLOCK_4_SIZE > 0 &&
+ // BLOCK_5_SIZE > 0 && BLOCK_6_SIZE ==0 && BLOCK_7_SIZE ==0 && BLOCK_8_SIZE ==0 &&
+ // BLOCK_9_SIZE == 0)
+ // (*this->factor_ptr_)(this->jets_0_, this->jets_1_, this->jets_2_, this->jets_3_,
+ // this->jets_4_, this->jets_5_,this->residuals_jet_);
+ // else if (BLOCK_0_SIZE > 0 && BLOCK_1_SIZE > 0 && BLOCK_2_SIZE > 0 && BLOCK_3_SIZE > 0 &&
+ // BLOCK_4_SIZE > 0 &&
+ // BLOCK_5_SIZE ==0 && BLOCK_6_SIZE ==0 && BLOCK_7_SIZE ==0 && BLOCK_8_SIZE ==0 &&
+ // BLOCK_9_SIZE == 0)
+ // (*this->factor_ptr_)(this->jets_0_, this->jets_1_, this->jets_2_, this->jets_3_,
+ // this->jets_4_, this->residuals_jet_);
+ // else if (BLOCK_0_SIZE > 0 && BLOCK_1_SIZE > 0 && BLOCK_2_SIZE > 0 && BLOCK_3_SIZE > 0 &&
+ // BLOCK_4_SIZE ==0 &&
+ // BLOCK_5_SIZE ==0 && BLOCK_6_SIZE ==0 && BLOCK_7_SIZE ==0 && BLOCK_8_SIZE ==0 &&
+ // BLOCK_9_SIZE == 0)
+ // (*this->factor_ptr_)(this->jets_0_, this->jets_1_, this->jets_2_, this->jets_3_,
+ // this->residuals_jet_);
+ // else if (BLOCK_0_SIZE > 0 && BLOCK_1_SIZE > 0 && BLOCK_2_SIZE > 0 && BLOCK_3_SIZE ==0 &&
+ // BLOCK_4_SIZE ==0 &&
+ // BLOCK_5_SIZE ==0 && BLOCK_6_SIZE ==0 && BLOCK_7_SIZE ==0 && BLOCK_8_SIZE ==0 &&
+ // BLOCK_9_SIZE == 0)
+ // (*this->factor_ptr_)(this->jets_0_, this->jets_1_, this->jets_2_, this->residuals_jet_);
+ // else if (BLOCK_0_SIZE > 0 && BLOCK_1_SIZE > 0 && BLOCK_2_SIZE ==0 && BLOCK_3_SIZE ==0 &&
+ // BLOCK_4_SIZE ==0 &&
+ // BLOCK_5_SIZE ==0 && BLOCK_6_SIZE ==0 && BLOCK_7_SIZE ==0 && BLOCK_8_SIZE ==0 &&
+ // BLOCK_9_SIZE == 0)
+ // (*this->factor_ptr_)(this->jets_0_, this->jets_1_, this->residuals_jet_);
+ // else if (BLOCK_0_SIZE > 0 && BLOCK_1_SIZE ==0 && BLOCK_2_SIZE ==0 && BLOCK_3_SIZE ==0 &&
+ // BLOCK_4_SIZE ==0 &&
+ // BLOCK_5_SIZE ==0 && BLOCK_6_SIZE ==0 && BLOCK_7_SIZE ==0 && BLOCK_8_SIZE ==0 &&
+ // BLOCK_9_SIZE == 0)
+ // (*this->factor_ptr_)(this->jets_0_, this->residuals_jet_);
+ // else
+ // assert("Wrong combination of zero sized blocks");
+ }
};
template <class FactorT,
const unsigned int MEASUREMENT_SIZE,
- unsigned int BLOCK_0_SIZE,
- unsigned int BLOCK_1_SIZE,
- unsigned int BLOCK_2_SIZE,
- unsigned int BLOCK_3_SIZE,
- unsigned int BLOCK_4_SIZE,
- unsigned int BLOCK_5_SIZE,
- unsigned int BLOCK_6_SIZE,
- unsigned int BLOCK_7_SIZE,
- unsigned int BLOCK_8_SIZE>
+ unsigned int BLOCK_0_SIZE,
+ unsigned int BLOCK_1_SIZE,
+ unsigned int BLOCK_2_SIZE,
+ unsigned int BLOCK_3_SIZE,
+ unsigned int BLOCK_4_SIZE,
+ unsigned int BLOCK_5_SIZE,
+ unsigned int BLOCK_6_SIZE,
+ unsigned int BLOCK_7_SIZE,
+ unsigned int BLOCK_8_SIZE>
class CostFunctionSparse<FactorT,
MEASUREMENT_SIZE,
BLOCK_0_SIZE,
@@ -147,380 +180,343 @@ class CostFunctionSparse<FactorT,
BLOCK_8_SIZE,
0>
{
- public:
- CostFunctionSparse(FactorT* _factor_ptr) :
- CostFunctionSparseBase<FactorT,
- MEASUREMENT_SIZE,
- BLOCK_0_SIZE,
- BLOCK_1_SIZE,
- BLOCK_2_SIZE,
- BLOCK_3_SIZE,
- BLOCK_4_SIZE,
- BLOCK_5_SIZE,
- BLOCK_6_SIZE,
- BLOCK_7_SIZE,
- BLOCK_8_SIZE,
- 0>(_factor_ptr)
- {
-
- }
-
- void callFunctor()
- {
- (*this->factor_ptr_)(this->jets_0_, this->jets_1_, this->jets_2_, this->jets_3_, this->jets_4_, this->jets_5_, this->jets_6_, this->jets_7_, this->jets_8_ ,this->residuals_jet_);
- }
+ public:
+ CostFunctionSparse(FactorT* _factor_ptr)
+ : CostFunctionSparseBase<FactorT,
+ MEASUREMENT_SIZE,
+ BLOCK_0_SIZE,
+ BLOCK_1_SIZE,
+ BLOCK_2_SIZE,
+ BLOCK_3_SIZE,
+ BLOCK_4_SIZE,
+ BLOCK_5_SIZE,
+ BLOCK_6_SIZE,
+ BLOCK_7_SIZE,
+ BLOCK_8_SIZE,
+ 0>(_factor_ptr)
+ {
+ }
+
+ void callFunctor()
+ {
+ (*this->factor_ptr_)(this->jets_0_,
+ this->jets_1_,
+ this->jets_2_,
+ this->jets_3_,
+ this->jets_4_,
+ this->jets_5_,
+ this->jets_6_,
+ this->jets_7_,
+ this->jets_8_,
+ this->residuals_jet_);
+ }
};
template <class FactorT,
const unsigned int MEASUREMENT_SIZE,
- unsigned int BLOCK_0_SIZE,
- unsigned int BLOCK_1_SIZE,
- unsigned int BLOCK_2_SIZE,
- unsigned int BLOCK_3_SIZE,
- unsigned int BLOCK_4_SIZE,
- unsigned int BLOCK_5_SIZE,
- unsigned int BLOCK_6_SIZE,
- unsigned int BLOCK_7_SIZE>
+ unsigned int BLOCK_0_SIZE,
+ unsigned int BLOCK_1_SIZE,
+ unsigned int BLOCK_2_SIZE,
+ unsigned int BLOCK_3_SIZE,
+ unsigned int BLOCK_4_SIZE,
+ unsigned int BLOCK_5_SIZE,
+ unsigned int BLOCK_6_SIZE,
+ unsigned int BLOCK_7_SIZE>
class CostFunctionSparse<FactorT,
- MEASUREMENT_SIZE,
- BLOCK_0_SIZE,
- BLOCK_1_SIZE,
- BLOCK_2_SIZE,
- BLOCK_3_SIZE,
- BLOCK_4_SIZE,
- BLOCK_5_SIZE,
- BLOCK_6_SIZE,
- BLOCK_7_SIZE,
- 0,
- 0> : CostFunctionSparseBase<FactorT,
- MEASUREMENT_SIZE,
- BLOCK_0_SIZE,
- BLOCK_1_SIZE,
- BLOCK_2_SIZE,
- BLOCK_3_SIZE,
- BLOCK_4_SIZE,
- BLOCK_5_SIZE,
- BLOCK_6_SIZE,
- BLOCK_7_SIZE,
- 0,
- 0>
+ MEASUREMENT_SIZE,
+ BLOCK_0_SIZE,
+ BLOCK_1_SIZE,
+ BLOCK_2_SIZE,
+ BLOCK_3_SIZE,
+ BLOCK_4_SIZE,
+ BLOCK_5_SIZE,
+ BLOCK_6_SIZE,
+ BLOCK_7_SIZE,
+ 0,
+ 0> : CostFunctionSparseBase<FactorT,
+ MEASUREMENT_SIZE,
+ BLOCK_0_SIZE,
+ BLOCK_1_SIZE,
+ BLOCK_2_SIZE,
+ BLOCK_3_SIZE,
+ BLOCK_4_SIZE,
+ BLOCK_5_SIZE,
+ BLOCK_6_SIZE,
+ BLOCK_7_SIZE,
+ 0,
+ 0>
{
- public:
- CostFunctionSparse(FactorT* _factor_ptr) :
- CostFunctionSparseBase<FactorT,
- MEASUREMENT_SIZE,
- BLOCK_0_SIZE,
- BLOCK_1_SIZE,
- BLOCK_2_SIZE,
- BLOCK_3_SIZE,
- BLOCK_4_SIZE,
- BLOCK_5_SIZE,
- BLOCK_6_SIZE,
- BLOCK_7_SIZE,
- 0,
- 0>(_factor_ptr)
- {
-
- }
-
- void callFunctor()
- {
- (*this->factor_ptr_)(this->jets_0_, this->jets_1_, this->jets_2_, this->jets_3_, this->jets_4_, this->jets_5_, this->jets_6_, this->jets_7_,this->residuals_jet_);
- }
+ public:
+ CostFunctionSparse(FactorT* _factor_ptr)
+ : CostFunctionSparseBase<FactorT,
+ MEASUREMENT_SIZE,
+ BLOCK_0_SIZE,
+ BLOCK_1_SIZE,
+ BLOCK_2_SIZE,
+ BLOCK_3_SIZE,
+ BLOCK_4_SIZE,
+ BLOCK_5_SIZE,
+ BLOCK_6_SIZE,
+ BLOCK_7_SIZE,
+ 0,
+ 0>(_factor_ptr)
+ {
+ }
+
+ void callFunctor()
+ {
+ (*this->factor_ptr_)(this->jets_0_,
+ this->jets_1_,
+ this->jets_2_,
+ this->jets_3_,
+ this->jets_4_,
+ this->jets_5_,
+ this->jets_6_,
+ this->jets_7_,
+ this->residuals_jet_);
+ }
};
template <class FactorT,
const unsigned int MEASUREMENT_SIZE,
- unsigned int BLOCK_0_SIZE,
- unsigned int BLOCK_1_SIZE,
- unsigned int BLOCK_2_SIZE,
- unsigned int BLOCK_3_SIZE,
- unsigned int BLOCK_4_SIZE,
- unsigned int BLOCK_5_SIZE,
- unsigned int BLOCK_6_SIZE>
+ unsigned int BLOCK_0_SIZE,
+ unsigned int BLOCK_1_SIZE,
+ unsigned int BLOCK_2_SIZE,
+ unsigned int BLOCK_3_SIZE,
+ unsigned int BLOCK_4_SIZE,
+ unsigned int BLOCK_5_SIZE,
+ unsigned int BLOCK_6_SIZE>
class CostFunctionSparse<FactorT,
- MEASUREMENT_SIZE,
- BLOCK_0_SIZE,
- BLOCK_1_SIZE,
- BLOCK_2_SIZE,
- BLOCK_3_SIZE,
- BLOCK_4_SIZE,
- BLOCK_5_SIZE,
- BLOCK_6_SIZE,
- 0,
- 0> : CostFunctionSparseBase<FactorT,
- MEASUREMENT_SIZE,
- BLOCK_0_SIZE,
- BLOCK_1_SIZE,
- BLOCK_2_SIZE,
- BLOCK_3_SIZE,
- BLOCK_4_SIZE,
- BLOCK_5_SIZE,
- BLOCK_6_SIZE,
- 0,
- 0>
+ MEASUREMENT_SIZE,
+ BLOCK_0_SIZE,
+ BLOCK_1_SIZE,
+ BLOCK_2_SIZE,
+ BLOCK_3_SIZE,
+ BLOCK_4_SIZE,
+ BLOCK_5_SIZE,
+ BLOCK_6_SIZE,
+ 0,
+ 0> : CostFunctionSparseBase<FactorT,
+ MEASUREMENT_SIZE,
+ BLOCK_0_SIZE,
+ BLOCK_1_SIZE,
+ BLOCK_2_SIZE,
+ BLOCK_3_SIZE,
+ BLOCK_4_SIZE,
+ BLOCK_5_SIZE,
+ BLOCK_6_SIZE,
+ 0,
+ 0>
{
- public:
- CostFunctionSparse(FactorT* _factor_ptr) :
- CostFunctionSparseBase<FactorT,
- MEASUREMENT_SIZE,
- BLOCK_0_SIZE,
- BLOCK_1_SIZE,
- BLOCK_2_SIZE,
- BLOCK_3_SIZE,
- BLOCK_4_SIZE,
- BLOCK_5_SIZE,
- BLOCK_6_SIZE,
- 0,
- 0>(_factor_ptr)
- {
-
- }
-
- void callFunctor()
- {
- (*this->factor_ptr_)(this->jets_0_, this->jets_1_, this->jets_2_, this->jets_3_, this->jets_4_, this->jets_5_, this->jets_6_,this->residuals_jet_);
- }
+ public:
+ CostFunctionSparse(FactorT* _factor_ptr)
+ : CostFunctionSparseBase<FactorT,
+ MEASUREMENT_SIZE,
+ BLOCK_0_SIZE,
+ BLOCK_1_SIZE,
+ BLOCK_2_SIZE,
+ BLOCK_3_SIZE,
+ BLOCK_4_SIZE,
+ BLOCK_5_SIZE,
+ BLOCK_6_SIZE,
+ 0,
+ 0>(_factor_ptr)
+ {
+ }
+
+ void callFunctor()
+ {
+ (*this->factor_ptr_)(this->jets_0_,
+ this->jets_1_,
+ this->jets_2_,
+ this->jets_3_,
+ this->jets_4_,
+ this->jets_5_,
+ this->jets_6_,
+ this->residuals_jet_);
+ }
};
template <class FactorT,
const unsigned int MEASUREMENT_SIZE,
- unsigned int BLOCK_0_SIZE,
- unsigned int BLOCK_1_SIZE,
- unsigned int BLOCK_2_SIZE,
- unsigned int BLOCK_3_SIZE,
- unsigned int BLOCK_4_SIZE,
- unsigned int BLOCK_5_SIZE>
+ unsigned int BLOCK_0_SIZE,
+ unsigned int BLOCK_1_SIZE,
+ unsigned int BLOCK_2_SIZE,
+ unsigned int BLOCK_3_SIZE,
+ unsigned int BLOCK_4_SIZE,
+ unsigned int BLOCK_5_SIZE>
class CostFunctionSparse<FactorT,
- MEASUREMENT_SIZE,
- BLOCK_0_SIZE,
- BLOCK_1_SIZE,
- BLOCK_2_SIZE,
- BLOCK_3_SIZE,
- BLOCK_4_SIZE,
- BLOCK_5_SIZE,
- 0,
- 0> : CostFunctionSparseBase<FactorT,
- MEASUREMENT_SIZE,
- BLOCK_0_SIZE,
- BLOCK_1_SIZE,
- BLOCK_2_SIZE,
- BLOCK_3_SIZE,
- BLOCK_4_SIZE,
- BLOCK_5_SIZE,
- 0,
- 0>
+ MEASUREMENT_SIZE,
+ BLOCK_0_SIZE,
+ BLOCK_1_SIZE,
+ BLOCK_2_SIZE,
+ BLOCK_3_SIZE,
+ BLOCK_4_SIZE,
+ BLOCK_5_SIZE,
+ 0,
+ 0> : CostFunctionSparseBase<FactorT,
+ MEASUREMENT_SIZE,
+ BLOCK_0_SIZE,
+ BLOCK_1_SIZE,
+ BLOCK_2_SIZE,
+ BLOCK_3_SIZE,
+ BLOCK_4_SIZE,
+ BLOCK_5_SIZE,
+ 0,
+ 0>
{
- public:
- CostFunctionSparse(FactorT* _factor_ptr) :
- CostFunctionSparseBase<FactorT,
- MEASUREMENT_SIZE,
- BLOCK_0_SIZE,
- BLOCK_1_SIZE,
- BLOCK_2_SIZE,
- BLOCK_3_SIZE,
- BLOCK_4_SIZE,
- BLOCK_5_SIZE,
- 0,
- 0>(_factor_ptr)
- {
-
- }
-
- void callFunctor()
- {
- (*this->factor_ptr_)(this->jets_0_, this->jets_1_, this->jets_2_, this->jets_3_, this->jets_4_, this->jets_5_,this->residuals_jet_);
- }
+ public:
+ CostFunctionSparse(FactorT* _factor_ptr)
+ : CostFunctionSparseBase<FactorT,
+ MEASUREMENT_SIZE,
+ BLOCK_0_SIZE,
+ BLOCK_1_SIZE,
+ BLOCK_2_SIZE,
+ BLOCK_3_SIZE,
+ BLOCK_4_SIZE,
+ BLOCK_5_SIZE,
+ 0,
+ 0>(_factor_ptr)
+ {
+ }
+
+ void callFunctor()
+ {
+ (*this->factor_ptr_)(this->jets_0_,
+ this->jets_1_,
+ this->jets_2_,
+ this->jets_3_,
+ this->jets_4_,
+ this->jets_5_,
+ this->residuals_jet_);
+ }
};
template <class FactorT,
const unsigned int MEASUREMENT_SIZE,
- unsigned int BLOCK_0_SIZE,
- unsigned int BLOCK_1_SIZE,
- unsigned int BLOCK_2_SIZE,
- unsigned int BLOCK_3_SIZE,
- unsigned int BLOCK_4_SIZE>
+ unsigned int BLOCK_0_SIZE,
+ unsigned int BLOCK_1_SIZE,
+ unsigned int BLOCK_2_SIZE,
+ unsigned int BLOCK_3_SIZE,
+ unsigned int BLOCK_4_SIZE>
class CostFunctionSparse<FactorT,
- MEASUREMENT_SIZE,
- BLOCK_0_SIZE,
- BLOCK_1_SIZE,
- BLOCK_2_SIZE,
- BLOCK_3_SIZE,
- BLOCK_4_SIZE,
- 0,
- 0> : CostFunctionSparseBase<FactorT,
- MEASUREMENT_SIZE,
- BLOCK_0_SIZE,
- BLOCK_1_SIZE,
- BLOCK_2_SIZE,
- BLOCK_3_SIZE,
- BLOCK_4_SIZE,
- 0,
- 0>
+ MEASUREMENT_SIZE,
+ BLOCK_0_SIZE,
+ BLOCK_1_SIZE,
+ BLOCK_2_SIZE,
+ BLOCK_3_SIZE,
+ BLOCK_4_SIZE,
+ 0,
+ 0> : CostFunctionSparseBase<FactorT,
+ MEASUREMENT_SIZE,
+ BLOCK_0_SIZE,
+ BLOCK_1_SIZE,
+ BLOCK_2_SIZE,
+ BLOCK_3_SIZE,
+ BLOCK_4_SIZE,
+ 0,
+ 0>
{
- public:
- CostFunctionSparse(FactorT* _factor_ptr) :
- CostFunctionSparseBase<FactorT,
- MEASUREMENT_SIZE,
- BLOCK_0_SIZE,
- BLOCK_1_SIZE,
- BLOCK_2_SIZE,
- BLOCK_3_SIZE,
- BLOCK_4_SIZE,
- 0,
- 0>(_factor_ptr)
- {
-
- }
-
- void callFunctor()
- {
- (*this->factor_ptr_)(this->jets_0_, this->jets_1_, this->jets_2_, this->jets_3_, this->jets_4_,this->residuals_jet_);
- }
+ public:
+ CostFunctionSparse(FactorT* _factor_ptr)
+ : CostFunctionSparseBase<FactorT,
+ MEASUREMENT_SIZE,
+ BLOCK_0_SIZE,
+ BLOCK_1_SIZE,
+ BLOCK_2_SIZE,
+ BLOCK_3_SIZE,
+ BLOCK_4_SIZE,
+ 0,
+ 0>(_factor_ptr)
+ {
+ }
+
+ void callFunctor()
+ {
+ (*this->factor_ptr_)(
+ this->jets_0_, this->jets_1_, this->jets_2_, this->jets_3_, this->jets_4_, this->residuals_jet_);
+ }
};
template <class FactorT,
const unsigned int MEASUREMENT_SIZE,
- unsigned int BLOCK_0_SIZE,
- unsigned int BLOCK_1_SIZE,
- unsigned int BLOCK_2_SIZE,
- unsigned int BLOCK_3_SIZE>
-class CostFunctionSparse<FactorT,
- MEASUREMENT_SIZE,
- BLOCK_0_SIZE,
- BLOCK_1_SIZE,
- BLOCK_2_SIZE,
- BLOCK_3_SIZE,
- 0,
- 0> : CostFunctionSparseBase<FactorT,
- MEASUREMENT_SIZE,
- BLOCK_0_SIZE,
- BLOCK_1_SIZE,
- BLOCK_2_SIZE,
- BLOCK_3_SIZE,
- 0,
- 0>
+ unsigned int BLOCK_0_SIZE,
+ unsigned int BLOCK_1_SIZE,
+ unsigned int BLOCK_2_SIZE,
+ unsigned int BLOCK_3_SIZE>
+class CostFunctionSparse<FactorT, MEASUREMENT_SIZE, BLOCK_0_SIZE, BLOCK_1_SIZE, BLOCK_2_SIZE, BLOCK_3_SIZE, 0, 0>
+ : CostFunctionSparseBase<FactorT, MEASUREMENT_SIZE, BLOCK_0_SIZE, BLOCK_1_SIZE, BLOCK_2_SIZE, BLOCK_3_SIZE, 0, 0>
{
- public:
- CostFunctionSparse(FactorT* _factor_ptr) :
- CostFunctionSparseBase<FactorT,
- MEASUREMENT_SIZE,
- BLOCK_0_SIZE,
- BLOCK_1_SIZE,
- BLOCK_2_SIZE,
- BLOCK_3_SIZE,
- 0,
- 0>(_factor_ptr)
- {
-
- }
-
- void callFunctor()
- {
- (*this->factor_ptr_)(this->jets_0_, this->jets_1_, this->jets_2_, this->jets_3_,this->residuals_jet_);
- }
+ public:
+ CostFunctionSparse(FactorT* _factor_ptr)
+ : CostFunctionSparseBase<FactorT,
+ MEASUREMENT_SIZE,
+ BLOCK_0_SIZE,
+ BLOCK_1_SIZE,
+ BLOCK_2_SIZE,
+ BLOCK_3_SIZE,
+ 0,
+ 0>(_factor_ptr)
+ {
+ }
+
+ void callFunctor()
+ {
+ (*this->factor_ptr_)(this->jets_0_, this->jets_1_, this->jets_2_, this->jets_3_, this->residuals_jet_);
+ }
};
template <class FactorT,
const unsigned int MEASUREMENT_SIZE,
- unsigned int BLOCK_0_SIZE,
- unsigned int BLOCK_1_SIZE,
- unsigned int BLOCK_2_SIZE>
-class CostFunctionSparse<FactorT,
- MEASUREMENT_SIZE,
- BLOCK_0_SIZE,
- BLOCK_1_SIZE,
- BLOCK_2_SIZE,
- 0,
- 0> : CostFunctionSparseBase<FactorT,
- MEASUREMENT_SIZE,
- BLOCK_0_SIZE,
- BLOCK_1_SIZE,
- BLOCK_2_SIZE,
- 0,
- 0>
+ unsigned int BLOCK_0_SIZE,
+ unsigned int BLOCK_1_SIZE,
+ unsigned int BLOCK_2_SIZE>
+class CostFunctionSparse<FactorT, MEASUREMENT_SIZE, BLOCK_0_SIZE, BLOCK_1_SIZE, BLOCK_2_SIZE, 0, 0>
+ : CostFunctionSparseBase<FactorT, MEASUREMENT_SIZE, BLOCK_0_SIZE, BLOCK_1_SIZE, BLOCK_2_SIZE, 0, 0>
{
- public:
- CostFunctionSparse(FactorT* _factor_ptr) :
- CostFunctionSparseBase<FactorT,
- MEASUREMENT_SIZE,
- BLOCK_0_SIZE,
- BLOCK_1_SIZE,
- BLOCK_2_SIZE,
- 0,
- 0>(_factor_ptr)
- {
-
- }
-
- void callFunctor()
- {
- (*this->factor_ptr_)(this->jets_0_, this->jets_1_, this->jets_2_,this->residuals_jet_);
- }
+ public:
+ CostFunctionSparse(FactorT* _factor_ptr)
+ : CostFunctionSparseBase<FactorT, MEASUREMENT_SIZE, BLOCK_0_SIZE, BLOCK_1_SIZE, BLOCK_2_SIZE, 0, 0>(
+ _factor_ptr)
+ {
+ }
+
+ void callFunctor()
+ {
+ (*this->factor_ptr_)(this->jets_0_, this->jets_1_, this->jets_2_, this->residuals_jet_);
+ }
};
-template <class FactorT,
- const unsigned int MEASUREMENT_SIZE,
- unsigned int BLOCK_0_SIZE,
- unsigned int BLOCK_1_SIZE>
-class CostFunctionSparse<FactorT,
- MEASUREMENT_SIZE,
- BLOCK_0_SIZE,
- BLOCK_1_SIZE,
- 0,
- 0> : CostFunctionSparseBase<FactorT,
- MEASUREMENT_SIZE,
- BLOCK_0_SIZE,
- BLOCK_1_SIZE,
- 0,
- 0>
+template <class FactorT, const unsigned int MEASUREMENT_SIZE, unsigned int BLOCK_0_SIZE, unsigned int BLOCK_1_SIZE>
+class CostFunctionSparse<FactorT, MEASUREMENT_SIZE, BLOCK_0_SIZE, BLOCK_1_SIZE, 0, 0>
+ : CostFunctionSparseBase<FactorT, MEASUREMENT_SIZE, BLOCK_0_SIZE, BLOCK_1_SIZE, 0, 0>
{
- public:
- CostFunctionSparse(FactorT* _factor_ptr) :
- CostFunctionSparseBase<FactorT,
- MEASUREMENT_SIZE,
- BLOCK_0_SIZE,
- BLOCK_1_SIZE,
- 0,
- 0>(_factor_ptr)
- {
-
- }
-
- void callFunctor()
- {
- (*this->factor_ptr_)(this->jets_0_, this->jets_1_,this->residuals_jet_);
- }
+ public:
+ CostFunctionSparse(FactorT* _factor_ptr)
+ : CostFunctionSparseBase<FactorT, MEASUREMENT_SIZE, BLOCK_0_SIZE, BLOCK_1_SIZE, 0, 0>(_factor_ptr)
+ {
+ }
+
+ void callFunctor()
+ {
+ (*this->factor_ptr_)(this->jets_0_, this->jets_1_, this->residuals_jet_);
+ }
};
-template <class FactorT,
- const unsigned int MEASUREMENT_SIZE,
- unsigned int BLOCK_0_SIZE>
-class CostFunctionSparse<FactorT,
- MEASUREMENT_SIZE,
- BLOCK_0_SIZE,
- 0,
- 0> : CostFunctionSparseBase<FactorT,
- MEASUREMENT_SIZE,
- BLOCK_0_SIZE,
- 0,
- 0>
+template <class FactorT, const unsigned int MEASUREMENT_SIZE, unsigned int BLOCK_0_SIZE>
+class CostFunctionSparse<FactorT, MEASUREMENT_SIZE, BLOCK_0_SIZE, 0, 0>
+ : CostFunctionSparseBase<FactorT, MEASUREMENT_SIZE, BLOCK_0_SIZE, 0, 0>
{
- public:
- CostFunctionSparse(FactorT* _factor_ptr) :
- CostFunctionSparseBase<FactorT,
- MEASUREMENT_SIZE,
- BLOCK_0_SIZE,
- 0,
- 0>(_factor_ptr)
- {
-
- }
-
- void callFunctor()
- {
- (*this->factor_ptr_)(this->jets_0_,this->residuals_jet_);
- }
+ public:
+ CostFunctionSparse(FactorT* _factor_ptr)
+ : CostFunctionSparseBase<FactorT, MEASUREMENT_SIZE, BLOCK_0_SIZE, 0, 0>(_factor_ptr)
+ {
+ }
+
+ void callFunctor()
+ {
+ (*this->factor_ptr_)(this->jets_0_, this->residuals_jet_);
+ }
};
-} //namespace wolf
+} // namespace wolf
diff --git a/include/core/solver_suitesparse/cost_function_sparse_base.h b/include/core/solver_suitesparse/cost_function_sparse_base.h
index 26e9dc23eebf57ab6b953ee599fffc613263a847..4983d82c1b9d2ec82fe39e35ae8b8f53fda17206 100644
--- a/include/core/solver_suitesparse/cost_function_sparse_base.h
+++ b/include/core/solver_suitesparse/cost_function_sparse_base.h
@@ -20,7 +20,7 @@
#pragma once
-//wolf includes
+// wolf includes
#include "core/common/wolf.h"
#include "cost_function_base.h"
@@ -29,174 +29,175 @@
namespace wolf
{
-
template <class FactorT,
const unsigned int MEASUREMENT_SIZE,
- unsigned int BLOCK_0_SIZE,
- unsigned int BLOCK_1_SIZE = 0,
- unsigned int BLOCK_2_SIZE = 0,
- unsigned int BLOCK_3_SIZE = 0,
- unsigned int BLOCK_4_SIZE = 0,
- unsigned int BLOCK_5_SIZE = 0,
- unsigned int BLOCK_6_SIZE = 0,
- unsigned int BLOCK_7_SIZE = 0,
- unsigned int BLOCK_8_SIZE = 0,
- unsigned int BLOCK_9_SIZE = 0>
+ unsigned int BLOCK_0_SIZE,
+ unsigned int BLOCK_1_SIZE = 0,
+ unsigned int BLOCK_2_SIZE = 0,
+ unsigned int BLOCK_3_SIZE = 0,
+ unsigned int BLOCK_4_SIZE = 0,
+ unsigned int BLOCK_5_SIZE = 0,
+ unsigned int BLOCK_6_SIZE = 0,
+ unsigned int BLOCK_7_SIZE = 0,
+ unsigned int BLOCK_8_SIZE = 0,
+ unsigned int BLOCK_9_SIZE = 0>
class CostFunctionSparseBase : CostFunctionBase
{
- typedef ceres::Jet<double, BLOCK_0_SIZE +
- BLOCK_1_SIZE +
- BLOCK_2_SIZE +
- BLOCK_3_SIZE +
- BLOCK_4_SIZE +
- BLOCK_5_SIZE +
- BLOCK_6_SIZE +
- BLOCK_7_SIZE +
- BLOCK_8_SIZE +
- BLOCK_9_SIZE> WolfJet;
- protected:
- FactorT* factor_ptr_;
- WolfJet jets_0_[BLOCK_0_SIZE];
- WolfJet jets_1_[BLOCK_1_SIZE];
- WolfJet jets_2_[BLOCK_2_SIZE];
- WolfJet jets_3_[BLOCK_3_SIZE];
- WolfJet jets_4_[BLOCK_4_SIZE];
- WolfJet jets_5_[BLOCK_5_SIZE];
- WolfJet jets_6_[BLOCK_6_SIZE];
- WolfJet jets_7_[BLOCK_7_SIZE];
- WolfJet jets_8_[BLOCK_8_SIZE];
- WolfJet jets_9_[BLOCK_9_SIZE];
- WolfJet residuals_jet_[MEASUREMENT_SIZE];
-
- public:
+ typedef ceres::Jet<double,
+ BLOCK_0_SIZE + BLOCK_1_SIZE + BLOCK_2_SIZE + BLOCK_3_SIZE + BLOCK_4_SIZE + BLOCK_5_SIZE +
+ BLOCK_6_SIZE + BLOCK_7_SIZE + BLOCK_8_SIZE + BLOCK_9_SIZE>
+ WolfJet;
- /** \brief Constructor with factor pointer
- *
- * Constructor with factor pointer
- *
- */
- CostFunctionSparseBase(FactorT* _factor_ptr);
+ protected:
+ FactorT* factor_ptr_;
+ WolfJet jets_0_[BLOCK_0_SIZE];
+ WolfJet jets_1_[BLOCK_1_SIZE];
+ WolfJet jets_2_[BLOCK_2_SIZE];
+ WolfJet jets_3_[BLOCK_3_SIZE];
+ WolfJet jets_4_[BLOCK_4_SIZE];
+ WolfJet jets_5_[BLOCK_5_SIZE];
+ WolfJet jets_6_[BLOCK_6_SIZE];
+ WolfJet jets_7_[BLOCK_7_SIZE];
+ WolfJet jets_8_[BLOCK_8_SIZE];
+ WolfJet jets_9_[BLOCK_9_SIZE];
+ WolfJet residuals_jet_[MEASUREMENT_SIZE];
- /** \brief Default destructor
- *
- * Default destructor
- *
- */
- virtual ~CostFunctionSparseBase();
+ public:
+ /** \brief Constructor with factor pointer
+ *
+ * Constructor with factor pointer
+ *
+ */
+ CostFunctionSparseBase(FactorT* _factor_ptr);
- /** \brief Evaluate residuals and jacobians of the factor in the current x
- *
- * Evaluate residuals and jacobians of the factor in the current x
- *
- */
- virtual void evaluateResidualJacobians();
+ /** \brief Default destructor
+ *
+ * Default destructor
+ *
+ */
+ virtual ~CostFunctionSparseBase();
- protected:
+ /** \brief Evaluate residuals and jacobians of the factor in the current x
+ *
+ * Evaluate residuals and jacobians of the factor in the current x
+ *
+ */
+ virtual void evaluateResidualJacobians();
- /** \brief Calls the functor of the factor evaluating jets
- *
- * Calls the functor of the factor evaluating jets
- *
- */
- virtual void callFunctor() = 0;
+ protected:
+ /** \brief Calls the functor of the factor evaluating jets
+ *
+ * Calls the functor of the factor evaluating jets
+ *
+ */
+ virtual void callFunctor() = 0;
- /** \brief Initialize the infinitesimal part of jets
- *
- * Initialize the infinitesimal part of jets with zeros and ones
- *
- */
- void initializeJets();
+ /** \brief Initialize the infinitesimal part of jets
+ *
+ * Initialize the infinitesimal part of jets with zeros and ones
+ *
+ */
+ void initializeJets();
- /** \brief Gets the evaluation point
- *
- * Gets the evaluation point from the state
- *
- */
- void evaluateX();
+ /** \brief Gets the evaluation point
+ *
+ * Gets the evaluation point from the state
+ *
+ */
+ void evaluateX();
};
template <class FactorT,
const unsigned int MEASUREMENT_SIZE,
- unsigned int BLOCK_0_SIZE,
- unsigned int BLOCK_1_SIZE,
- unsigned int BLOCK_2_SIZE,
- unsigned int BLOCK_3_SIZE,
- unsigned int BLOCK_4_SIZE,
- unsigned int BLOCK_5_SIZE,
- unsigned int BLOCK_6_SIZE,
- unsigned int BLOCK_7_SIZE,
- unsigned int BLOCK_8_SIZE,
- unsigned int BLOCK_9_SIZE>
+ unsigned int BLOCK_0_SIZE,
+ unsigned int BLOCK_1_SIZE,
+ unsigned int BLOCK_2_SIZE,
+ unsigned int BLOCK_3_SIZE,
+ unsigned int BLOCK_4_SIZE,
+ unsigned int BLOCK_5_SIZE,
+ unsigned int BLOCK_6_SIZE,
+ unsigned int BLOCK_7_SIZE,
+ unsigned int BLOCK_8_SIZE,
+ unsigned int BLOCK_9_SIZE>
CostFunctionSparseBase<FactorT,
- MEASUREMENT_SIZE,
- BLOCK_0_SIZE,
- BLOCK_1_SIZE,
- BLOCK_2_SIZE,
- BLOCK_3_SIZE,
- BLOCK_4_SIZE,
- BLOCK_5_SIZE,
- BLOCK_6_SIZE,
- BLOCK_7_SIZE,
- BLOCK_8_SIZE,
- BLOCK_9_SIZE>::CostFunctionSparseBase(FactorT* _factor_ptr) :
- CostFunctionBase(MEASUREMENT_SIZE, BLOCK_0_SIZE, BLOCK_1_SIZE, BLOCK_2_SIZE, BLOCK_3_SIZE, BLOCK_4_SIZE, BLOCK_5_SIZE, BLOCK_6_SIZE, BLOCK_7_SIZE, BLOCK_8_SIZE,BLOCK_9_SIZE),
- factor_ptr_(_factor_ptr)
+ MEASUREMENT_SIZE,
+ BLOCK_0_SIZE,
+ BLOCK_1_SIZE,
+ BLOCK_2_SIZE,
+ BLOCK_3_SIZE,
+ BLOCK_4_SIZE,
+ BLOCK_5_SIZE,
+ BLOCK_6_SIZE,
+ BLOCK_7_SIZE,
+ BLOCK_8_SIZE,
+ BLOCK_9_SIZE>::CostFunctionSparseBase(FactorT* _factor_ptr)
+ : CostFunctionBase(MEASUREMENT_SIZE,
+ BLOCK_0_SIZE,
+ BLOCK_1_SIZE,
+ BLOCK_2_SIZE,
+ BLOCK_3_SIZE,
+ BLOCK_4_SIZE,
+ BLOCK_5_SIZE,
+ BLOCK_6_SIZE,
+ BLOCK_7_SIZE,
+ BLOCK_8_SIZE,
+ BLOCK_9_SIZE),
+ factor_ptr_(_factor_ptr)
{
initializeJets();
}
template <class FactorT,
const unsigned int MEASUREMENT_SIZE,
- unsigned int BLOCK_0_SIZE,
- unsigned int BLOCK_1_SIZE,
- unsigned int BLOCK_2_SIZE,
- unsigned int BLOCK_3_SIZE,
- unsigned int BLOCK_4_SIZE,
- unsigned int BLOCK_5_SIZE,
- unsigned int BLOCK_6_SIZE,
- unsigned int BLOCK_7_SIZE,
- unsigned int BLOCK_8_SIZE,
- unsigned int BLOCK_9_SIZE>
+ unsigned int BLOCK_0_SIZE,
+ unsigned int BLOCK_1_SIZE,
+ unsigned int BLOCK_2_SIZE,
+ unsigned int BLOCK_3_SIZE,
+ unsigned int BLOCK_4_SIZE,
+ unsigned int BLOCK_5_SIZE,
+ unsigned int BLOCK_6_SIZE,
+ unsigned int BLOCK_7_SIZE,
+ unsigned int BLOCK_8_SIZE,
+ unsigned int BLOCK_9_SIZE>
CostFunctionSparseBase<FactorT,
- MEASUREMENT_SIZE,
- BLOCK_0_SIZE,
- BLOCK_1_SIZE,
- BLOCK_2_SIZE,
- BLOCK_3_SIZE,
- BLOCK_4_SIZE,
- BLOCK_5_SIZE,
- BLOCK_6_SIZE,
- BLOCK_7_SIZE,
- BLOCK_8_SIZE,
- BLOCK_9_SIZE>::~CostFunctionSparseBase()
+ MEASUREMENT_SIZE,
+ BLOCK_0_SIZE,
+ BLOCK_1_SIZE,
+ BLOCK_2_SIZE,
+ BLOCK_3_SIZE,
+ BLOCK_4_SIZE,
+ BLOCK_5_SIZE,
+ BLOCK_6_SIZE,
+ BLOCK_7_SIZE,
+ BLOCK_8_SIZE,
+ BLOCK_9_SIZE>::~CostFunctionSparseBase()
{
-
}
template <class FactorT,
const unsigned int MEASUREMENT_SIZE,
- unsigned int BLOCK_0_SIZE,
- unsigned int BLOCK_1_SIZE,
- unsigned int BLOCK_2_SIZE,
- unsigned int BLOCK_3_SIZE,
- unsigned int BLOCK_4_SIZE,
- unsigned int BLOCK_5_SIZE,
- unsigned int BLOCK_6_SIZE,
- unsigned int BLOCK_7_SIZE,
- unsigned int BLOCK_8_SIZE,
- unsigned int BLOCK_9_SIZE>
+ unsigned int BLOCK_0_SIZE,
+ unsigned int BLOCK_1_SIZE,
+ unsigned int BLOCK_2_SIZE,
+ unsigned int BLOCK_3_SIZE,
+ unsigned int BLOCK_4_SIZE,
+ unsigned int BLOCK_5_SIZE,
+ unsigned int BLOCK_6_SIZE,
+ unsigned int BLOCK_7_SIZE,
+ unsigned int BLOCK_8_SIZE,
+ unsigned int BLOCK_9_SIZE>
void CostFunctionSparseBase<FactorT,
- MEASUREMENT_SIZE,
- BLOCK_0_SIZE,
- BLOCK_1_SIZE,
- BLOCK_2_SIZE,
- BLOCK_3_SIZE,
- BLOCK_4_SIZE,
- BLOCK_5_SIZE,
- BLOCK_6_SIZE,
- BLOCK_7_SIZE,
- BLOCK_8_SIZE,
- BLOCK_9_SIZE>::evaluateResidualJacobians()
+ MEASUREMENT_SIZE,
+ BLOCK_0_SIZE,
+ BLOCK_1_SIZE,
+ BLOCK_2_SIZE,
+ BLOCK_3_SIZE,
+ BLOCK_4_SIZE,
+ BLOCK_5_SIZE,
+ BLOCK_6_SIZE,
+ BLOCK_7_SIZE,
+ BLOCK_8_SIZE,
+ BLOCK_9_SIZE>::evaluateResidualJacobians()
{
evaluateX();
@@ -204,7 +205,7 @@ void CostFunctionSparseBase<FactorT,
// fill the jacobian matrices
int jacobian_location = 0;
- for (int i = 0; i<n_blocks_; i++)
+ for (int i = 0; i < n_blocks_; i++)
{
for (int row = 0; row < MEASUREMENT_SIZE; row++)
jacobians_.at(i)->row(row) = residuals_jet_[row].v.segment(jacobian_location, block_sizes_.at(i));
@@ -213,123 +214,102 @@ void CostFunctionSparseBase<FactorT,
}
// fill the residual vector
- for (int i = 0; i < MEASUREMENT_SIZE; i++)
- residual_(i) = residuals_jet_[i].a;
+ for (int i = 0; i < MEASUREMENT_SIZE; i++) residual_(i) = residuals_jet_[i].a;
}
template <class FactorT,
-const unsigned int MEASUREMENT_SIZE,
- unsigned int BLOCK_0_SIZE,
- unsigned int BLOCK_1_SIZE,
- unsigned int BLOCK_2_SIZE,
- unsigned int BLOCK_3_SIZE,
- unsigned int BLOCK_4_SIZE,
- unsigned int BLOCK_5_SIZE,
- unsigned int BLOCK_6_SIZE,
- unsigned int BLOCK_7_SIZE,
- unsigned int BLOCK_8_SIZE,
- unsigned int BLOCK_9_SIZE>
- void CostFunctionSparseBase<FactorT,
- MEASUREMENT_SIZE,
- BLOCK_0_SIZE,
- BLOCK_1_SIZE,
- BLOCK_2_SIZE,
- BLOCK_3_SIZE,
- BLOCK_4_SIZE,
- BLOCK_5_SIZE,
- BLOCK_6_SIZE,
- BLOCK_7_SIZE,
- BLOCK_8_SIZE,
- BLOCK_9_SIZE>::initializeJets()
+ const unsigned int MEASUREMENT_SIZE,
+ unsigned int BLOCK_0_SIZE,
+ unsigned int BLOCK_1_SIZE,
+ unsigned int BLOCK_2_SIZE,
+ unsigned int BLOCK_3_SIZE,
+ unsigned int BLOCK_4_SIZE,
+ unsigned int BLOCK_5_SIZE,
+ unsigned int BLOCK_6_SIZE,
+ unsigned int BLOCK_7_SIZE,
+ unsigned int BLOCK_8_SIZE,
+ unsigned int BLOCK_9_SIZE>
+void CostFunctionSparseBase<FactorT,
+ MEASUREMENT_SIZE,
+ BLOCK_0_SIZE,
+ BLOCK_1_SIZE,
+ BLOCK_2_SIZE,
+ BLOCK_3_SIZE,
+ BLOCK_4_SIZE,
+ BLOCK_5_SIZE,
+ BLOCK_6_SIZE,
+ BLOCK_7_SIZE,
+ BLOCK_8_SIZE,
+ BLOCK_9_SIZE>::initializeJets()
{
int last_jet_idx = 0;
// JET 0
- for (int i = 0; i < BLOCK_0_SIZE; i++)
- jets_0_[i] = WolfJet(0, last_jet_idx++);
+ for (int i = 0; i < BLOCK_0_SIZE; i++) jets_0_[i] = WolfJet(0, last_jet_idx++);
// JET 1
- for (int i = 0; i < BLOCK_1_SIZE; i++)
- jets_1_[i] = WolfJet(0, last_jet_idx++);
+ for (int i = 0; i < BLOCK_1_SIZE; i++) jets_1_[i] = WolfJet(0, last_jet_idx++);
// JET 2
- for (int i = 0; i < BLOCK_2_SIZE; i++)
- jets_2_[i] = WolfJet(0, last_jet_idx++);
+ for (int i = 0; i < BLOCK_2_SIZE; i++) jets_2_[i] = WolfJet(0, last_jet_idx++);
// JET 3
- for (int i = 0; i < BLOCK_3_SIZE; i++)
- jets_3_[i] = WolfJet(0, last_jet_idx++);
+ for (int i = 0; i < BLOCK_3_SIZE; i++) jets_3_[i] = WolfJet(0, last_jet_idx++);
// JET 4
- for (int i = 0; i < BLOCK_4_SIZE; i++)
- jets_4_[i] = WolfJet(0, last_jet_idx++);
+ for (int i = 0; i < BLOCK_4_SIZE; i++) jets_4_[i] = WolfJet(0, last_jet_idx++);
// JET 5
- for (int i = 0; i < BLOCK_5_SIZE; i++)
- jets_5_[i] = WolfJet(0, last_jet_idx++);
+ for (int i = 0; i < BLOCK_5_SIZE; i++) jets_5_[i] = WolfJet(0, last_jet_idx++);
// JET 6
- for (int i = 0; i < BLOCK_6_SIZE; i++)
- jets_6_[i] = WolfJet(0, last_jet_idx++);
+ for (int i = 0; i < BLOCK_6_SIZE; i++) jets_6_[i] = WolfJet(0, last_jet_idx++);
// JET 7
- for (int i = 0; i < BLOCK_7_SIZE; i++)
- jets_7_[i] = WolfJet(0, last_jet_idx++);
+ for (int i = 0; i < BLOCK_7_SIZE; i++) jets_7_[i] = WolfJet(0, last_jet_idx++);
// JET 8
- for (int i = 0; i < BLOCK_8_SIZE; i++)
- jets_8_[i] = WolfJet(0, last_jet_idx++);
+ for (int i = 0; i < BLOCK_8_SIZE; i++) jets_8_[i] = WolfJet(0, last_jet_idx++);
// JET 9
- for (int i = 0; i < BLOCK_9_SIZE; i++)
- jets_9_[i] = WolfJet(0, last_jet_idx++);
+ for (int i = 0; i < BLOCK_9_SIZE; i++) jets_9_[i] = WolfJet(0, last_jet_idx++);
}
template <class FactorT,
-const unsigned int MEASUREMENT_SIZE,
- unsigned int BLOCK_0_SIZE,
- unsigned int BLOCK_1_SIZE,
- unsigned int BLOCK_2_SIZE,
- unsigned int BLOCK_3_SIZE,
- unsigned int BLOCK_4_SIZE,
- unsigned int BLOCK_5_SIZE,
- unsigned int BLOCK_6_SIZE,
- unsigned int BLOCK_7_SIZE,
- unsigned int BLOCK_8_SIZE,
- unsigned int BLOCK_9_SIZE>
- void CostFunctionSparseBase<FactorT,
- MEASUREMENT_SIZE,
- BLOCK_0_SIZE,
- BLOCK_1_SIZE,
- BLOCK_2_SIZE,
- BLOCK_3_SIZE,
- BLOCK_4_SIZE,
- BLOCK_5_SIZE,
- BLOCK_6_SIZE,
- BLOCK_7_SIZE,
- BLOCK_8_SIZE,
- BLOCK_9_SIZE>::evaluateX()
+ const unsigned int MEASUREMENT_SIZE,
+ unsigned int BLOCK_0_SIZE,
+ unsigned int BLOCK_1_SIZE,
+ unsigned int BLOCK_2_SIZE,
+ unsigned int BLOCK_3_SIZE,
+ unsigned int BLOCK_4_SIZE,
+ unsigned int BLOCK_5_SIZE,
+ unsigned int BLOCK_6_SIZE,
+ unsigned int BLOCK_7_SIZE,
+ unsigned int BLOCK_8_SIZE,
+ unsigned int BLOCK_9_SIZE>
+void CostFunctionSparseBase<FactorT,
+ MEASUREMENT_SIZE,
+ BLOCK_0_SIZE,
+ BLOCK_1_SIZE,
+ BLOCK_2_SIZE,
+ BLOCK_3_SIZE,
+ BLOCK_4_SIZE,
+ BLOCK_5_SIZE,
+ BLOCK_6_SIZE,
+ BLOCK_7_SIZE,
+ BLOCK_8_SIZE,
+ BLOCK_9_SIZE>::evaluateX()
{
// JET 0
- for (int i = 0; i < BLOCK_0_SIZE; i++)
- jets_0_[i].a = *(factor_ptr_->getStateScalarPtrVector().at(0)+i);
+ for (int i = 0; i < BLOCK_0_SIZE; i++) jets_0_[i].a = *(factor_ptr_->getStateScalarPtrVector().at(0) + i);
// JET 1
- for (int i = 0; i < BLOCK_1_SIZE; i++)
- jets_1_[i].a = *(factor_ptr_->getStateScalarPtrVector().at(1)+i);
+ for (int i = 0; i < BLOCK_1_SIZE; i++) jets_1_[i].a = *(factor_ptr_->getStateScalarPtrVector().at(1) + i);
// JET 2
- for (int i = 0; i < BLOCK_2_SIZE; i++)
- jets_2_[i].a = *(factor_ptr_->getStateScalarPtrVector().at(2)+i);
+ for (int i = 0; i < BLOCK_2_SIZE; i++) jets_2_[i].a = *(factor_ptr_->getStateScalarPtrVector().at(2) + i);
// JET 3
- for (int i = 0; i < BLOCK_3_SIZE; i++)
- jets_3_[i].a = *(factor_ptr_->getStateScalarPtrVector().at(3)+i);
+ for (int i = 0; i < BLOCK_3_SIZE; i++) jets_3_[i].a = *(factor_ptr_->getStateScalarPtrVector().at(3) + i);
// JET 4
- for (int i = 0; i < BLOCK_4_SIZE; i++)
- jets_4_[i].a = *(factor_ptr_->getStateScalarPtrVector().at(4)+i);
+ for (int i = 0; i < BLOCK_4_SIZE; i++) jets_4_[i].a = *(factor_ptr_->getStateScalarPtrVector().at(4) + i);
// JET 5
- for (int i = 0; i < BLOCK_5_SIZE; i++)
- jets_5_[i].a = *(factor_ptr_->getStateScalarPtrVector().at(5)+i);
+ for (int i = 0; i < BLOCK_5_SIZE; i++) jets_5_[i].a = *(factor_ptr_->getStateScalarPtrVector().at(5) + i);
// JET 6
- for (int i = 0; i < BLOCK_6_SIZE; i++)
- jets_6_[i].a = *(factor_ptr_->getStateScalarPtrVector().at(6)+i);
+ for (int i = 0; i < BLOCK_6_SIZE; i++) jets_6_[i].a = *(factor_ptr_->getStateScalarPtrVector().at(6) + i);
// JET 7
- for (int i = 0; i < BLOCK_7_SIZE; i++)
- jets_7_[i].a = *(factor_ptr_->getStateScalarPtrVector().at(7)+i);
+ for (int i = 0; i < BLOCK_7_SIZE; i++) jets_7_[i].a = *(factor_ptr_->getStateScalarPtrVector().at(7) + i);
// JET 8
- for (int i = 0; i < BLOCK_8_SIZE; i++)
- jets_8_[i].a = *(factor_ptr_->getStateScalarPtrVector().at(8)+i);
+ for (int i = 0; i < BLOCK_8_SIZE; i++) jets_8_[i].a = *(factor_ptr_->getStateScalarPtrVector().at(8) + i);
// JET 9
- for (int i = 0; i < BLOCK_9_SIZE; i++)
- jets_9_[i].a = *(factor_ptr_->getStateScalarPtrVector().at(9)+i);
+ for (int i = 0; i < BLOCK_9_SIZE; i++) jets_9_[i].a = *(factor_ptr_->getStateScalarPtrVector().at(9) + i);
}
-} // wolf namespace
+} // namespace wolf
diff --git a/include/core/solver_suitesparse/qr_solver.h b/include/core/solver_suitesparse/qr_solver.h
index 40bae47e7fda25fba87e1bf7961707de96c313e2..a1ca79663e69f5f3daafb187590d1e6603b2686f 100644
--- a/include/core/solver_suitesparse/qr_solver.h
+++ b/include/core/solver_suitesparse/qr_solver.h
@@ -20,12 +20,12 @@
#pragma once
-//std includes
+// std includes
#include <core/factor/factor_odom_2d.h>
#include <iostream>
#include <ctime>
-//Wolf includes
+// Wolf includes
#include "core/state_block/state_block.h"
#include "../factor_sparse.h"
#include "core/factor/factor_corner_2d.h"
@@ -44,561 +44,583 @@
namespace wolf
{
-
class SolverQR
{
- protected:
- ProblemPtr problem_ptr_;
- Eigen::SparseQR<Eigen::SparseMatrix<double>, Eigen::NaturalOrdering<int>> solver_;
- Eigen::SparseMatrix<double> A_, R_;
- Eigen::VectorXd b_, x_incr_;
- std::vector<StateBlockPtr> nodes_;
- std::vector<FactorBasePtr> factors_;
- std::vector<CostFunctionBasePtr> cost_functions_;
-
- // ordering
- Eigen::SparseMatrix<int> A_nodes_;
- Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic, int> acc_node_permutation_;
- std::map<double*, unsigned int> id_2_idx_;
- Eigen::CCOLAMDOrdering<int> orderer_;
- Eigen::VectorXi node_ordering_restrictions_;
- Eigen::ArrayXi node_locations_;
- std::vector<unsigned int> factor_locations_;
- unsigned int n_new_factors_;
-
- // time
- clock_t t_ordering_, t_solving_, t_managing_;
- double time_ordering_, time_solving_, time_managing_;
-
- public:
- SolverQR(ProblemPtr problem_ptr_) :
- problem_ptr_(problem_ptr_), A_(0, 0), R_(0, 0), A_nodes_(0, 0), acc_node_permutation_(0), n_new_factors_(
- 0), time_ordering_(0), time_solving_(0), time_managing_(0)
+ protected:
+ ProblemPtr problem_ptr_;
+ Eigen::SparseQR<Eigen::SparseMatrix<double>, Eigen::NaturalOrdering<int>> solver_;
+ Eigen::SparseMatrix<double> A_, R_;
+ Eigen::VectorXd b_, x_incr_;
+ std::vector<StateBlockPtr> nodes_;
+ std::vector<FactorBasePtr> factors_;
+ std::vector<CostFunctionBasePtr> cost_functions_;
+
+ // ordering
+ Eigen::SparseMatrix<int> A_nodes_;
+ Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic, int> acc_node_permutation_;
+ std::map<double *, unsigned int> id_2_idx_;
+ Eigen::CCOLAMDOrdering<int> orderer_;
+ Eigen::VectorXi node_ordering_restrictions_;
+ Eigen::ArrayXi node_locations_;
+ std::vector<unsigned int> factor_locations_;
+ unsigned int n_new_factors_;
+
+ // time
+ clock_t t_ordering_, t_solving_, t_managing_;
+ double time_ordering_, time_solving_, time_managing_;
+
+ public:
+ SolverQR(ProblemPtr problem_ptr_)
+ : problem_ptr_(problem_ptr_),
+ A_(0, 0),
+ R_(0, 0),
+ A_nodes_(0, 0),
+ acc_node_permutation_(0),
+ n_new_factors_(0),
+ time_ordering_(0),
+ time_solving_(0),
+ time_managing_(0)
+ {
+ node_locations_.resize(0);
+ factor_locations_.resize(0);
+ }
+
+ virtual ~SolverQR() {}
+
+ void update()
+ {
+ // UPDATE STATE BLOCKS
+ while (!problem_ptr_->getStateBlockNotificationList().empty())
{
- node_locations_.resize(0);
- factor_locations_.resize(0);
- }
-
- virtual ~SolverQR()
- {
-
- }
-
- void update()
- {
- // UPDATE STATE BLOCKS
- while (!problem_ptr_->getStateBlockNotificationList().empty())
+ switch (problem_ptr_->getStateBlockNotificationList().front().notification_)
{
- switch (problem_ptr_->getStateBlockNotificationList().front().notification_)
- {
- case ADD:
- {
- addStateBlock(problem_ptr_->getStateBlockNotificationList().front().state_block_ptr_);
- break;
- }
- case UPDATE:
- {
- updateStateBlockStatus(problem_ptr_->getStateBlockNotificationList().front().state_block_ptr_);
- break;
- }
- case REMOVE:
- {
- // TODO removeStateBlock((double *)(problem_ptr_->getStateBlockNotificationList().front().scalar_ptr_));
- break;
- }
- default:
- throw std::runtime_error("SolverQR::update: State Block notification must be ADD, UPATE or REMOVE.");
+ case ADD: {
+ addStateBlock(problem_ptr_->getStateBlockNotificationList().front().state_block_ptr_);
+ break;
+ }
+ case UPDATE: {
+ updateStateBlockStatus(problem_ptr_->getStateBlockNotificationList().front().state_block_ptr_);
+ break;
}
- problem_ptr_->getStateBlockNotificationList().pop_front();
+ case REMOVE: {
+ // TODO removeStateBlock((double
+ // *)(problem_ptr_->getStateBlockNotificationList().front().scalar_ptr_));
+ break;
+ }
+ default:
+ throw std::runtime_error(
+ "SolverQR::update: State Block notification must be ADD, UPATE or REMOVE.");
}
- // UPDATE FACTORS
- while (!problem_ptr_->getFactorNotificationList().empty())
+ problem_ptr_->getStateBlockNotificationList().pop_front();
+ }
+ // UPDATE FACTORS
+ while (!problem_ptr_->getFactorNotificationList().empty())
+ {
+ switch (problem_ptr_->getFactorNotificationList().front().notification_)
{
- switch (problem_ptr_->getFactorNotificationList().front().notification_)
- {
- case ADD:
- {
- addFactor(problem_ptr_->getFactorNotificationList().front().factor_ptr_);
- break;
- }
- case REMOVE:
- {
- // TODO: removeFactor(problem_ptr_->getFactorNotificationList().front().id_);
- break;
- }
- default:
- throw std::runtime_error("SolverQR::update: Factor notification must be ADD or REMOVE.");
+ case ADD: {
+ addFactor(problem_ptr_->getFactorNotificationList().front().factor_ptr_);
+ break;
+ }
+ case REMOVE: {
+ // TODO: removeFactor(problem_ptr_->getFactorNotificationList().front().id_);
+ break;
}
- problem_ptr_->getFactorNotificationList().pop_front();
+ default:
+ throw std::runtime_error("SolverQR::update: Factor notification must be ADD or REMOVE.");
}
+ problem_ptr_->getFactorNotificationList().pop_front();
}
+ }
+
+ void addStateBlock(StateBlockPtr _state_ptr)
+ {
+ t_managing_ = clock();
- void addStateBlock(StateBlockPtr _state_ptr)
+ std::cout << "adding state unit " << _state_ptr->get() << std::endl;
+ if (!_state_ptr->isFixed())
{
- t_managing_ = clock();
+ nodes_.push_back(_state_ptr);
+ id_2_idx_[_state_ptr->get()] = nodes_.size() - 1;
- std::cout << "adding state unit " << _state_ptr->get() << std::endl;
- if (!_state_ptr->isFixed())
- {
- nodes_.push_back(_state_ptr);
- id_2_idx_[_state_ptr->get()] = nodes_.size() - 1;
+ std::cout << "idx " << id_2_idx_[_state_ptr->get()] << std::endl;
+
+ // Resize accumulated permutations
+ augmentPermutation(acc_node_permutation_, nNodes());
- std::cout << "idx " << id_2_idx_[_state_ptr->get()] << std::endl;
+ // Resize state
+ x_incr_.conservativeResize(x_incr_.size() + _state_ptr->getSize());
- // Resize accumulated permutations
- augmentPermutation(acc_node_permutation_, nNodes());
+ // Resize problem
+ A_.conservativeResize(A_.rows(), A_.cols() + _state_ptr->getSize());
+ R_.conservativeResize(R_.cols() + _state_ptr->getSize(), R_.cols() + _state_ptr->getSize());
+ }
+ time_managing_ += ((double)clock() - t_managing_) / CLOCKS_PER_SEC;
+ }
- // Resize state
- x_incr_.conservativeResize(x_incr_.size() + _state_ptr->getSize());
+ void updateStateBlockStatus(StateBlockPtr _state_ptr)
+ {
+ // TODO
+ }
- // Resize problem
- A_.conservativeResize(A_.rows(), A_.cols() + _state_ptr->getSize());
- R_.conservativeResize(R_.cols() + _state_ptr->getSize(), R_.cols() + _state_ptr->getSize());
+ void addFactor(FactorBasePtr _factor_ptr)
+ {
+ std::cout << "adding factor " << _factor_ptr->id() << std::endl;
+ t_managing_ = clock();
- }
- time_managing_ += ((double)clock() - t_managing_) / CLOCKS_PER_SEC;
- }
+ factors_.push_back(_factor_ptr);
+ cost_functions_.push_back(createCostFunction(_factor_ptr));
- void updateStateBlockStatus(StateBlockPtr _state_ptr)
- {
- //TODO
- }
+ unsigned int meas_dim = _factor_ptr->getSize();
- void addFactor(FactorBasePtr _factor_ptr)
- {
- std::cout << "adding factor " << _factor_ptr->id() << std::endl;
- t_managing_ = clock();
+ std::vector<Eigen::MatrixXd> jacobians(_factor_ptr->getStateBlockPtrVector().size());
+ Eigen::VectorXd error(meas_dim);
- factors_.push_back(_factor_ptr);
- cost_functions_.push_back(createCostFunction(_factor_ptr));
+ cost_functions_.back()->evaluateResidualJacobians();
+ cost_functions_.back()->getResidual(error);
+ cost_functions_.back()->getJacobians(jacobians);
- unsigned int meas_dim = _factor_ptr->getSize();
+ std::vector<unsigned int> idxs;
+ for (unsigned int i = 0; i < _factor_ptr->getStateBlockPtrVector().size(); i++)
+ if (!_factor_ptr->getStateBlockPtrVector().at(i)->isFixed())
+ idxs.push_back(id_2_idx_[_factor_ptr->getStateBlockPtrVector().at(i)->get()]);
- std::vector<Eigen::MatrixXd> jacobians(_factor_ptr->getStateBlockPtrVector().size());
- Eigen::VectorXd error(meas_dim);
+ n_new_factors_++;
+ factor_locations_.push_back(A_.rows());
- cost_functions_.back()->evaluateResidualJacobians();
- cost_functions_.back()->getResidual(error);
- cost_functions_.back()->getJacobians(jacobians);
+ // Resize problem
+ A_.conservativeResize(A_.rows() + meas_dim, A_.cols());
+ b_.conservativeResize(b_.size() + meas_dim);
+ A_nodes_.conservativeResize(factors_.size(), nNodes());
- std::vector<unsigned int> idxs;
- for (unsigned int i = 0; i < _factor_ptr->getStateBlockPtrVector().size(); i++)
- if (!_factor_ptr->getStateBlockPtrVector().at(i)->isFixed())
- idxs.push_back(id_2_idx_[_factor_ptr->getStateBlockPtrVector().at(i)->get()]);
+ // ADD MEASUREMENTS
+ for (unsigned int j = 0; j < idxs.size(); j++)
+ {
+ assert((unsigned int)(acc_node_permutation_.indices()(idxs.at(j))) == nodeOrder(idxs.at(j)));
+ assert(jacobians.at(j).cols() == nodeDim(idxs.at(j)));
+ assert(jacobians.at(j).rows() == meas_dim);
- n_new_factors_++;
- factor_locations_.push_back(A_.rows());
+ addSparseBlock(jacobians.at(j), A_, A_.rows() - meas_dim, nodeLocation(idxs.at(j)));
- // Resize problem
- A_.conservativeResize(A_.rows() + meas_dim, A_.cols());
- b_.conservativeResize(b_.size() + meas_dim);
- A_nodes_.conservativeResize(factors_.size(), nNodes());
+ A_nodes_.coeffRef(A_nodes_.rows() - 1, nodeOrder(idxs.at(j))) = 1;
+ }
- // ADD MEASUREMENTS
- for (unsigned int j = 0; j < idxs.size(); j++)
- {
- assert((unsigned int )(acc_node_permutation_.indices()(idxs.at(j))) == nodeOrder(idxs.at(j)));
- assert(jacobians.at(j).cols() == nodeDim(idxs.at(j)));
- assert(jacobians.at(j).rows() == meas_dim);
+ // error
+ b_.tail(meas_dim) = error;
- addSparseBlock(jacobians.at(j), A_, A_.rows() - meas_dim, nodeLocation(idxs.at(j)));
+ time_managing_ += ((double)clock() - t_managing_) / CLOCKS_PER_SEC;
+ }
- A_nodes_.coeffRef(A_nodes_.rows() - 1, nodeOrder(idxs.at(j))) = 1;
- }
+ void ordering(const int &_first_ordered_idx)
+ {
+ std::cout << "ordering from idx " << _first_ordered_idx << std::endl;
+ t_ordering_ = clock();
+
+ // full problem ordering
+ if (_first_ordered_idx == -1)
+ {
+ // ordering ordering factors
+ node_ordering_restrictions_.resize(nNodes());
+ node_ordering_restrictions_ = A_nodes_.bottomRows(1).transpose();
- // error
- b_.tail(meas_dim) = error;
+ // computing nodes partial ordering_
+ A_nodes_.makeCompressed();
+ Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic, int> incr_permutation_nodes(nNodes());
+ orderer_(A_nodes_, incr_permutation_nodes, node_ordering_restrictions_.data());
- time_managing_ += ((double)clock() - t_managing_) / CLOCKS_PER_SEC;
+ // node ordering to variable ordering
+ Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic, int> incr_permutation(A_.cols());
+ nodePermutation2VariablesPermutation(incr_permutation_nodes, incr_permutation);
+
+ // apply partial_ordering orderings
+ A_nodes_ = (A_nodes_ * incr_permutation_nodes.transpose()).sparseView();
+ A_ = (A_ * incr_permutation.transpose()).sparseView();
+
+ // ACCUMULATING PERMUTATIONS
+ accumulatePermutation(incr_permutation_nodes);
}
- void ordering(const int & _first_ordered_idx)
+ // partial ordering
+ else
{
- std::cout << "ordering from idx " << _first_ordered_idx << std::endl;
- t_ordering_ = clock();
+ unsigned int first_ordered_node = nodeOrder(_first_ordered_idx); // nodes_.at(_first_ordered_idx).order;
+ unsigned int ordered_nodes = nNodes() - first_ordered_node;
- // full problem ordering
- if (_first_ordered_idx == -1)
+ if (ordered_nodes > 2) // only reordering when involved nodes in the measurement are not the two last ones
{
- // ordering ordering factors
- node_ordering_restrictions_.resize(nNodes());
- node_ordering_restrictions_ = A_nodes_.bottomRows(1).transpose();
+ // SUBPROBLEM ORDERING (from first node variable to last one)
+ // std::cout << "ordering partial_ordering problem: " << _first_ordered_node << " to "<< n_nodes_ - 1
+ // << std::endl;
+ Eigen::SparseMatrix<int> sub_A_nodes_ = A_nodes_.rightCols(ordered_nodes);
+
+ // _partial_ordering ordering_ factors
+ node_ordering_restrictions_.resize(ordered_nodes);
+ node_ordering_restrictions_ = sub_A_nodes_.bottomRows(1).transpose();
// computing nodes partial ordering_
- A_nodes_.makeCompressed();
- Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic, int> incr_permutation_nodes(nNodes());
- orderer_(A_nodes_, incr_permutation_nodes, node_ordering_restrictions_.data());
+ sub_A_nodes_.makeCompressed();
+ Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic, int> partial_permutation_nodes(ordered_nodes);
+ orderer_(sub_A_nodes_, partial_permutation_nodes, node_ordering_restrictions_.data());
// node ordering to variable ordering
- Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic, int> incr_permutation(A_.cols());
- nodePermutation2VariablesPermutation(incr_permutation_nodes, incr_permutation);
+ unsigned int ordered_variables =
+ A_.cols() - nodeLocation(_first_ordered_idx); // nodes_.at(_first_ordered_idx).location;
+ // std::cout << "first_ordered_node " << first_ordered_node << std::endl;
+ // std::cout << "A_.cols() " << A_.cols() << std::endl;
+ // std::cout << "nodes_.at(_first_ordered_idx).location " <<
+ // nodes_.at(_first_ordered_idx).location << std::endl; std::cout <<
+ // "ordered_variables " << ordered_variables << std::endl;
+ Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic, int> partial_permutation(ordered_variables);
+ nodePermutation2VariablesPermutation(partial_permutation_nodes, partial_permutation);
// apply partial_ordering orderings
- A_nodes_ = (A_nodes_ * incr_permutation_nodes.transpose()).sparseView();
- A_ = (A_ * incr_permutation.transpose()).sparseView();
+ A_nodes_.rightCols(ordered_nodes) =
+ (A_nodes_.rightCols(ordered_nodes) * partial_permutation_nodes.transpose()).sparseView();
+ A_.rightCols(ordered_variables) =
+ (A_.rightCols(ordered_variables) * partial_permutation.transpose()).sparseView();
+ R_.rightCols(ordered_variables) =
+ (R_.rightCols(ordered_variables) * partial_permutation.transpose()).sparseView();
// ACCUMULATING PERMUTATIONS
- accumulatePermutation(incr_permutation_nodes);
+ accumulatePermutation(partial_permutation_nodes);
}
-
- // partial ordering
- else
- {
- unsigned int first_ordered_node = nodeOrder(_first_ordered_idx); //nodes_.at(_first_ordered_idx).order;
- unsigned int ordered_nodes = nNodes() - first_ordered_node;
-
- if (ordered_nodes > 2) // only reordering when involved nodes in the measurement are not the two last ones
- {
- // SUBPROBLEM ORDERING (from first node variable to last one)
- //std::cout << "ordering partial_ordering problem: " << _first_ordered_node << " to "<< n_nodes_ - 1 << std::endl;
- Eigen::SparseMatrix<int> sub_A_nodes_ = A_nodes_.rightCols(ordered_nodes);
-
- // _partial_ordering ordering_ factors
- node_ordering_restrictions_.resize(ordered_nodes);
- node_ordering_restrictions_ = sub_A_nodes_.bottomRows(1).transpose();
-
- // computing nodes partial ordering_
- sub_A_nodes_.makeCompressed();
- Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic, int> partial_permutation_nodes(ordered_nodes);
- orderer_(sub_A_nodes_, partial_permutation_nodes, node_ordering_restrictions_.data());
-
- // node ordering to variable ordering
- unsigned int ordered_variables = A_.cols() - nodeLocation(_first_ordered_idx); //nodes_.at(_first_ordered_idx).location;
-// std::cout << "first_ordered_node " << first_ordered_node << std::endl;
-// std::cout << "A_.cols() " << A_.cols() << std::endl;
-// std::cout << "nodes_.at(_first_ordered_idx).location " << nodes_.at(_first_ordered_idx).location << std::endl;
-// std::cout << "ordered_variables " << ordered_variables << std::endl;
- Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic, int> partial_permutation(ordered_variables);
- nodePermutation2VariablesPermutation(partial_permutation_nodes, partial_permutation);
-
- // apply partial_ordering orderings
- A_nodes_.rightCols(ordered_nodes) = (A_nodes_.rightCols(ordered_nodes)
- * partial_permutation_nodes.transpose()).sparseView();
- A_.rightCols(ordered_variables) =
- (A_.rightCols(ordered_variables) * partial_permutation.transpose()).sparseView();
- R_.rightCols(ordered_variables) =
- (R_.rightCols(ordered_variables) * partial_permutation.transpose()).sparseView();
-
- // ACCUMULATING PERMUTATIONS
- accumulatePermutation(partial_permutation_nodes);
- }
- }
- time_ordering_ += ((double)clock() - t_ordering_) / CLOCKS_PER_SEC;
}
-
- unsigned int findFirstOrderedNode()
+ time_ordering_ += ((double)clock() - t_ordering_) / CLOCKS_PER_SEC;
+ }
+
+ unsigned int findFirstOrderedNode()
+ {
+ unsigned int first_ordered_node = nNodes();
+ unsigned int first_ordered_idx;
+ for (unsigned int i = 0; i < n_new_factors_; i++)
{
- unsigned int first_ordered_node = nNodes();
- unsigned int first_ordered_idx;
- for (unsigned int i = 0; i < n_new_factors_; i++)
+ FactorBasePtr ct_ptr = factors_.at(factors_.size() - 1 - i);
+ std::cout << "factor: " << i << " id: " << factors_.at(factors_.size() - 1 - i)->id() << std::endl;
+ for (unsigned int j = 0; j < ct_ptr->getStateBlockPtrVector().size(); j++)
{
- FactorBasePtr ct_ptr = factors_.at(factors_.size() - 1 - i);
- std::cout << "factor: " << i << " id: " << factors_.at(factors_.size() - 1 - i)->id()
- << std::endl;
- for (unsigned int j = 0; j < ct_ptr->getStateBlockPtrVector().size(); j++)
+ if (!ct_ptr->getStateBlockPtrVector().at(j)->isFixed())
{
- if (!ct_ptr->getStateBlockPtrVector().at(j)->isFixed())
+ unsigned int idx = id_2_idx_[ct_ptr->getStateBlockPtrVector().at(j)->get()];
+ // std::cout << "estimated idx " << idx << std::endl;
+ // std::cout << "node_order(idx) " << node_order(idx) << std::endl;
+ // std::cout << "first_ordered_node " << first_ordered_node << std::endl;
+ if (first_ordered_node > nodeOrder(idx)) // nodes_.at(idx).order)
{
- unsigned int idx = id_2_idx_[ct_ptr->getStateBlockPtrVector().at(j)->get()];
- //std::cout << "estimated idx " << idx << std::endl;
- //std::cout << "node_order(idx) " << node_order(idx) << std::endl;
- //std::cout << "first_ordered_node " << first_ordered_node << std::endl;
- if (first_ordered_node > nodeOrder(idx)) //nodes_.at(idx).order)
- {
- first_ordered_idx = idx;
- //std::cout << "first_ordered_idx " << first_ordered_idx << std::endl;
- first_ordered_node = nodeOrder(idx);
- //std::cout << "first_ordered_node " << first_ordered_node << std::endl;
- }
+ first_ordered_idx = idx;
+ // std::cout << "first_ordered_idx " << first_ordered_idx << std::endl;
+ first_ordered_node = nodeOrder(idx);
+ // std::cout << "first_ordered_node " << first_ordered_node << std::endl;
}
}
}
- //std::cout << "found first_ordered_node " << first_ordered_node << std::endl;
- //std::cout << "found first_ordered_idx " << first_ordered_idx << std::endl;
-
- return first_ordered_idx;
}
+ // std::cout << "found first_ordered_node " << first_ordered_node << std::endl;
+ // std::cout << "found first_ordered_idx " << first_ordered_idx << std::endl;
- bool solve(const unsigned int mode)
- {
- if (n_new_factors_ == 0)
- return 1;
-
- std::cout << "solving mode " << mode << std::endl;
+ return first_ordered_idx;
+ }
- bool batch, order;
- unsigned int first_ordered_idx;
-
- switch (mode)
- {
- case 0:
- {
- batch = true;
- order = false;
- break;
- }
- case 1:
- {
- batch = true;
- order = (nNodes() > 1);
- break;
- }
- case 2:
- {
- first_ordered_idx = findFirstOrderedNode();
- batch = (nodeOrder(first_ordered_idx) == 0);
- order = (nNodes() > 1);
- }
- }
+ bool solve(const unsigned int mode)
+ {
+ if (n_new_factors_ == 0) return 1;
- // BATCH
- if (batch)
- {
- // REORDER
- if (order)
- ordering(-1);
+ std::cout << "solving mode " << mode << std::endl;
- //printProblem();
+ bool batch, order;
+ unsigned int first_ordered_idx;
- // SOLVE
- t_solving_ = clock();
- A_.makeCompressed();
- solver_.compute(A_);
- if (solver_.info() != Eigen::Success)
- {
- std::cout << "decomposition failed" << std::endl;
- return 0;
- }
- x_incr_ = solver_.solve(-b_);
- R_ = solver_.matrixR();
- //std::cout << "R" << std::endl << MatrixXd::Identity(R_.cols(), R_.cols()) * R_ << std::endl;
- time_solving_ += ((double)clock() - t_solving_) / CLOCKS_PER_SEC;
+ switch (mode)
+ {
+ case 0: {
+ batch = true;
+ order = false;
+ break;
}
- // INCREMENTAL
- else
- {
- // REORDER SUBPROBLEM
- ordering(first_ordered_idx);
- //printProblem();
-
- // SOLVE ORDERED SUBPROBLEM
- t_solving_ = clock();
- A_nodes_.makeCompressed();
- A_.makeCompressed();
-
- // finding measurements block
- Eigen::SparseMatrix<int> measurements_to_initial = A_nodes_.col(nodeOrder(first_ordered_idx));
- //std::cout << "measurements_to_initial " << measurements_to_initial << std::endl;
- //std::cout << "measurements_to_initial.innerIndex()[measurements_to_initial.outerIndex()[0]] " << measurements_to_initial.innerIndex()[measurements_to_initial.outerIndex()[0]] << std::endl;
- unsigned int first_ordered_measurement =
- measurements_to_initial.innerIndex()[measurements_to_initial.outerIndex()[0]];
- unsigned int ordered_measurements = A_.rows() - factor_locations_.at(first_ordered_measurement);
- unsigned int ordered_variables = A_.cols() - nodeLocation(first_ordered_idx); //nodes_.at(first_ordered_idx).location;
- unsigned int unordered_variables = nodeLocation(first_ordered_idx); //nodes_.at(first_ordered_idx).location;
-
- Eigen::SparseMatrix<double> A_partial = A_.bottomRightCorner(ordered_measurements, ordered_variables);
- solver_.compute(A_partial);
- if (solver_.info() != Eigen::Success)
- {
- std::cout << "decomposition failed" << std::endl;
- return 0;
- }
- //std::cout << "R new" << std::endl << MatrixXd::Identity(A_partial.cols(), A_partial.cols()) * solver_.matrixR() << std::endl;
- x_incr_.tail(ordered_variables) = solver_.solve(-b_.tail(ordered_measurements));
-
- // store new part of R
- eraseSparseBlock(R_, unordered_variables, unordered_variables, ordered_variables, ordered_variables);
- //std::cout << "R" << std::endl << MatrixXd::Identity(R_.rows(), R_.rows()) * R_ << std::endl;
- addSparseBlock(solver_.matrixR(), R_, unordered_variables, unordered_variables);
- //std::cout << "R" << std::endl << MatrixXd::Identity(R_.rows(), R_.rows()) * R_ << std::endl;
- R_.makeCompressed();
-
- // solving not ordered subproblem
- if (unordered_variables > 0)
- {
- //std::cout << "--------------------- solving unordered part" << std::endl;
- Eigen::SparseMatrix<double> R1 = R_.topLeftCorner(unordered_variables, unordered_variables);
- //std::cout << "R1" << std::endl << MatrixXd::Identity(R1.rows(), R1.rows()) * R1 << std::endl;
- Eigen::SparseMatrix<double> R2 = R_.topRightCorner(unordered_variables, ordered_variables);
- //std::cout << "R2" << std::endl << MatrixXd::Identity(R2.rows(), R2.rows()) * R2 << std::endl;
- solver_.compute(R1);
- if (solver_.info() != Eigen::Success)
- {
- std::cout << "decomposition failed" << std::endl;
- return 0;
- }
- x_incr_.head(unordered_variables) = solver_.solve(
- -b_.head(unordered_variables) + R2 * x_incr_.tail(ordered_variables));
- }
-
+ case 1: {
+ batch = true;
+ order = (nNodes() > 1);
+ break;
}
- // UPDATE X VALUE
- for (unsigned int i = 0; i < nodes_.size(); i++)
- {
- Eigen::Map < Eigen::VectorXd > x_i(nodes_.at(i)->get(), nodes_.at(i)->getSize());
- x_i += x_incr_.segment(nodeLocation(i), nodes_.at(i)->getSize());
+ case 2: {
+ first_ordered_idx = findFirstOrderedNode();
+ batch = (nodeOrder(first_ordered_idx) == 0);
+ order = (nNodes() > 1);
}
- // Zero the error
- b_.setZero();
-
- time_solving_ += ((double)clock() - t_solving_) / CLOCKS_PER_SEC;
- n_new_factors_ = 0;
- return 1;
}
- void nodePermutation2VariablesPermutation(const Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic, int> &_perm_nodes,
- Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic, int> &perm_variables)
+ // BATCH
+ if (batch)
{
- //std::cout << "perm_nodes: " << _perm_nodes.indices().transpose() << std::endl;
- nodePermutation2nodeLocations(_perm_nodes, node_locations_);
- //std::cout << "locations: " << locations.transpose() << std::endl;
- //std::cout << "perm_variables: " << perm_variables.indices().transpose() << std::endl;
+ // REORDER
+ if (order) ordering(-1);
+
+ // printProblem();
- unsigned int last_idx = 0;
- for (unsigned int i = 0; i < node_locations_.size(); i++)
+ // SOLVE
+ t_solving_ = clock();
+ A_.makeCompressed();
+ solver_.compute(A_);
+ if (solver_.info() != Eigen::Success)
{
- perm_variables.indices().segment(last_idx, nodeDim(i)) = Eigen::VectorXi::LinSpaced(
- nodeDim(i), node_locations_(i), node_locations_(i) + nodeDim(i) - 1);
- last_idx += nodeDim(i);
- //std::cout << i << " perm_variables: " << perm_variables.indices().transpose() << std::endl;
+ std::cout << "decomposition failed" << std::endl;
+ return 0;
}
- //std::cout << "perm_variables: " << perm_variables.indices().transpose() << std::endl;
- }
-
- void nodePermutation2nodeLocations(const Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic, int> &_perm_nodes,
- Eigen::ArrayXi& locations)
- {
- locations = _perm_nodes.indices().array();
-
- for (unsigned int i = 0; i < locations.size(); i++)
- locations = (locations > locations(i)).select(locations + nodeDim(i) - 1, locations);
- }
-
- void augmentPermutation(Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic, int> &perm, unsigned int new_size)
- {
- unsigned int old_size = perm.indices().size();
- unsigned int dim = new_size - old_size;
-
- Eigen::VectorXi new_indices(new_size);
- new_indices.head(old_size) = perm.indices();
- new_indices.tail(dim) = Eigen::ArrayXi::LinSpaced(dim, old_size, new_size - 1);
- perm.resize(new_size);
- perm.indices() = new_indices;
- std::cout << "permutation augmented" << std::endl;
-
- // resize and update locations
- node_locations_.conservativeResize(node_locations_.size() + 1);
- node_locations_(node_locations_.size() - 1) = x_incr_.size();
- std::cout << "node_locations_ augmented" << std::endl;
+ x_incr_ = solver_.solve(-b_);
+ R_ = solver_.matrixR();
+ // std::cout << "R" << std::endl << MatrixXd::Identity(R_.cols(), R_.cols()) * R_ << std::endl;
+ time_solving_ += ((double)clock() - t_solving_) / CLOCKS_PER_SEC;
}
-
- void accumulatePermutation(const Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic, int> &perm)
+ // INCREMENTAL
+ else
{
- //std::cout << std::endl << "old acc_permutation_nodes_ " << acc_permutation_nodes_.indices().transpose() << std::endl;
- //std::cout << "incr perm " << perm.indices().transpose() << std::endl;
-
- // acumulate permutation
- if (perm.size() == acc_node_permutation_.size()) //full permutation
- acc_node_permutation_ = perm * acc_node_permutation_;
- else //partial permutation
+ // REORDER SUBPROBLEM
+ ordering(first_ordered_idx);
+ // printProblem();
+
+ // SOLVE ORDERED SUBPROBLEM
+ t_solving_ = clock();
+ A_nodes_.makeCompressed();
+ A_.makeCompressed();
+
+ // finding measurements block
+ Eigen::SparseMatrix<int> measurements_to_initial = A_nodes_.col(nodeOrder(first_ordered_idx));
+ // std::cout << "measurements_to_initial " << measurements_to_initial << std::endl;
+ // std::cout << "measurements_to_initial.innerIndex()[measurements_to_initial.outerIndex()[0]] " <<
+ // measurements_to_initial.innerIndex()[measurements_to_initial.outerIndex()[0]] << std::endl;
+ unsigned int first_ordered_measurement =
+ measurements_to_initial.innerIndex()[measurements_to_initial.outerIndex()[0]];
+ unsigned int ordered_measurements = A_.rows() - factor_locations_.at(first_ordered_measurement);
+ unsigned int ordered_variables =
+ A_.cols() - nodeLocation(first_ordered_idx); // nodes_.at(first_ordered_idx).location;
+ unsigned int unordered_variables =
+ nodeLocation(first_ordered_idx); // nodes_.at(first_ordered_idx).location;
+
+ Eigen::SparseMatrix<double> A_partial = A_.bottomRightCorner(ordered_measurements, ordered_variables);
+ solver_.compute(A_partial);
+ if (solver_.info() != Eigen::Success)
{
- Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic, int> incr_permutation_nodes(
- Eigen::VectorXi::LinSpaced(nNodes(), 0, nNodes() - 1)); // identity permutation
- incr_permutation_nodes.indices().tail(perm.size()) = perm.indices()
- + Eigen::VectorXi::Constant(perm.size(), nNodes() - perm.size());
- //std::cout << "incr perm " << incr_permutation_nodes.indices().transpose() << std::endl;
- acc_node_permutation_ = incr_permutation_nodes * acc_node_permutation_;
+ std::cout << "decomposition failed" << std::endl;
+ return 0;
+ }
+ // std::cout << "R new" << std::endl << MatrixXd::Identity(A_partial.cols(), A_partial.cols()) *
+ // solver_.matrixR() << std::endl;
+ x_incr_.tail(ordered_variables) = solver_.solve(-b_.tail(ordered_measurements));
+
+ // store new part of R
+ eraseSparseBlock(R_, unordered_variables, unordered_variables, ordered_variables, ordered_variables);
+ // std::cout << "R" << std::endl << MatrixXd::Identity(R_.rows(), R_.rows()) * R_ << std::endl;
+ addSparseBlock(solver_.matrixR(), R_, unordered_variables, unordered_variables);
+ // std::cout << "R" << std::endl << MatrixXd::Identity(R_.rows(), R_.rows()) * R_ << std::endl;
+ R_.makeCompressed();
+
+ // solving not ordered subproblem
+ if (unordered_variables > 0)
+ {
+ // std::cout << "--------------------- solving unordered part" << std::endl;
+ Eigen::SparseMatrix<double> R1 = R_.topLeftCorner(unordered_variables, unordered_variables);
+ // std::cout << "R1" << std::endl << MatrixXd::Identity(R1.rows(), R1.rows()) * R1 << std::endl;
+ Eigen::SparseMatrix<double> R2 = R_.topRightCorner(unordered_variables, ordered_variables);
+ // std::cout << "R2" << std::endl << MatrixXd::Identity(R2.rows(), R2.rows()) * R2 << std::endl;
+ solver_.compute(R1);
+ if (solver_.info() != Eigen::Success)
+ {
+ std::cout << "decomposition failed" << std::endl;
+ return 0;
+ }
+ x_incr_.head(unordered_variables) =
+ solver_.solve(-b_.head(unordered_variables) + R2 * x_incr_.tail(ordered_variables));
}
- //std::cout << "new acc_permutation_nodes_ " << acc_permutation_nodes_.indices().transpose() << std::endl;
-
- // update nodes orders and locations
- nodePermutation2nodeLocations(acc_node_permutation_, node_locations_);
- }
-
- unsigned int nodeDim(const unsigned int _idx)
- {
- assert(_idx < nNodes());
- return nodes_.at(_idx)->getSize();
}
-
- unsigned int nodeOrder(const unsigned int _idx)
+ // UPDATE X VALUE
+ for (unsigned int i = 0; i < nodes_.size(); i++)
{
- assert(_idx < nNodes());
- assert(_idx < acc_node_permutation_.indices().size());
- return acc_node_permutation_.indices()(_idx);
+ Eigen::Map<Eigen::VectorXd> x_i(nodes_.at(i)->get(), nodes_.at(i)->getSize());
+ x_i += x_incr_.segment(nodeLocation(i), nodes_.at(i)->getSize());
}
-
- unsigned int nodeLocation(const unsigned int _idx)
+ // Zero the error
+ b_.setZero();
+
+ time_solving_ += ((double)clock() - t_solving_) / CLOCKS_PER_SEC;
+ n_new_factors_ = 0;
+ return 1;
+ }
+
+ void nodePermutation2VariablesPermutation(
+ const Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic, int> &_perm_nodes,
+ Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic, int> & perm_variables)
+ {
+ // std::cout << "perm_nodes: " << _perm_nodes.indices().transpose() << std::endl;
+ nodePermutation2nodeLocations(_perm_nodes, node_locations_);
+ // std::cout << "locations: " << locations.transpose() << std::endl;
+ // std::cout << "perm_variables: " << perm_variables.indices().transpose() << std::endl;
+
+ unsigned int last_idx = 0;
+ for (unsigned int i = 0; i < node_locations_.size(); i++)
{
- assert(_idx < nNodes());
- assert(_idx < node_locations_.size());
- return node_locations_(_idx);
+ perm_variables.indices().segment(last_idx, nodeDim(i)) =
+ Eigen::VectorXi::LinSpaced(nodeDim(i), node_locations_(i), node_locations_(i) + nodeDim(i) - 1);
+ last_idx += nodeDim(i);
+ // std::cout << i << " perm_variables: " << perm_variables.indices().transpose() << std::endl;
}
-
- unsigned int nNodes()
+ // std::cout << "perm_variables: " << perm_variables.indices().transpose() << std::endl;
+ }
+
+ void nodePermutation2nodeLocations(
+ const Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic, int> &_perm_nodes,
+ Eigen::ArrayXi & locations)
+ {
+ locations = _perm_nodes.indices().array();
+
+ for (unsigned int i = 0; i < locations.size(); i++)
+ locations = (locations > locations(i)).select(locations + nodeDim(i) - 1, locations);
+ }
+
+ void augmentPermutation(Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic, int> &perm, unsigned int new_size)
+ {
+ unsigned int old_size = perm.indices().size();
+ unsigned int dim = new_size - old_size;
+
+ Eigen::VectorXi new_indices(new_size);
+ new_indices.head(old_size) = perm.indices();
+ new_indices.tail(dim) = Eigen::ArrayXi::LinSpaced(dim, old_size, new_size - 1);
+ perm.resize(new_size);
+ perm.indices() = new_indices;
+ std::cout << "permutation augmented" << std::endl;
+
+ // resize and update locations
+ node_locations_.conservativeResize(node_locations_.size() + 1);
+ node_locations_(node_locations_.size() - 1) = x_incr_.size();
+ std::cout << "node_locations_ augmented" << std::endl;
+ }
+
+ void accumulatePermutation(const Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic, int> &perm)
+ {
+ // std::cout << std::endl << "old acc_permutation_nodes_ " << acc_permutation_nodes_.indices().transpose() <<
+ // std::endl; std::cout << "incr perm " << perm.indices().transpose() << std::endl;
+
+ // acumulate permutation
+ if (perm.size() == acc_node_permutation_.size()) // full permutation
+ acc_node_permutation_ = perm * acc_node_permutation_;
+ else // partial permutation
{
- return nodes_.size();
+ Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic, int> incr_permutation_nodes(
+ Eigen::VectorXi::LinSpaced(nNodes(), 0, nNodes() - 1)); // identity permutation
+ incr_permutation_nodes.indices().tail(perm.size()) =
+ perm.indices() + Eigen::VectorXi::Constant(perm.size(), nNodes() - perm.size());
+ // std::cout << "incr perm " << incr_permutation_nodes.indices().transpose() << std::endl;
+ acc_node_permutation_ = incr_permutation_nodes * acc_node_permutation_;
}
-
- CostFunctionBasePtr createCostFunction(FactorBasePtr _fac_ptr)
+ // std::cout << "new acc_permutation_nodes_ " << acc_permutation_nodes_.indices().transpose() << std::endl;
+
+ // update nodes orders and locations
+ nodePermutation2nodeLocations(acc_node_permutation_, node_locations_);
+ }
+
+ unsigned int nodeDim(const unsigned int _idx)
+ {
+ assert(_idx < nNodes());
+ return nodes_.at(_idx)->getSize();
+ }
+
+ unsigned int nodeOrder(const unsigned int _idx)
+ {
+ assert(_idx < nNodes());
+ assert(_idx < acc_node_permutation_.indices().size());
+ return acc_node_permutation_.indices()(_idx);
+ }
+
+ unsigned int nodeLocation(const unsigned int _idx)
+ {
+ assert(_idx < nNodes());
+ assert(_idx < node_locations_.size());
+ return node_locations_(_idx);
+ }
+
+ unsigned int nNodes()
+ {
+ return nodes_.size();
+ }
+
+ CostFunctionBasePtr createCostFunction(FactorBasePtr _fac_ptr)
+ {
+ // std::cout << "adding fac " << _fac_ptr->id() << std::endl;
+ //_fac_ptr->print();
+
+ switch (_fac_ptr->getTypeId())
{
- //std::cout << "adding fac " << _fac_ptr->id() << std::endl;
- //_fac_ptr->print();
-
- switch (_fac_ptr->getTypeId())
- {
- case FAC_GPS_FIX_2d:
- {
- FactorGPS2d* specific_ptr = (FactorGPS2d*)(_fac_ptr);
- return (CostFunctionBasePtr)(new CostFunctionSparse<FactorGPS2d, specific_ptr->residualSize,
- specific_ptr->block0Size, specific_ptr->block1Size, specific_ptr->block2Size,
- specific_ptr->block3Size, specific_ptr->block4Size, specific_ptr->block5Size,
- specific_ptr->block6Size, specific_ptr->block7Size, specific_ptr->block8Size,
- specific_ptr->block9Size>(specific_ptr));
- break;
- }
- case FAC_ODOM_2d:
- {
- FactorOdom2d* specific_ptr = (FactorOdom2d*)(_fac_ptr);
- return (CostFunctionBasePtr)new CostFunctionSparse<FactorOdom2d, specific_ptr->residualSize,
- specific_ptr->block0Size, specific_ptr->block1Size, specific_ptr->block2Size,
- specific_ptr->block3Size, specific_ptr->block4Size, specific_ptr->block5Size,
- specific_ptr->block6Size, specific_ptr->block7Size, specific_ptr->block8Size,
- specific_ptr->block9Size>(specific_ptr);
- break;
- }
- case FAC_CORNER_2d:
- {
- FactorCorner2d* specific_ptr = (FactorCorner2d*)(_fac_ptr);
- return (CostFunctionBasePtr)new CostFunctionSparse<FactorCorner2d, specific_ptr->residualSize,
- specific_ptr->block0Size, specific_ptr->block1Size, specific_ptr->block2Size,
- specific_ptr->block3Size, specific_ptr->block4Size, specific_ptr->block5Size,
- specific_ptr->block6Size, specific_ptr->block7Size, specific_ptr->block8Size,
- specific_ptr->block9Size>(specific_ptr);
- break;
- }
- default:
- std::cout << "Unknown factor type! Please add it in the CeresWrapper::createCostFunction()"
- << std::endl;
-
- return nullptr;
+ case FAC_GPS_FIX_2d: {
+ FactorGPS2d *specific_ptr = (FactorGPS2d *)(_fac_ptr);
+ return (CostFunctionBasePtr)(new CostFunctionSparse<FactorGPS2d,
+ specific_ptr->residualSize,
+ specific_ptr->block0Size,
+ specific_ptr->block1Size,
+ specific_ptr->block2Size,
+ specific_ptr->block3Size,
+ specific_ptr->block4Size,
+ specific_ptr->block5Size,
+ specific_ptr->block6Size,
+ specific_ptr->block7Size,
+ specific_ptr->block8Size,
+ specific_ptr->block9Size>(specific_ptr));
+ break;
}
- }
-
- void printResults()
- {
- std::cout << " solved in " << time_solving_ * 1e3 << " ms | " << R_.nonZeros() << " nonzeros in R"
- << std::endl;
- std::cout << "x = " << x_incr_.transpose() << std::endl;
- }
+ case FAC_ODOM_2d: {
+ FactorOdom2d *specific_ptr = (FactorOdom2d *)(_fac_ptr);
+ return (CostFunctionBasePtr) new CostFunctionSparse<FactorOdom2d,
+ specific_ptr->residualSize,
+ specific_ptr->block0Size,
+ specific_ptr->block1Size,
+ specific_ptr->block2Size,
+ specific_ptr->block3Size,
+ specific_ptr->block4Size,
+ specific_ptr->block5Size,
+ specific_ptr->block6Size,
+ specific_ptr->block7Size,
+ specific_ptr->block8Size,
+ specific_ptr->block9Size>(specific_ptr);
+ break;
+ }
+ case FAC_CORNER_2d: {
+ FactorCorner2d *specific_ptr = (FactorCorner2d *)(_fac_ptr);
+ return (CostFunctionBasePtr) new CostFunctionSparse<FactorCorner2d,
+ specific_ptr->residualSize,
+ specific_ptr->block0Size,
+ specific_ptr->block1Size,
+ specific_ptr->block2Size,
+ specific_ptr->block3Size,
+ specific_ptr->block4Size,
+ specific_ptr->block5Size,
+ specific_ptr->block6Size,
+ specific_ptr->block7Size,
+ specific_ptr->block8Size,
+ specific_ptr->block9Size>(specific_ptr);
+ break;
+ }
+ default:
+ std::cout << "Unknown factor type! Please add it in the CeresWrapper::createCostFunction()"
+ << std::endl;
- void printProblem()
- {
- std::cout << std::endl << "A_nodes_: " << std::endl
- << Eigen::MatrixXi::Identity(A_nodes_.rows(), A_nodes_.rows()) * A_nodes_ << std::endl << std::endl;
- //std::cout << "A_: " << std::endl << MatrixXd::Identity(A_.rows(), A_.rows()) * A_ << std::endl << std::endl;
- std::cout << "b_: " << std::endl << b_.transpose() << std::endl << std::endl;
+ return nullptr;
}
+ }
+
+ void printResults()
+ {
+ std::cout << " solved in " << time_solving_ * 1e3 << " ms | " << R_.nonZeros() << " nonzeros in R"
+ << std::endl;
+ std::cout << "x = " << x_incr_.transpose() << std::endl;
+ }
+
+ void printProblem()
+ {
+ std::cout << std::endl
+ << "A_nodes_: " << std::endl
+ << Eigen::MatrixXi::Identity(A_nodes_.rows(), A_nodes_.rows()) * A_nodes_ << std::endl
+ << std::endl;
+ // std::cout << "A_: " << std::endl << MatrixXd::Identity(A_.rows(), A_.rows()) * A_ << std::endl <<
+ // std::endl;
+ std::cout << "b_: " << std::endl << b_.transpose() << std::endl << std::endl;
+ }
};
-} // namespace wolf
+} // namespace wolf
diff --git a/include/core/solver_suitesparse/solver_QR.h b/include/core/solver_suitesparse/solver_QR.h
index 6ddcda8a0bf22503b3e97fbf8c5fbdfda3fbb671..a65e87bcb16c824f9a51c0eae904907ca3616e86 100644
--- a/include/core/solver_suitesparse/solver_QR.h
+++ b/include/core/solver_suitesparse/solver_QR.h
@@ -24,19 +24,19 @@ using namespace Eigen;
class SolverQR
{
- protected:
- SparseQR < SparseMatrix<double>, NaturalOrdering<int>> solver_;
- SparseMatrix<double> A, A_ordered, R;
- VectorXd b, x, x_ordered, x_ordered_partial;
- int n_measurements = 0;
- int n_nodes = 0;
+ protected:
+ SparseQR<SparseMatrix<double>, NaturalOrdering<int>> solver_;
+ SparseMatrix<double> A, A_ordered, R;
+ VectorXd b, x, x_ordered, x_ordered_partial;
+ int n_measurements = 0;
+ int n_nodes = 0;
- // ordering variables
- SparseMatrix<int> A_nodes_ordered;
- PermutationMatrix<Dynamic, Dynamic, int> acc_permutation_nodes_matrix;
+ // ordering variables
+ SparseMatrix<int> A_nodes_ordered;
+ PermutationMatrix<Dynamic, Dynamic, int> acc_permutation_nodes_matrix;
- CCOLAMDOrdering<int> ordering, partial_ordering;
- VectorXi nodes_ordering_factors;
+ CCOLAMDOrdering<int> ordering, partial_ordering;
+ VectorXi nodes_ordering_factors;
- private:
+ private:
};
diff --git a/include/core/solver_suitesparse/solver_manager.h b/include/core/solver_suitesparse/solver_manager.h
index a7660386e87bdaa18ff4879156429f1c636e46df..95c1d31fd2fd1b3c0152a862c6c640a1f6e88d3e 100644
--- a/include/core/solver_suitesparse/solver_manager.h
+++ b/include/core/solver_suitesparse/solver_manager.h
@@ -20,7 +20,7 @@
#pragma once
-//wolf includes
+// wolf includes
#include "core/factor/factor_GPS_2d.h"
#include "core/common/wolf.h"
#include "core/state_block/state_block.h"
@@ -38,28 +38,27 @@
class SolverManager
{
- protected:
+ protected:
+ public:
+ SolverManager(ceres::Problem::Options _options);
- public:
- SolverManager(ceres::Problem::Options _options);
+ ~SolverManager();
- ~SolverManager();
+ ceres::Solver::Summary solve(const ceres::Solver::Options& _ceres_options);
- ceres::Solver::Summary solve(const ceres::Solver::Options& _ceres_options);
+ // void computeCovariances(WolfProblemPtr _problem_ptr);
- //void computeCovariances(WolfProblemPtr _problem_ptr);
+ void update(const WolfProblemPtr _problem_ptr);
- void update(const WolfProblemPtr _problem_ptr);
+ void addFactor(FactorBasePtr _fac_ptr);
- void addFactor(FactorBasePtr _fac_ptr);
+ void removeFactor(const unsigned int& _fac_idx);
- void removeFactor(const unsigned int& _fac_idx);
+ void addStateUnit(StateBlockPtr _st_ptr);
- void addStateUnit(StateBlockPtr _st_ptr);
+ void removeAllStateUnits();
- void removeAllStateUnits();
+ void updateStateUnitStatus(StateBlockPtr _st_ptr);
- void updateStateUnitStatus(StateBlockPtr _st_ptr);
-
- ceres::CostFunction* createCostFunction(FactorBasePtr _fac_ptr);
+ ceres::CostFunction* createCostFunction(FactorBasePtr _fac_ptr);
};
diff --git a/include/core/solver_suitesparse/sparse_utils.h b/include/core/solver_suitesparse/sparse_utils.h
index 4c746537b3b836b4bcb83210104f58cd2fe39648..cbab6408b6742f966f271c0686d7ae7765219f73 100644
--- a/include/core/solver_suitesparse/sparse_utils.h
+++ b/include/core/solver_suitesparse/sparse_utils.h
@@ -25,7 +25,6 @@
namespace wolf
{
-
void eraseBlockRow(Eigen::SparseMatrixd& A, const unsigned int& _row, const unsigned int& _n_rows)
{
A.prune([](int i, int, double) { return i >= _row && i < _row + _n_rows; });
@@ -36,22 +35,28 @@ void eraseBlockCol(Eigen::SparseMatrixd& A, const unsigned int& _col, const unsi
A.prune([](int, int j, double) { return j >= _col && j < _col + _n_cols; });
}
-void addSparseBlock(const Eigen::MatrixXd& ins, Eigen::SparseMatrixd& original, const unsigned int& row, const unsigned int& col)
+void addSparseBlock(const Eigen::MatrixXd& ins,
+ Eigen::SparseMatrixd& original,
+ const unsigned int& row,
+ const unsigned int& col)
{
for (auto ins_row = 0; ins_row < ins.rows(); ins_row++)
for (auto ins_col = 0; ins_col < ins.cols(); ins_col++)
- original.coeffRef(row+ins_row, col+ins_col) += ins(ins_row,ins_col);
+ original.coeffRef(row + ins_row, col + ins_col) += ins(ins_row, ins_col);
original.makeCompressed();
}
-void insertSparseBlock(const Eigen::MatrixXd& ins, Eigen::SparseMatrixd& original, const unsigned int& row, const unsigned int& col)
+void insertSparseBlock(const Eigen::MatrixXd& ins,
+ Eigen::SparseMatrixd& original,
+ const unsigned int& row,
+ const unsigned int& col)
{
for (auto ins_row = 0; ins_row < ins.rows(); ins_row++)
for (auto ins_col = 0; ins_col < ins.cols(); ins_col++)
- original.insert(row+ins_row, col+ins_col) = ins(ins_row,ins_col);
+ original.insert(row + ins_row, col + ins_col) = ins(ins_row, ins_col);
original.makeCompressed();
}
-}
+} // namespace wolf
diff --git a/include/core/state_block/factory_state_block.h b/include/core/state_block/factory_state_block.h
index 94bf908ef3d8c56242f99fea25b1e3ed3193410f..cd35e83a241b589de8ce48b6be629d4aa771eac9 100644
--- a/include/core/state_block/factory_state_block.h
+++ b/include/core/state_block/factory_state_block.h
@@ -25,7 +25,6 @@
namespace wolf
{
-
/** \brief StateBlock factory class
*
* This factory can create objects of class StateBlock and classes deriving from StateBlock.
@@ -82,32 +81,32 @@ namespace wolf
*
*/
typedef Factory<std::shared_ptr<StateBlock>, const Eigen::VectorXd&, bool> FactoryStateBlock;
-template<>
+template <>
inline std::string FactoryStateBlock::getClass() const
{
return "FactoryStateBlock";
}
typedef Factory<std::shared_ptr<StateBlock>> FactoryStateBlockIdentity;
-template<>
+template <>
inline std::string FactoryStateBlockIdentity::getClass() const
{
return "FactoryStateBlockIdentity";
}
typedef Factory<Eigen::VectorXd> FactoryStateBlockIdentityVector;
-template<>
+template <>
inline std::string FactoryStateBlockIdentityVector::getClass() const
{
return "FactoryStateBlockIdentityVector";
}
-#define WOLF_REGISTER_STATEBLOCK(StateBlockType) \
-namespace \
-{ \
-const bool WOLF_UNUSED StateBlockType##Registered = \
- FactoryStateBlock::registerCreator(#StateBlockType, StateBlockType::create); \
-const bool WOLF_UNUSED StateBlockType##IdentityRegistered = \
- FactoryStateBlockIdentity::registerCreator(#StateBlockType, StateBlockType::createIdentity); \
-const bool WOLF_UNUSED StateBlockType##IdentityVectorRegistered = \
- FactoryStateBlockIdentityVector::registerCreator(#StateBlockType, StateBlockType::Identity); \
-}
-}
+#define WOLF_REGISTER_STATEBLOCK(StateBlockType) \
+ namespace \
+ { \
+ const bool WOLF_UNUSED StateBlockType##Registered = \
+ FactoryStateBlock::registerCreator(#StateBlockType, StateBlockType::create); \
+ const bool WOLF_UNUSED StateBlockType##IdentityRegistered = \
+ FactoryStateBlockIdentity::registerCreator(#StateBlockType, StateBlockType::createIdentity); \
+ const bool WOLF_UNUSED StateBlockType##IdentityVectorRegistered = \
+ FactoryStateBlockIdentityVector::registerCreator(#StateBlockType, StateBlockType::Identity); \
+ }
+} // namespace wolf
diff --git a/include/core/state_block/local_parametrization_angle.h b/include/core/state_block/local_parametrization_angle.h
index affd72fde8848ecacf89de0bcfe38de2b254d7b3..36e5542fdd44a4211e3b81480fbc83031a40f870 100644
--- a/include/core/state_block/local_parametrization_angle.h
+++ b/include/core/state_block/local_parametrization_angle.h
@@ -25,25 +25,24 @@
namespace wolf
{
-
class LocalParametrizationAngle : public LocalParametrizationBase
{
- public:
- LocalParametrizationAngle();
- ~LocalParametrizationAngle() override;
+ public:
+ LocalParametrizationAngle();
+ ~LocalParametrizationAngle() override;
- bool plus(Eigen::Map<const Eigen::VectorXd>& _h, Eigen::Map<const Eigen::VectorXd>& _delta,
- Eigen::Map<Eigen::VectorXd>& _h_plus_delta) const override;
- bool computeJacobian(Eigen::Map<const Eigen::VectorXd>& _h,
- Eigen::Map<Eigen::MatrixRowXd>& _jacobian) const override;
- bool minus(Eigen::Map<const Eigen::VectorXd>& _x1,
- Eigen::Map<const Eigen::VectorXd>& _x2,
- Eigen::Map<Eigen::VectorXd>& _x2_minus_x1) override;
- bool isValid(const Eigen::VectorXd& state, double tolerance) override;
+ bool plus(Eigen::Map<const Eigen::VectorXd>& _h,
+ Eigen::Map<const Eigen::VectorXd>& _delta,
+ Eigen::Map<Eigen::VectorXd>& _h_plus_delta) const override;
+ bool computeJacobian(Eigen::Map<const Eigen::VectorXd>& _h,
+ Eigen::Map<Eigen::MatrixRowXd>& _jacobian) const override;
+ bool minus(Eigen::Map<const Eigen::VectorXd>& _x1,
+ Eigen::Map<const Eigen::VectorXd>& _x2,
+ Eigen::Map<Eigen::VectorXd>& _x2_minus_x1) override;
+ bool isValid(const Eigen::VectorXd& state, double tolerance) override;
};
-inline LocalParametrizationAngle::LocalParametrizationAngle() :
- LocalParametrizationBase(1,1)
+inline LocalParametrizationAngle::LocalParametrizationAngle() : LocalParametrizationBase(1, 1)
{
//
}
@@ -55,14 +54,14 @@ inline LocalParametrizationAngle::~LocalParametrizationAngle()
inline bool LocalParametrizationAngle::plus(Eigen::Map<const Eigen::VectorXd>& _h,
Eigen::Map<const Eigen::VectorXd>& _delta,
- Eigen::Map<Eigen::VectorXd>& _h_plus_delta) const
+ Eigen::Map<Eigen::VectorXd>& _h_plus_delta) const
{
_h_plus_delta(0) = pi2pi(_h(0) + _delta(0));
return true;
}
inline bool LocalParametrizationAngle::computeJacobian(Eigen::Map<const Eigen::VectorXd>& _h,
- Eigen::Map<Eigen::MatrixRowXd>& _jacobian) const
+ Eigen::Map<Eigen::MatrixRowXd>& _jacobian) const
{
_jacobian(0) = 1.0;
return true;
@@ -70,17 +69,17 @@ inline bool LocalParametrizationAngle::computeJacobian(Eigen::Map<const Eigen::V
inline bool LocalParametrizationAngle::minus(Eigen::Map<const Eigen::VectorXd>& _x1,
Eigen::Map<const Eigen::VectorXd>& _x2,
- Eigen::Map<Eigen::VectorXd>& _x2_minus_x1)
+ Eigen::Map<Eigen::VectorXd>& _x2_minus_x1)
{
- _x2_minus_x1(0) = pi2pi(_x2(0)-_x1(0));
+ _x2_minus_x1(0) = pi2pi(_x2(0) - _x1(0));
return true;
}
inline bool LocalParametrizationAngle::isValid(const Eigen::VectorXd& _state, double tolerance)
{
- //Any real is a valid angle because we use the pi2pi function. Also
- //the types don't match. In this case the argument is
- //Eigen::Map not Eigen::VectorXd
+ // Any real is a valid angle because we use the pi2pi function. Also
+ // the types don't match. In this case the argument is
+ // Eigen::Map not Eigen::VectorXd
return true;
}
} /* namespace wolf */
diff --git a/include/core/state_block/local_parametrization_base.h b/include/core/state_block/local_parametrization_base.h
index d1c3010e8e2c44361b6ea7a19a87ff36c069fa1a..6fa3dec2dbd12e767b0adee824a095df6a46f91b 100644
--- a/include/core/state_block/local_parametrization_base.h
+++ b/include/core/state_block/local_parametrization_base.h
@@ -22,30 +22,30 @@
#include "core/common/wolf.h"
-namespace wolf {
+namespace wolf
+{
+class LocalParametrizationBase
+{
+ protected:
+ unsigned int global_size_;
+ unsigned int local_size_;
-class LocalParametrizationBase{
- protected:
- unsigned int global_size_;
- unsigned int local_size_;
- public:
- LocalParametrizationBase(unsigned int _global_size, unsigned int _local_size);
- virtual ~LocalParametrizationBase();
+ public:
+ LocalParametrizationBase(unsigned int _global_size, unsigned int _local_size);
+ virtual ~LocalParametrizationBase();
- bool plus(const Eigen::VectorXd& _x,
- const Eigen::VectorXd& _delta,
- Eigen::VectorXd& _x_plus_delta) const;
- virtual bool plus(Eigen::Map<const Eigen::VectorXd>& _x,
- Eigen::Map<const Eigen::VectorXd>& _delta,
- Eigen::Map<Eigen::VectorXd>& _x_plus_delta) const = 0;
- virtual bool computeJacobian(Eigen::Map<const Eigen::VectorXd>& _x,
- Eigen::Map<Eigen::MatrixRowXd>& _jacobian) const = 0;
- virtual bool minus(Eigen::Map<const Eigen::VectorXd>& _x1,
- Eigen::Map<const Eigen::VectorXd>& _x2,
- Eigen::Map<Eigen::VectorXd>& _x2_minus_x1) = 0;
- virtual bool isValid(const Eigen::VectorXd& state, double tolerance) = 0;
- unsigned int getLocalSize() const;
- unsigned int getGlobalSize() const;
+ bool plus(const Eigen::VectorXd& _x, const Eigen::VectorXd& _delta, Eigen::VectorXd& _x_plus_delta) const;
+ virtual bool plus(Eigen::Map<const Eigen::VectorXd>& _x,
+ Eigen::Map<const Eigen::VectorXd>& _delta,
+ Eigen::Map<Eigen::VectorXd>& _x_plus_delta) const = 0;
+ virtual bool computeJacobian(Eigen::Map<const Eigen::VectorXd>& _x,
+ Eigen::Map<Eigen::MatrixRowXd>& _jacobian) const = 0;
+ virtual bool minus(Eigen::Map<const Eigen::VectorXd>& _x1,
+ Eigen::Map<const Eigen::VectorXd>& _x2,
+ Eigen::Map<Eigen::VectorXd>& _x2_minus_x1) = 0;
+ virtual bool isValid(const Eigen::VectorXd& state, double tolerance) = 0;
+ unsigned int getLocalSize() const;
+ unsigned int getGlobalSize() const;
};
-} // namespace wolf
+} // namespace wolf
diff --git a/include/core/state_block/local_parametrization_homogeneous.h b/include/core/state_block/local_parametrization_homogeneous.h
index d7e066e9afac1ee73595e73c3c534ee1d38ac840..f3be4e47e1c5d8740492078b78624d22d726c229 100644
--- a/include/core/state_block/local_parametrization_homogeneous.h
+++ b/include/core/state_block/local_parametrization_homogeneous.h
@@ -22,8 +22,8 @@
#include "core/state_block/local_parametrization_base.h"
-namespace wolf {
-
+namespace wolf
+{
/**
* \brief Local parametrization for homogeneous vectors.
*
@@ -38,7 +38,8 @@ namespace wolf {
* where \f$\otimes\f$ is the quaternion product, and \f$d{\bf q} = {\bf q}(d {\bf x})\f$ is a unit delta_quaternion
* equivalent to a rotation \f$d{\bf x}\f$, obtained with
*
- * \f[{\bf q}(d{\bf x}) = \left[\begin{array}{c} \frac{ d {\bf x}}{ |d{\bf x}|} \sin(|d{\bf x}|) \\ \cos(|d{\bf x}|) \end{array}\right]\f]
+ * \f[{\bf q}(d{\bf x}) = \left[\begin{array}{c} \frac{ d {\bf x}}{ |d{\bf x}|} \sin(|d{\bf x}|) \\ \cos(|d{\bf x}|)
+ * \end{array}\right]\f]
*
* Contrary to the case of quaternions, it is not required that \f${\bf x}\f$ be a unit homogeneous vector.
* In this case, the updated \f${\bf x}^+={\bf x}\oplus d {\bf x}\f$ has the same norm as \f${\bf x}\f$.
@@ -48,18 +49,19 @@ namespace wolf {
*/
class LocalParametrizationHomogeneous : public LocalParametrizationBase
{
- public:
- LocalParametrizationHomogeneous();
- ~LocalParametrizationHomogeneous() override;
+ public:
+ LocalParametrizationHomogeneous();
+ ~LocalParametrizationHomogeneous() override;
- bool plus(Eigen::Map<const Eigen::VectorXd>& _h,
- Eigen::Map<const Eigen::VectorXd>& _delta,
- Eigen::Map<Eigen::VectorXd>& _h_plus_delta) const override;
- bool computeJacobian(Eigen::Map<const Eigen::VectorXd>& _h, Eigen::Map<Eigen::MatrixRowXd>& _jacobian) const override;
- bool minus(Eigen::Map<const Eigen::VectorXd>& _h1,
- Eigen::Map<const Eigen::VectorXd>& _h2,
- Eigen::Map<Eigen::VectorXd>& _h2_minus_h1) override;
- bool isValid(const Eigen::VectorXd& state, double tolerance) override;
+ bool plus(Eigen::Map<const Eigen::VectorXd>& _h,
+ Eigen::Map<const Eigen::VectorXd>& _delta,
+ Eigen::Map<Eigen::VectorXd>& _h_plus_delta) const override;
+ bool computeJacobian(Eigen::Map<const Eigen::VectorXd>& _h,
+ Eigen::Map<Eigen::MatrixRowXd>& _jacobian) const override;
+ bool minus(Eigen::Map<const Eigen::VectorXd>& _h1,
+ Eigen::Map<const Eigen::VectorXd>& _h2,
+ Eigen::Map<Eigen::VectorXd>& _h2_minus_h1) override;
+ bool isValid(const Eigen::VectorXd& state, double tolerance) override;
};
-} // namespace wolf
+} // namespace wolf
diff --git a/include/core/state_block/local_parametrization_quaternion.h b/include/core/state_block/local_parametrization_quaternion.h
index 777930c7d581ace2019aa06373d8fc7bd23c84ce..f037dba0e61a9a4ad02d0ea5770971360dfe90b2 100644
--- a/include/core/state_block/local_parametrization_quaternion.h
+++ b/include/core/state_block/local_parametrization_quaternion.h
@@ -22,18 +22,19 @@
#include "core/state_block/local_parametrization_base.h"
-namespace wolf {
-
+namespace wolf
+{
/**
- * \brief Local or global orientation error
- *
- * Local or global orientation error.
- *
- * This enum controls whether the orientation error is composed locally or globally to an orientation specification.
- *
- * See LocalParametrizationQuaternion for more information.
- */
-typedef enum {
+ * \brief Local or global orientation error
+ *
+ * Local or global orientation error.
+ *
+ * This enum controls whether the orientation error is composed locally or globally to an orientation specification.
+ *
+ * See LocalParametrizationQuaternion for more information.
+ */
+typedef enum
+{
DQ_LOCAL,
DQ_GLOBAL
} QuaternionDeltaReference;
@@ -62,38 +63,35 @@ typedef enum {
template <QuaternionDeltaReference DeltaReference = DQ_LOCAL>
class LocalParametrizationQuaternion : public LocalParametrizationBase
{
+ public:
+ LocalParametrizationQuaternion() : LocalParametrizationBase(4, 3)
+ {
+ //
+ }
-public:
-
- LocalParametrizationQuaternion() :
- LocalParametrizationBase(4, 3)
- {
- //
- }
-
- ~LocalParametrizationQuaternion() override
- {
- //
- }
+ ~LocalParametrizationQuaternion() override
+ {
+ //
+ }
- bool plus(Eigen::Map<const Eigen::VectorXd>& _q,
- Eigen::Map<const Eigen::VectorXd>& _delta_theta,
- Eigen::Map<Eigen::VectorXd>& _q_plus_delta_theta) const override;
+ bool plus(Eigen::Map<const Eigen::VectorXd>& _q,
+ Eigen::Map<const Eigen::VectorXd>& _delta_theta,
+ Eigen::Map<Eigen::VectorXd>& _q_plus_delta_theta) const override;
- bool computeJacobian(Eigen::Map<const Eigen::VectorXd>& _q,
- Eigen::Map<Eigen::MatrixRowXd>& _jacobian) const override;
- bool minus(Eigen::Map<const Eigen::VectorXd>& _q1,
- Eigen::Map<const Eigen::VectorXd>& _q2,
- Eigen::Map<Eigen::VectorXd>& _q2_minus_q1) override;
- // inline bool isValid(const Eigen::VectorXd& state) override;
- // template<QuaternionDeltaReference DeltaReference>
- bool isValid(const Eigen::VectorXd& _state, double tolerance) override
- {
- return _state.size() == global_size_ && fabs(1.0 - _state.norm()) < tolerance;
- }
+ bool computeJacobian(Eigen::Map<const Eigen::VectorXd>& _q,
+ Eigen::Map<Eigen::MatrixRowXd>& _jacobian) const override;
+ bool minus(Eigen::Map<const Eigen::VectorXd>& _q1,
+ Eigen::Map<const Eigen::VectorXd>& _q2,
+ Eigen::Map<Eigen::VectorXd>& _q2_minus_q1) override;
+ // inline bool isValid(const Eigen::VectorXd& state) override;
+ // template<QuaternionDeltaReference DeltaReference>
+ bool isValid(const Eigen::VectorXd& _state, double tolerance) override
+ {
+ return _state.size() == global_size_ && fabs(1.0 - _state.norm()) < tolerance;
+ }
};
typedef LocalParametrizationQuaternion<DQ_GLOBAL> LocalParametrizationQuaternionGlobal;
typedef LocalParametrizationQuaternion<DQ_LOCAL> LocalParametrizationQuaternionLocal;
-} // namespace wolf
+} // namespace wolf
diff --git a/include/core/state_block/state_angle.h b/include/core/state_block/state_angle.h
index 67c5feb3715ccbd79eb3cb9d19ff0e9d0e755108..cbec8d9641dba0459aadf0b535c98d5a06d03abc 100644
--- a/include/core/state_block/state_angle.h
+++ b/include/core/state_block/state_angle.h
@@ -25,33 +25,31 @@
namespace wolf
{
-
class StateAngle : public StateBlock
{
- public:
- StateAngle(double _angle, bool _fixed = false, bool _transformable = true);
- StateAngle(const VectorXd& _angle, bool _fixed = false, bool _transformable = true);
-
- static Eigen::VectorXd Identity();
+ public:
+ StateAngle(double _angle, bool _fixed = false, bool _transformable = true);
+ StateAngle(const VectorXd& _angle, bool _fixed = false, bool _transformable = true);
+
+ static Eigen::VectorXd Identity();
- WOLF_STATE_BLOCK_CREATE(StateAngle);
+ WOLF_STATE_BLOCK_CREATE(StateAngle);
- ~StateAngle() override;
+ ~StateAngle() override;
- virtual void transform(const VectorComposite& _transformation) override;
+ virtual void transform(const VectorComposite& _transformation) override;
};
-inline StateAngle::StateAngle(double _angle, bool _fixed, bool _transformable) :
- StateBlock("StateAngle", 1, _fixed, std::make_shared<LocalParametrizationAngle>(), _transformable)
+inline StateAngle::StateAngle(double _angle, bool _fixed, bool _transformable)
+ : StateBlock("StateAngle", 1, _fixed, std::make_shared<LocalParametrizationAngle>(), _transformable)
{
state_(0) = pi2pi(_angle);
}
-inline StateAngle::StateAngle(const VectorXd& _angle, bool _fixed, bool _transformable) :
- StateBlock("StateAngle", 1, _fixed, std::make_shared<LocalParametrizationAngle>(), _transformable)
+inline StateAngle::StateAngle(const VectorXd& _angle, bool _fixed, bool _transformable)
+ : StateBlock("StateAngle", 1, _fixed, std::make_shared<LocalParametrizationAngle>(), _transformable)
{
- if(_angle.size() != 1)
- throw std::runtime_error("The angle state vector must be of size 1!");
+ if (_angle.size() != 1) throw std::runtime_error("The angle state vector must be of size 1!");
state_(0) = pi2pi(_angle(0));
}
@@ -68,7 +66,7 @@ inline StateAngle::~StateAngle()
inline void StateAngle::transform(const VectorComposite& _transformation)
{
- if (isTransformable()) setState(_transformation.at('O') + getState()); // 2D rotation is a sum of angles
+ if (isTransformable()) setState(_transformation.at('O') + getState()); // 2D rotation is a sum of angles
}
} /* namespace wolf */
diff --git a/include/core/state_block/state_block.h b/include/core/state_block/state_block.h
index 835b6d9ec99960f17d1440bfe2056e282b20009f..cc5f509fea8a8d11dc5c2d303dbe717672f77087 100644
--- a/include/core/state_block/state_block.h
+++ b/include/core/state_block/state_block.h
@@ -23,44 +23,45 @@
/*
* Macro for defining StateBlocks creators.
*
- * Place a call to this macro inside your class declaration
+ * Place a call to this macro inside your class declaration
* (in the state_block_class.h file), preferably just after the constructors.
*
* In order to use this macro, the derived state_block class, StateBlockClass,
* must have the constructor callable as:
*
* StateBlockClass(const Eigen::VectorXd& _state, bool _fixed)
- *
+ *
* And the static method:
- *
+ *
* static Eigen::VectorXd Identity()
*/
-#define WOLF_STATE_BLOCK_CREATE(StateBlockClass) \
-static StateBlockPtr create(const Eigen::VectorXd& _state, bool _fixed) \
-{ \
- return std::make_shared<StateBlockClass>(_state, _fixed); \
-} \
-static StateBlockPtr createIdentity() \
-{ \
- return std::make_shared<StateBlockClass>(StateBlockClass::Identity(), false); \
-} \
+#define WOLF_STATE_BLOCK_CREATE(StateBlockClass) \
+ static StateBlockPtr create(const Eigen::VectorXd& _state, bool _fixed) \
+ { \
+ return std::make_shared<StateBlockClass>(_state, _fixed); \
+ } \
+ static StateBlockPtr createIdentity() \
+ { \
+ return std::make_shared<StateBlockClass>(StateBlockClass::Identity(), false); \
+ }
// Fwd references
-namespace wolf{
+namespace wolf
+{
class NodeBase;
class LocalParametrizationBase;
-}
+} // namespace wolf
-//Wolf includes
+// Wolf includes
#include "core/common/wolf.h"
#include "core/composite/vector_composite.h"
-//std includes
+// std includes
#include <iostream>
#include <mutex>
-namespace wolf {
-
+namespace wolf
+{
/** \brief class StateBlock
*
* This class implements a state block for general nonlinear optimization. It offers the following functionality:
@@ -72,200 +73,221 @@ namespace wolf {
*/
class StateBlock : public std::enable_shared_from_this<StateBlock>
{
-public:
-
- protected:
-
- std::string type_; ///< Type of the derived class
- std::atomic_bool fixed_; ///< Key to indicate whether the state is fixed or not
-
- std::atomic<SizeEigen> state_size_; ///< State vector size
- Eigen::VectorXd state_; ///< State vector storing the state values
- mutable std::mutex mut_state_; ///< State vector mutex
-
- LocalParametrizationBasePtr local_param_ptr_; ///< Local parametrization useful for optimizing in the tangent space to the manifold
-
- std::atomic_bool state_updated_; ///< Flag to indicate whether the state has been updated or not
- std::atomic_bool fix_updated_; ///< Flag to indicate whether the status has been updated or not
- std::atomic_bool local_param_updated_; ///< Flag to indicate whether the local_parameterization has been updated or not
-
- bool transformable_; ///< Flag to indicate if the state block can be transformed to another reference frame
-
- public:
- /** \brief Constructor from size
- *
- * \param _size is state size
- * \param _fixed Indicates this state is not estimated and thus acts as a fixed parameter
- * \param _local_param_ptr pointer to the local parametrization for the block
- */
- StateBlock(const std::string& _type, const SizeEigen _size, bool _fixed = false, LocalParametrizationBasePtr _local_param_ptr = nullptr, bool _transformable = true);
-
- /** \brief Constructor from vector
- *
- * \param _state is state vector
- * \param _fixed Indicates this state is not estimated and thus acts as a fixed parameter
- * \param _local_param_ptr pointer to the local parametrization for the block
- **/
- StateBlock(const std::string& _type, const Eigen::VectorXd& _state, bool _fixed = false, LocalParametrizationBasePtr _local_param_ptr = nullptr, bool _transformable = true);
-
- ///< Explicitly not copyable/movable
- StateBlock(const StateBlock& o) = delete;
- StateBlock(StateBlock&& o) = delete;
- StateBlock& operator=(const StateBlock& o) = delete;
-
- /** \brief Destructor
- **/
- virtual ~StateBlock();
-
- /** \brief Returns the type of the state block
- **/
- std::string getType() const;
-
- /** \brief Returns the state vector
- **/
- Eigen::VectorXd getState() const;
-
- /** \brief Returns the state vector data array
- **/
- double* getStateData();
-
- /** \brief Sets the state vector
- **/
- void setState(const Eigen::VectorXd& _state, const bool _notify = true);
-
- /** \brief Returns the state size
- **/
- SizeEigen getSize() const;
-
- /**\brief Returns the size of the local parametrization
- */
- SizeEigen getLocalSize() const;
+ public:
+ protected:
+ std::string type_; ///< Type of the derived class
+ std::atomic_bool fixed_; ///< Key to indicate whether the state is fixed or not
+
+ std::atomic<SizeEigen> state_size_; ///< State vector size
+ Eigen::VectorXd state_; ///< State vector storing the state values
+ mutable std::mutex mut_state_; ///< State vector mutex
+
+ LocalParametrizationBasePtr
+ local_param_ptr_; ///< Local parametrization useful for optimizing in the tangent space to the manifold
+
+ std::atomic_bool state_updated_; ///< Flag to indicate whether the state has been updated or not
+ std::atomic_bool fix_updated_; ///< Flag to indicate whether the status has been updated or not
+ std::atomic_bool
+ local_param_updated_; ///< Flag to indicate whether the local_parameterization has been updated or not
+
+ bool transformable_; ///< Flag to indicate if the state block can be transformed to another reference frame
+
+ public:
+ /** \brief Constructor from size
+ *
+ * \param _size is state size
+ * \param _fixed Indicates this state is not estimated and thus acts as a fixed parameter
+ * \param _local_param_ptr pointer to the local parametrization for the block
+ */
+ StateBlock(const std::string& _type,
+ const SizeEigen _size,
+ bool _fixed = false,
+ LocalParametrizationBasePtr _local_param_ptr = nullptr,
+ bool _transformable = true);
+
+ /** \brief Constructor from vector
+ *
+ * \param _state is state vector
+ * \param _fixed Indicates this state is not estimated and thus acts as a fixed parameter
+ * \param _local_param_ptr pointer to the local parametrization for the block
+ **/
+ StateBlock(const std::string& _type,
+ const Eigen::VectorXd& _state,
+ bool _fixed = false,
+ LocalParametrizationBasePtr _local_param_ptr = nullptr,
+ bool _transformable = true);
+
+ ///< Explicitly not copyable/movable
+ StateBlock(const StateBlock& o) = delete;
+ StateBlock(StateBlock&& o) = delete;
+ StateBlock& operator=(const StateBlock& o) = delete;
+
+ /** \brief Destructor
+ **/
+ virtual ~StateBlock();
+
+ /** \brief Returns the type of the state block
+ **/
+ std::string getType() const;
+
+ /** \brief Returns the state vector
+ **/
+ Eigen::VectorXd getState() const;
+
+ /** \brief Returns the state vector data array
+ **/
+ double* getStateData();
+
+ /** \brief Sets the state vector
+ **/
+ void setState(const Eigen::VectorXd& _state, const bool _notify = true);
+
+ /** \brief Returns the state size
+ **/
+ SizeEigen getSize() const;
+
+ /**\brief Returns the size of the local parametrization
+ */
+ SizeEigen getLocalSize() const;
+
+ /** \brief Returns if the state is fixed (not estimated)
+ **/
+ bool isFixed() const;
+
+ /** \brief Sets the state as fixed
+ **/
+ void fix();
+
+ /** \brief Sets the state as estimated
+ **/
+ void unfix();
+
+ /** \brief Sets the state status
+ **/
+ void setFixed(bool _fixed);
+
+ /** \brief Returns if the state has a local parametrization
+ **/
+ bool hasLocalParametrization() const;
+
+ /** \brief Returns the state local parametrization ptr
+ **/
+ LocalParametrizationBasePtr getLocalParametrization() const;
+
+ /** \brief Sets a local parametrization
+ **/
+ void setLocalParametrization(LocalParametrizationBasePtr _local_param);
+
+ /** \brief Removes the state_block local parametrization
+ **/
+ void removeLocalParametrization();
+
+ /** \brief Return if state has been updated
+ **/
+ bool stateUpdated() const;
+
+ /** \brief Return if fix/unfix has been updated
+ **/
+ bool fixUpdated() const;
+
+ /** \brief Return if local parameterization has been updated
+ **/
+ bool localParamUpdated() const;
+
+ /** \brief Set state_updated_ to false
+ **/
+ void resetStateUpdated();
+
+ /** \brief Set fix_updated_ to false
+ **/
+ void resetFixUpdated();
+
+ /** \brief Set localk_param_updated_ to false
+ **/
+ void resetLocalParamUpdated();
+
+ virtual void setIdentity(bool _notify = true);
+ void setZero(bool _notify = true);
+
+ virtual Eigen::VectorXd identity() const;
+ Eigen::VectorXd zero() const;
- /** \brief Returns if the state is fixed (not estimated)
- **/
- bool isFixed() const;
+ /**
+ * \brief perturb all states states with random noise
+ * following an uniform distribution in [ -amplitude, amplitude ]
+ */
+ void perturb(double amplitude = 0.1);
- /** \brief Sets the state as fixed
- **/
- void fix();
+ bool isTransformable() const
+ {
+ return transformable_;
+ }
- /** \brief Sets the state as estimated
- **/
- void unfix();
+ void setTransformable(bool _trf = true)
+ {
+ transformable_ = _trf;
+ }
- /** \brief Sets the state status
- **/
- void setFixed(bool _fixed);
+ void setNonTransformable()
+ {
+ transformable_ = false;
+ }
- /** \brief Returns if the state has a local parametrization
- **/
- bool hasLocalParametrization() const;
+ virtual void transform(const VectorComposite& _transformation) = 0;
- /** \brief Returns the state local parametrization ptr
- **/
- LocalParametrizationBasePtr getLocalParametrization() const;
-
- /** \brief Sets a local parametrization
- **/
- void setLocalParametrization(LocalParametrizationBasePtr _local_param);
-
- /** \brief Removes the state_block local parametrization
- **/
- void removeLocalParametrization();
-
- /** \brief Return if state has been updated
- **/
- bool stateUpdated() const;
-
- /** \brief Return if fix/unfix has been updated
- **/
- bool fixUpdated() const;
-
- /** \brief Return if local parameterization has been updated
- **/
- bool localParamUpdated() const;
-
- /** \brief Set state_updated_ to false
- **/
- void resetStateUpdated();
-
- /** \brief Set fix_updated_ to false
- **/
- void resetFixUpdated();
-
- /** \brief Set localk_param_updated_ to false
- **/
- void resetLocalParamUpdated();
-
- virtual void setIdentity(bool _notify = true);
- void setZero (bool _notify = true);
-
- virtual Eigen::VectorXd identity() const;
- Eigen::VectorXd zero() const;
-
- /**
- * \brief perturb all states states with random noise
- * following an uniform distribution in [ -amplitude, amplitude ]
- */
- void perturb(double amplitude = 0.1);
-
- bool isTransformable() const {
- return transformable_;
- }
-
- void setTransformable(bool _trf = true) {transformable_ = _trf;}
-
- void setNonTransformable() {transformable_ = false;}
-
- virtual void transform(const VectorComposite& _transformation) = 0;
-
- void plus(const Eigen::VectorXd& _dv);
-
- bool isValid(double tolerance = Constants::EPS);
+ void plus(const Eigen::VectorXd& _dv);
+ bool isValid(double tolerance = Constants::EPS);
};
-
-} // namespace wolf
+} // namespace wolf
// IMPLEMENTATION
#include "core/state_block/local_parametrization_base.h"
#include "core/common/node_base.h"
#include "core/problem/problem.h"
-namespace wolf {
-
-inline StateBlock::StateBlock(const std::string& _type, const Eigen::VectorXd& _state, bool _fixed, LocalParametrizationBasePtr _local_param_ptr, bool _transformable) :
- type_(_type),
- fixed_(_fixed),
- state_size_(_state.size()),
- state_(_state),
- local_param_ptr_(_local_param_ptr),
- state_updated_(false),
- fix_updated_(false),
- local_param_updated_(false),
- transformable_(_transformable)
+namespace wolf
{
-// std::cout << "constructed +sb" << std::endl;
+inline StateBlock::StateBlock(const std::string& _type,
+ const Eigen::VectorXd& _state,
+ bool _fixed,
+ LocalParametrizationBasePtr _local_param_ptr,
+ bool _transformable)
+ : type_(_type),
+ fixed_(_fixed),
+ state_size_(_state.size()),
+ state_(_state),
+ local_param_ptr_(_local_param_ptr),
+ state_updated_(false),
+ fix_updated_(false),
+ local_param_updated_(false),
+ transformable_(_transformable)
+{
+ // std::cout << "constructed +sb" << std::endl;
}
-inline StateBlock::StateBlock(const std::string& _type, const SizeEigen _size, bool _fixed, LocalParametrizationBasePtr _local_param_ptr, bool _transformable) :
- type_(_type),
- fixed_(_fixed),
- state_size_(_size),
- state_(Eigen::VectorXd::Zero(_size)),
- local_param_ptr_(_local_param_ptr),
- state_updated_(false),
- fix_updated_(false),
- local_param_updated_(false),
- transformable_(_transformable)
+inline StateBlock::StateBlock(const std::string& _type,
+ const SizeEigen _size,
+ bool _fixed,
+ LocalParametrizationBasePtr _local_param_ptr,
+ bool _transformable)
+ : type_(_type),
+ fixed_(_fixed),
+ state_size_(_size),
+ state_(Eigen::VectorXd::Zero(_size)),
+ local_param_ptr_(_local_param_ptr),
+ state_updated_(false),
+ fix_updated_(false),
+ local_param_updated_(false),
+ transformable_(_transformable)
{
// std::cout << "constructed +sb" << std::endl;
}
inline StateBlock::~StateBlock()
{
-// std::cout << "destructed -sb" << std::endl;
+ // std::cout << "destructed -sb" << std::endl;
}
inline std::string StateBlock::getType() const
@@ -286,8 +308,7 @@ inline SizeEigen StateBlock::getSize() const
inline SizeEigen StateBlock::getLocalSize() const
{
- if(local_param_ptr_)
- return local_param_ptr_->getLocalSize();
+ if (local_param_ptr_) return local_param_ptr_->getLocalSize();
return getSize();
}
@@ -318,14 +339,14 @@ inline LocalParametrizationBasePtr StateBlock::getLocalParametrization() const
inline void StateBlock::removeLocalParametrization()
{
- assert(local_param_ptr_ != nullptr && "state block without local parametrization");
+ assert(local_param_ptr_ != nullptr && "state block without local parametrization");
local_param_ptr_.reset();
local_param_updated_.store(true);
}
inline void StateBlock::setLocalParametrization(LocalParametrizationBasePtr _local_param)
{
- assert(_local_param != nullptr && "setting a null local parametrization");
+ assert(_local_param != nullptr && "setting a null local parametrization");
local_param_ptr_ = _local_param;
local_param_updated_.store(true);
}
@@ -367,7 +388,7 @@ inline double* StateBlock::getStateData()
inline void StateBlock::setIdentity(bool _notify)
{
- setState( Eigen::VectorXd::Zero(state_size_), _notify );
+ setState(Eigen::VectorXd::Zero(state_size_), _notify);
}
inline void StateBlock::setZero(bool _notify)
@@ -390,4 +411,4 @@ inline bool StateBlock::isValid(double tolerance)
return local_param_ptr_ ? local_param_ptr_->isValid(state_, tolerance) : true;
}
-}// namespace wolf
+} // namespace wolf
diff --git a/include/core/state_block/state_block_derived.h b/include/core/state_block/state_block_derived.h
index 508a405350d89ff9794f72d69af78b01fe32b9a8..d105cf2da5955749c97cae423635950c0b112c08 100644
--- a/include/core/state_block/state_block_derived.h
+++ b/include/core/state_block/state_block_derived.h
@@ -24,7 +24,6 @@
namespace wolf
{
-
/**
* @brief State block for general parameters
*
@@ -38,15 +37,16 @@ template <size_t size>
class StateParams : public StateBlock
{
public:
- StateParams(const Eigen::VectorXd& _state, bool _fixed = false) :
- StateBlock("StateParams" + toString(size), _state, _fixed, nullptr, false)
+ StateParams(const Eigen::VectorXd& _state, bool _fixed = false)
+ : StateBlock("StateParams" + toString(size), _state, _fixed, nullptr, false)
{
- if (_state.size() != size)
- throw std::runtime_error("Wrong vector size for StateParams" + toString(size) + ":" + toString(_state.size()));
+ if (_state.size() != size)
+ throw std::runtime_error("Wrong vector size for StateParams" + toString(size) + ":" +
+ toString(_state.size()));
}
static Eigen::VectorXd Identity()
{
- return Eigen::Matrix<double,size,1>::Zero();
+ return Eigen::Matrix<double, size, 1>::Zero();
}
WOLF_STATE_BLOCK_CREATE(StateParams<size>);
void transform(const VectorComposite& _transformation) override
@@ -81,7 +81,7 @@ class StatePoint2d : public StateBlock
StatePoint2d(const Eigen::VectorXd& _state, bool _fixed = false, bool _transformable = true)
: StateBlock("StatePoint2d", _state, _fixed, nullptr, _transformable)
{
- if (_state.size() != 2)
+ if (_state.size() != 2)
throw std::runtime_error("Wrong vector size for StatePoint2d: " + toString(_state.size()));
}
static Eigen::VectorXd Identity()
@@ -111,7 +111,7 @@ class StateVector2d : public StateBlock
StateVector2d(const Eigen::VectorXd& _state, bool _fixed = false, bool _transformable = true)
: StateBlock("StateVector2d", _state, _fixed, nullptr, _transformable)
{
- if (_state.size() != 2)
+ if (_state.size() != 2)
throw std::runtime_error("Wrong vector size for StateVector2d: " + toString(_state.size()));
}
static Eigen::VectorXd Identity()
@@ -137,12 +137,10 @@ class StateVector2d : public StateBlock
class StatePoint3d : public StateBlock
{
public:
- StatePoint3d(const Eigen::VectorXd& _state,
- bool _fixed = false,
- bool _transformable = true)
+ StatePoint3d(const Eigen::VectorXd& _state, bool _fixed = false, bool _transformable = true)
: StateBlock("StatePoint3d", _state, _fixed, nullptr, _transformable)
{
- if (_state.size() != 3)
+ if (_state.size() != 3)
throw std::runtime_error("Wrong vector size for StatePoint3d: " + toString(_state.size()));
}
static Eigen::VectorXd Identity()
@@ -173,7 +171,7 @@ class StateVector3d : public StateBlock
StateVector3d(const Eigen::VectorXd& _state, bool _fixed = false, bool _transformable = true)
: StateBlock("StateVector3d", _state, _fixed, nullptr, _transformable)
{
- if (_state.size() != 3)
+ if (_state.size() != 3)
throw std::runtime_error("Wrong vector size for StateVector3d: " + toString(_state.size()));
}
static Eigen::VectorXd Identity()
diff --git a/include/core/state_block/state_homogeneous_3d.h b/include/core/state_block/state_homogeneous_3d.h
index 608de913b16afae02fae8f0f13ffd5a2379ae8ce..2da7eba410d893a00dd275cf9aa1946245038e63 100644
--- a/include/core/state_block/state_homogeneous_3d.h
+++ b/include/core/state_block/state_homogeneous_3d.h
@@ -23,36 +23,35 @@
#include "core/state_block/state_block.h"
#include "core/state_block/local_parametrization_homogeneous.h"
-namespace wolf {
-
+namespace wolf
+{
class StateHomogeneous3d : public StateBlock
{
- public:
- StateHomogeneous3d(bool _fixed = false, bool _transformable = true);
- StateHomogeneous3d(const Eigen::VectorXd _state, bool _fixed = false, bool _transformable = true);
- ~StateHomogeneous3d() override;
- static Eigen::VectorXd Identity();
- WOLF_STATE_BLOCK_CREATE(StateHomogeneous3d);
+ public:
+ StateHomogeneous3d(bool _fixed = false, bool _transformable = true);
+ StateHomogeneous3d(const Eigen::VectorXd _state, bool _fixed = false, bool _transformable = true);
+ ~StateHomogeneous3d() override;
+ static Eigen::VectorXd Identity();
+ WOLF_STATE_BLOCK_CREATE(StateHomogeneous3d);
- void setIdentity(bool _notify = true) override;
- Eigen::VectorXd identity() const override;
+ void setIdentity(bool _notify = true) override;
+ Eigen::VectorXd identity() const override;
- virtual void transform(const VectorComposite& _transformation) override;
+ virtual void transform(const VectorComposite& _transformation) override;
};
-inline StateHomogeneous3d::StateHomogeneous3d(const Eigen::VectorXd _state, bool _fixed, bool _transformable) :
- StateBlock("StateHomogeneous3d", _state, _fixed, nullptr, _transformable)
+inline StateHomogeneous3d::StateHomogeneous3d(const Eigen::VectorXd _state, bool _fixed, bool _transformable)
+ : StateBlock("StateHomogeneous3d", _state, _fixed, nullptr, _transformable)
{
- if(_state.size() != 4)
- throw std::runtime_error("Homogeneous 3d must be size 4.");
+ if (_state.size() != 4) throw std::runtime_error("Homogeneous 3d must be size 4.");
local_param_ptr_ = std::make_shared<LocalParametrizationHomogeneous>();
}
-inline StateHomogeneous3d::StateHomogeneous3d(bool _fixed, bool _transformable) :
- StateBlock("StateHomogeneous3d", 4, _fixed, nullptr, _transformable)
+inline StateHomogeneous3d::StateHomogeneous3d(bool _fixed, bool _transformable)
+ : StateBlock("StateHomogeneous3d", 4, _fixed, nullptr, _transformable)
{
local_param_ptr_ = std::make_shared<LocalParametrizationHomogeneous>();
- state_ << 0, 0, 0, 1; // Set origin
+ state_ << 0, 0, 0, 1; // Set origin
}
inline StateHomogeneous3d::~StateHomogeneous3d()
@@ -78,7 +77,8 @@ inline Eigen::VectorXd StateHomogeneous3d::Identity()
inline void StateHomogeneous3d::transform(const VectorComposite& _transformation)
{
if (transformable_)
- setState((Quaterniond(_transformation.at('O').data()) * Quaterniond(getState().data())).coeffs()); // TODO is this correct?????? probably not!!!
+ setState((Quaterniond(_transformation.at('O').data()) * Quaterniond(getState().data()))
+ .coeffs()); // TODO is this correct?????? probably not!!!
}
-} // namespace wolf
+} // namespace wolf
diff --git a/include/core/state_block/state_quaternion.h b/include/core/state_block/state_quaternion.h
index 00d3c00bd62d2b56ba50d028e4cc29809b30a692..30dbf4b9dcc834fca80699bd2c57c825443b30ef 100644
--- a/include/core/state_block/state_quaternion.h
+++ b/include/core/state_block/state_quaternion.h
@@ -23,49 +23,58 @@
#include "core/state_block/state_block.h"
#include "core/state_block/local_parametrization_quaternion.h"
-namespace wolf {
-
+namespace wolf
+{
class StateQuaternion : public StateBlock
{
- public:
- StateQuaternion(bool _fixed = false, bool _transformable = true);
- StateQuaternion(const Eigen::VectorXd& _state, bool _fixed = false, bool _transformable = true);
- StateQuaternion(const Eigen::Quaterniond& _quaternion, bool _fixed = false, bool _transformable = true);
- ~StateQuaternion() override;
- static Eigen::VectorXd Identity();
- WOLF_STATE_BLOCK_CREATE(StateQuaternion);
-
- void setIdentity(bool _notify = true) override;
- Eigen::VectorXd identity() const override;
-
- virtual void transform(const VectorComposite& _transformation) override;
+ public:
+ StateQuaternion(bool _fixed = false, bool _transformable = true);
+ StateQuaternion(const Eigen::VectorXd& _state, bool _fixed = false, bool _transformable = true);
+ StateQuaternion(const Eigen::Quaterniond& _quaternion, bool _fixed = false, bool _transformable = true);
+ ~StateQuaternion() override;
+ static Eigen::VectorXd Identity();
+ WOLF_STATE_BLOCK_CREATE(StateQuaternion);
+
+ void setIdentity(bool _notify = true) override;
+ Eigen::VectorXd identity() const override;
+
+ virtual void transform(const VectorComposite& _transformation) override;
};
-inline StateQuaternion::StateQuaternion(const Eigen::Quaterniond& _quaternion, bool _fixed, bool _transformable) :
- StateBlock("StateQuaternion", _quaternion.coeffs(), _fixed, std::make_shared<LocalParametrizationQuaternion<DQ_LOCAL>>(), _transformable)
+inline StateQuaternion::StateQuaternion(const Eigen::Quaterniond& _quaternion, bool _fixed, bool _transformable)
+ : StateBlock("StateQuaternion",
+ _quaternion.coeffs(),
+ _fixed,
+ std::make_shared<LocalParametrizationQuaternion<DQ_LOCAL>>(),
+ _transformable)
{
// TODO: remove these lines after issue #381 is addressed
- if(not isValid(1e-5))
- throw std::runtime_error("Quaternion unit norm is worse than 1e-5 tolerance!");
-
+ if (not isValid(1e-5)) throw std::runtime_error("Quaternion unit norm is worse than 1e-5 tolerance!");
+
state_.normalize();
}
-inline StateQuaternion::StateQuaternion(const Eigen::VectorXd& _state, bool _fixed, bool _transformable) :
- StateBlock("StateQuaternion", _state, _fixed, std::make_shared<LocalParametrizationQuaternion<DQ_LOCAL>>(), _transformable)
+inline StateQuaternion::StateQuaternion(const Eigen::VectorXd& _state, bool _fixed, bool _transformable)
+ : StateBlock("StateQuaternion",
+ _state,
+ _fixed,
+ std::make_shared<LocalParametrizationQuaternion<DQ_LOCAL>>(),
+ _transformable)
{
- if(state_.size() != 4)
- throw std::runtime_error("The quaternion state vector must be of size 4!");
+ if (state_.size() != 4) throw std::runtime_error("The quaternion state vector must be of size 4!");
// TODO: remove these lines after issue #381 is addressed
- if(not isValid(1e-5))
- throw std::runtime_error("Quaternion unit norm is worse than 1e-5 tolerance!");
+ if (not isValid(1e-5)) throw std::runtime_error("Quaternion unit norm is worse than 1e-5 tolerance!");
state_.normalize();
}
-inline StateQuaternion::StateQuaternion(bool _fixed, bool _transformable) :
- StateBlock("StateQuaternion", 4, _fixed, std::make_shared<LocalParametrizationQuaternion<DQ_LOCAL>>(), _transformable)
+inline StateQuaternion::StateQuaternion(bool _fixed, bool _transformable)
+ : StateBlock("StateQuaternion",
+ 4,
+ _fixed,
+ std::make_shared<LocalParametrizationQuaternion<DQ_LOCAL>>(),
+ _transformable)
{
state_ = Eigen::Quaterniond::Identity().coeffs();
//
@@ -97,4 +106,4 @@ inline void StateQuaternion::transform(const VectorComposite& _transformation)
setState((Quaterniond(_transformation.at('O').data()) * Quaterniond(getState().data())).coeffs());
}
-} // namespace wolf
+} // namespace wolf
diff --git a/include/core/trajectory/trajectory_base.h b/include/core/trajectory/trajectory_base.h
index 60627e9d97770b37355e1f5ec3cbec5da3426830..5f16217e22fc415cd26c2044383d56413080b0a2 100644
--- a/include/core/trajectory/trajectory_base.h
+++ b/include/core/trajectory/trajectory_base.h
@@ -68,26 +68,21 @@ class TrajectoryBase : public NodeBase, public std::enable_shared_from_this<Traj
bool hasTimeStamp(const TimeStamp& _ts) const;
bool hasFrame(FrameBaseConstPtr _frame) const;
- virtual void printHeader(int depth, //
+ virtual void printHeader(int depth, //
bool factored_by, //
- bool metric, //
+ bool metric, //
bool state_blocks,
std::ostream& stream,
std::string _tabs = "") const;
- void print(int depth, //
+ void print(int depth, //
bool factored_by, //
- bool metric, //
+ bool metric, //
bool state_blocks,
std::ostream& stream = std::cout,
std::string _tabs = "") const;
- virtual CheckLog localCheck(bool _verbose,
- std::ostream& _stream,
- std::string _tabs = "") const;
- bool check(CheckLog& _log,
- bool _verbose,
- std::ostream& _stream,
- std::string _tabs = "") const;
+ virtual CheckLog localCheck(bool _verbose, std::ostream& _stream, std::string _tabs = "") const;
+ bool check(CheckLog& _log, bool _verbose, std::ostream& _stream, std::string _tabs = "") const;
private:
FrameBasePtr addFrame(FrameBasePtr _frame_ptr);
@@ -152,10 +147,11 @@ inline bool TrajectoryBase::hasTimeStamp(const TimeStamp& _ts) const
inline bool TrajectoryBase::hasFrame(FrameBaseConstPtr _frame) const
{
- return std::find_if(
- frame_map_.cbegin(), frame_map_.cend(), [&_frame](const std::pair<TimeStamp, FrameBaseConstPtr>& frm_it) {
- return frm_it.second == _frame;
- }) != frame_map_.end();
+ return std::find_if(frame_map_.cbegin(),
+ frame_map_.cend(),
+ [&_frame](const std::pair<TimeStamp, FrameBaseConstPtr>& frm_it) {
+ return frm_it.second == _frame;
+ }) != frame_map_.end();
}
} // namespace wolf
diff --git a/include/core/tree_manager/factory_tree_manager.h b/include/core/tree_manager/factory_tree_manager.h
index 7fcbe3ec641e6690af6817477b2c47d28b867344..802d1a6997d4a636b503792f49277c39052e197a 100644
--- a/include/core/tree_manager/factory_tree_manager.h
+++ b/include/core/tree_manager/factory_tree_manager.h
@@ -28,28 +28,32 @@
namespace wolf
{
-
-typedef Factory<std::shared_ptr<TreeManagerBase>,
- const YAML::Node&> FactoryTreeManager;
-template<>
-inline std::string FactoryTreeManager::getClass() const
+typedef Factory<std::shared_ptr<TreeManagerBase>, const YAML::Node&, const std::vector<std::string>&>
+ FactoryTreeManagerNode;
+template <>
+inline std::string FactoryTreeManagerNode::getClass() const
{
- return "FactoryTreeManager";
+ return "FactoryTreeManagerNode";
}
-typedef Factory<std::shared_ptr<TreeManagerBase>,
- const std::string&,
- const std::vector<std::string>&> FactoryTreeManagerYaml;
-template<>
-inline std::string FactoryTreeManagerYaml::getClass() const
+typedef Factory<std::shared_ptr<TreeManagerBase>, const std::string&, const std::vector<std::string>&>
+ FactoryTreeManagerFile;
+template <>
+inline std::string FactoryTreeManagerFile::getClass() const
{
- return "FactoryTreeManagerYaml";
+ return "FactoryTreeManagerFile";
}
-#define WOLF_REGISTER_TREE_MANAGER(TreeManagerType) \
- namespace{ const bool WOLF_UNUSED TreeManagerType##Registered = \
- wolf::FactoryTreeManager::registerCreator(#TreeManagerType, TreeManagerType::create); } \
- namespace{ const bool WOLF_UNUSED TreeManagerType##YamlRegistered = \
- wolf::FactoryTreeManagerYaml::registerCreator(#TreeManagerType, TreeManagerType::create); } \
+#define WOLF_REGISTER_TREE_MANAGER(TreeManagerType) \
+ namespace \
+ { \
+ const bool WOLF_UNUSED TreeManagerType##Registered = \
+ wolf::FactoryTreeManagerNode::registerCreator(#TreeManagerType, TreeManagerType::create); \
+ } \
+ namespace \
+ { \
+ const bool WOLF_UNUSED TreeManagerType##YamlRegistered = \
+ wolf::FactoryTreeManagerFile::registerCreator(#TreeManagerType, TreeManagerType::create); \
+ }
} /* namespace wolf */
diff --git a/include/core/tree_manager/tree_manager_base.h b/include/core/tree_manager/tree_manager_base.h
index 3e43ccfab8eab538827bbc3212df9e0fc96ba4a3..d6b1881487d4de0162c5585d58f7720a4c7c87dc 100644
--- a/include/core/tree_manager/tree_manager_base.h
+++ b/include/core/tree_manager/tree_manager_base.h
@@ -23,7 +23,6 @@
// Wolf includes
#include "core/common/wolf.h"
#include "core/common/node_base.h"
-#include "core/common/params_base.h"
#include "core/problem/problem.h"
#include "core/tree_manager/factory_tree_manager.h"
@@ -38,65 +37,50 @@ namespace wolf
* In order to use this macro, the derived processor class, ProcessorClass,
* must have a constructor available with the API:
*
- * TreeManagerClass(const ParamsTreeManagerPtr _params);
- *
- * Also, there should be the schema file 'TreeManagerClass.schema' containing the specifications
+ * TreeManagerClass(const YAML::Node& _params);
+ *
+ * Also, there should be the schema file 'TreeManagerClass.schema' containing the specifications
* of the user input yaml file.
*/
-#define WOLF_TREE_MANAGER_CREATE(TreeManagerClass, ParamsTreeManagerClass) \
-static TreeManagerBasePtr create(const YAML::Node& _input_node) \
-{ \
- auto params = std::make_shared<ParamsTreeManagerClass>(_input_node); \
- \
- return std::make_shared<TreeManagerClass>(params); \
-} \
-static TreeManagerBasePtr create(const std::string& _yaml_filepath, \
- const std::vector<std::string>& _folders_schema) \
-{ \
- auto server = yaml_schema_cpp::YamlServer(_folders_schema, _yaml_filepath); \
- \
- if (not server.applySchema(#TreeManagerClass)) \
- { \
- WOLF_ERROR(server.getLog()); \
- return nullptr; \
- } \
- auto params = std::make_shared<ParamsTreeManagerClass>(server.getNode()); \
- \
- return std::make_shared<TreeManagerClass>(params); \
-} \
-
-struct ParamsTreeManagerBase : public ParamsBase
-{
- ParamsTreeManagerBase() = default;
- ParamsTreeManagerBase(const YAML::Node& _input_node):
- ParamsBase(_input_node)
- {
+#define WOLF_TREE_MANAGER_CREATE(TreeManagerClass) \
+ static TreeManagerBasePtr create(const YAML::Node& _input_node, \
+ const std::vector<std::string>& _folders_schema = {}) \
+ { \
+ if (not _folders_schema.empty()) \
+ { \
+ auto server = yaml_schema_cpp::YamlServer(_folders_schema); \
+ server.setYaml(_input_node); \
+ if (not server.applySchema(#TreeManagerClass)) \
+ { \
+ throw std::runtime_error(server.getLog()); \
+ } \
+ } \
+ return std::shared_ptr<TreeManagerClass>(new TreeManagerClass(_input_node)); \
+ } \
+ static TreeManagerBasePtr create(const std::string& _yaml_filepath, \
+ const std::vector<std::string>& _folders_schema) \
+ { \
+ auto server = yaml_schema_cpp::YamlServer(_folders_schema, _yaml_filepath); \
+ if (not server.applySchema(#TreeManagerClass)) \
+ { \
+ throw std::runtime_error(server.getLog()); \
+ } \
+ return create(server.getNode(), {}); \
}
- ~ParamsTreeManagerBase() override = default;
-
- std::string print() const override
- {
- return "";
- }
-};
-
class TreeManagerBase : public NodeBase
{
- public:
- TreeManagerBase(const std::string& _type, ParamsTreeManagerBasePtr _params) :
- NodeBase("TREE_MANAGER", _type),
- params_(_params)
- {}
+ public:
+ TreeManagerBase(const std::string& _type, const YAML::Node& _params) : NodeBase("TREE_MANAGER", _type) {}
- virtual ~TreeManagerBase() {}
+ virtual ~TreeManagerBase() {}
- virtual void keyFrameCallback(FrameBasePtr _key_frame) = 0;
+ virtual void keyFrameCallback(FrameBasePtr _key_frame) = 0;
- bool hasChildren() const override{return true;};
-
- protected:
- ParamsTreeManagerBasePtr params_;
+ bool hasChildren() const override
+ {
+ return true;
+ };
};
} /* namespace wolf */
diff --git a/include/core/tree_manager/tree_manager_sliding_window.h b/include/core/tree_manager/tree_manager_sliding_window.h
index 9f0c0e8c0749de3f15ef073e8f3e2aca6868fafd..377230c025853387691f0779d119d27df9ae9d71 100644
--- a/include/core/tree_manager/tree_manager_sliding_window.h
+++ b/include/core/tree_manager/tree_manager_sliding_window.h
@@ -24,50 +24,23 @@
namespace wolf
{
-
-WOLF_STRUCT_PTR_TYPEDEFS(ParamsTreeManagerSlidingWindow)
WOLF_PTR_TYPEDEFS(TreeManagerSlidingWindow)
-struct ParamsTreeManagerSlidingWindow : public ParamsTreeManagerBase
-{
- ParamsTreeManagerSlidingWindow() = default;
- ParamsTreeManagerSlidingWindow(const YAML::Node& _node_input) :
- ParamsTreeManagerBase(_node_input)
- {
- n_frames = _node_input["n_frames"] .as<unsigned int>();
- n_fix_first_frames = _node_input["n_fix_first_frames"] .as<unsigned int>();
- viral_remove_parent_without_children = _node_input["viral_remove_parent_without_children"] .as<bool>();
- if (n_frames <= n_fix_first_frames)
- throw std::runtime_error("TreeManagerSlidingWindow: Wrong parameter value. 'n_fix_first_frames' should be lower than 'n_frames'!");
- }
- std::string print() const override
- {
- return ParamsTreeManagerBase::print() + "\n"
- + "n_frames: " + toString(n_frames) + "\n"
- + "n_fix_first_frames: " + toString(n_fix_first_frames) + "\n"
- + "viral_remove_parent_without_children: " + toString(viral_remove_parent_without_children) + "\n";
- }
-
- unsigned int n_frames;
- unsigned int n_fix_first_frames;
- bool viral_remove_parent_without_children;
-};
-
class TreeManagerSlidingWindow : public TreeManagerBase
{
- public:
- TreeManagerSlidingWindow(ParamsTreeManagerSlidingWindowPtr _params) :
- TreeManagerBase("TreeManagerSlidingWindow", _params),
- params_sw_(_params)
- {};
- WOLF_TREE_MANAGER_CREATE(TreeManagerSlidingWindow, ParamsTreeManagerSlidingWindow)
+ public:
+ TreeManagerSlidingWindow(const YAML::Node& _params);
+ WOLF_TREE_MANAGER_CREATE(TreeManagerSlidingWindow)
- ~TreeManagerSlidingWindow() override{}
+ ~TreeManagerSlidingWindow() override {}
- void keyFrameCallback(FrameBasePtr _frame) override;
+ void keyFrameCallback(FrameBasePtr _frame) override;
- protected:
- ParamsTreeManagerSlidingWindowPtr params_sw_;
+ protected:
+ // PARAMS
+ unsigned int n_frames_;
+ unsigned int n_fix_first_frames_;
+ bool viral_remove_parent_without_children_;
};
} /* namespace wolf */
diff --git a/include/core/tree_manager/tree_manager_sliding_window_dual_rate.h b/include/core/tree_manager/tree_manager_sliding_window_dual_rate.h
index 33dd309665cd24a77b86a6cbcdd065a0488c551e..fc77e1374dc82a9aaf1e97f8e00700306549488d 100644
--- a/include/core/tree_manager/tree_manager_sliding_window_dual_rate.h
+++ b/include/core/tree_manager/tree_manager_sliding_window_dual_rate.h
@@ -24,48 +24,25 @@
namespace wolf
{
-
-WOLF_STRUCT_PTR_TYPEDEFS(ParamsTreeManagerSlidingWindowDualRate)
WOLF_PTR_TYPEDEFS(TreeManagerSlidingWindowDualRate)
-struct ParamsTreeManagerSlidingWindowDualRate : public ParamsTreeManagerSlidingWindow
+class TreeManagerSlidingWindowDualRate : public TreeManagerSlidingWindow
{
- ParamsTreeManagerSlidingWindowDualRate() = default;
- ParamsTreeManagerSlidingWindowDualRate(const YAML::Node& _input_node) :
- ParamsTreeManagerSlidingWindow(_input_node)
- {
- n_frames_recent = _input_node["n_frames_recent"].as<unsigned int>();
- rate_old_frames = _input_node["rate_old_frames"].as<unsigned int>();
- if (n_frames_recent >= n_frames)
- {
- throw std::runtime_error("ParamsTreeManagerSlidingWindowDualRate: 'n_frames_recent' should be smaller than 'n_frames'");
- }
- }
- std::string print() const override
- {
- return ParamsTreeManagerBase::print() + "\n"
- + "n_frames_recent: " + toString(n_frames_recent) + "\n"
- + "rate_old_frames: " + toString(rate_old_frames) + "\n";
- }
+ public:
+ TreeManagerSlidingWindowDualRate(const YAML::Node& _params);
- unsigned int n_frames_recent, rate_old_frames;
-};
+ WOLF_TREE_MANAGER_CREATE(TreeManagerSlidingWindowDualRate)
-class TreeManagerSlidingWindowDualRate : public TreeManagerSlidingWindow
-{
- public:
- TreeManagerSlidingWindowDualRate(ParamsTreeManagerSlidingWindowDualRatePtr _params);
-
- WOLF_TREE_MANAGER_CREATE(TreeManagerSlidingWindowDualRate, ParamsTreeManagerSlidingWindowDualRate)
+ ~TreeManagerSlidingWindowDualRate() override {}
- ~TreeManagerSlidingWindowDualRate() override{}
+ void keyFrameCallback(FrameBasePtr _frame) override;
- void keyFrameCallback(FrameBasePtr _frame) override;
+ protected:
+ unsigned int count_frames_; ///< counter of frames
- protected:
- ParamsTreeManagerSlidingWindowDualRatePtr params_swdr_;
- unsigned int count_frames_;
- //TrajectoryIter first_recent_frame_it_;
+ // PARAMS
+ unsigned int n_frames_recent_;
+ unsigned int rate_old_frames_;
};
} /* namespace wolf */
diff --git a/include/core/utils/eigen_assert.h b/include/core/utils/eigen_assert.h
index a6035cc8b236e8199146a0daac0b5e67d1298e6b..c478277fc3a82d8ccccc7e7ab945bbf5242c0cff 100644
--- a/include/core/utils/eigen_assert.h
+++ b/include/core/utils/eigen_assert.h
@@ -22,8 +22,8 @@
#include <Eigen/Dense>
-namespace Eigen {
-
+namespace Eigen
+{
//////////////////////////////////////////////////////////
/** Check Eigen Matrix sizes, both statically and dynamically
*
@@ -32,7 +32,8 @@ namespace Eigen {
* The WOLF project implements many template functions using Eigen Matrix and Quaterniond, in different versions
* (Static size, Dynamic size, Map, Matrix expression).
*
- * Eigen provides some macros for STATIC assert of matrix sizes, the most common of them are (see Eigen's StaticAssert.h):
+ * Eigen provides some macros for STATIC assert of matrix sizes, the most common of them are (see Eigen's
+ * StaticAssert.h):
*
* EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE
* EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE
@@ -40,7 +41,8 @@ namespace Eigen {
*
* but they do not work if the evaluated types are of dynamic size.
*
- * In order to achieve full templatization over both dynamic and static sizes, we use extensively a prototype of this kind:
+ * In order to achieve full templatization over both dynamic and static sizes, we use extensively a prototype of this
+ * kind:
*
* template<typename Derived>
* inline Eigen::Matrix<typename Derived::Scalar, 3, 3> function(const Eigen::MatrixBase<Derived>& _v){
@@ -57,44 +59,44 @@ namespace Eigen {
* The function : MatrixSizeCheck <Rows, Cols>::check(M) checks that the Matrix M is of size ( Rows x Cols ).
* This check is performed statically or dynamically, depending on the type of argument provided.
*/
-template<int Size, int DesiredSize>
+template <int Size, int DesiredSize>
struct StaticDimCheck
{
- template<typename T>
- StaticDimCheck(const T&)
- {
- static_assert(Size == DesiredSize, "Size of static Vector or Matrix does not match");
- }
+ template <typename T>
+ StaticDimCheck(const T&)
+ {
+ static_assert(Size == DesiredSize, "Size of static Vector or Matrix does not match");
+ }
};
-template<int DesiredSize>
+template <int DesiredSize>
struct StaticDimCheck<Eigen::Dynamic, DesiredSize>
{
- template<typename T>
- StaticDimCheck(const T& t)
- {
- if (t != DesiredSize) std::cerr << "t : " << t << " != DesiredSize : " << DesiredSize << std::endl;
- assert(t == DesiredSize && "Size of dynamic Vector or Matrix does not match");
- }
+ template <typename T>
+ StaticDimCheck(const T& t)
+ {
+ if (t != DesiredSize) std::cerr << "t : " << t << " != DesiredSize : " << DesiredSize << std::endl;
+ assert(t == DesiredSize && "Size of dynamic Vector or Matrix does not match");
+ }
};
-template<int DesiredR, int DesiredC>
+template <int DesiredR, int DesiredC>
struct MatrixSizeCheck
{
- /** \brief Assert matrix size dynamically or statically
- *
- * @param t the variable for which we wish to assert the size.
- *
- * Usage: to assert that t is size (Rows x Cols)
- *
- * MatrixSizeCheck<Rows, Cols>::check(t);
- */
- template<typename T>
- static void check(const Eigen::MatrixBase<T>& t)
- {
- StaticDimCheck<Eigen::MatrixBase<T>::RowsAtCompileTime, DesiredR>(t.rows());
- StaticDimCheck<Eigen::MatrixBase<T>::ColsAtCompileTime, DesiredC>(t.cols());
- }
+ /** \brief Assert matrix size dynamically or statically
+ *
+ * @param t the variable for which we wish to assert the size.
+ *
+ * Usage: to assert that t is size (Rows x Cols)
+ *
+ * MatrixSizeCheck<Rows, Cols>::check(t);
+ */
+ template <typename T>
+ static void check(const Eigen::MatrixBase<T>& t)
+ {
+ StaticDimCheck<Eigen::MatrixBase<T>::RowsAtCompileTime, DesiredR>(t.rows());
+ StaticDimCheck<Eigen::MatrixBase<T>::ColsAtCompileTime, DesiredC>(t.cols());
+ }
};
template <int Dim>
diff --git a/include/core/utils/graph_search.h b/include/core/utils/graph_search.h
index 142f40d06626df24dd9e502852fb8028172df66c..b5e4abcd6e5e37f6dba5f8b2157c422e1cdafef2 100644
--- a/include/core/utils/graph_search.h
+++ b/include/core/utils/graph_search.h
@@ -28,34 +28,30 @@
namespace wolf
{
-
class GraphSearch
{
- private:
-
- std::map<NodeStateBlocksPtr, std::pair<FactorBasePtr,NodeStateBlocksPtr>> parents_;
-
- public:
-
- GraphSearch();
+ private:
+ std::map<NodeStateBlocksPtr, std::pair<FactorBasePtr, NodeStateBlocksPtr>> parents_;
- ~GraphSearch();
+ public:
+ GraphSearch();
- FactorBasePtrList computeShortestPath(NodeStateBlocksPtr node1,
- NodeStateBlocksPtr node2,
- const unsigned int max_graph_dist = 0);
+ ~GraphSearch();
- std::set<NodeStateBlocksPtr> getNeighborFrames(const std::set<NodeStateBlocksPtr>& nodes);
+ FactorBasePtrList computeShortestPath(NodeStateBlocksPtr node1,
+ NodeStateBlocksPtr node2,
+ const unsigned int max_graph_dist = 0);
- static FactorBasePtrList shortestPath(NodeStateBlocksPtr node1,
- NodeStateBlocksPtr node2,
- const unsigned int max_graph_dist = 0)
- {
- GraphSearch graph_search;
+ std::set<NodeStateBlocksPtr> getNeighborFrames(const std::set<NodeStateBlocksPtr>& nodes);
- return graph_search.computeShortestPath(node1, node2, max_graph_dist);
- }
+ static FactorBasePtrList shortestPath(NodeStateBlocksPtr node1,
+ NodeStateBlocksPtr node2,
+ const unsigned int max_graph_dist = 0)
+ {
+ GraphSearch graph_search;
+ return graph_search.computeShortestPath(node1, node2, max_graph_dist);
+ }
};
} // namespace wolf
diff --git a/include/core/utils/loader.h b/include/core/utils/loader.h
index 23a54c750e8f5a6c785c5c60c84dc4d1e150251a..8255d1ec4506c14a3c722cffc722ad4580028302 100644
--- a/include/core/utils/loader.h
+++ b/include/core/utils/loader.h
@@ -27,7 +27,6 @@
namespace wolf
{
-
class Loader
{
protected:
diff --git a/include/core/utils/logging.h b/include/core/utils/logging.h
index 09af0b8d11ee66997bde8db73be9ea170e296afd..86b02e32fa48426f951689410836cdd7dd4a256b 100644
--- a/include/core/utils/logging.h
+++ b/include/core/utils/logging.h
@@ -31,235 +31,231 @@
// Wolf includes
#include "core/utils/singleton.h"
-namespace wolf {
-namespace internal {
-namespace do_not_enter_where_the_wolf_lives {
-
+namespace wolf
+{
+namespace internal
+{
+namespace do_not_enter_where_the_wolf_lives
+{
#define __INTERNAL_WOLF_MAIN_LOGGER_NAME_ "wolf_main_console"
-static const auto repeated_brace = std::make_tuple("{}",
- "{}{}",
- "{}{}{}",
- "{}{}{}{}",
- "{}{}{}{}{}",
- "{}{}{}{}{}{}",
- "{}{}{}{}{}{}{}",
- "{}{}{}{}{}{}{}{}",
- "{}{}{}{}{}{}{}{}{}",
- "{}{}{}{}{}{}{}{}{}{}",
- "{}{}{}{}{}{}{}{}{}{}{}",
- "{}{}{}{}{}{}{}{}{}{}{}{}",
- "{}{}{}{}{}{}{}{}{}{}{}{}{}",
- "{}{}{}{}{}{}{}{}{}{}{}{}{}{}",
- "{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}",
- "{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}",
- "{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}",
- "{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}",
- "{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}",
- "{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}",
- "{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}",
- "{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}",
- "{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}",
- "{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}",
- "{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}"); // up to 25 args. Should be fine
+static const auto repeated_brace =
+ std::make_tuple("{}",
+ "{}{}",
+ "{}{}{}",
+ "{}{}{}{}",
+ "{}{}{}{}{}",
+ "{}{}{}{}{}{}",
+ "{}{}{}{}{}{}{}",
+ "{}{}{}{}{}{}{}{}",
+ "{}{}{}{}{}{}{}{}{}",
+ "{}{}{}{}{}{}{}{}{}{}",
+ "{}{}{}{}{}{}{}{}{}{}{}",
+ "{}{}{}{}{}{}{}{}{}{}{}{}",
+ "{}{}{}{}{}{}{}{}{}{}{}{}{}",
+ "{}{}{}{}{}{}{}{}{}{}{}{}{}{}",
+ "{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}",
+ "{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}",
+ "{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}",
+ "{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}",
+ "{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}",
+ "{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}",
+ "{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}",
+ "{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}",
+ "{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}",
+ "{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}",
+ "{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}"); // up to 25 args. Should be fine
class Logger
{
-public:
+ public:
+ Logger(const std::string& name);
- Logger(const std::string& name);
+ Logger(std::string&& name);
- Logger(std::string&& name);
+ ~Logger();
- ~Logger();
+ // Not copyable/movable
+ Logger(Logger&) = delete;
+ void operator=(Logger&) = delete;
+ Logger(Logger&&) = delete;
+ void operator=(Logger&&) = delete;
- // Not copyable/movable
- Logger(Logger&) = delete;
- void operator=(Logger&) = delete;
- Logger(Logger&&) = delete;
- void operator=(Logger&&) = delete;
+ template <typename... Args>
+ void info(Args&&... args) const;
- template<typename... Args>
- void info(Args&&... args) const;
+ template <typename... Args>
+ void warn(Args&&... args) const;
- template<typename... Args>
- void warn(Args&&... args) const;
+ template <typename... Args>
+ void error(Args&&... args) const;
- template<typename... Args>
- void error(Args&&... args) const;
+ template <typename... Args>
+ void debug(Args&&... args) const;
- template<typename... Args>
- void debug(Args&&... args) const;
+ template <typename... Args>
+ void trace(Args&&... args) const;
- template<typename... Args>
- void trace(Args&&... args) const;
+ bool set_async_queue(const std::size_t q_size);
- bool set_async_queue(const std::size_t q_size);
+ void set_pattern(const std::string& p);
- void set_pattern(const std::string& p);
+ protected:
+ const std::string log_name_;
-protected:
-
- const std::string log_name_;
-
- std::shared_ptr<spdlog::logger> console_;
+ std::shared_ptr<spdlog::logger> console_;
};
-inline Logger::Logger(const std::string& name) :
- log_name_(name)
+inline Logger::Logger(const std::string& name) : log_name_(name)
{
- // Create main logger
- console_ = spdlog::stdout_color_mt(log_name_);
+ // Create main logger
+ console_ = spdlog::stdout_color_mt(log_name_);
#ifdef _WOLF_TRACE
- console_->set_level(spdlog::level::trace);
+ console_->set_level(spdlog::level::trace);
#endif
- // Enable asynchronous logging
- // Queue size must be a power of 2
- spdlog::set_async_mode(4096);
-
- if (log_name_ == __INTERNAL_WOLF_MAIN_LOGGER_NAME_)
- // Logging pattern is :
- // [thread num][hour:minutes:seconds.nanoseconds][log type] #log-content
- //set_pattern("[%t][%H:%M:%S.%F][%l] %v");
- // [log type][MM/DD/YY - hour:minutes:seconds.nanoseconds] #log-content
-// set_pattern("[%l][%x - %H:%M:%S.%F] %v");
- set_pattern("[%l][%H:%M:%S] %v");
- else
- // Logging pattern is :
- // [logger name][thread num][hour:minutes:seconds.nanoseconds][log type] #log-content
- //set_pattern("[" + log_name_ + "]" +"[%t][%H:%M:%S.%F][%l] %v");
- // [log type][MM/DD/YY - hour:minutes:seconds.nanoseconds][logger name] #log-content
- set_pattern("[%l][%x - %H:%M:%S.%F][" + log_name_ + "] %v");
+ // Enable asynchronous logging
+ // Queue size must be a power of 2
+ spdlog::set_async_mode(4096);
+
+ if (log_name_ == __INTERNAL_WOLF_MAIN_LOGGER_NAME_)
+ // Logging pattern is :
+ // [thread num][hour:minutes:seconds.nanoseconds][log type] #log-content
+ // set_pattern("[%t][%H:%M:%S.%F][%l] %v");
+ // [log type][MM/DD/YY - hour:minutes:seconds.nanoseconds] #log-content
+ // set_pattern("[%l][%x - %H:%M:%S.%F] %v");
+ set_pattern("[%l][%H:%M:%S] %v");
+ else
+ // Logging pattern is :
+ // [logger name][thread num][hour:minutes:seconds.nanoseconds][log type] #log-content
+ // set_pattern("[" + log_name_ + "]" +"[%t][%H:%M:%S.%F][%l] %v");
+ // [log type][MM/DD/YY - hour:minutes:seconds.nanoseconds][logger name] #log-content
+ set_pattern("[%l][%x - %H:%M:%S.%F][" + log_name_ + "] %v");
}
-inline Logger::Logger(std::string&& name) :
- log_name_(std::forward<std::string>(name))
+inline Logger::Logger(std::string&& name) : log_name_(std::forward<std::string>(name))
{
- // Create main logger
- console_ = spdlog::stdout_color_mt(log_name_);
+ // Create main logger
+ console_ = spdlog::stdout_color_mt(log_name_);
#ifdef _WOLF_TRACE
- console_->set_level(spdlog::level::trace);
+ console_->set_level(spdlog::level::trace);
#endif
- // Enable asynchronous logging
- // Queue size must be a power of 2
- spdlog::set_async_mode(4096);
-
- if (log_name_ == __INTERNAL_WOLF_MAIN_LOGGER_NAME_)
- // Logging pattern is :
- // [thread num][hour:minutes:seconds.nanoseconds][log type] #log-content
- //set_pattern("[%t][%H:%M:%S.%F][%l] %v");
- // [log type][MM/DD/YY - hour:minutes:seconds.nanoseconds] #log-content
-// set_pattern("[%l][%x - %H:%M:%S.%F] %v");
- set_pattern("[%l][%H:%M:%S] %v");
- else
- // Logging pattern is :
- // [logger name][thread num][hour:minutes:seconds.nanoseconds][log type] #log-content
- //set_pattern("[" + log_name_ + "]" +"[%t][%H:%M:%S.%F][%l] %v");
- // [log type][MM/DD/YY - hour:minutes:seconds.nanoseconds][logger name] #log-content
- set_pattern("[%l][%x - %H:%M:%S.%F][" + log_name_ + "] %v");
+ // Enable asynchronous logging
+ // Queue size must be a power of 2
+ spdlog::set_async_mode(4096);
+
+ if (log_name_ == __INTERNAL_WOLF_MAIN_LOGGER_NAME_)
+ // Logging pattern is :
+ // [thread num][hour:minutes:seconds.nanoseconds][log type] #log-content
+ // set_pattern("[%t][%H:%M:%S.%F][%l] %v");
+ // [log type][MM/DD/YY - hour:minutes:seconds.nanoseconds] #log-content
+ // set_pattern("[%l][%x - %H:%M:%S.%F] %v");
+ set_pattern("[%l][%H:%M:%S] %v");
+ else
+ // Logging pattern is :
+ // [logger name][thread num][hour:minutes:seconds.nanoseconds][log type] #log-content
+ // set_pattern("[" + log_name_ + "]" +"[%t][%H:%M:%S.%F][%l] %v");
+ // [log type][MM/DD/YY - hour:minutes:seconds.nanoseconds][logger name] #log-content
+ set_pattern("[%l][%x - %H:%M:%S.%F][" + log_name_ + "] %v");
}
inline Logger::~Logger()
{
- spdlog::drop(log_name_);
+ spdlog::drop(log_name_);
}
-template<typename... Args>
+template <typename... Args>
void Logger::info(Args&&... args) const
{
- console_->info(std::get<sizeof...(args)-1>(repeated_brace), std::forward<Args>(args)...);
+ console_->info(std::get<sizeof...(args) - 1>(repeated_brace), std::forward<Args>(args)...);
}
-template<typename... Args>
+template <typename... Args>
void Logger::warn(Args&&... args) const
{
- console_->warn(std::get<sizeof...(args)-1>(repeated_brace), std::forward<Args>(args)...);
+ console_->warn(std::get<sizeof...(args) - 1>(repeated_brace), std::forward<Args>(args)...);
}
-template<typename... Args>
+template <typename... Args>
void Logger::error(Args&&... args) const
{
- console_->error(std::get<sizeof...(args)-1>(repeated_brace), std::forward<Args>(args)...);
+ console_->error(std::get<sizeof...(args) - 1>(repeated_brace), std::forward<Args>(args)...);
}
-template<typename... Args>
+template <typename... Args>
void Logger::debug(Args&&... args) const
{
- console_->debug(std::get<sizeof...(args)-1>(repeated_brace), std::forward<Args>(args)...);
+ console_->debug(std::get<sizeof...(args) - 1>(repeated_brace), std::forward<Args>(args)...);
}
-template<typename... Args>
+template <typename... Args>
void Logger::trace(Args&&... args) const
{
- console_->trace(std::get<sizeof...(args)-1>(repeated_brace), std::forward<Args>(args)...);
+ console_->trace(std::get<sizeof...(args) - 1>(repeated_brace), std::forward<Args>(args)...);
}
inline bool Logger::set_async_queue(const std::size_t q_size)
{
- bool p2 = q_size%2 == 0;
+ bool p2 = q_size % 2 == 0;
- if (p2) spdlog::set_async_mode(q_size);
+ if (p2) spdlog::set_async_mode(q_size);
- return q_size;
+ return q_size;
}
inline void Logger::set_pattern(const std::string& p)
{
- console_->set_pattern(p);
+ console_->set_pattern(p);
}
using LoggerPtr = std::unique_ptr<Logger>;
class LoggerManager
{
-public:
-
- LoggerManager() = default;
- ~LoggerManager() = default;
-
- bool exists(const std::string& name) const
- {
- std::lock_guard<std::mutex> lock(mut_);
- return existsImpl(name);
- }
-
- const Logger& getLogger(const std::string& name) /*const*/
- {
- std::lock_guard<std::mutex> lock(mut_);
-
- if (!existsImpl(name)) addLogger(name);
-
- return *(logger_map_.at(name));
- }
-
-protected:
-
- mutable std::mutex mut_;
-
- std::map<const std::string, const LoggerPtr> logger_map_;
-
- bool addLogger(const std::string& name)
- {
- /// @note would be easier with cpp17 map.try_emplace...
- const bool created = existsImpl(name) ?
- false :
- logger_map_.emplace(name, std::make_unique<Logger>(name)).second;
- return created;
- }
-
- bool existsImpl(const std::string& name) const
- {
- return (logger_map_.find(name) != logger_map_.end());
- //return (spdlog::get(name) != nullptr);
- }
+ public:
+ LoggerManager() = default;
+ ~LoggerManager() = default;
+
+ bool exists(const std::string& name) const
+ {
+ std::lock_guard<std::mutex> lock(mut_);
+ return existsImpl(name);
+ }
+
+ const Logger& getLogger(const std::string& name) /*const*/
+ {
+ std::lock_guard<std::mutex> lock(mut_);
+
+ if (!existsImpl(name)) addLogger(name);
+
+ return *(logger_map_.at(name));
+ }
+
+ protected:
+ mutable std::mutex mut_;
+
+ std::map<const std::string, const LoggerPtr> logger_map_;
+
+ bool addLogger(const std::string& name)
+ {
+ /// @note would be easier with cpp17 map.try_emplace...
+ const bool created =
+ existsImpl(name) ? false : logger_map_.emplace(name, std::make_unique<Logger>(name)).second;
+ return created;
+ }
+
+ bool existsImpl(const std::string& name) const
+ {
+ return (logger_map_.find(name) != logger_map_.end());
+ // return (spdlog::get(name) != nullptr);
+ }
};
} /* namespace do_not_enter_where_the_wolf_lives */
-using WolfLogger = Singleton<do_not_enter_where_the_wolf_lives::Logger>;
+using WolfLogger = Singleton<do_not_enter_where_the_wolf_lives::Logger>;
using WolfLoggerManager = Singleton<do_not_enter_where_the_wolf_lives::LoggerManager>;
} /* namespace internal */
@@ -270,34 +266,42 @@ using WolfLoggerManager = Singleton<do_not_enter_where_the_wolf_lives::LoggerMan
/// @brief NAMED LOGGING
-#define WOLF_ASYNC_QUEUE_LOG_NAMED(name, ...) wolf::internal::WolfLoggerManager::get().getLogger(name).set_async_queue(x);
+#define WOLF_ASYNC_QUEUE_LOG_NAMED(name, ...) \
+ wolf::internal::WolfLoggerManager::get().getLogger(name).set_async_queue(x);
#define WOLF_INFO_NAMED(name, ...) wolf::internal::WolfLoggerManager::get().getLogger(name).info(__VA_ARGS__);
-#define WOLF_INFO_NAMED_COND(name, cond, ...) if (cond) WOLF_INFO_NAMED(name, __VA_ARGS__);
+#define WOLF_INFO_NAMED_COND(name, cond, ...) \
+ if (cond) WOLF_INFO_NAMED(name, __VA_ARGS__);
#define WOLF_WARN_NAMED(name, ...) wolf::internal::WolfLoggerManager::get().getLogger(name).warn(__VA_ARGS__);
-#define WOLF_WARN_NAMED_COND(name, cond, ...) if (cond) WOLF_WARN_NAMED(name, __VA_ARGS__);
+#define WOLF_WARN_NAMED_COND(name, cond, ...) \
+ if (cond) WOLF_WARN_NAMED(name, __VA_ARGS__);
#define WOLF_ERROR_NAMED(name, ...) wolf::internal::WolfLoggerManager::get().getLogger(name).error(__VA_ARGS__);
-#define WOLF_ERROR_NAMED_COND(name, cond, ...) if (cond) WOLF_ERROR_NAMED(name, __VA_ARGS__);
+#define WOLF_ERROR_NAMED_COND(name, cond, ...) \
+ if (cond) WOLF_ERROR_NAMED(name, __VA_ARGS__);
#ifdef _WOLF_DEBUG
- #define WOLF_DEBUG_NAMED(name, ...) wolf::internal::WolfLoggerManager::get().getLogger(name).debug(__VA_ARGS__);
- #define WOLF_DEBUG_NAMED_COND(name, cond, ...) if (cond) WOLF_DEBUG_NAMED(name, __VA_ARGS__);
+#define WOLF_DEBUG_NAMED(name, ...) wolf::internal::WolfLoggerManager::get().getLogger(name).debug(__VA_ARGS__);
+#define WOLF_DEBUG_NAMED_COND(name, cond, ...) \
+ if (cond) WOLF_DEBUG_NAMED(name, __VA_ARGS__);
#else
- #define WOLF_DEBUG_NAMED(name, ...)
- #define WOLF_DEBUG_NAMED_COND(cond, name, ...)
+#define WOLF_DEBUG_NAMED(name, ...)
+#define WOLF_DEBUG_NAMED_COND(cond, name, ...)
#endif
#ifdef _WOLF_TRACE
- #define WOLF_TRACE_NAMED(name, ...) \
- {char this_file[] = __FILE__;\
- wolf::internal::WolfLoggerManager::get().getLogger(name).trace("[", basename(this_file), " L", __LINE__, \
- " : ", __FUNCTION__, "] ", __VA_ARGS__);}
- #define WOLF_TRACE_NAMED_COND(name, cond, ...) if (cond) WOLF_TRACE_NAMED_COND(name, __VA_ARGS__);
+#define WOLF_TRACE_NAMED(name, ...) \
+ { \
+ char this_file[] = __FILE__; \
+ wolf::internal::WolfLoggerManager::get().getLogger(name).trace( \
+ "[", basename(this_file), " L", __LINE__, " : ", __FUNCTION__, "] ", __VA_ARGS__); \
+ }
+#define WOLF_TRACE_NAMED_COND(name, cond, ...) \
+ if (cond) WOLF_TRACE_NAMED_COND(name, __VA_ARGS__);
#else
- #define WOLF_TRACE_NAMED(...)
- #define WOLF_TRACE_NAMED_cond(name, cond, ...)
+#define WOLF_TRACE_NAMED(...)
+#define WOLF_TRACE_NAMED_cond(name, cond, ...)
#endif
/// @brief MAIN LOGGING
@@ -305,29 +309,36 @@ using WolfLoggerManager = Singleton<do_not_enter_where_the_wolf_lives::LoggerMan
#define WOLF_ASYNC_QUEUE_LOG(x) wolf::internal::WolfLogger::get(__INTERNAL_WOLF_MAIN_LOGGER_NAME_).set_async_queue(x);
#define WOLF_INFO(...) wolf::internal::WolfLogger::get(__INTERNAL_WOLF_MAIN_LOGGER_NAME_).info(__VA_ARGS__);
-#define WOLF_INFO_COND(cond, ...) if (cond) WOLF_INFO(__VA_ARGS__);
+#define WOLF_INFO_COND(cond, ...) \
+ if (cond) WOLF_INFO(__VA_ARGS__);
#define WOLF_WARN(...) wolf::internal::WolfLogger::get(__INTERNAL_WOLF_MAIN_LOGGER_NAME_).warn(__VA_ARGS__);
-#define WOLF_WARN_COND(cond, ...) if (cond) WOLF_WARN(__VA_ARGS__);
+#define WOLF_WARN_COND(cond, ...) \
+ if (cond) WOLF_WARN(__VA_ARGS__);
#define WOLF_ERROR(...) wolf::internal::WolfLogger::get(__INTERNAL_WOLF_MAIN_LOGGER_NAME_).error(__VA_ARGS__);
-#define WOLF_ERROR_COND(cond, ...) if (cond) WOLF_ERROR(__VA_ARGS__);
+#define WOLF_ERROR_COND(cond, ...) \
+ if (cond) WOLF_ERROR(__VA_ARGS__);
#ifdef _WOLF_DEBUG
- #define WOLF_DEBUG(...) wolf::internal::WolfLogger::get(__INTERNAL_WOLF_MAIN_LOGGER_NAME_).debug(__VA_ARGS__);
- #define WOLF_DEBUG_COND(cond, ...) if (cond) WOLF_DEBUG(__VA_ARGS__);
+#define WOLF_DEBUG(...) wolf::internal::WolfLogger::get(__INTERNAL_WOLF_MAIN_LOGGER_NAME_).debug(__VA_ARGS__);
+#define WOLF_DEBUG_COND(cond, ...) \
+ if (cond) WOLF_DEBUG(__VA_ARGS__);
#else
- #define WOLF_DEBUG(...)
- #define WOLF_DEBUG_COND(cond, ...)
+#define WOLF_DEBUG(...)
+#define WOLF_DEBUG_COND(cond, ...)
#endif
#ifdef _WOLF_TRACE
- #define WOLF_TRACE(...) \
- {char this_file[] = __FILE__;\
- wolf::internal::WolfLogger::get(__INTERNAL_WOLF_MAIN_LOGGER_NAME_).trace("[", basename(this_file), " L", __LINE__, \
- " : ", __FUNCTION__, "] ", __VA_ARGS__);}
- #define WOLF_TRACE_COND(cond, ...) if (cond) WOLF_TRACE(__VA_ARGS__);
+#define WOLF_TRACE(...) \
+ { \
+ char this_file[] = __FILE__; \
+ wolf::internal::WolfLogger::get(__INTERNAL_WOLF_MAIN_LOGGER_NAME_) \
+ .trace("[", basename(this_file), " L", __LINE__, " : ", __FUNCTION__, "] ", __VA_ARGS__); \
+ }
+#define WOLF_TRACE_COND(cond, ...) \
+ if (cond) WOLF_TRACE(__VA_ARGS__);
#else
- #define WOLF_TRACE(...)
- #define WOLF_TRACE_COND(cond, ...)
+#define WOLF_TRACE(...)
+#define WOLF_TRACE_COND(cond, ...)
#endif
diff --git a/include/core/utils/singleton.h b/include/core/utils/singleton.h
index c92f18c75ecc3dd48538411ae9fadeef3531579a..408073eb1751cd6e84ee319cfa2890740c424030 100644
--- a/include/core/utils/singleton.h
+++ b/include/core/utils/singleton.h
@@ -22,9 +22,10 @@
#include <memory>
-namespace wolf {
-namespace internal {
-
+namespace wolf
+{
+namespace internal
+{
/**
* \brief A thread-safer (?) Singleton implementation with
* argument forwarding.
@@ -32,29 +33,27 @@ namespace internal {
template <class T>
class Singleton
{
- using SingletonOPtr = std::unique_ptr<T>;
-
-public:
-
- template <typename... Args>
- static T& get(Args&&... args)
- {
- static SingletonOPtr instance_(new T(std::forward<Args>(args)...));
+ using SingletonOPtr = std::unique_ptr<T>;
- return *instance_;
- }
+ public:
+ template <typename... Args>
+ static T& get(Args&&... args)
+ {
+ static SingletonOPtr instance_(new T(std::forward<Args>(args)...));
- constexpr Singleton(const Singleton&) = delete;
- constexpr Singleton(const Singleton&&) = delete;
+ return *instance_;
+ }
- constexpr Singleton& operator=(const Singleton&) const = delete;
- constexpr Singleton& operator=(const Singleton&&) const = delete;
+ constexpr Singleton(const Singleton&) = delete;
+ constexpr Singleton(const Singleton&&) = delete;
-protected:
+ constexpr Singleton& operator=(const Singleton&) const = delete;
+ constexpr Singleton& operator=(const Singleton&&) const = delete;
- constexpr Singleton() = default;
- virtual ~Singleton() = default;
+ protected:
+ constexpr Singleton() = default;
+ virtual ~Singleton() = default;
};
-} // namespace internal
-} // namespace wolf
+} // namespace internal
+} // namespace wolf
diff --git a/include/core/common/params_base.h b/include/core/utils/string_utils.h
similarity index 80%
rename from include/core/common/params_base.h
rename to include/core/utils/string_utils.h
index 8ae98bcabfd39f63314523fe5c18222f811b3c81..a5ad22f67d5e8d138ee952901a23649dc194ae99 100644
--- a/include/core/common/params_base.h
+++ b/include/core/utils/string_utils.h
@@ -20,34 +20,18 @@
#pragma once
-#include "yaml-cpp/yaml.h"
+#include <Eigen/Dense>
+#include <ctime>
+#include <iostream>
+#include <string>
+#include <sstream>
-namespace wolf
+namespace wolf
{
+// date now in string
+std::string dateTimeNow();
-struct ParamsBase
-{
- ParamsBase() = default;
- ParamsBase(const YAML::Node& _n)
- {
- //
- }
-
- virtual ~ParamsBase() = default;
- virtual std::string print() const = 0;
-};
-
-template<typename Derived>
-std::string toString(const Eigen::DenseBase<Derived>& mat)
-{
- std::stringstream ss;
- if (mat.cols() == 1)
- ss << mat.transpose();
- else
- ss << std::endl << mat;
- return ss.str();
-}
-
+// to string
std::string toString(bool _arg);
std::string toString(int _arg);
std::string toString(long _arg);
@@ -59,6 +43,17 @@ std::string toString(float _arg);
std::string toString(double _arg);
std::string toString(long double _arg);
+template <typename Derived>
+std::string toString(const Eigen::DenseBase<Derived>& mat)
+{
+ std::stringstream ss;
+ if (mat.cols() == 1)
+ ss << mat.transpose();
+ else
+ ss << std::endl << mat;
+ return ss.str();
+}
+
inline std::string toString(bool _arg)
{
return (_arg ? "true" : "false");
@@ -108,4 +103,17 @@ inline std::string toString(long double _arg)
{
return std::to_string(_arg);
}
+
+inline std::string dateTimeNow()
+{
+ // Get date and time for archiving purposes
+ std::time_t rawtime;
+ std::time(&rawtime);
+ const std::tm* timeinfo = std::localtime(&rawtime);
+ char time_char[30];
+ std::strftime(time_char, sizeof(time_char), "%d/%m/%Y at %H:%M:%S", timeinfo);
+ std::string date_time(time_char);
+ return date_time;
}
+
+} // namespace wolf
diff --git a/include/core/utils/utils_gtest.h b/include/core/utils/utils_gtest.h
index 97f082db2e91c3ac2cbd0bf6c79adc5f6f31af9e..5f85f806e6902b8a1574262ef7dd93e3b7b109d7 100644
--- a/include/core/utils/utils_gtest.h
+++ b/include/core/utils/utils_gtest.h
@@ -66,36 +66,40 @@ namespace internal
{
enum GTestColor
{
- COLOR_DEFAULT,
- COLOR_RED,
- COLOR_GREEN,
- COLOR_YELLOW
+ COLOR_DEFAULT,
+ COLOR_RED,
+ COLOR_GREEN,
+ COLOR_YELLOW
};
extern void ColoredPrintf(GTestColor color, const char* fmt, ...);
-#define PRINTF(...) \
- do { testing::internal::ColoredPrintf(testing::internal::COLOR_GREEN,\
- "[ ] "); \
- testing::internal::ColoredPrintf(testing::internal::COLOR_YELLOW, __VA_ARGS__); } \
- while(0)
-
-#define PRINT_TEST_FINISHED \
-{ \
- const ::testing::TestInfo* const test_info = \
- ::testing::UnitTest::GetInstance()->current_test_info(); \
- PRINTF(std::string("Finished test case ").append(test_info->test_case_name()).\
- append(" of test ").append(test_info->name()).append(".\n").c_str()); \
-}
+#define PRINTF(...) \
+ do \
+ { \
+ testing::internal::ColoredPrintf(testing::internal::COLOR_GREEN, "[ ] "); \
+ testing::internal::ColoredPrintf(testing::internal::COLOR_YELLOW, __VA_ARGS__); \
+ } while (0)
+
+#define PRINT_TEST_FINISHED \
+ { \
+ const ::testing::TestInfo* const test_info = ::testing::UnitTest::GetInstance()->current_test_info(); \
+ PRINTF(std::string("Finished test case ") \
+ .append(test_info->test_case_name()) \
+ .append(" of test ") \
+ .append(test_info->name()) \
+ .append(".\n") \
+ .c_str()); \
+ }
// C++ stream interface
class TestCout : public std::stringstream
{
-public:
- ~TestCout() override
- {
- PRINTF("%s\n", str().c_str());
- }
+ public:
+ ~TestCout() override
+ {
+ PRINTF("%s\n", str().c_str());
+ }
};
/* Usage :
@@ -120,72 +124,104 @@ TEST(Test, Foo)
*/
#define TEST_COUT testing::internal::TestCout()
-} // namespace internal
+} // namespace internal
/* Macros related to testing Eigen classes:
*/
-#define EXPECT_MATRIX_APPROX(C_expect, C_actual, precision) EXPECT_PRED2([](const Eigen::MatrixXd lhs, const Eigen::MatrixXd rhs) { \
- return (lhs - rhs).isMuchSmallerThan(1, precision); \
- }, \
- C_expect, C_actual);
-
-#define ASSERT_MATRIX_APPROX(C_expect, C_actual, precision) ASSERT_PRED2([](const Eigen::MatrixXd lhs, const Eigen::MatrixXd rhs) { \
- return (lhs - rhs).isMuchSmallerThan(1, precision); \
- }, \
- C_expect, C_actual);
-
-#define EXPECT_QUATERNION_APPROX(C_expect, C_actual, precision) EXPECT_PRED2([](const Eigen::VectorXd lhs, const Eigen::VectorXd rhs) { \
- return Eigen::Quaterniond(Eigen::Vector4d(lhs)).angularDistance(Eigen::Quaterniond(Eigen::Vector4d(rhs))) < precision; \
- }, \
- Quaterniond(C_expect).coeffs(), Quaterniond(C_actual).coeffs());
-
-#define ASSERT_QUATERNION_APPROX(C_expect, C_actual, precision) ASSERT_PRED2([](const Eigen::VectorXd lhs, const Eigen::VectorXd rhs) { \
- return Eigen::Quaterniond(Eigen::Vector4d(lhs)).angularDistance(Eigen::Quaterniond(Eigen::Vector4d(rhs))) < precision; \
- }, \
- Quaterniond(C_expect).coeffs(), Quaterniond(C_actual).coeffs());
-
-#define EXPECT_QUATERNION_VECTOR_APPROX(C_expect, C_actual, precision) EXPECT_PRED2([](const Eigen::VectorXd lhs, const Eigen::VectorXd rhs) { \
- return Eigen::Quaterniond(Eigen::Vector4d(lhs)).angularDistance(Eigen::Quaterniond(Eigen::Vector4d(rhs))) < precision; \
- }, \
- C_expect, C_actual);
-
-#define ASSERT_QUATERNION_VECTOR_APPROX(C_expect, C_actual, precision) ASSERT_PRED2([](const Eigen::VectorXd lhs, const Eigen::VectorXd rhs) { \
- return Eigen::Quaterniond(Eigen::Vector4d(lhs)).angularDistance(Eigen::Quaterniond(Eigen::Vector4d(rhs))) < precision; \
- }, \
- C_expect, C_actual);
-
-#define EXPECT_POSE2d_APPROX(C_expect, C_actual, precision) EXPECT_PRED2([](const Eigen::VectorXd lhs, const Eigen::VectorXd rhs) { \
- assert(lhs.size() == 3 and rhs.size() == 3); \
- Eigen::VectorXd er = lhs - rhs; \
- er(2) = pi2pi((double)er(2)); \
- return er.isMuchSmallerThan(1, precision); \
- }, \
- C_expect, C_actual);
-
-#define ASSERT_POSE2d_APPROX(C_expect, C_actual, precision) ASSERT_PRED2([](const Eigen::VectorXd lhs, const Eigen::VectorXd rhs) { \
- assert(lhs.size() == 3 and rhs.size() == 3); \
- Eigen::VectorXd er = lhs - rhs; \
- er(2) = pi2pi((double)er(2)); \
- return er.isMuchSmallerThan(1, precision); \
- }, \
- C_expect, C_actual);
-
-#define EXPECT_POSE3d_APPROX(C_expect, C_actual, precision) EXPECT_PRED2([](const Eigen::VectorXd lhs, const Eigen::VectorXd rhs) { \
- assert(lhs.size() == 7 and rhs.size() == 7); \
- Eigen::Vector4d er; \
- er.head(3) = lhs.head(3) - rhs.head(3); \
- er(3) = Eigen::Quaterniond(Eigen::Vector4d(lhs.tail(4))).angularDistance(Eigen::Quaterniond(Eigen::Vector4d(rhs.tail(4)))); \
- return er.isMuchSmallerThan(1, precision); \
- }, \
- C_expect, C_actual);
-
-#define ASSERT_POSE3d_APPROX(C_expect, C_actual, precision) EXPECT_PRED2([](const Eigen::VectorXd lhs, const Eigen::VectorXd rhs) { \
- assert(lhs.size() == 7 and rhs.size() == 7); \
- Eigen::Vector4d er; \
- er.head(3) = lhs.head(3) - rhs.head(3); \
- er(3) = Eigen::Quaterniond(Eigen::Vector4d(lhs.tail(4))).angularDistance(Eigen::Quaterniond(Eigen::Vector4d(rhs.tail(4)))); \
- return er.isMuchSmallerThan(1, precision); \
- }, \
- C_expect, C_actual);
-
-} // namespace testing
+#define EXPECT_MATRIX_APPROX(C_expect, C_actual, precision) \
+ EXPECT_PRED2([](const Eigen::MatrixXd lhs, \
+ const Eigen::MatrixXd rhs) { return (lhs - rhs).isMuchSmallerThan(1, precision); }, \
+ C_expect, \
+ C_actual);
+
+#define ASSERT_MATRIX_APPROX(C_expect, C_actual, precision) \
+ ASSERT_PRED2([](const Eigen::MatrixXd lhs, \
+ const Eigen::MatrixXd rhs) { return (lhs - rhs).isMuchSmallerThan(1, precision); }, \
+ C_expect, \
+ C_actual);
+
+#define EXPECT_QUATERNION_APPROX(C_expect, C_actual, precision) \
+ EXPECT_PRED2( \
+ [](const Eigen::VectorXd lhs, const Eigen::VectorXd rhs) { \
+ return Eigen::Quaterniond(Eigen::Vector4d(lhs)) \
+ .angularDistance(Eigen::Quaterniond(Eigen::Vector4d(rhs))) < precision; \
+ }, \
+ Quaterniond(C_expect).coeffs(), \
+ Quaterniond(C_actual).coeffs());
+
+#define ASSERT_QUATERNION_APPROX(C_expect, C_actual, precision) \
+ ASSERT_PRED2( \
+ [](const Eigen::VectorXd lhs, const Eigen::VectorXd rhs) { \
+ return Eigen::Quaterniond(Eigen::Vector4d(lhs)) \
+ .angularDistance(Eigen::Quaterniond(Eigen::Vector4d(rhs))) < precision; \
+ }, \
+ Quaterniond(C_expect).coeffs(), \
+ Quaterniond(C_actual).coeffs());
+
+#define EXPECT_QUATERNION_VECTOR_APPROX(C_expect, C_actual, precision) \
+ EXPECT_PRED2( \
+ [](const Eigen::VectorXd lhs, const Eigen::VectorXd rhs) { \
+ return Eigen::Quaterniond(Eigen::Vector4d(lhs)) \
+ .angularDistance(Eigen::Quaterniond(Eigen::Vector4d(rhs))) < precision; \
+ }, \
+ C_expect, \
+ C_actual);
+
+#define ASSERT_QUATERNION_VECTOR_APPROX(C_expect, C_actual, precision) \
+ ASSERT_PRED2( \
+ [](const Eigen::VectorXd lhs, const Eigen::VectorXd rhs) { \
+ return Eigen::Quaterniond(Eigen::Vector4d(lhs)) \
+ .angularDistance(Eigen::Quaterniond(Eigen::Vector4d(rhs))) < precision; \
+ }, \
+ C_expect, \
+ C_actual);
+
+#define EXPECT_POSE2d_APPROX(C_expect, C_actual, precision) \
+ EXPECT_PRED2( \
+ [](const Eigen::VectorXd lhs, const Eigen::VectorXd rhs) { \
+ assert(lhs.size() == 3 and rhs.size() == 3); \
+ Eigen::VectorXd er = lhs - rhs; \
+ er(2) = pi2pi((double)er(2)); \
+ return er.isMuchSmallerThan(1, precision); \
+ }, \
+ C_expect, \
+ C_actual);
+
+#define ASSERT_POSE2d_APPROX(C_expect, C_actual, precision) \
+ ASSERT_PRED2( \
+ [](const Eigen::VectorXd lhs, const Eigen::VectorXd rhs) { \
+ assert(lhs.size() == 3 and rhs.size() == 3); \
+ Eigen::VectorXd er = lhs - rhs; \
+ er(2) = pi2pi((double)er(2)); \
+ return er.isMuchSmallerThan(1, precision); \
+ }, \
+ C_expect, \
+ C_actual);
+
+#define EXPECT_POSE3d_APPROX(C_expect, C_actual, precision) \
+ EXPECT_PRED2( \
+ [](const Eigen::VectorXd lhs, const Eigen::VectorXd rhs) { \
+ assert(lhs.size() == 7 and rhs.size() == 7); \
+ Eigen::Vector4d er; \
+ er.head(3) = lhs.head(3) - rhs.head(3); \
+ er(3) = Eigen::Quaterniond(Eigen::Vector4d(lhs.tail(4))) \
+ .angularDistance(Eigen::Quaterniond(Eigen::Vector4d(rhs.tail(4)))); \
+ return er.isMuchSmallerThan(1, precision); \
+ }, \
+ C_expect, \
+ C_actual);
+
+#define ASSERT_POSE3d_APPROX(C_expect, C_actual, precision) \
+ EXPECT_PRED2( \
+ [](const Eigen::VectorXd lhs, const Eigen::VectorXd rhs) { \
+ assert(lhs.size() == 7 and rhs.size() == 7); \
+ Eigen::Vector4d er; \
+ er.head(3) = lhs.head(3) - rhs.head(3); \
+ er(3) = Eigen::Quaterniond(Eigen::Vector4d(lhs.tail(4))) \
+ .angularDistance(Eigen::Quaterniond(Eigen::Vector4d(rhs.tail(4)))); \
+ return er.isMuchSmallerThan(1, precision); \
+ }, \
+ C_expect, \
+ C_actual);
+
+} // namespace testing
diff --git a/schema/map/MapBase.schema b/schema/map/MapBase.schema
index bf5f648e68b1b63eb0a4d67ac26ed99c7aa8dc0e..c78f4158bfe0c1c114179b48d244012829d69df0 100644
--- a/schema/map/MapBase.schema
+++ b/schema/map/MapBase.schema
@@ -5,15 +5,9 @@ type:
_doc: Type of the Map used in the problem.
plugin:
- _mandatory: false
+ _mandatory: true
_type: string
- _default: core
_doc: Name of the wolf plugin where the map type is implemented.
-
-filename:
- _mandatory: false
- _type: string
- _doc: Optional. Absolute path of the YAML file containing the landmarks.
landmarks:
_mandatory: false
diff --git a/schema/processor/ProcessorBase.schema b/schema/processor/ProcessorBase.schema
index ccab8c38c48c339bbb9a56244a11d724a54102bb..51e17501855865df34a2da4f03b7f0e33134278e 100644
--- a/schema/processor/ProcessorBase.schema
+++ b/schema/processor/ProcessorBase.schema
@@ -6,7 +6,7 @@ name:
time_tolerance:
_mandatory: true
_type: double
- _doc: The time tolerance for joining captures into frames [s].
+ _doc: Maximum time difference between a Keyframe time stamp and a particular Capture of this processor to allow assigning this Capture to the Keyframe. [s].
keyframe_vote:
voting_active:
diff --git a/schema/processor/ProcessorLandmarkExternal.schema b/schema/processor/ProcessorLandmarkExternal.schema
index dd93d6ee380b84c86eb3c7c7847f054d99f2fe5a..c8afe136ee3be6f5c850dc6fbef1962d6aeddfeb 100644
--- a/schema/processor/ProcessorLandmarkExternal.schema
+++ b/schema/processor/ProcessorLandmarkExternal.schema
@@ -25,9 +25,9 @@ keyframe_vote:
use_orientation:
_mandatory: true
_type: bool
- _doc: "If the orientation measurement is considered"
+ _doc: "If the orientation measurement is considered when emplacing factors"
-match dist_th:
+match_dist_th:
_mandatory: true
_type: double
_doc: "the threshold in distance for considering a match between landmarks and detections"
\ No newline at end of file
diff --git a/schema/processor/ProcessorLoopClosure.schema b/schema/processor/ProcessorLoopClosure.schema
new file mode 100644
index 0000000000000000000000000000000000000000..08cfc8e2d6ac2605a0b0d09466e82a103d1096eb
--- /dev/null
+++ b/schema/processor/ProcessorLoopClosure.schema
@@ -0,0 +1,5 @@
+follow: ProcessorBase.schema
+max_loops:
+ _mandatory: true
+ _type: int
+ _doc: The maximum amount of loops that can be closed each keyframe callback. -1 (or any negative value) is unlimited loops.
\ No newline at end of file
diff --git a/schema/processor/ProcessorMotion.schema b/schema/processor/ProcessorMotion.schema
index a3f95576dad9761ff4e152c9073c4d71b78c8c58..6f4c5b1c8fe84c44147f10d956a68d0840ad5bb3 100644
--- a/schema/processor/ProcessorMotion.schema
+++ b/schema/processor/ProcessorMotion.schema
@@ -9,14 +9,14 @@ keyframe_vote:
_mandatory: $voting_active
_type: unsigned int
_doc: Maximum size of the buffer of motions.
- dist_traveled:
+ max_dist_traveled:
_mandatory: $voting_active
_type: double
- _doc: Threshold of distance traveled to create a new frame [m].
- angle_turned:
+ _doc: Distance traveled threshold to create a new frame [m].
+ max_angle_turned:
_mandatory: $voting_active
_type: double
- _doc: Threshold of angle turned to create a new frame [rad].
+ _doc: Angle turned threshold to create a new frame [rad].
unmeasured_perturbation_std:
_mandatory: true
_type: double
diff --git a/schema/sensor/SensorBase.schema b/schema/sensor/SensorBase.schema
index de1f6dbaa73829d921f4fff2113af2b0a15e9e9d..7bd4c6fb9eda769e30c3d2557ddcfad8ab6852aa 100644
--- a/schema/sensor/SensorBase.schema
+++ b/schema/sensor/SensorBase.schema
@@ -1,4 +1,10 @@
name:
_mandatory: true
_type: string
- _doc: The sensor's name. It has to be unique.
\ No newline at end of file
+ _doc: The sensor's name. It has to be unique.
+
+states:
+ keys:
+ _mandatory: true
+ _type: string
+ _doc: The keys corresponding to the states of the sensor, to be filled with _value in the derived schemas.
\ No newline at end of file
diff --git a/schema/sensor/SensorDiffDrive.schema b/schema/sensor/SensorDiffDrive.schema
index a3ef51b03cf6353277de9cbc3987b83bb843546f..14ff63ce81264765f6cc8dd36cc36839df1c35b0 100644
--- a/schema/sensor/SensorDiffDrive.schema
+++ b/schema/sensor/SensorDiffDrive.schema
@@ -11,6 +11,11 @@ ticks_std_factor:
_doc: ratio of displacement variance to rotation, for odometry noise calculation.
states:
+ keys:
+ _value: POI
+ _mandatory: false
+ _type: string
+ _doc: The keys corresponding to the states of the sensor.
P:
follow: SpecStateSensorP2d.schema
O:
diff --git a/schema/sensor/SensorMotionModel.schema b/schema/sensor/SensorMotionModel.schema
index 9ff4a8f839cc1bec658f6c111fc6214889d29b27..536be42fbee0425ba36cf7c5bd8c0cd2ecdceb7a 100644
--- a/schema/sensor/SensorMotionModel.schema
+++ b/schema/sensor/SensorMotionModel.schema
@@ -1 +1,7 @@
-follow: SensorBase.schema
\ No newline at end of file
+follow: SensorBase.schema
+states:
+ keys:
+ _value: ""
+ _mandatory: false
+ _type: string
+ _doc: The keys corresponding to the states of the sensor.
\ No newline at end of file
diff --git a/schema/sensor/SensorOdom2d.schema b/schema/sensor/SensorOdom2d.schema
index dd89f77c9d3d523a08c9ca0b877f2e81ae556b56..5e74d3d7aeba8d549cbca036e234bca69b3dfc7d 100644
--- a/schema/sensor/SensorOdom2d.schema
+++ b/schema/sensor/SensorOdom2d.schema
@@ -26,6 +26,11 @@ min_rot_var:
_doc: minimum rotation variance, for odometry noise calculation.
states:
+ keys:
+ _value: PO
+ _mandatory: false
+ _type: string
+ _doc: The keys corresponding to the states of the sensor.
P:
follow: SpecStateSensorP2d.schema
O:
diff --git a/schema/sensor/SensorOdom3d.schema b/schema/sensor/SensorOdom3d.schema
index 1307dc6ea0bf88265cc3f99affafeec1ccfbe156..dca332c76e77781eee42b0f890125fd11e85256b 100644
--- a/schema/sensor/SensorOdom3d.schema
+++ b/schema/sensor/SensorOdom3d.schema
@@ -26,6 +26,11 @@ min_rot_var:
_doc: minimum rotation variance, for odometry noise calculation.
states:
+ keys:
+ _value: PO
+ _mandatory: false
+ _type: string
+ _doc: The keys corresponding to the states of the sensor.
P:
follow: SpecStateSensorP3d.schema
O:
diff --git a/schema/sensor/SensorPose2d.schema b/schema/sensor/SensorPose2d.schema
index 5863bd1bdcd83c96b2ef109913633ebb97309665..b597a4036ebf3099425f63c35c75e9c3840d48f4 100644
--- a/schema/sensor/SensorPose2d.schema
+++ b/schema/sensor/SensorPose2d.schema
@@ -6,6 +6,11 @@ std_noise:
_doc: Vector containing the standard deviation of the position and orientation measurements (square root of the covariance matrix diagonal).
states:
+ keys:
+ _value: PO
+ _mandatory: false
+ _type: string
+ _doc: The keys corresponding to the states of the sensor.
P:
follow: SpecStateSensorP2d.schema
O:
diff --git a/schema/sensor/SensorPose3d.schema b/schema/sensor/SensorPose3d.schema
index 8c1b7d2081825abc24c7177fc3ba70ce58fdcd4e..f044419086964a0abe2be7cb4d28825b94077974 100644
--- a/schema/sensor/SensorPose3d.schema
+++ b/schema/sensor/SensorPose3d.schema
@@ -6,6 +6,11 @@ std_noise:
_doc: Vector containing the standard deviation of the position and orientation measurements (square root of the covariance matrix diagonal).
states:
+ keys:
+ _value: PO
+ _mandatory: false
+ _type: string
+ _doc: The keys corresponding to the states of the sensor.
P:
follow: SpecStateSensorP3d.schema
O:
diff --git a/schema/sensor/SpecStateSensorO2d.schema b/schema/sensor/SpecStateSensorO2d.schema
index a930bc522410aad3e4f4d120346b4f37b18f3e74..4fa80b9cd425f1ee00b70f23c646a81dce081569 100644
--- a/schema/sensor/SpecStateSensorO2d.schema
+++ b/schema/sensor/SpecStateSensorO2d.schema
@@ -1,16 +1,6 @@
follow: SpecStateSensor.schema
-
-type:
- _type: string
- _mandatory: false
- _value: StateAngle
- _doc: The derived type of the StateBlock
-
-state:
- _type: Vector1d
- _mandatory: true
- _doc: A vector containing the state values
-
+follow: SpecStateO2d.schema
+
drift_std:
_type: Vector1d
_mandatory: false
diff --git a/schema/sensor/SpecStateSensorO3d.schema b/schema/sensor/SpecStateSensorO3d.schema
index ae0ae0c134e86ce01d78d1d3e278f1524f05172e..74747dfd1b301070f32b83aca127160ff6c8f821 100644
--- a/schema/sensor/SpecStateSensorO3d.schema
+++ b/schema/sensor/SpecStateSensorO3d.schema
@@ -1,15 +1,5 @@
follow: SpecStateSensor.schema
-
-type:
- _type: string
- _mandatory: false
- _value: StateQuaternion
- _doc: The derived type of the State in 'O'
-
-state:
- _type: Vector4d
- _mandatory: true
- _doc: A vector containing the state values. It should be a quaternion (i.e. four values and normalized)
+follow: SpecStateO3d.schema
drift_std:
_type: Vector3d
diff --git a/schema/sensor/SpecStateSensorP2d.schema b/schema/sensor/SpecStateSensorP2d.schema
index b13e6182b6dbd4be797aa24fb4a04340d97b593c..5391ee50ebb67987ee836034f0846a2d71f79b03 100644
--- a/schema/sensor/SpecStateSensorP2d.schema
+++ b/schema/sensor/SpecStateSensorP2d.schema
@@ -1,15 +1,5 @@
follow: SpecStateSensor.schema
-
-type:
- _type: string
- _mandatory: false
- _value: StatePoint2d
- _doc: The derived type of the StateBlock
-
-state:
- _type: Vector2d
- _mandatory: true
- _doc: A vector containing the state values
+follow: SpecStateP2d.schema
drift_std:
_type: Vector2d
diff --git a/schema/sensor/SpecStateSensorP3d.schema b/schema/sensor/SpecStateSensorP3d.schema
index 004c67ccf326033132962931c2d0b34db983b83a..7fbc5cba5b32f7948b34ccbf07f4c23d8a503ca3 100644
--- a/schema/sensor/SpecStateSensorP3d.schema
+++ b/schema/sensor/SpecStateSensorP3d.schema
@@ -1,15 +1,5 @@
follow: SpecStateSensor.schema
-
-type:
- _type: string
- _mandatory: true
- _value: StatePoint3d
- _doc: The derived type of the state in 'P'
-
-state:
- _type: Vector3d
- _mandatory: true
- _doc: A vector containing the state 'P' values
+follow: SpecStateP3d.schema
drift_std:
_type: Vector3d
diff --git a/src/capture/capture_base.cpp b/src/capture/capture_base.cpp
index a6e566b54921d449ac6b3f632b84de5fa86c140b..12658d3ab25cbb5805d9795c4a15790a31fc5671 100644
--- a/src/capture/capture_base.cpp
+++ b/src/capture/capture_base.cpp
@@ -21,54 +21,77 @@
#include "core/capture/capture_base.h"
#include "core/sensor/sensor_base.h"
-namespace wolf{
-
+namespace wolf
+{
using namespace Eigen;
unsigned int CaptureBase::capture_id_count_ = 0;
CaptureBase::CaptureBase(const std::string& _type,
- const TimeStamp& _ts,
- SensorBasePtr _sensor_ptr,
- StateBlockPtr _p_ptr,
- StateBlockPtr _o_ptr,
- StateBlockPtr _intr_ptr) :
- NodeStateBlocks("CAPTURE", _type),
- frame_ptr_(), // nullptr
- sensor_ptr_(_sensor_ptr),
- capture_id_(++capture_id_count_),
- time_stamp_(_ts)
+ const TimeStamp& _ts,
+ SensorBasePtr _sensor_ptr,
+ StateBlockPtr _p_ptr,
+ StateBlockPtr _o_ptr,
+ StateBlockPtr _intr_ptr)
+ : NodeStateBlocks("CAPTURE", _type),
+ frame_ptr_(), // nullptr
+ sensor_ptr_(_sensor_ptr),
+ capture_id_(++capture_id_count_),
+ time_stamp_(_ts)
{
assert(time_stamp_.ok() && "Creating a capture with an invalid timestamp!");
if (_sensor_ptr)
{
if (_sensor_ptr->getP() != nullptr and _sensor_ptr->isStateBlockDynamic('P'))
{
- WOLF_ERROR_COND(not _p_ptr, "In CaptureBase constructor of type ", _type, " the sensor ", _sensor_ptr->getType(), " ", _sensor_ptr->getName(), " has state P dynamic but provided _p_ptr is nullptr")
+ WOLF_ERROR_COND(not _p_ptr,
+ "In CaptureBase constructor of type ",
+ _type,
+ " the sensor ",
+ _sensor_ptr->getType(),
+ " ",
+ _sensor_ptr->getName(),
+ " has state P dynamic but provided _p_ptr is nullptr")
assert(_p_ptr && "Pointer to dynamic position params is null!");
addStateBlock('P', _p_ptr);
}
else
- assert(_p_ptr == nullptr && "Provided dynamic sensor position but the sensor position is static or doesn't exist");
+ assert(_p_ptr == nullptr &&
+ "Provided dynamic sensor position but the sensor position is static or doesn't exist");
if (_sensor_ptr->getO() != nullptr and _sensor_ptr->isStateBlockDynamic('O'))
{
- WOLF_ERROR_COND(not _o_ptr, "In CaptureBase constructor of type ", _type, " the sensor ", _sensor_ptr->getType(), " ", _sensor_ptr->getName(), " has state O dynamic but provided _o_ptr is nullptr")
+ WOLF_ERROR_COND(not _o_ptr,
+ "In CaptureBase constructor of type ",
+ _type,
+ " the sensor ",
+ _sensor_ptr->getType(),
+ " ",
+ _sensor_ptr->getName(),
+ " has state O dynamic but provided _o_ptr is nullptr")
assert(_o_ptr && "Pointer to dynamic orientation params is null!");
addStateBlock('O', _o_ptr);
}
else
- assert(_o_ptr == nullptr && "Provided dynamic sensor orientation but the sensor orientation is static or doesn't exist");
+ assert(_o_ptr == nullptr &&
+ "Provided dynamic sensor orientation but the sensor orientation is static or doesn't exist");
if (_sensor_ptr->getIntrinsic() != nullptr and _sensor_ptr->isStateBlockDynamic('I'))
{
- WOLF_ERROR_COND(not _intr_ptr, "In CaptureBase constructor of type ", _type, " the sensor ", _sensor_ptr->getType(), " ", _sensor_ptr->getName(), " has state I dynamic but provided _i_ptr is nullptr")
+ WOLF_ERROR_COND(not _intr_ptr,
+ "In CaptureBase constructor of type ",
+ _type,
+ " the sensor ",
+ _sensor_ptr->getType(),
+ " ",
+ _sensor_ptr->getName(),
+ " has state I dynamic but provided _i_ptr is nullptr")
assert(_intr_ptr && "Pointer to dynamic intrinsic params is null!");
addStateBlock('I', _intr_ptr);
}
else
- assert(_intr_ptr == nullptr && "Provided dynamic sensor intrinsics but the sensor intrinsics are static or don't exist");
-
+ assert(_intr_ptr == nullptr &&
+ "Provided dynamic sensor intrinsics but the sensor intrinsics are static or don't exist");
}
else if (_p_ptr || _o_ptr || _intr_ptr)
{
@@ -83,20 +106,18 @@ void CaptureBase::remove(bool viral_remove_parent_without_children)
* In case multi-threading, solver can be called while removing.
* This order avoids SolverManager to ignore notifications (efficiency)
*/
- if (is_removing_)
- return;
+ if (is_removing_) return;
- is_removing_ = true;
+ is_removing_ = true;
CaptureBasePtr this_C = shared_from_this_capture(); // keep this alive while removing it
// SensorBase::last_capture_
- if (getSensor() and getSensor()->getLastCapture() == this_C)
- getSensor()->updateLastCapture();
+ if (getSensor() and getSensor()->getLastCapture() == this_C) getSensor()->updateLastCapture();
// remove downstream
while (not feature_list_.empty())
{
- feature_list_.front()->remove(viral_remove_parent_without_children); // remove downstream
+ feature_list_.front()->remove(viral_remove_parent_without_children); // remove downstream
}
// remove from upstream
@@ -106,7 +127,7 @@ void CaptureBase::remove(bool viral_remove_parent_without_children)
F->removeCapture(this_C);
if (viral_remove_parent_without_children and not F->hasChildren())
{
- F->remove(viral_remove_parent_without_children); // remove upstream
+ F->remove(viral_remove_parent_without_children); // remove upstream
}
}
@@ -121,7 +142,9 @@ bool CaptureBase::hasChildren() const
bool CaptureBase::process()
{
- assert (getSensor() != nullptr && "Attempting to process a capture with no associated sensor. Either set the capture's sensor or call sensor->process(capture) instead.");
+ assert(getSensor() != nullptr &&
+ "Attempting to process a capture with no associated sensor. Either set the capture's sensor or call "
+ "sensor->process(capture) instead.");
return getSensor()->process(shared_from_this_capture());
}
@@ -153,22 +176,18 @@ FactorBasePtrList CaptureBase::getFactorList()
void CaptureBase::getFactorList(FactorBaseConstPtrList& _fac_list) const
{
for (auto f_ptr : getFeatureList())
- if (not f_ptr->isRemoving())
- f_ptr->getFactorList(_fac_list);
+ if (not f_ptr->isRemoving()) f_ptr->getFactorList(_fac_list);
}
void CaptureBase::getFactorList(FactorBasePtrList& _fac_list)
{
for (auto f_ptr : getFeatureList())
- if (not f_ptr->isRemoving())
- f_ptr->getFactorList(_fac_list);
+ if (not f_ptr->isRemoving()) f_ptr->getFactorList(_fac_list);
}
-bool CaptureBase::hasFeature(const FeatureBaseConstPtr &_feature) const
+bool CaptureBase::hasFeature(const FeatureBaseConstPtr& _feature) const
{
- return std::find(feature_list_.begin(),
- feature_list_.end(),
- _feature) != feature_list_.end();
+ return std::find(feature_list_.begin(), feature_list_.end(), _feature) != feature_list_.end();
}
StateBlockConstPtr CaptureBase::getStateBlock(const char& _key) const
@@ -180,7 +199,7 @@ StateBlockConstPtr CaptureBase::getStateBlock(const char& _key) const
else
return getSensor()->getStateBlock(_key);
}
- else // No sensor associated, or sensor without this state block: assume sensor params are here
+ else // No sensor associated, or sensor without this state block: assume sensor params are here
return NodeStateBlocks::getStateBlock(_key);
}
@@ -193,7 +212,7 @@ StateBlockPtr CaptureBase::getStateBlock(const char& _key)
else
return getSensor()->getStateBlock(_key);
}
- else // No sensor associated, or sensor without this state block: assume sensor params are here
+ else // No sensor associated, or sensor without this state block: assume sensor params are here
return NodeStateBlocks::getStateBlock(_key);
}
@@ -209,9 +228,15 @@ void CaptureBase::unfix()
void CaptureBase::move(FrameBasePtr _frm_ptr)
{
- WOLF_WARN_COND(this->getFrame() == nullptr, "Moving Capture ", id(), " at ts=", getTimeStamp(), " not linked to any frame. Consider just linking it with link() instead of move()!");
+ WOLF_WARN_COND(this->getFrame() == nullptr,
+ "Moving Capture ",
+ id(),
+ " at ts=",
+ getTimeStamp(),
+ " not linked to any frame. Consider just linking it with link() instead of move()!");
- assert((this->getFrame() == nullptr || not this->getFrame()->getProblem()) && "Forbidden: trying to move a capture already linked to a KF!");
+ assert((this->getFrame() == nullptr || not this->getFrame()->getProblem()) &&
+ "Forbidden: trying to move a capture already linked to a KF!");
// Unlink
unlink();
@@ -227,7 +252,7 @@ void CaptureBase::link(FrameBasePtr _frm_ptr)
WOLF_WARN_COND(_frm_ptr == nullptr, "Linking Capture ", id(), " to a nullptr");
- if(_frm_ptr)
+ if (_frm_ptr)
{
_frm_ptr->addCapture(shared_from_this_capture());
this->setFrame(_frm_ptr);
@@ -237,10 +262,10 @@ void CaptureBase::link(FrameBasePtr _frm_ptr)
void CaptureBase::unlink()
{
- WOLF_WARN_COND(this->getFrame() == nullptr, "Unlinking a not linked Capture ", id(), ". Nothing to do, skipping...");
+ WOLF_WARN_COND(
+ this->getFrame() == nullptr, "Unlinking a not linked Capture ", id(), ". Nothing to do, skipping...");
- if (not this->getFrame())
- return;
+ if (not this->getFrame()) return;
assert(getFactorList().empty() && " unlinking a capture constrained by factors!");
@@ -251,8 +276,7 @@ void CaptureBase::unlink()
void CaptureBase::setProblem(ProblemPtr _problem)
{
- if (_problem == nullptr || _problem == this->getProblem())
- return;
+ if (_problem == nullptr || _problem == this->getProblem()) return;
assert(getFrame() && "Cannot set problem: Capture has no Frame!");
@@ -260,111 +284,115 @@ void CaptureBase::setProblem(ProblemPtr _problem)
// update SensorBase::last_capture_
if (getSensor() and
- (not getSensor()->getLastCapture() or
- getSensor()->getLastCapture()->getTimeStamp() < time_stamp_))
- getSensor()->setLastCapture(shared_from_this_capture());
+ (not getSensor()->getLastCapture() or getSensor()->getLastCapture()->getTimeStamp() < time_stamp_))
+ getSensor()->setLastCapture(shared_from_this_capture());
- for (auto ft : feature_list_)
- ft->setProblem(_problem);
+ for (auto ft : feature_list_) ft->setProblem(_problem);
}
-void CaptureBase::printHeader(int _depth, bool _factored_by, bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const
+void CaptureBase::printHeader(int _depth,
+ bool _factored_by,
+ bool _metric,
+ bool _state_blocks,
+ std::ostream& _stream,
+ std::string _tabs) const
{
- _stream << _tabs << "Cap" << id()
- << " " << getType()
- << " ts = " << std::setprecision(3) << getTimeStamp();
+ _stream << _tabs << "Cap" << id() << " " << getType() << " ts = " << std::setprecision(3) << getTimeStamp();
- if(getSensor() != nullptr)
+ if (getSensor() != nullptr)
{
_stream << " -> Sen" << getSensor()->id();
}
else
_stream << " -> Sen-";
- _stream << ((_depth < 3) ? " -- " + std::to_string(getFeatureList().size()) + "f" : "");
+ _stream << ((_depth < 3) ? " -- " + toString(getFeatureList().size()) + "f" : "");
if (_factored_by)
{
_stream << " <-- ";
for (auto fac : getFactoredBySet())
- if (fac)
- _stream << "Fac" << fac->id() << " \t";
+ if (fac) _stream << "Fac" << fac->id() << " \t";
}
_stream << std::endl;
- if (getStateBlockMap().size() > 0)
- printState(_factored_by, _metric, _state_blocks, _stream, _tabs);
+ if (getStateBlockMap().size() > 0) printState(_factored_by, _metric, _state_blocks, _stream, _tabs);
}
-void CaptureBase::print(int _depth, bool _factored_by, bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const
+void CaptureBase::print(int _depth,
+ bool _factored_by,
+ bool _metric,
+ bool _state_blocks,
+ std::ostream& _stream,
+ std::string _tabs) const
{
printHeader(_depth, _factored_by, _metric, _state_blocks, _stream, _tabs);
if (_depth >= 3)
for (auto f : getFeatureList())
- if (f)
- f->print(_depth, _factored_by, _metric, _state_blocks, _stream, _tabs + " ");
+ if (f) f->print(_depth, _factored_by, _metric, _state_blocks, _stream, _tabs + " ");
}
CheckLog CaptureBase::localCheck(bool _verbose, std::ostream& _stream, std::string _tabs) const
{
- CheckLog log;
+ CheckLog log;
std::stringstream inconsistency_explanation;
if (_verbose)
{
_stream << _tabs << "Cap" << id() << " @ " << this << " -> Sen";
- if (getSensor()) _stream << getSensor()->id();
- else _stream << "-";
+ if (getSensor())
+ _stream << getSensor()->id();
+ else
+ _stream << "-";
_stream << std::endl;
- _stream << _tabs << " " << "-> Prb @ " << getProblem() << std::endl;
- _stream << _tabs << " " << "-> Frm" << getFrame()->id() << " @ " << getFrame() << std::endl;
+ _stream << _tabs << " "
+ << "-> Prb @ " << getProblem() << std::endl;
+ _stream << _tabs << " "
+ << "-> Frm" << getFrame()->id() << " @ " << getFrame() << std::endl;
}
// check problem and frame pointers
auto frm_ptr = getFrame();
- inconsistency_explanation << "Capture problem pointer " << getProblem()
- << " different from frame problem pointer " << frm_ptr->getProblem() << "\n";
+ inconsistency_explanation << "Capture problem pointer " << getProblem() << " different from frame problem pointer "
+ << frm_ptr->getProblem() << "\n";
log.assertTrue((getProblem() == frm_ptr->getProblem()), inconsistency_explanation);
// check frame
- inconsistency_explanation << "Cap" << id() << " @ " << this
- << " ---> Frm" << frm_ptr->id() << " @ " << frm_ptr
- << " -X-> Frm" << id() << "\n";
- auto frm_cap_list = frm_ptr->getCaptureList();
+ inconsistency_explanation << "Cap" << id() << " @ " << this << " ---> Frm" << frm_ptr->id() << " @ " << frm_ptr
+ << " -X-> Frm" << id() << "\n";
+ auto frm_cap_list = frm_ptr->getCaptureList();
auto frame_has_cap = std::find(frm_cap_list.begin(), frm_cap_list.end(), shared_from_this_capture());
log.assertTrue(frame_has_cap != frm_cap_list.end(), inconsistency_explanation);
-
// check features
- for(auto f : getFeatureList())
+ for (auto f : getFeatureList())
{
- inconsistency_explanation << "Cap " << id() << " @ " << this
- << " ---> Ftr" << f->id() << " @ " << f
- << " -X-> Cap" << id();
+ inconsistency_explanation << "Cap " << id() << " @ " << this << " ---> Ftr" << f->id() << " @ " << f
+ << " -X-> Cap" << id();
log.assertTrue((f->getCapture() == shared_from_this_capture()), inconsistency_explanation);
}
- //Check that the capture is within time tolerance of some processor
- auto frame = getFrame();
+ // Check that the capture is within time tolerance of some processor
+ auto frame = getFrame();
double time_diff = fabs(getTimeStamp() - frame->getTimeStamp());
- //It looks like some gtests add captures by hand, without using processors, so the processor list is empty.
- //This initialization is needed because if the list empty the execution does not go into the loop and the
- //assertion fails
- bool match_any_prc_timetolerance=false;
- if(getSensor() != nullptr )
+ // It looks like some gtests add captures by hand, without using processors, so the processor list is empty.
+ // This initialization is needed because if the list empty the execution does not go into the loop and the
+ // assertion fails
+ bool match_any_prc_timetolerance = false;
+ if (getSensor() != nullptr)
{
match_any_prc_timetolerance = getSensor()->getProcessorList().empty();
- for(auto prc : getSensor()->getProcessorList())
+ for (auto prc : getSensor()->getProcessorList())
{
match_any_prc_timetolerance = match_any_prc_timetolerance or (time_diff <= prc->getTimeTolerance());
}
- inconsistency_explanation << "Cap " << id() << " @ " << this
- << " ts =" << getTimeStamp() << "Frm" << frame->id()
- << " ts = " << frame->getTimeStamp() << " their time difference (" << time_diff << ") does not match any time tolerance of"
- << " any processor in sensor " << getSensor()->id() << "\n";
+ inconsistency_explanation << "Cap " << id() << " @ " << this << " ts =" << getTimeStamp() << "Frm"
+ << frame->id() << " ts = " << frame->getTimeStamp() << " their time difference ("
+ << time_diff << ") does not match any time tolerance of"
+ << " any processor in sensor " << getSensor()->id() << "\n";
log.assertTrue((match_any_prc_timetolerance), inconsistency_explanation);
}
-
+
// check NodeStateBlocks
auto node_log = checkStatesAndFactoredBy(_verbose, _stream, _tabs);
log.compose(node_log);
@@ -377,7 +405,7 @@ bool CaptureBase::check(CheckLog& _log, bool _verbose, std::ostream& _stream, st
auto local_log = localCheck(_verbose, _stream, _tabs);
_log.compose(local_log);
- for(auto f : getFeatureList()) f->check(_log, _verbose, _stream, _tabs + " ");
+ for (auto f : getFeatureList()) f->check(_log, _verbose, _stream, _tabs + " ");
return _log.is_consistent_;
}
-} // namespace wolf
+} // namespace wolf
diff --git a/src/capture/capture_diff_drive.cpp b/src/capture/capture_diff_drive.cpp
index b6c628d33bccb95cf87948c45480ae2e1603e945..30dd73d05521f7526d9bb919cd7419573cdfc07c 100644
--- a/src/capture/capture_diff_drive.cpp
+++ b/src/capture/capture_diff_drive.cpp
@@ -21,27 +21,27 @@
#include "core/capture/capture_diff_drive.h"
#include "core/math/rotations.h"
-namespace wolf {
-
-CaptureDiffDrive::CaptureDiffDrive(const TimeStamp& _ts,
- const SensorBasePtr& _sensor_ptr,
+namespace wolf
+{
+CaptureDiffDrive::CaptureDiffDrive(const TimeStamp& _ts,
+ const SensorBasePtr& _sensor_ptr,
const Eigen::VectorXd& _data,
const Eigen::MatrixXd& _data_cov,
- CaptureBasePtr _capture_origin_ptr,
- StateBlockPtr _p_ptr,
- StateBlockPtr _o_ptr,
- StateBlockPtr _intr_ptr) :
- CaptureMotion("CaptureDiffDrive",
- _ts,
- _sensor_ptr,
- _data,
- _data_cov,
- _capture_origin_ptr,
- _p_ptr,
- _o_ptr,
- _intr_ptr)
+ CaptureBasePtr _capture_origin_ptr,
+ StateBlockPtr _p_ptr,
+ StateBlockPtr _o_ptr,
+ StateBlockPtr _intr_ptr)
+ : CaptureMotion("CaptureDiffDrive",
+ _ts,
+ _sensor_ptr,
+ _data,
+ _data_cov,
+ _capture_origin_ptr,
+ _p_ptr,
+ _o_ptr,
+ _intr_ptr)
{
//
}
-} // namespace wolf
+} // namespace wolf
diff --git a/src/capture/capture_landmarks_external.cpp b/src/capture/capture_landmarks_external.cpp
index a2394cf9f5136ea86a9eefe6b9958024419a5205..7ef37912443ac717072f6240146a93b7846e7a7b 100644
--- a/src/capture/capture_landmarks_external.cpp
+++ b/src/capture/capture_landmarks_external.cpp
@@ -20,33 +20,36 @@
#include "core/capture/capture_landmarks_external.h"
-namespace wolf{
-
-CaptureLandmarksExternal::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) :
- CaptureBase("CaptureLandmarksExternal", _ts, _sensor_ptr)
+namespace wolf
+{
+CaptureLandmarksExternal::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)
+ : CaptureBase("CaptureLandmarksExternal", _ts, _sensor_ptr)
{
- if (_ids.size() != _detections.size() or
- _ids.size() != _covs.size() or
- _ids.size() != _qualities.size())
- throw std::runtime_error("CaptureLandmarksExternal constructor: '_ids', '_detections', '_covs', '_qualities' should have the same size.");
-
+ if (_ids.size() != _detections.size() or _ids.size() != _covs.size() or _ids.size() != _qualities.size())
+ 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));
}
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 Eigen::VectorXd& _detection,
+ const Eigen::MatrixXd& _cov,
+ const double& _quality)
{
detections_.push_back(LandmarkDetection{_id, _detection, _cov, _quality});
}
-} // namespace wolf
+} // namespace wolf
diff --git a/src/capture/capture_motion.cpp b/src/capture/capture_motion.cpp
index e6a35dbe303c0bb03ca4d908da0ed998abc3f0cd..c153969ae757639b75ff74718a593a07417aa8d3 100644
--- a/src/capture/capture_motion.cpp
+++ b/src/capture/capture_motion.cpp
@@ -22,35 +22,37 @@
namespace wolf
{
-
-CaptureMotion::CaptureMotion(const std::string & _type,
- const TimeStamp& _ts,
- SensorBasePtr _sensor_ptr,
+CaptureMotion::CaptureMotion(const std::string& _type,
+ const TimeStamp& _ts,
+ SensorBasePtr _sensor_ptr,
const Eigen::VectorXd& _data,
- CaptureBasePtr _capture_origin_ptr) :
- CaptureBase(_type, _ts, _sensor_ptr),
- data_(_data),
- data_cov_(_sensor_ptr ? _sensor_ptr->computeNoiseCov(_data) : Eigen::MatrixXd::Zero(_data.rows(), _data.rows())), // Someone should test this zero and do something smart accordingly
- buffer_(),
- capture_origin_ptr_(_capture_origin_ptr)
+ CaptureBasePtr _capture_origin_ptr)
+ : CaptureBase(_type, _ts, _sensor_ptr),
+ data_(_data),
+ data_cov_(_sensor_ptr ? _sensor_ptr->computeNoiseCov(_data)
+ : Eigen::MatrixXd::Zero(
+ _data.rows(),
+ _data.rows())), // Someone should test this zero and do something smart accordingly
+ buffer_(),
+ capture_origin_ptr_(_capture_origin_ptr)
{
//
}
-CaptureMotion::CaptureMotion(const std::string & _type,
- const TimeStamp& _ts,
- SensorBasePtr _sensor_ptr,
+CaptureMotion::CaptureMotion(const std::string& _type,
+ const TimeStamp& _ts,
+ SensorBasePtr _sensor_ptr,
const Eigen::VectorXd& _data,
const Eigen::MatrixXd& _data_cov,
- CaptureBasePtr _capture_origin_ptr,
- StateBlockPtr _p_ptr ,
- StateBlockPtr _o_ptr ,
- StateBlockPtr _intr_ptr ) :
- CaptureBase(_type, _ts, _sensor_ptr, _p_ptr, _o_ptr, _intr_ptr),
- data_(_data),
- data_cov_(_data_cov),
- buffer_(),
- capture_origin_ptr_(_capture_origin_ptr)
+ CaptureBasePtr _capture_origin_ptr,
+ StateBlockPtr _p_ptr,
+ StateBlockPtr _o_ptr,
+ StateBlockPtr _intr_ptr)
+ : CaptureBase(_type, _ts, _sensor_ptr, _p_ptr, _o_ptr, _intr_ptr),
+ data_(_data),
+ data_cov_(_data_cov),
+ buffer_(),
+ capture_origin_ptr_(_capture_origin_ptr)
{
//
}
@@ -60,17 +62,15 @@ CaptureMotion::~CaptureMotion()
//
}
-bool CaptureMotion::containsTimeStamp (const TimeStamp& _ts, double _time_tolerance) const
+bool CaptureMotion::containsTimeStamp(const TimeStamp& _ts, double _time_tolerance) const
{
assert(_ts.ok() and this->time_stamp_.ok());
// the same capture is within tolerance
- if (this->time_stamp_ - _time_tolerance <= _ts && _ts <= this->time_stamp_ + _time_tolerance)
- return true;
+ if (this->time_stamp_ - _time_tolerance <= _ts && _ts <= this->time_stamp_ + _time_tolerance) return true;
// buffer is empty, and the capture is not within tolerance
- if (this->getBuffer().empty())
- return false;
+ if (this->getBuffer().empty()) return false;
// buffer encloses timestamp, if ts is:
// from : buffer.first.ts - tt
@@ -83,16 +83,16 @@ bool CaptureMotion::containsTimeStamp (const TimeStamp& _ts, double _time_tolera
return false;
}
-
-
-
-void CaptureMotion::printHeader(int _depth, bool _factored_by, bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const
+void CaptureMotion::printHeader(int _depth,
+ bool _factored_by,
+ bool _metric,
+ bool _state_blocks,
+ std::ostream& _stream,
+ std::string _tabs) const
{
- _stream << _tabs << "CapM" << id()
- << " " << getType()
- << " ts = " << std::setprecision(3) << getTimeStamp();
+ _stream << _tabs << "CapM" << id() << " " << getType() << " ts = " << std::setprecision(3) << getTimeStamp();
- if(getSensor() != nullptr)
+ if (getSensor() != nullptr)
{
_stream << " -> Sen" << getSensor()->id();
}
@@ -102,38 +102,37 @@ void CaptureMotion::printHeader(int _depth, bool _factored_by, bool _metric, boo
if (auto OC = getOriginCapture())
{
_stream << " -> Origin: Cap" << OC->id();
- if (auto OF = OC->getFrame())
- _stream << " ; Frm" << OF->id();
+ if (auto OF = OC->getFrame()) _stream << " ; Frm" << OF->id();
}
- _stream << ((_depth < 3) ? " -- " + std::to_string(getFeatureList().size()) + "f" : "");
+ _stream << ((_depth < 3) ? " -- " + toString(getFeatureList().size()) + "f" : "");
if (_factored_by)
{
_stream << " <-- ";
for (auto fac : getFactoredBySet())
- if (fac)
- _stream << "Fac" << fac->id() << " \t";
+ if (fac) _stream << "Fac" << fac->id() << " \t";
}
_stream << std::endl;
- if (getStateBlockMap().size() > 0)
- printState(_factored_by, _metric, _state_blocks, _stream, _tabs);
+ if (getStateBlockMap().size() > 0) printState(_factored_by, _metric, _state_blocks, _stream, _tabs);
- _stream << _tabs << " " << "buffer size : " << getBuffer().size();
+ _stream << _tabs << " "
+ << "buffer size : " << getBuffer().size();
if (getBuffer().size() > 0) _stream << " ; nbr of data samples : " << getBuffer().size() - 1;
_stream << std::endl;
- if ( _metric && ! getBuffer().empty())
+ if (_metric && !getBuffer().empty())
{
- _stream << _tabs << " " << "delta preint : (" << getDeltaPreint().transpose() << ")" << std::endl;
-// if (hasCalibration())
-// {
-// _stream << _tabs << " " << "calib preint : (" << getCalibrationPreint().transpose() << ")" << std::endl;
-// _stream << _tabs << " " << "jacob preint : (" << getJacobianCalib().row(0) << ")" << std::endl;
-// _stream << _tabs << " " << "calib current: (" << getCalibration().transpose() << ")" << std::endl;
-// _stream << _tabs << " " << "delta correct: (" << getDeltaCorrected(getCalibration()).transpose() << ")" << std::endl;
-// }
+ _stream << _tabs << " "
+ << "delta preint : (" << getDeltaPreint().transpose() << ")" << std::endl;
+ // if (hasCalibration())
+ // {
+ // _stream << _tabs << " " << "calib preint : (" << getCalibrationPreint().transpose() << ")" <<
+ // std::endl; _stream << _tabs << " " << "jacob preint : (" << getJacobianCalib().row(0) << ")" <<
+ // std::endl; _stream << _tabs << " " << "calib current: (" << getCalibration().transpose() << ")"
+ // << std::endl; _stream << _tabs << " " << "delta correct: (" <<
+ // getDeltaCorrected(getCalibration()).transpose() << ")" << std::endl;
+ // }
}
-
-}
}
+} // namespace wolf
diff --git a/src/capture/capture_odom_2d.cpp b/src/capture/capture_odom_2d.cpp
index 447a301effd1122279874d1154119c59963d57fd..de755b1b5c7efec7c5ec2772b0ae87926722b5ee 100644
--- a/src/capture/capture_odom_2d.cpp
+++ b/src/capture/capture_odom_2d.cpp
@@ -17,33 +17,26 @@
//
// 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/>.
-/*
- * capture_odom_2d.cpp
- *
- * Created on: Oct 16, 2017
- * Author: jsola
- */
#include "core/capture/capture_odom_2d.h"
namespace wolf
{
-
-CaptureOdom2d::CaptureOdom2d(const TimeStamp& _init_ts,
- SensorBasePtr _sensor_ptr,
+CaptureOdom2d::CaptureOdom2d(const TimeStamp& _init_ts,
+ SensorBasePtr _sensor_ptr,
const Eigen::VectorXd& _data,
- CaptureBasePtr _capture_origin_ptr):
- CaptureMotion("CaptureOdom2d", _init_ts, _sensor_ptr, _data, _capture_origin_ptr)
+ CaptureBasePtr _capture_origin_ptr)
+ : CaptureMotion("CaptureOdom2d", _init_ts, _sensor_ptr, _data, _capture_origin_ptr)
{
//
}
-CaptureOdom2d::CaptureOdom2d(const TimeStamp& _init_ts,
- SensorBasePtr _sensor_ptr,
+CaptureOdom2d::CaptureOdom2d(const TimeStamp& _init_ts,
+ SensorBasePtr _sensor_ptr,
const Eigen::VectorXd& _data,
const Eigen::MatrixXd& _data_cov,
- CaptureBasePtr _capture_origin_ptr):
- CaptureMotion("CaptureOdom2d", _init_ts, _sensor_ptr, _data, _data_cov, _capture_origin_ptr)
+ CaptureBasePtr _capture_origin_ptr)
+ : CaptureMotion("CaptureOdom2d", _init_ts, _sensor_ptr, _data, _data_cov, _capture_origin_ptr)
{
//
}
diff --git a/src/capture/capture_odom_3d.cpp b/src/capture/capture_odom_3d.cpp
index eb3b3bff5225433792be43e9eae544bef2adf19f..e68ed98376dd5ec2e3cee26f64ce7576650c0830 100644
--- a/src/capture/capture_odom_3d.cpp
+++ b/src/capture/capture_odom_3d.cpp
@@ -17,33 +17,26 @@
//
// 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/>.
-/*
- * capture_odom_3d.cpp
- *
- * Created on: Oct 16, 2017
- * Author: jsola
- */
#include "core/capture/capture_odom_3d.h"
namespace wolf
{
-
-CaptureOdom3d::CaptureOdom3d(const TimeStamp& _init_ts,
- SensorBasePtr _sensor_ptr,
+CaptureOdom3d::CaptureOdom3d(const TimeStamp& _init_ts,
+ SensorBasePtr _sensor_ptr,
const Eigen::VectorXd& _data,
- CaptureBasePtr _capture_origin_ptr):
- CaptureMotion("CaptureOdom3d", _init_ts, _sensor_ptr, _data, _capture_origin_ptr)
+ CaptureBasePtr _capture_origin_ptr)
+ : CaptureMotion("CaptureOdom3d", _init_ts, _sensor_ptr, _data, _capture_origin_ptr)
{
//
}
-CaptureOdom3d::CaptureOdom3d(const TimeStamp& _init_ts,
- SensorBasePtr _sensor_ptr,
+CaptureOdom3d::CaptureOdom3d(const TimeStamp& _init_ts,
+ SensorBasePtr _sensor_ptr,
const Eigen::VectorXd& _data,
const Eigen::MatrixXd& _data_cov,
- CaptureBasePtr _capture_origin_ptr):
- CaptureMotion("CaptureOdom3d", _init_ts, _sensor_ptr, _data, _data_cov, _capture_origin_ptr)
+ CaptureBasePtr _capture_origin_ptr)
+ : CaptureMotion("CaptureOdom3d", _init_ts, _sensor_ptr, _data, _data_cov, _capture_origin_ptr)
{
//
}
@@ -54,4 +47,3 @@ CaptureOdom3d::~CaptureOdom3d()
}
} /* namespace wolf */
-
diff --git a/src/capture/capture_pose.cpp b/src/capture/capture_pose.cpp
index 5910874bb92f8ead6504835a160051e90a3cee74..ce8a4d543c9f6fd5ac0b1036f719b40503028ec8 100644
--- a/src/capture/capture_pose.cpp
+++ b/src/capture/capture_pose.cpp
@@ -22,7 +22,6 @@
namespace wolf
{
-
CapturePose::CapturePose(const TimeStamp& _ts,
SensorBasePtr _sensor_ptr,
const Eigen::VectorXd& _data,
diff --git a/src/capture/capture_void.cpp b/src/capture/capture_void.cpp
index 2c960af33c8e8b818b40d024511b23d88d9e4559..a18c8db422e9ed20f37e75e2d445c43bf94c859f 100644
--- a/src/capture/capture_void.cpp
+++ b/src/capture/capture_void.cpp
@@ -20,17 +20,17 @@
#include "core/capture/capture_void.h"
-namespace wolf {
-
-CaptureVoid::CaptureVoid(const TimeStamp& _ts, SensorBasePtr _sensor_ptr) :
- CaptureBase("CaptureVoid", _ts, _sensor_ptr)
+namespace wolf
+{
+CaptureVoid::CaptureVoid(const TimeStamp& _ts, SensorBasePtr _sensor_ptr)
+ : CaptureBase("CaptureVoid", _ts, _sensor_ptr)
{
//
}
CaptureVoid::~CaptureVoid()
{
- //std::cout << "deleting CaptureVoid " << id() << std::endl;
+ // std::cout << "deleting CaptureVoid " << id() << std::endl;
}
-} // namespace wolf
+} // namespace wolf
diff --git a/src/ceres_wrapper/local_parametrization_wrapper.cpp b/src/ceres_wrapper/local_parametrization_wrapper.cpp
index 9143b059f6c737e6efdc16f10d888e9eba2073f0..eb1631397088389010aeb15b41a9513d2aafeecf 100644
--- a/src/ceres_wrapper/local_parametrization_wrapper.cpp
+++ b/src/ceres_wrapper/local_parametrization_wrapper.cpp
@@ -20,22 +20,21 @@
#include "core/ceres_wrapper/local_parametrization_wrapper.h"
-namespace wolf {
-
+namespace wolf
+{
bool LocalParametrizationWrapper::Plus(const double* x_raw, const double* delta_raw, double* x_plus_delta_raw) const
{
Eigen::Map<const Eigen::VectorXd> x_raw_map((double*)x_raw, GlobalSize());
Eigen::Map<const Eigen::VectorXd> delta_raw_map((double*)delta_raw, LocalSize());
- Eigen::Map<Eigen::VectorXd> x_plus_map((double*)x_plus_delta_raw, GlobalSize());
+ Eigen::Map<Eigen::VectorXd> x_plus_map((double*)x_plus_delta_raw, GlobalSize());
return local_parametrization_ptr_->plus(x_raw_map, delta_raw_map, x_plus_map);
};
bool LocalParametrizationWrapper::ComputeJacobian(const double* x, double* jacobian) const
{
- Eigen::Map<const Eigen::VectorXd> x_map((double*)x, GlobalSize());
- Eigen::Map<Eigen::MatrixRowXd> jacobian_map((double*)jacobian, GlobalSize(), LocalSize());
+ Eigen::Map<const Eigen::VectorXd> x_map((double*)x, GlobalSize());
+ Eigen::Map<Eigen::MatrixRowXd> jacobian_map((double*)jacobian, GlobalSize(), LocalSize());
return local_parametrization_ptr_->computeJacobian(x_map, jacobian_map);
};
-} // namespace wolf
-
+} // namespace wolf
diff --git a/src/ceres_wrapper/qr_manager.cpp b/src/ceres_wrapper/qr_manager.cpp
index 0e3e8c7017ba1ebd2108ccffaa300e98227e22ab..1862081067ac1b879ec011182f25578b0a9bca7b 100644
--- a/src/ceres_wrapper/qr_manager.cpp
+++ b/src/ceres_wrapper/qr_manager.cpp
@@ -17,25 +17,19 @@
//
// 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/>.
-/*
- * qr_manager.cpp
- *
- * Created on: Jun 7, 2017
- * Author: jvallve
- */
#include "qr_manager.h"
-namespace wolf {
-
-QRManager::QRManager(ProblemPtr _wolf_problem, const unsigned int& _N_batch) :
- SolverManager(_wolf_problem),
- A_(), // empty matrix
- b_(),
- any_state_block_removed_(false),
- new_state_blocks_(0),
- N_batch_(_N_batch),
- pending_changes_(false)
+namespace wolf
+{
+QRManager::QRManager(ProblemPtr _wolf_problem, const unsigned int& _N_batch)
+ : SolverManager(_wolf_problem),
+ A_(), // empty matrix
+ b_(),
+ any_state_block_removed_(false),
+ new_state_blocks_(0),
+ N_batch_(_N_batch),
+ pending_changes_(false)
{
//
}
@@ -52,8 +46,7 @@ std::string QRManager::solve(const unsigned int& _report_level)
update();
// Decomposition
- if (!computeDecomposition())
- return std::string("decomposition failed\n");
+ if (!computeDecomposition()) return std::string("decomposition failed\n");
// Solve
Eigen::VectorXd x_incr = solver_.solve(-b_);
@@ -61,7 +54,8 @@ std::string QRManager::solve(const unsigned int& _report_level)
// update state blocks
for (auto sb_pair : sb_2_col_)
- sb_pair.first->setState(sb_pair.first->getState() + x_incr.segment(sb_pair.second, sb_pair.first->getSize()), false);
+ sb_pair.first->setState(sb_pair.first->getState() + x_incr.segment(sb_pair.second, sb_pair.first->getSize()),
+ false);
if (_report_level == 1)
return std::string("Success!\n");
@@ -78,34 +72,40 @@ void QRManager::computeCovariances(CovarianceBlocksToBeComputed _blocks)
void QRManager::computeCovariances(const StateBlockPtrList& _sb_list)
{
- //std::cout << "computing covariances.." << std::endl;
+ // std::cout << "computing covariances.." << std::endl;
update();
computeDecomposition();
-// std::cout << "R is " << solver_.matrixR().rows() << "x" << solver_.matrixR().cols() << std::endl;
-// std::cout << Eigen::MatrixXd(solver_.matrixR()) << std::endl;
+ // std::cout << "R is " << solver_.matrixR().rows() << "x" << solver_.matrixR().cols() << std::endl;
+ // std::cout << Eigen::MatrixXd(solver_.matrixR()) << std::endl;
covariance_solver_.compute(solver_.matrixR().topRows(solver_.matrixR().cols()));
- Eigen::SparseMatrix<double, Eigen::ColMajor> I(A_.cols(),A_.cols());
+ Eigen::SparseMatrix<double, Eigen::ColMajor> I(A_.cols(), A_.cols());
I.setIdentity();
Eigen::SparseMatrix<double, Eigen::ColMajor> iR = covariance_solver_.solve(I);
- Eigen::MatrixXd Sigma_full = solver_.colsPermutation() * iR * iR.transpose() * solver_.colsPermutation().transpose();
+ Eigen::MatrixXd Sigma_full =
+ solver_.colsPermutation() * iR * iR.transpose() * solver_.colsPermutation().transpose();
-// std::cout << "A' A = \n" << Eigen::MatrixXd(A_.transpose() * A_)<< std::endl;
-// std::cout << "P iR iR' P' = \n" << Eigen::MatrixXd(P * iR * iR.transpose() * P.transpose()) << std::endl;
-// std::cout << "Sigma * Lambda = \n" << Eigen::MatrixXd(Sigma_full * A_.transpose() * A_) << std::endl;
-// std::cout << "Permutation: \n" << solver_.colsPermutation() << std::endl;
-// std::cout << "Sigma = \n" << Sigma_full << std::endl;
+ // std::cout << "A' A = \n" << Eigen::MatrixXd(A_.transpose() * A_)<< std::endl;
+ // std::cout << "P iR iR' P' = \n" << Eigen::MatrixXd(P * iR * iR.transpose() * P.transpose()) << std::endl;
+ // std::cout << "Sigma * Lambda = \n" << Eigen::MatrixXd(Sigma_full * A_.transpose() * A_) << std::endl;
+ // std::cout << "Permutation: \n" << solver_.colsPermutation() << std::endl;
+ // std::cout << "Sigma = \n" << Sigma_full << std::endl;
// STORE DESIRED COVARIANCES
for (auto sb_row = _sb_list.begin(); sb_row != _sb_list.end(); sb_row++)
- for (auto sb_col = sb_row; sb_col!=_sb_list.end(); sb_col++)
+ for (auto sb_col = sb_row; sb_col != _sb_list.end(); sb_col++)
{
- //std::cout << "cov state block " << sb_col->get() << std::endl;
+ // std::cout << "cov state block " << sb_col->get() << std::endl;
assert(sb_2_col_.find(*sb_col) != sb_2_col_.end() && "state block not found");
- //std::cout << "block: " << sb_2_col_[*sb_row] << "," << sb_2_col_[*sb_col] << std::endl << Sigma_full.block(sb_2_col_[*sb_row], sb_2_col_[*sb_col], (*sb_row)->getSize(), (*sb_col)->getSize()) << std::endl;
- wolf_problem_->addCovarianceBlock(*sb_row, *sb_col, Sigma_full.block(sb_2_col_[*sb_row], sb_2_col_[*sb_col], (*sb_row)->getSize(), (*sb_col)->getSize()));
+ // std::cout << "block: " << sb_2_col_[*sb_row] << "," << sb_2_col_[*sb_col] << std::endl <<
+ // Sigma_full.block(sb_2_col_[*sb_row], sb_2_col_[*sb_col], (*sb_row)->getSize(), (*sb_col)->getSize()) <<
+ // std::endl;
+ wolf_problem_->addCovarianceBlock(
+ *sb_row,
+ *sb_col,
+ Sigma_full.block(sb_2_col_[*sb_row], sb_2_col_[*sb_col], (*sb_row)->getSize(), (*sb_col)->getSize()));
}
}
@@ -132,24 +132,22 @@ bool QRManager::computeDecomposition()
}
// resize and setZero A, b
- A_.resize(meas_size,state_size);
+ A_.resize(meas_size, state_size);
b_.resize(meas_size);
}
if (any_state_block_removed_ || new_state_blocks_ >= N_batch_)
{
// relinearize all factors
- for (auto fac_pair : fac_2_row_)
- relinearizeFactor(fac_pair.first);
+ for (auto fac_pair : fac_2_row_) relinearizeFactor(fac_pair.first);
any_state_block_removed_ = false;
- new_state_blocks_ = 0;
+ new_state_blocks_ = 0;
}
// Decomposition
solver_.compute(A_);
- if (solver_.info() != Eigen::Success)
- return false;
+ if (solver_.info() != Eigen::Success) return false;
}
pending_changes_ = false;
@@ -159,7 +157,7 @@ bool QRManager::computeDecomposition()
void QRManager::addFactor(FactorBasePtr _fac_ptr)
{
- //std::cout << "add factor " << _fac_ptr->id() << std::endl;
+ // std::cout << "add factor " << _fac_ptr->id() << std::endl;
assert(fac_2_row_.find(_fac_ptr) == fac_2_row_.end() && "adding existing factor");
fac_2_row_[_fac_ptr] = A_.rows();
A_.conservativeResize(A_.rows() + _fac_ptr->getSize(), A_.cols());
@@ -175,7 +173,7 @@ void QRManager::addFactor(FactorBasePtr _fac_ptr)
void QRManager::removeFactor(FactorBasePtr _fac_ptr)
{
- //std::cout << "remove factor " << _fac_ptr->id() << std::endl;
+ // std::cout << "remove factor " << _fac_ptr->id() << std::endl;
assert(fac_2_row_.find(_fac_ptr) != fac_2_row_.end() && "removing unknown factor");
eraseBlockRow(A_, fac_2_row_[_fac_ptr], _fac_ptr->getSize());
b_.segment(fac_2_row_[_fac_ptr], _fac_ptr->getSize()).setZero();
@@ -185,7 +183,7 @@ void QRManager::removeFactor(FactorBasePtr _fac_ptr)
void QRManager::addStateBlock(StateBlockPtr _st_ptr)
{
- //std::cout << "add state block " << _st_ptr.get() << std::endl;
+ // std::cout << "add state block " << _st_ptr.get() << std::endl;
assert(sb_2_col_.find(_st_ptr) == sb_2_col_.end() && "adding existing state block");
sb_2_col_[_st_ptr] = A_.cols();
A_.conservativeResize(A_.rows(), A_.cols() + _st_ptr->getSize());
@@ -196,7 +194,7 @@ void QRManager::addStateBlock(StateBlockPtr _st_ptr)
void QRManager::removeStateBlock(StateBlockPtr _st_ptr)
{
- //std::cout << "remove state block " << _st_ptr.get() << std::endl;
+ // std::cout << "remove state block " << _st_ptr.get() << std::endl;
assert(sb_2_col_.find(_st_ptr) != sb_2_col_.end() && "removing unknown state block");
eraseBlockCol(A_, sb_2_col_[_st_ptr], _st_ptr->getSize());
@@ -210,34 +208,33 @@ void QRManager::removeStateBlock(StateBlockPtr _st_ptr)
void QRManager::updateStateBlockStatus(StateBlockPtr _st_ptr)
{
- //std::cout << "update state block " << _st_ptr.get() << std::endl;
+ // std::cout << "update state block " << _st_ptr.get() << std::endl;
if (_st_ptr->isFixed())
{
- if (sb_2_col_.find(_st_ptr) != sb_2_col_.end())
- removeStateBlock(_st_ptr);
+ if (sb_2_col_.find(_st_ptr) != sb_2_col_.end()) removeStateBlock(_st_ptr);
}
- else
- if (sb_2_col_.find(_st_ptr) == sb_2_col_.end())
- addStateBlock(_st_ptr);
+ else if (sb_2_col_.find(_st_ptr) == sb_2_col_.end())
+ addStateBlock(_st_ptr);
}
void QRManager::relinearizeFactor(FactorBasePtr _fac_ptr)
{
// evaluate factor
std::vector<const double*> fac_states_ptr;
- for (auto sb : _fac_ptr->getStateBlockPtrVector())
- fac_states_ptr.push_back(sb->get());
- Eigen::VectorXd residual(_fac_ptr->getSize());
+ for (auto sb : _fac_ptr->getStateBlockPtrVector()) fac_states_ptr.push_back(sb->get());
+ Eigen::VectorXd residual(_fac_ptr->getSize());
std::vector<Eigen::MatrixXd> jacobians;
- _fac_ptr->evaluate(fac_states_ptr,residual,jacobians);
+ _fac_ptr->evaluate(fac_states_ptr, residual, jacobians);
// Fill jacobians
Eigen::SparseMatrixd A_block_row(_fac_ptr->getSize(), A_.cols());
for (auto i = 0; i < jacobians.size(); i++)
if (!_fac_ptr->getStateBlockPtrVector()[i]->isFixed())
{
- assert(sb_2_col_.find(_fac_ptr->getStateBlockPtrVector()[i]) != sb_2_col_.end() && "factor involving a state block not added");
- assert(A_.cols() >= sb_2_col_[_fac_ptr->getStateBlockPtrVector()[i]] + jacobians[i].cols() - 1 && "bad A number of cols");
+ assert(sb_2_col_.find(_fac_ptr->getStateBlockPtrVector()[i]) != sb_2_col_.end() &&
+ "factor involving a state block not added");
+ assert(A_.cols() >= sb_2_col_[_fac_ptr->getStateBlockPtrVector()[i]] + jacobians[i].cols() - 1 &&
+ "bad A number of cols");
// insert since A_block_row has just been created so it's empty for sure
insertSparseBlock(jacobians[i], A_block_row, 0, sb_2_col_[_fac_ptr->getStateBlockPtrVector()[i]]);
}
diff --git a/src/ceres_wrapper/solver_ceres.cpp b/src/ceres_wrapper/solver_ceres.cpp
index c242d4911d5e8487cc102f480a8cea1f249b8f2d..dc00901999032bc39d1bf4b9cc167ac69d46e07e 100644
--- a/src/ceres_wrapper/solver_ceres.cpp
+++ b/src/ceres_wrapper/solver_ceres.cpp
@@ -29,35 +29,80 @@
namespace wolf
{
-
-SolverCeres::SolverCeres(const ProblemPtr& _wolf_problem) :
- SolverCeres(_wolf_problem, std::make_shared<ParamsCeres>())
-{
-}
-
-SolverCeres::SolverCeres(const ProblemPtr& _wolf_problem,
- const ParamsCeresPtr& _params)
- : SolverManager(_wolf_problem, _params)
- , params_ceres_(_params)
- , n_iter_acc_(0)
- , n_iter_max_(0)
- , n_convergence_(0)
- , n_interrupted_(0)
- , n_no_convergence_(0)
+SolverCeres::SolverCeres(const ProblemPtr& _wolf_problem, const YAML::Node& _params)
+ : SolverManager(_wolf_problem, _params),
+ n_iter_acc_(0),
+ n_iter_max_(0),
+ n_convergence_(0),
+ n_interrupted_(0),
+ n_no_convergence_(0)
{
- covariance_ = std::make_unique<ceres::Covariance>(params_ceres_->covariance_options);
- ceres_problem_ = std::make_unique<ceres::Problem>(params_ceres_->problem_options);
-
- if (params_ceres_->interrupt_on_problem_change)
- getSolverOptions().callbacks.push_back(new IterationUpdateCallback(wolf_problem_,
- params_ceres_->min_num_iterations,
- params_ceres_->verbose != SolverManager::ReportVerbosity::QUIET));
+ // LOAD PARAMETERS ======================================================
+ // CERES SOLVER OPTIONS
+ solver_options_ = ceres::Solver::Options();
+ solver_options_.max_num_iterations = _params["max_num_iterations"].as<int>();
+ solver_options_.function_tolerance = _params["function_tolerance"].as<double>();
+ solver_options_.gradient_tolerance = _params["gradient_tolerance"].as<double>();
+ solver_options_.num_threads = _params["n_threads"].as<int>();
+
+ std::string minimizer = _params["minimizer"].as<std::string>();
+ if (minimizer == "LEVENBERG_MARQUARDT" or minimizer == "levenberg_marquardt")
+ {
+ solver_options_.minimizer_type = ceres::TRUST_REGION;
+ solver_options_.trust_region_strategy_type = ceres::LEVENBERG_MARQUARDT;
+ }
+ else if (minimizer == "DOGLEG" or minimizer == "dogleg")
+ {
+ solver_options_.minimizer_type = ceres::TRUST_REGION;
+ solver_options_.trust_region_strategy_type = ceres::DOGLEG;
+ }
+ else if (minimizer == "LBFGS" or minimizer == "lbfgs")
+ {
+ solver_options_.minimizer_type = ceres::LINE_SEARCH;
+ solver_options_.line_search_direction_type = ceres::LBFGS;
+ }
+ else if (minimizer == "BFGS" or minimizer == "bfgs")
+ {
+ solver_options_.minimizer_type = ceres::LINE_SEARCH;
+ solver_options_.line_search_direction_type = ceres::BFGS;
+ }
+ else
+ {
+ throw std::runtime_error(
+ "ParamsCeres: Wrong parameter 'minimizer'. Should be 'LEVENBERG_MARQUARDT', 'DOGLEG', 'LBFGS' or "
+ "'BFGS' (upper or lowercase)");
+ }
+ if (solver_options_.minimizer_type == ceres::TRUST_REGION) // specific options for TRUST REGION
+ {
+ solver_options_.use_nonmonotonic_steps = _params["use_nonmonotonic_steps"].as<bool>();
+ if (solver_options_.use_nonmonotonic_steps)
+ solver_options_.max_consecutive_nonmonotonic_steps =
+ _params["max_consecutive_nonmonotonic_steps"].as<int>();
+ }
+ // interrupt solver whenever the problem is updated (via ceres::iterationCallback)
+ if (_params["interrupt_on_problem_change"].as<bool>())
+ solver_options_.callbacks.push_back(
+ new IterationUpdateCallback(wolf_problem_,
+ _params["min_num_iterations"].as<int>(),
+ verbose_ != SolverManager::ReportVerbosity::QUIET));
+
+ // PROBLEM OPTIONS
+ auto problem_options = ceres::Problem::Options();
+ problem_options.cost_function_ownership = ceres::DO_NOT_TAKE_OWNERSHIP;
+ problem_options.loss_function_ownership = ceres::TAKE_OWNERSHIP;
+ problem_options.local_parameterization_ownership = ceres::DO_NOT_TAKE_OWNERSHIP;
+ ceres_problem_ = std::make_unique<ceres::Problem>(problem_options);
+
+ // COVARIANCE OPTIONS
+ auto covariance_options = ceres::Covariance::Options();
+ covariance_options.apply_loss_function = false;
+ covariance_options.num_threads = solver_options_.num_threads;
+ covariance_ = std::make_unique<ceres::Covariance>(covariance_options);
}
SolverCeres::~SolverCeres()
{
- while (!fac_2_residual_idx_.empty())
- removeFactorDerived(fac_2_residual_idx_.begin()->first);
+ while (!fac_2_residual_idx_.empty()) removeFactorDerived(fac_2_residual_idx_.begin()->first);
while (!state_blocks_.empty())
{
@@ -65,21 +110,21 @@ SolverCeres::~SolverCeres()
state_blocks_.erase(state_blocks_.begin());
}
- while (!getSolverOptions().callbacks.empty())
+ while (!solver_options_.callbacks.empty())
{
- delete getSolverOptions().callbacks.back();
- getSolverOptions().callbacks.pop_back();
+ delete solver_options_.callbacks.back();
+ solver_options_.callbacks.pop_back();
}
}
std::string SolverCeres::solveDerived(const ReportVerbosity report_level)
{
// run Ceres Solver
- ceres::Solve(getSolverOptions(), ceres_problem_.get(), &summary_);
+ ceres::Solve(solver_options_, ceres_problem_.get(), &summary_);
std::string report;
- //return report
+ // return report
if (report_level == ReportVerbosity::BRIEF)
report = summary_.BriefReport();
else if (report_level == ReportVerbosity::FULL)
@@ -102,7 +147,7 @@ std::string SolverCeres::solveDerived(const ReportVerbosity report_level)
}
bool SolverCeres::computeCovariancesDerived(const CovarianceBlocksToBeComputed _blocks)
-{
+{
// update problem
update();
@@ -112,12 +157,11 @@ bool SolverCeres::computeCovariancesDerived(const CovarianceBlocksToBeComputed _
switch (_blocks)
{
- case CovarianceBlocksToBeComputed::ALL:
- {
+ case CovarianceBlocksToBeComputed::ALL: {
// first create a vector containing all state blocks
std::vector<StateBlockPtr> all_state_blocks, landmark_state_blocks;
- //frame state blocks
- for(auto fr_pair : wolf_problem_->getTrajectory()->getFrameMap())
+ // frame state blocks
+ for (auto fr_pair : wolf_problem_->getTrajectory()->getFrameMap())
for (auto key : fr_pair.second->getKeys())
{
const auto& sb = fr_pair.second->getStateBlock(key);
@@ -125,32 +169,31 @@ bool SolverCeres::computeCovariancesDerived(const CovarianceBlocksToBeComputed _
}
// landmark state blocks
- for(auto l_ptr : wolf_problem_->getMap()->getLandmarkList())
+ for (auto l_ptr : wolf_problem_->getMap()->getLandmarkList())
for (auto key : l_ptr->getKeys())
{
const auto& sb = l_ptr->getStateBlock(key);
all_state_blocks.push_back(sb);
}
-
+
// double loop all against all (without repetitions)
for (unsigned int i = 0; i < all_state_blocks.size(); i++)
{
assert(isStateBlockRegisteredDerived(all_state_blocks[i]));
- for (unsigned int j = i; j < all_state_blocks.size(); j++)
+ for (unsigned int j = i; j < all_state_blocks.size(); j++)
{
assert(isStateBlockRegisteredDerived(all_state_blocks[i]));
- state_block_pairs.emplace_back(all_state_blocks[i],all_state_blocks[j]);
+ state_block_pairs.emplace_back(all_state_blocks[i], all_state_blocks[j]);
double_pairs.emplace_back(getAssociatedMemBlockPtr(all_state_blocks[i]),
getAssociatedMemBlockPtr(all_state_blocks[j]));
}
}
break;
}
- case CovarianceBlocksToBeComputed::ALL_MARGINALS:
- {
+ case CovarianceBlocksToBeComputed::ALL_MARGINALS: {
// first create a vector containing all state blocks
- for(auto fr_pair : wolf_problem_->getTrajectory()->getFrameMap())
+ for (auto fr_pair : wolf_problem_->getTrajectory()->getFrameMap())
for (auto key1 : fr_pair.second->getKeys())
{
const auto& sb1 = fr_pair.second->getStateBlock(key1);
@@ -163,13 +206,12 @@ bool SolverCeres::computeCovariancesDerived(const CovarianceBlocksToBeComputed _
state_block_pairs.emplace_back(sb1, sb2);
double_pairs.emplace_back(getAssociatedMemBlockPtr(sb1), getAssociatedMemBlockPtr(sb2));
- if (sb1 == sb2)
- break;
+ if (sb1 == sb2) break;
}
}
// landmark state blocks
- for(auto l_ptr : wolf_problem_->getMap()->getLandmarkList())
+ for (auto l_ptr : wolf_problem_->getMap()->getLandmarkList())
for (auto key1 : l_ptr->getKeys())
{
const auto& sb1 = l_ptr->getStateBlock(key1);
@@ -182,16 +224,14 @@ bool SolverCeres::computeCovariancesDerived(const CovarianceBlocksToBeComputed _
state_block_pairs.emplace_back(sb1, sb2);
double_pairs.emplace_back(getAssociatedMemBlockPtr(sb1), getAssociatedMemBlockPtr(sb2));
- if (sb1 == sb2)
- break;
+ if (sb1 == sb2) break;
}
}
break;
}
- case CovarianceBlocksToBeComputed::ROBOT_LANDMARKS:
- {
- //robot-robot
+ case CovarianceBlocksToBeComputed::ROBOT_LANDMARKS: {
+ // robot-robot
auto last_key_frame = wolf_problem_->getLastFrame();
assert(isStateBlockRegisteredDerived(last_key_frame->getP()));
@@ -209,7 +249,7 @@ bool SolverCeres::computeCovariancesDerived(const CovarianceBlocksToBeComputed _
getAssociatedMemBlockPtr(last_key_frame->getO()));
// landmarks
- for(auto l_ptr : wolf_problem_->getMap()->getLandmarkList())
+ for (auto l_ptr : wolf_problem_->getMap()->getLandmarkList())
for (auto key : l_ptr->getKeys())
{
const auto& sb = l_ptr->getStateBlock(key);
@@ -231,66 +271,65 @@ bool SolverCeres::computeCovariancesDerived(const CovarianceBlocksToBeComputed _
state_block_pairs.emplace_back(sb, sb2);
double_pairs.emplace_back(getAssociatedMemBlockPtr(sb), getAssociatedMemBlockPtr(sb2));
- if (sb == sb2)
- break;
+ if (sb == sb2) break;
}
}
-
+
break;
}
- case CovarianceBlocksToBeComputed::GAUSS:
- {
+ case CovarianceBlocksToBeComputed::GAUSS: {
// State blocks:
// - Last KF: PV
// last KF
FrameBasePtr frame = wolf_problem_->getLastFrame();
- if (not frame)
- return false;
+ if (not frame) return false;
while (frame)
{
- if (frame->has('P') and
- isStateBlockRegisteredDerived(frame->getStateBlock('P')) and
- frame->has('V') and
- isStateBlockRegisteredDerived(frame->getStateBlock('V')))
+ if (frame->has('P') and isStateBlockRegisteredDerived(frame->getStateBlock('P')) and
+ frame->has('V') and isStateBlockRegisteredDerived(frame->getStateBlock('V')))
{
break;
}
- //else
- WOLF_DEBUG("SolverCeres::computeCovariances: Frame ", frame->id(), " hasn't the state blocks or they are not registered. Trying with the previous one...");
+ // else
+ WOLF_DEBUG("SolverCeres::computeCovariances: Frame ",
+ frame->id(),
+ " hasn't the state blocks or they are not registered. Trying with the previous one...");
frame = frame->getPreviousFrame();
}
if (not frame)
{
- WOLF_WARN("SolverCeres::computeCovariances: Couldn't find a frame with valid and registered state blocks P and V, returning...");
+ WOLF_WARN(
+ "SolverCeres::computeCovariances: Couldn't find a frame with valid and registered state blocks P "
+ "and V, returning...");
return false;
}
// only marginals of P and V
- WOLF_DEBUG("SolverCeres::computeCovariances: storing pairs of frame ", frame->id(), " with state blocks: "
- "\nP @", frame->getStateBlock('P'),
- "\nV @", frame->getStateBlock('V'));
- state_block_pairs.emplace_back(frame->getStateBlock('P'),
- frame->getStateBlock('P'));
+ WOLF_DEBUG("SolverCeres::computeCovariances: storing pairs of frame ",
+ frame->id(),
+ " with state blocks: "
+ "\nP @",
+ frame->getStateBlock('P'),
+ "\nV @",
+ frame->getStateBlock('V'));
+ state_block_pairs.emplace_back(frame->getStateBlock('P'), frame->getStateBlock('P'));
double_pairs.emplace_back(getAssociatedMemBlockPtr(frame->getStateBlock('P')),
getAssociatedMemBlockPtr(frame->getStateBlock('P')));
- state_block_pairs.emplace_back(frame->getStateBlock('V'),
- frame->getStateBlock('V'));
+ state_block_pairs.emplace_back(frame->getStateBlock('V'), frame->getStateBlock('V'));
double_pairs.emplace_back(getAssociatedMemBlockPtr(frame->getStateBlock('V')),
getAssociatedMemBlockPtr(frame->getStateBlock('V')));
break;
}
- case CovarianceBlocksToBeComputed::GAUSS_TUCAN:
- {
+ case CovarianceBlocksToBeComputed::GAUSS_TUCAN: {
// State blocks:
// - Last KF: PV
// last KF
FrameBasePtr frame = wolf_problem_->getLastFrame();
- if (not frame)
- return false;
+ if (not frame) return false;
StateBlockPtr sb_gnss_T;
while (frame)
@@ -303,60 +342,60 @@ bool SolverCeres::computeCovariancesDerived(const CovarianceBlocksToBeComputed _
break;
}
- if (frame->has('P') and
- isStateBlockRegisteredDerived(frame->getStateBlock('P')) and
- frame->has('O') and
- isStateBlockRegisteredDerived(frame->getStateBlock('O')) and
- frame->has('V') and
- isStateBlockRegisteredDerived(frame->getStateBlock('V')) and
- sb_gnss_T and
+ if (frame->has('P') and isStateBlockRegisteredDerived(frame->getStateBlock('P')) and
+ frame->has('O') and isStateBlockRegisteredDerived(frame->getStateBlock('O')) and
+ frame->has('V') and isStateBlockRegisteredDerived(frame->getStateBlock('V')) and sb_gnss_T and
isStateBlockRegisteredDerived(sb_gnss_T))
{
break;
}
// else
- WOLF_DEBUG("SolverCeres::computeCovariances: Frame ", frame->id(), " hasn't the state blocks or they are not registered. Trying with the previous one...");
+ WOLF_DEBUG("SolverCeres::computeCovariances: Frame ",
+ frame->id(),
+ " hasn't the state blocks or they are not registered. Trying with the previous one...");
frame = frame->getPreviousFrame();
}
if (not frame)
{
- WOLF_WARN("SolverCeres::computeCovariances: Couldn't find a frame with valid and registered state blocks P, O, V and CaptureGnss with T, returning...");
+ WOLF_WARN(
+ "SolverCeres::computeCovariances: Couldn't find a frame with valid and registered state blocks P, "
+ "O, V and CaptureGnss with T, returning...");
return false;
}
// only marginals of P, O, V and T
- WOLF_DEBUG("SolverCeres::computeCovariances: storing pairs of frame ", frame->id(), " with state blocks: "
- "\nP @", frame->getStateBlock('P'),
- "\nO @", frame->getStateBlock('O'),
- "\nV @", frame->getStateBlock('V'),
- "\nT @", sb_gnss_T);
- state_block_pairs.emplace_back(frame->getStateBlock('P'),
- frame->getStateBlock('P'));
+ WOLF_DEBUG("SolverCeres::computeCovariances: storing pairs of frame ",
+ frame->id(),
+ " with state blocks: "
+ "\nP @",
+ frame->getStateBlock('P'),
+ "\nO @",
+ frame->getStateBlock('O'),
+ "\nV @",
+ frame->getStateBlock('V'),
+ "\nT @",
+ sb_gnss_T);
+ state_block_pairs.emplace_back(frame->getStateBlock('P'), frame->getStateBlock('P'));
double_pairs.emplace_back(getAssociatedMemBlockPtr(frame->getStateBlock('P')),
getAssociatedMemBlockPtr(frame->getStateBlock('P')));
- state_block_pairs.emplace_back(frame->getStateBlock('O'),
- frame->getStateBlock('O'));
+ state_block_pairs.emplace_back(frame->getStateBlock('O'), frame->getStateBlock('O'));
double_pairs.emplace_back(getAssociatedMemBlockPtr(frame->getStateBlock('O')),
getAssociatedMemBlockPtr(frame->getStateBlock('O')));
- state_block_pairs.emplace_back(frame->getStateBlock('V'),
- frame->getStateBlock('V'));
+ state_block_pairs.emplace_back(frame->getStateBlock('V'), frame->getStateBlock('V'));
double_pairs.emplace_back(getAssociatedMemBlockPtr(frame->getStateBlock('V')),
getAssociatedMemBlockPtr(frame->getStateBlock('V')));
state_block_pairs.emplace_back(sb_gnss_T, sb_gnss_T);
- double_pairs.emplace_back(getAssociatedMemBlockPtr(sb_gnss_T),
- getAssociatedMemBlockPtr(sb_gnss_T));
+ double_pairs.emplace_back(getAssociatedMemBlockPtr(sb_gnss_T), getAssociatedMemBlockPtr(sb_gnss_T));
break;
}
- case CovarianceBlocksToBeComputed::GAUSS_TUCAN_ONLY_POSITION:
- {
+ case CovarianceBlocksToBeComputed::GAUSS_TUCAN_ONLY_POSITION: {
// State blocks:
// - Last KF: PV
// last KF
FrameBasePtr frame = wolf_problem_->getLastFrame();
- if (not frame)
- return false;
+ if (not frame) return false;
StateBlockPtr sb_gnss_T;
while (frame)
@@ -369,42 +408,46 @@ bool SolverCeres::computeCovariancesDerived(const CovarianceBlocksToBeComputed _
break;
}
- if (frame->has('P') and
- isStateBlockRegisteredDerived(frame->getStateBlock('P')) and
- sb_gnss_T and
+ if (frame->has('P') and isStateBlockRegisteredDerived(frame->getStateBlock('P')) and sb_gnss_T and
isStateBlockRegisteredDerived(sb_gnss_T))
{
break;
}
// else
- WOLF_DEBUG("SolverCeres::computeCovariances: Frame ", frame->id(), " hasn't the state blocks or they are not registered. Trying with the previous one...");
+ WOLF_DEBUG("SolverCeres::computeCovariances: Frame ",
+ frame->id(),
+ " hasn't the state blocks or they are not registered. Trying with the previous one...");
frame = frame->getPreviousFrame();
}
if (not frame)
{
- WOLF_WARN("SolverCeres::computeCovariances: Couldn't find a frame with valid and registered state blocks P and CaptureGnss with T, returning...");
+ WOLF_WARN(
+ "SolverCeres::computeCovariances: Couldn't find a frame with valid and registered state blocks P "
+ "and CaptureGnss with T, returning...");
return false;
}
// only marginals of P and T
- WOLF_DEBUG("SolverCeres::computeCovariances: storing pairs of frame ", frame->id(), " with state blocks: "
- "\nP @", frame->getStateBlock('P'),
- "\nT @", sb_gnss_T);
- state_block_pairs.emplace_back(frame->getStateBlock('P'),
- frame->getStateBlock('P'));
+ WOLF_DEBUG("SolverCeres::computeCovariances: storing pairs of frame ",
+ frame->id(),
+ " with state blocks: "
+ "\nP @",
+ frame->getStateBlock('P'),
+ "\nT @",
+ sb_gnss_T);
+ state_block_pairs.emplace_back(frame->getStateBlock('P'), frame->getStateBlock('P'));
double_pairs.emplace_back(getAssociatedMemBlockPtr(frame->getStateBlock('P')),
getAssociatedMemBlockPtr(frame->getStateBlock('P')));
state_block_pairs.emplace_back(sb_gnss_T, sb_gnss_T);
- double_pairs.emplace_back(getAssociatedMemBlockPtr(sb_gnss_T),
- getAssociatedMemBlockPtr(sb_gnss_T));
+ double_pairs.emplace_back(getAssociatedMemBlockPtr(sb_gnss_T), getAssociatedMemBlockPtr(sb_gnss_T));
break;
}
default:
- throw std::runtime_error("SolverCeres::computeCovariances: Unknown CovarianceBlocksToBeComputed enum value");
-
+ throw std::runtime_error(
+ "SolverCeres::computeCovariances: Unknown CovarianceBlocksToBeComputed enum value");
}
- //std::cout << "pairs... " << double_pairs.size() << std::endl;
+ // std::cout << "pairs... " << double_pairs.size() << std::endl;
// COMPUTE DESIRED COVARIANCES
if (covariance_->Compute(double_pairs, ceres_problem_.get()))
@@ -415,7 +458,8 @@ bool SolverCeres::computeCovariancesDerived(const CovarianceBlocksToBeComputed _
// STORE DESIRED COVARIANCES
for (unsigned int i = 0; i < double_pairs.size(); i++)
{
- Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> cov(state_block_pairs[i].first->getLocalSize(),state_block_pairs[i].second->getLocalSize());
+ Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> cov(
+ state_block_pairs[i].first->getLocalSize(), state_block_pairs[i].second->getLocalSize());
covariance_->GetCovarianceBlockInTangentSpace(double_pairs[i].first, double_pairs[i].second, cov.data());
wolf_problem_->addCovarianceBlock(state_block_pairs[i].first, state_block_pairs[i].second, cov);
// std::cout << "covariance got switch: " << std::endl << cov << std::endl;
@@ -429,7 +473,7 @@ bool SolverCeres::computeCovariancesDerived(const CovarianceBlocksToBeComputed _
bool SolverCeres::computeCovariancesDerived(const std::vector<StateBlockPtr>& st_list)
{
- //std::cout << "SolverCeres: computing covariances..." << std::endl;
+ // std::cout << "SolverCeres: computing covariances..." << std::endl;
// update problem
update();
@@ -441,25 +485,26 @@ bool SolverCeres::computeCovariancesDerived(const std::vector<StateBlockPtr>& st
// double loop all against all (without repetitions)
for (auto st_it1 = st_list.begin(); st_it1 != st_list.end(); st_it1++)
{
- if (*st_it1 == nullptr){
+ if (*st_it1 == nullptr)
+ {
continue;
}
assert(isStateBlockRegisteredDerived(*st_it1));
for (auto st_it2 = st_it1; st_it2 != st_list.end(); st_it2++)
{
- if (*st_it2 == nullptr){
+ if (*st_it2 == nullptr)
+ {
continue;
}
assert(isStateBlockRegisteredDerived(*st_it2));
state_block_pairs.emplace_back(*st_it1, *st_it2);
- double_pairs.emplace_back(getAssociatedMemBlockPtr((*st_it1)),
- getAssociatedMemBlockPtr((*st_it2)));
+ double_pairs.emplace_back(getAssociatedMemBlockPtr((*st_it1)), getAssociatedMemBlockPtr((*st_it2)));
}
}
- //std::cout << "pairs... " << double_pairs.size() << std::endl;
+ // std::cout << "pairs... " << double_pairs.size() << std::endl;
// COMPUTE DESIRED COVARIANCES
if (covariance_->Compute(double_pairs, ceres_problem_.get()))
@@ -470,7 +515,8 @@ bool SolverCeres::computeCovariancesDerived(const std::vector<StateBlockPtr>& st
// STORE DESIRED COVARIANCES
for (unsigned int i = 0; i < double_pairs.size(); i++)
{
- Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> cov(state_block_pairs[i].first->getLocalSize(),state_block_pairs[i].second->getLocalSize());
+ Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> cov(
+ state_block_pairs[i].first->getLocalSize(), state_block_pairs[i].second->getLocalSize());
covariance_->GetCovarianceBlockInTangentSpace(double_pairs[i].first, double_pairs[i].second, cov.data());
wolf_problem_->addCovarianceBlock(state_block_pairs[i].first, state_block_pairs[i].second, cov);
// std::cout << "covariance got from st_list: " << std::endl << cov << std::endl;
@@ -486,7 +532,7 @@ void SolverCeres::addFactorDerived(const FactorBasePtr& fac_ptr)
{
assert(!isFactorRegisteredDerived(fac_ptr) && "adding a factor that is already in the fac_2_costfunction_ map");
- auto cost_func_ptr = createCostFunction(fac_ptr);
+ auto cost_func_ptr = createCostFunction(fac_ptr);
fac_2_costfunction_[fac_ptr] = cost_func_ptr;
std::vector<double*> res_block_mem;
@@ -494,32 +540,37 @@ void SolverCeres::addFactorDerived(const FactorBasePtr& fac_ptr)
for (const StateBlockPtr& st : fac_ptr->getStateBlockPtrVector())
{
assert(isStateBlockRegisteredDerived(st) && "adding a factor that involves a floating or not registered sb");
- res_block_mem.emplace_back( getAssociatedMemBlockPtr(st) );
+ res_block_mem.emplace_back(getAssociatedMemBlockPtr(st));
}
auto loss_func_ptr = (fac_ptr->getApplyLossFunction()) ? new ceres::CauchyLoss(0.5) : nullptr;
- //std::cout << "Added residual block corresponding to constraint " << std::static_pointer_cast<CostFunctionWrapper>(cost_func_ptr)->getConstraintPtr()->getType() << std::static_pointer_cast<CostFunctionWrapper>(cost_func_ptr)->getConstraintPtr()->id() <<std::endl;
+ // std::cout << "Added residual block corresponding to constraint " <<
+ // std::static_pointer_cast<CostFunctionWrapper>(cost_func_ptr)->getConstraintPtr()->getType() <<
+ // std::static_pointer_cast<CostFunctionWrapper>(cost_func_ptr)->getConstraintPtr()->id() <<std::endl;
- assert((unsigned int)(ceres_problem_->NumResidualBlocks()) == fac_2_residual_idx_.size() && "ceres residuals different from wrapper residuals");
- assert(fac_2_residual_idx_.find(fac_ptr) == fac_2_residual_idx_.end() && "adding a factor that is already in the fac_2_residual_idx_ map");
+ assert((unsigned int)(ceres_problem_->NumResidualBlocks()) == fac_2_residual_idx_.size() &&
+ "ceres residuals different from wrapper residuals");
+ assert(fac_2_residual_idx_.find(fac_ptr) == fac_2_residual_idx_.end() &&
+ "adding a factor that is already in the fac_2_residual_idx_ map");
- fac_2_residual_idx_[fac_ptr] = ceres_problem_->AddResidualBlock(cost_func_ptr.get(),
- loss_func_ptr,
- res_block_mem);
+ fac_2_residual_idx_[fac_ptr] = ceres_problem_->AddResidualBlock(cost_func_ptr.get(), loss_func_ptr, res_block_mem);
- assert((unsigned int)(ceres_problem_->NumResidualBlocks()) == fac_2_residual_idx_.size() && "ceres residuals different from wrapper residuals");
+ assert((unsigned int)(ceres_problem_->NumResidualBlocks()) == fac_2_residual_idx_.size() &&
+ "ceres residuals different from wrapper residuals");
}
void SolverCeres::removeFactorDerived(const FactorBasePtr& _fac_ptr)
{
- assert(fac_2_residual_idx_.find(_fac_ptr) != fac_2_residual_idx_.end() && "removing a factor that is not in the fac_2_residual map");
+ assert(fac_2_residual_idx_.find(_fac_ptr) != fac_2_residual_idx_.end() &&
+ "removing a factor that is not in the fac_2_residual map");
ceres_problem_->RemoveResidualBlock(fac_2_residual_idx_[_fac_ptr]);
fac_2_residual_idx_.erase(_fac_ptr);
fac_2_costfunction_.erase(_fac_ptr);
- assert((unsigned int)(ceres_problem_->NumResidualBlocks()) == fac_2_residual_idx_.size() && "ceres residuals different from wrapper residuals");
+ assert((unsigned int)(ceres_problem_->NumResidualBlocks()) == fac_2_residual_idx_.size() &&
+ "ceres residuals different from wrapper residuals");
}
void SolverCeres::addStateBlockDerived(const StateBlockPtr& state_ptr)
@@ -532,25 +583,24 @@ void SolverCeres::addStateBlockDerived(const StateBlockPtr& state_ptr)
if (state_ptr->hasLocalParametrization())
{
- auto p = state_blocks_local_param_.emplace(state_ptr,
- std::make_shared<LocalParametrizationWrapper>(state_ptr->getLocalParametrization()));
+ auto p = state_blocks_local_param_.emplace(
+ state_ptr, std::make_shared<LocalParametrizationWrapper>(state_ptr->getLocalParametrization()));
local_parametrization_ptr = p.first->second.get();
}
- ceres_problem_->AddParameterBlock(getAssociatedMemBlockPtr(state_ptr),
- state_ptr->getSize(),
- local_parametrization_ptr);
+ ceres_problem_->AddParameterBlock(
+ getAssociatedMemBlockPtr(state_ptr), state_ptr->getSize(), local_parametrization_ptr);
- if (state_ptr->isFixed())
- ceres_problem_->SetParameterBlockConstant(getAssociatedMemBlockPtr(state_ptr));
+ if (state_ptr->isFixed()) ceres_problem_->SetParameterBlockConstant(getAssociatedMemBlockPtr(state_ptr));
updateStateBlockStatusDerived(state_ptr);
}
void SolverCeres::removeStateBlockDerived(const StateBlockPtr& state_ptr)
{
- //std::cout << "SolverCeres::removeStateBlock " << state_ptr.get() << " - " << getAssociatedMemBlockPtr(state_ptr) << std::endl;
+ // std::cout << "SolverCeres::removeStateBlock " << state_ptr.get() << " - " << getAssociatedMemBlockPtr(state_ptr)
+ // << std::endl;
assert(state_ptr);
assert(isStateBlockRegisteredDerived(state_ptr) && "removing a state block that is not in ceres");
@@ -591,8 +641,7 @@ void SolverCeres::updateStateBlockLocalParametrizationDerived(const StateBlockPt
}
// Remove all involved factors (it does not remove any parameter block)
- for (auto fac : involved_factors)
- removeFactorDerived(fac);
+ for (auto fac : involved_factors) removeFactorDerived(fac);
// Remove state block (it removes all involved residual blocks but they just were removed)
removeStateBlockDerived(state_ptr);
@@ -601,8 +650,7 @@ void SolverCeres::updateStateBlockLocalParametrizationDerived(const StateBlockPt
addStateBlockDerived(state_ptr);
// Add all involved factors
- for (auto fac : involved_factors)
- addFactorDerived(fac);
+ for (auto fac : involved_factors) addFactorDerived(fac);
}
bool SolverCeres::hasConverged()
@@ -617,8 +665,7 @@ bool SolverCeres::wasStopped()
unsigned int SolverCeres::iterations()
{
- if (summary_.num_successful_steps + summary_.num_unsuccessful_steps < 1)
- return 0;
+ if (summary_.num_successful_steps + summary_.num_unsuccessful_steps < 1) return 0;
return summary_.num_successful_steps + summary_.num_unsuccessful_steps;
}
@@ -650,7 +697,7 @@ ceres::CostFunctionPtr SolverCeres::createCostFunction(const FactorBasePtr& _fac
return createNumericDiffCostFunction(_fac_ptr);
else
- throw std::invalid_argument( "Wrong Jacobian Method!" );
+ throw std::invalid_argument("Wrong Jacobian Method!");
}
bool SolverCeres::checkDerived(std::string prefix) const
@@ -662,17 +709,31 @@ bool SolverCeres::checkDerived(std::string prefix) const
ceres_problem_->NumResidualBlocks() != fac_2_costfunction_.size() or
ceres_problem_->NumResidualBlocks() != fac_2_residual_idx_.size())
{
- WOLF_ERROR("SolverCeres::check: number of residuals mismatch - in ", prefix, ":\n\tceres_problem_->NumResidualBlocks(): ", ceres_problem_->NumResidualBlocks(), "\n\tfactors_.size(): ", factors_.size(), "\n\tfac_2_costfunction_.size(): ", fac_2_costfunction_.size(), "\n\tfac_2_residual_idx_.size(): ", fac_2_residual_idx_.size());
+ WOLF_ERROR("SolverCeres::check: number of residuals mismatch - in ",
+ prefix,
+ ":\n\tceres_problem_->NumResidualBlocks(): ",
+ ceres_problem_->NumResidualBlocks(),
+ "\n\tfactors_.size(): ",
+ factors_.size(),
+ "\n\tfac_2_costfunction_.size(): ",
+ fac_2_costfunction_.size(),
+ "\n\tfac_2_residual_idx_.size(): ",
+ fac_2_residual_idx_.size());
ok = false;
}
if (ceres_problem_->NumParameterBlocks() != state_blocks_.size())
{
- WOLF_ERROR("SolverCeres::check: number of parameters mismatch ((ceres_problem_->NumParameterBlocks(): ", ceres_problem_->NumParameterBlocks(), " != state_blocks_.size(): ", state_blocks_.size(), ") - in ", prefix);
+ WOLF_ERROR("SolverCeres::check: number of parameters mismatch ((ceres_problem_->NumParameterBlocks(): ",
+ ceres_problem_->NumParameterBlocks(),
+ " != state_blocks_.size(): ",
+ state_blocks_.size(),
+ ") - in ",
+ prefix);
ok = false;
}
// Check parameter blocks
- //for (auto&& state_block_pair : state_blocks_)
+ // for (auto&& state_block_pair : state_blocks_)
for (const auto& state_block_pair : state_blocks_)
{
if (!ceres_problem_->HasParameterBlock(state_block_pair.second.data()))
@@ -688,25 +749,33 @@ bool SolverCeres::checkDerived(std::string prefix) const
// SolverManager::factors_
if (factors_.count(fac_res_pair.first) == 0)
{
- WOLF_ERROR("SolverCeres::check: factor ", fac_res_pair.first->id(), " (in fac_2_residual_idx_) not in factors_ - in ", prefix);
+ WOLF_ERROR("SolverCeres::check: factor ",
+ fac_res_pair.first->id(),
+ " (in fac_2_residual_idx_) not in factors_ - in ",
+ prefix);
ok = false;
}
// costfunction - residual
if (fac_2_costfunction_.count(fac_res_pair.first) == 0)
{
- WOLF_ERROR("SolverCeres::check: any factor in fac_2_residual_idx_ is not in fac_2_costfunction_ - in ", prefix);
+ WOLF_ERROR("SolverCeres::check: any factor in fac_2_residual_idx_ is not in fac_2_costfunction_ - in ",
+ prefix);
ok = false;
}
- //if (fac_2_costfunction_[fac_res_pair.first].get() != ceres_problem_->GetCostFunctionForResidualBlock(fac_res_pair.second))
- if (fac_2_costfunction_.at(fac_res_pair.first).get() != ceres_problem_->GetCostFunctionForResidualBlock(fac_res_pair.second))
+ // if (fac_2_costfunction_[fac_res_pair.first].get() !=
+ // ceres_problem_->GetCostFunctionForResidualBlock(fac_res_pair.second))
+ if (fac_2_costfunction_.at(fac_res_pair.first).get() !=
+ ceres_problem_->GetCostFunctionForResidualBlock(fac_res_pair.second))
{
WOLF_ERROR("SolverCeres::check: fac_2_costfunction_ and ceres mismatch - in ", prefix);
ok = false;
}
// factor - residual
- if (fac_res_pair.first != static_cast<const CostFunctionWrapper*>(ceres_problem_->GetCostFunctionForResidualBlock(fac_res_pair.second))->getFactor())
+ if (fac_res_pair.first != static_cast<const CostFunctionWrapper*>(
+ ceres_problem_->GetCostFunctionForResidualBlock(fac_res_pair.second))
+ ->getFactor())
{
WOLF_ERROR("SolverCeres::check: fac_2_residual_idx_ and ceres mismatch - in ", prefix);
ok = false;
@@ -717,7 +786,12 @@ bool SolverCeres::checkDerived(std::string prefix) const
ceres_problem_->GetParameterBlocksForResidualBlock(fac_res_pair.second, ¶m_blocks);
if (param_blocks.size() != fac_res_pair.first->getStateBlockPtrVector().size())
{
- WOLF_ERROR("SolverCeres::check: number parameter blocks in ceres residual", param_blocks.size(), " different from number of state blocks in factor ", fac_res_pair.first->getStateBlockPtrVector().size(), " - in ", prefix);
+ WOLF_ERROR("SolverCeres::check: number parameter blocks in ceres residual",
+ param_blocks.size(),
+ " different from number of state blocks in factor ",
+ fac_res_pair.first->getStateBlockPtrVector().size(),
+ " - in ",
+ prefix);
ok = false;
}
else
@@ -737,11 +811,14 @@ bool SolverCeres::checkDerived(std::string prefix) const
return ok;
}
-void insertSparseBlock(const Eigen::MatrixXd& ins, Eigen::SparseMatrixd& original, const unsigned int& row, const unsigned int& col)
+void insertSparseBlock(const Eigen::MatrixXd& ins,
+ Eigen::SparseMatrixd& original,
+ const unsigned int& row,
+ const unsigned int& col)
{
for (auto ins_row = 0; ins_row < ins.rows(); ins_row++)
for (auto ins_col = 0; ins_col < ins.cols(); ins_col++)
- original.insert(row+ins_row, col+ins_col) = ins(ins_row,ins_col);
+ original.insert(row + ins_row, col + ins_col) = ins(ins_row, ins_col);
original.makeCompressed();
}
@@ -753,17 +830,19 @@ const Eigen::SparseMatrixd SolverCeres::computeHessian() const
// fac_2_residual_idx_
// fac_2_costfunction_
// state_blocks_local_param_
- int ix_row = 0; // index of the factor/measurement in the
- for (auto fac_res_pair: fac_2_residual_idx_){
+ int ix_row = 0; // index of the factor/measurement in the
+ for (auto fac_res_pair : fac_2_residual_idx_)
+ {
FactorBasePtr fac_ptr = fac_res_pair.first;
ix_row += fac_ptr->getSize();
// retrieve all stateblocks data related to this factor
std::vector<const double*> fac_states_ptr;
- for (auto sb : fac_res_pair.first->getStateBlockPtrVector()){
+ for (auto sb : fac_res_pair.first->getStateBlockPtrVector())
+ {
fac_states_ptr.push_back(sb->getStateData());
}
-
+
// retrieve residual value, not used afterwards in this case since we just care about jacobians
Eigen::VectorXd residual(fac_ptr->getSize());
// retrieve jacobian in group size, not local size
@@ -781,56 +860,58 @@ const Eigen::SparseMatrixd SolverCeres::computeHessian() const
assert((A.cols() >= sb->getLocalSize() + jacobians[i].cols() - 1) && "bad A number of cols");
// insert since A_block_row has just been created so it's empty for sure
- if (sb->hasLocalParametrization()){
- // if the state block has a local parameterization, we need to right multiply by the manifold element / tangent element jacobian
- Eigen::MatrixXd J_manif_tang(sb->getSize(), sb->getLocalSize());
- Eigen::Map<Eigen::MatrixRowXd> J_manif_tang_map(J_manif_tang.data(), sb->getSize(), sb->getLocalSize());
+ if (sb->hasLocalParametrization())
+ {
+ // if the state block has a local parameterization, we need to right multiply by the manifold
+ // element / tangent element jacobian
+ Eigen::MatrixXd J_manif_tang(sb->getSize(), sb->getLocalSize());
+ Eigen::Map<Eigen::MatrixRowXd> J_manif_tang_map(
+ J_manif_tang.data(), sb->getSize(), sb->getLocalSize());
Eigen::Map<const Eigen::VectorXd> sb_state_map(sb->getStateData(), sb->getSize());
sb->getLocalParametrization()->computeJacobian(sb_state_map, J_manif_tang_map);
- insertSparseBlock(jacobians[i] * J_manif_tang, A_block_row, 0, sb->getLocalSize()); // (to_insert, matrix_to_fill, row, col)
+ insertSparseBlock(jacobians[i] * J_manif_tang,
+ A_block_row,
+ 0,
+ sb->getLocalSize()); // (to_insert, matrix_to_fill, row, col)
}
- else {
+ else
+ {
insertSparseBlock(jacobians[i], A_block_row, 0, sb->getLocalSize());
}
}
}
-
+
// fill the weighted jacobian matrix block row
A.block(ix_row, 0, fac_ptr->getSize(), A.cols());
-
}
- // compute the hessian matrix from the weighted jacobian
+ // compute the hessian matrix from the weighted jacobian
H = A.transpose() * A;
return H;
}
-bool SolverCeres::hasThisLocalParametrizationDerived(const StateBlockPtr& st,
+bool SolverCeres::hasThisLocalParametrizationDerived(const StateBlockPtr& st,
const LocalParametrizationBasePtr& local_param)
{
- return state_blocks_local_param_.count(st) == 1
- && state_blocks_local_param_.at(st)->getLocalParametrization() == local_param
- && ceres_problem_->GetParameterization(getAssociatedMemBlockPtr(st))
- == state_blocks_local_param_.at(st).get();
+ return state_blocks_local_param_.count(st) == 1 &&
+ state_blocks_local_param_.at(st)->getLocalParametrization() == local_param &&
+ ceres_problem_->GetParameterization(getAssociatedMemBlockPtr(st)) == state_blocks_local_param_.at(st).get();
}
void SolverCeres::printProfilingDerived(std::ostream& _stream) const
{
_stream << "Iterations:"
- << "\n\tUser-defined limit: " << params_ceres_->solver_options.max_num_iterations
+ << "\n\tUser-defined limit: " << solver_options_.max_num_iterations
<< "\n\tAverage iterations: " << n_iter_acc_ / n_solve_
- << "\n\tMax. iterations: " << n_iter_max_
- << "\nTermination:"
+ << "\n\tMax. iterations: " << n_iter_max_ << "\nTermination:"
<< "\n\tConvergence: " << 1e2 * n_convergence_ / n_solve_ << " %"
<< "\n\tInterrupted by problem change: " << 1e2 * n_interrupted_ / n_solve_ << " %"
- << "\n\tMax. iterations reached: " << 1e2 * n_no_convergence_ / n_solve_ << " %"
- << std::endl;
+ << "\n\tMax. iterations reached: " << 1e2 * n_no_convergence_ / n_solve_ << " %" << std::endl;
}
// register in the factory
WOLF_REGISTER_SOLVER(SolverCeres)
-} // namespace wolf
-
+} // namespace wolf
diff --git a/src/common/node_base.cpp b/src/common/node_base.cpp
index 5d9393c6125caf0444aee7498b74e0247496ddb3..b680bc94b2addeb98127691965319e82620e72c5 100644
--- a/src/common/node_base.cpp
+++ b/src/common/node_base.cpp
@@ -20,9 +20,9 @@
#include "core/common/node_base.h"
-namespace wolf {
-
-//init static node counter
+namespace wolf
+{
+// init static node counter
unsigned int NodeBase::node_id_count_ = 0;
-} // namespace wolf
+} // namespace wolf
diff --git a/src/common/node_state_blocks.cpp b/src/common/node_state_blocks.cpp
index 93b98d1c6a877ebee298c165f366d60274eb2dd1..9ee3c37cf95b8d7e0dc58944b8141fa2de4a446d 100644
--- a/src/common/node_state_blocks.cpp
+++ b/src/common/node_state_blocks.cpp
@@ -143,18 +143,19 @@ StateBlockPtr NodeStateBlocks::addStateBlock(const char& _sb_key, const StateBlo
return _sb;
}
-void NodeStateBlocks::emplaceFactorPrior(const char& _key,
- const Eigen::VectorXd& _x,
- const Eigen::MatrixXd& _cov,
- unsigned int _start_idx)
+void NodeStateBlocks::emplaceFactorStateBlock(const char& _key,
+ const Eigen::VectorXd& _x,
+ const Eigen::MatrixXd& _cov,
+ unsigned int _start_idx)
{
- assert(isCovariance(_cov) and "NodeStateBlocks::emplaceFactorPrior: provided _cov is not a covariance matrix");
+ assert(isCovariance(_cov) and
+ "NodeStateBlocks::emplaceFactorStateBlock: provided _cov is not a covariance matrix");
StateBlockPtr _sb = getStateBlock(_key);
// CHECKS
// missing key
- if (not _sb) throw std::runtime_error("NodeStateBlocks::emplaceFactorPrior: missing key " + _key);
+ if (not _sb) throw std::runtime_error("NodeStateBlocks::emplaceFactorStateBlock: missing key " + _key);
// Overparametrized states
if (_sb->getSize() != _sb->getLocalSize())
@@ -162,27 +163,29 @@ void NodeStateBlocks::emplaceFactorPrior(const char& _key,
// not segment of state not allowed
if (_start_idx != 0)
throw std::runtime_error(
- "NodeStateBlocks::emplaceFactorPrior: Prior for a segment of a state with local size different from "
+ "NodeStateBlocks::emplaceFactorStateBlock: Prior for a segment of a state with local size different "
+ "from "
"size "
"not allowed");
// state size
if (_x.size() != _sb->getSize())
- throw std::runtime_error("NodeStateBlocks::emplaceFactorPrior: Bad state size");
+ throw std::runtime_error("NodeStateBlocks::emplaceFactorStateBlock: Bad state size");
// cov size
if (_cov.cols() != _sb->getLocalSize())
- throw std::runtime_error("NodeStateBlocks::emplaceFactorPrior: Bad covariance size");
+ throw std::runtime_error("NodeStateBlocks::emplaceFactorStateBlock: Bad covariance size");
}
else
{
// cov and state size
if (_cov.cols() != _x.size())
throw std::runtime_error(
- "NodeStateBlocks::emplaceFactorPrior: Inconsistend measurement and covariance sizes");
+ "NodeStateBlocks::emplaceFactorStateBlock: Inconsistend measurement and covariance sizes");
// state size
if (_x.size() + _start_idx > _sb->getSize())
throw std::runtime_error(
- "NodeStateBlocks::emplaceFactorPrior: Prior for a segment of a state with local size different from "
+ "NodeStateBlocks::emplaceFactorStateBlock: Prior for a segment of a state with local size different "
+ "from "
"size "
"not allowed");
}
@@ -190,8 +193,8 @@ void NodeStateBlocks::emplaceFactorPrior(const char& _key,
// existing prior
if (factor_prior_map_.count(_key))
throw std::runtime_error(
- std::string("NodeStateBlocks::emplaceFactorPrior: There already is a factor prior in this key ") + _key +
- ", factor " + toString(factor_prior_map_.at(_key)->id()));
+ std::string("NodeStateBlocks::emplaceFactorStateBlock: There already is a factor prior in this key ") +
+ _key + ", factor " + toString(factor_prior_map_.at(_key)->id()));
// SET STATE
if (_x.size() == _sb->getSize())
@@ -224,7 +227,7 @@ void NodeStateBlocks::emplaceFactorPrior(const char& _key,
}
}
-void NodeStateBlocks::applyPriors(const SpecStateComposite& _specs)
+void NodeStateBlocks::emplacePriors(const SpecStateComposite& _specs)
{
for (auto spec_pair : _specs)
{
@@ -262,9 +265,10 @@ void NodeStateBlocks::applyPriors(const SpecStateComposite& _specs)
if (prior.isFixed()) state_block_composite_.at(key)->fix();
// emplace factor prior (if specified)
- if (prior.isFactor()) emplaceFactorPrior(key, prior.getState(), prior.getNoiseStd().cwiseAbs2().asDiagonal());
+ if (prior.isFactor())
+ emplaceFactorStateBlock(key, prior.getState(), prior.getNoiseStd().cwiseAbs2().asDiagonal());
- // set state (already set inside emplaceFactorPrior)
+ // set state (already set inside emplaceFactorStateBlock)
else
state_block_composite_.at(key)->setState(prior.getState());
}
@@ -674,8 +678,8 @@ CheckLog NodeStateBlocks::checkStatesAndFactoredBy(bool _verbose, std::ostream&
// check factor_prior_map_ does not have any factor not in factored_by
for (auto fac_pri : getPriorFactorMap())
{
- inconsistency_explanation << "Fac" << fac_pri.second->id() << " prior factor in " << getCategory() << id() << " @ "
- << shared_from_this().get() << " not found in factored_by_set_\n";
+ inconsistency_explanation << "Fac" << fac_pri.second->id() << " prior factor in " << getCategory() << id()
+ << " @ " << shared_from_this().get() << " not found in factored_by_set_\n";
log.assertTrue(getFactoredBySet().count(fac_pri.second), inconsistency_explanation);
}
return log;
diff --git a/src/common/time_stamp.cpp b/src/common/time_stamp.cpp
index 627e39310398e654454b7195ca65af24ea0f96ee..7756ac8a2768fe13fda51b5aab53caaaef2d7bb1 100644
--- a/src/common/time_stamp.cpp
+++ b/src/common/time_stamp.cpp
@@ -20,9 +20,9 @@
#include "core/common/time_stamp.h"
-namespace wolf {
-
-TimeStamp TimeStamp::Now ( )
+namespace wolf
+{
+TimeStamp TimeStamp::Now()
{
TimeStamp t(0);
t.setToNow();
@@ -31,46 +31,41 @@ TimeStamp TimeStamp::Now ( )
std::ostream& operator<<(std::ostream& os, const TimeStamp& _ts)
{
- if (!_ts.ok())
- os << "TimeStamp is invalid! ";
- os << _ts.getSeconds() << "." << std::setfill('0') << std::setw(9) << std::right <<_ts.getNanoSeconds(); // write obj to stream
+ if (!_ts.ok()) os << "TimeStamp is invalid! ";
+ os << _ts.getSeconds() << "." << std::setfill('0') << std::setw(9) << std::right
+ << _ts.getNanoSeconds(); // write obj to stream
os << std::setfill(' ');
return os;
}
-TimeStamp::TimeStamp() :
- //time_stamp_(0)
- time_stamp_nano_(0)
- ,
- is_valid_(false)
+TimeStamp::TimeStamp()
+ : // time_stamp_(0)
+ time_stamp_nano_(0),
+ is_valid_(false)
{
-// setToNow();
+ // setToNow();
}
-TimeStamp::TimeStamp(const TimeStamp& _ts) :
- time_stamp_nano_(_ts.time_stamp_nano_),
- is_valid_(_ts.is_valid_)
+TimeStamp::TimeStamp(const TimeStamp& _ts) : time_stamp_nano_(_ts.time_stamp_nano_), is_valid_(_ts.is_valid_)
{
//
}
-TimeStamp::TimeStamp(const double& _ts) :
- time_stamp_nano_(_ts > 0 ? (unsigned long int)(_ts*1e9) : 0),
- is_valid_(_ts >= 0)
+TimeStamp::TimeStamp(const double& _ts)
+ : time_stamp_nano_(_ts > 0 ? (unsigned long int)(_ts * 1e9) : 0), is_valid_(_ts >= 0)
{
//
}
-TimeStamp::TimeStamp(const unsigned long int& _sec, const unsigned long int& _nsec) :
- time_stamp_nano_(_sec*NANOSECS+_nsec),
- is_valid_(true)
+TimeStamp::TimeStamp(const unsigned long int& _sec, const unsigned long int& _nsec)
+ : time_stamp_nano_(_sec * NANOSECS + _nsec), is_valid_(true)
{
//
}
TimeStamp::~TimeStamp()
{
- //nothing to do
+ // nothing to do
}
void TimeStamp::setToNow()
@@ -81,30 +76,30 @@ void TimeStamp::setToNow()
setOk();
}
-TimeStamp TimeStamp::operator +(const double& dt) const
+TimeStamp TimeStamp::operator+(const double& dt) const
{
TimeStamp ts(*this);
ts += dt;
return ts;
}
-TimeStamp TimeStamp::operator -(const double& dt) const
+TimeStamp TimeStamp::operator-(const double& dt) const
{
TimeStamp ts(*this);
ts -= dt;
return ts;
}
-void TimeStamp::operator -=(const double& dt)
+void TimeStamp::operator-=(const double& dt)
{
- unsigned long int dt_nano = (unsigned long int)(dt*NANOSECS);
- is_valid_ = (time_stamp_nano_ >= dt_nano);
- time_stamp_nano_ = (dt_nano > time_stamp_nano_ ? 0 : time_stamp_nano_ - dt_nano);
+ unsigned long int dt_nano = (unsigned long int)(dt * NANOSECS);
+ is_valid_ = (time_stamp_nano_ >= dt_nano);
+ time_stamp_nano_ = (dt_nano > time_stamp_nano_ ? 0 : time_stamp_nano_ - dt_nano);
}
-void TimeStamp::print(std::ostream & ost) const
+void TimeStamp::print(std::ostream& ost) const
{
ost << *this;
}
-} // namespace wolf
+} // namespace wolf
diff --git a/src/composite/matrix_composite.cpp b/src/composite/matrix_composite.cpp
index fcd838c5199003a398e262658cb83f113071da28..adced770869c033e24c0e7681288ca8711d6cf05 100644
--- a/src/composite/matrix_composite.cpp
+++ b/src/composite/matrix_composite.cpp
@@ -20,9 +20,9 @@
#include "core/composite/matrix_composite.h"
-namespace wolf{
-
-bool MatrixComposite::emplace(const char &_row, const char &_col, const Eigen::MatrixXd &_mat_blk)
+namespace wolf
+{
+bool MatrixComposite::emplace(const char& _row, const char& _col, const Eigen::MatrixXd& _mat_blk)
{
// check rows
if (size_rows_.count(_row) == 0)
@@ -41,22 +41,20 @@ bool MatrixComposite::emplace(const char &_row, const char &_col, const Eigen::M
return true;
}
-bool MatrixComposite::at(const char &_row, const char &_col, Eigen::MatrixXd &_mat_blk) const
+bool MatrixComposite::at(const char& _row, const char& _col, Eigen::MatrixXd& _mat_blk) const
{
auto row_it = this->find(_row);
- if(row_it != this->end())
- return false;
+ if (row_it != this->end()) return false;
auto col_it = row_it->second.find(_col);
- if(col_it != row_it->second.end())
- return false;
+ if (col_it != row_it->second.end()) return false;
_mat_blk = col_it->second;
return true;
}
-Eigen::MatrixXd& MatrixComposite::at(const char &_row, const char &_col)
+Eigen::MatrixXd& MatrixComposite::at(const char& _row, const char& _col)
{
auto row_it = this->find(_row);
assert(row_it != this->end() && "Requested matrix block does not exist in matrix composite.");
@@ -67,7 +65,7 @@ Eigen::MatrixXd& MatrixComposite::at(const char &_row, const char &_col)
return col_it->second;
}
-const Eigen::MatrixXd& MatrixComposite::at(const char &_row, const char &_col) const
+const Eigen::MatrixXd& MatrixComposite::at(const char& _row, const char& _col) const
{
auto row_it = this->find(_row);
assert(row_it != this->end() && "Requested matrix block does not exist in matrix composite.");
@@ -78,24 +76,24 @@ const Eigen::MatrixXd& MatrixComposite::at(const char &_row, const char &_col) c
return col_it->second;
}
-wolf::MatrixComposite MatrixComposite::operator *(const MatrixComposite &_N) const
+wolf::MatrixComposite MatrixComposite::operator*(const MatrixComposite& _N) const
{
MatrixComposite MN;
- for (const auto &pair_i_Mi : (*this))
+ for (const auto& pair_i_Mi : (*this))
{
- const auto &i = pair_i_Mi.first;
- const auto &Mi = pair_i_Mi.second;
+ const auto& i = pair_i_Mi.first;
+ const auto& Mi = pair_i_Mi.second;
- for (const auto &pair_k_Nk : _N)
+ for (const auto& pair_k_Nk : _N)
{
- const auto &k = pair_k_Nk.first;
- const auto &Nk = pair_k_Nk.second;
+ const auto& k = pair_k_Nk.first;
+ const auto& Nk = pair_k_Nk.second;
- for (const auto &pair_j_Nkj : Nk)
+ for (const auto& pair_j_Nkj : Nk)
{
- const auto &j = pair_j_Nkj.first;
- const auto &Nkj = pair_j_Nkj.second;
- const auto &Mik = Mi.at(k);
+ const auto& j = pair_j_Nkj.first;
+ const auto& Nkj = pair_j_Nkj.second;
+ const auto& Mik = Mi.at(k);
if (MN.count(i, j) == 0)
MN.emplace(i, j, Mik * Nkj);
@@ -107,19 +105,19 @@ wolf::MatrixComposite MatrixComposite::operator *(const MatrixComposite &_N) con
return MN;
}
-wolf::MatrixComposite MatrixComposite::operator +(const MatrixComposite &_N) const
+wolf::MatrixComposite MatrixComposite::operator+(const MatrixComposite& _N) const
{
MatrixComposite MpN;
- for (const auto &pair_i_Mi : *this)
+ for (const auto& pair_i_Mi : *this)
{
- const auto &i = pair_i_Mi.first;
- const auto &Mi = pair_i_Mi.second;
+ const auto& i = pair_i_Mi.first;
+ const auto& Mi = pair_i_Mi.second;
for (const auto& pair_j_Mij : Mi)
{
- const auto& j = pair_j_Mij.first;
+ const auto& j = pair_j_Mij.first;
const auto& Mij = pair_j_Mij.second;
- const auto& Nij = _N.at(i,j);
+ const auto& Nij = _N.at(i, j);
MpN.emplace(i, j, Mij + Nij);
}
@@ -127,19 +125,19 @@ wolf::MatrixComposite MatrixComposite::operator +(const MatrixComposite &_N) con
return MpN;
}
-wolf::MatrixComposite MatrixComposite::operator -(const MatrixComposite &_N) const
+wolf::MatrixComposite MatrixComposite::operator-(const MatrixComposite& _N) const
{
MatrixComposite MpN;
- for (const auto &pair_i_Mi : *this)
+ for (const auto& pair_i_Mi : *this)
{
- const auto &i = pair_i_Mi.first;
- const auto &Mi = pair_i_Mi.second;
+ const auto& i = pair_i_Mi.first;
+ const auto& Mi = pair_i_Mi.second;
for (const auto& pair_j_Mij : Mi)
{
- const auto& j = pair_j_Mij.first;
+ const auto& j = pair_j_Mij.first;
const auto& Mij = pair_j_Mij.second;
- const auto& Nij = _N.at(i,j);
+ const auto& Nij = _N.at(i, j);
MpN.emplace(i, j, Mij - Nij);
}
@@ -147,34 +145,34 @@ wolf::MatrixComposite MatrixComposite::operator -(const MatrixComposite &_N) con
return MpN;
}
-MatrixComposite MatrixComposite::operator - (void) const
+MatrixComposite MatrixComposite::operator-(void) const
{
MatrixComposite m;
for (const auto& pair_rkey_row : *this)
{
- m.unordered_map<char,unordered_map<char, MatrixXd>>::emplace(pair_rkey_row.first, unordered_map<char, MatrixXd>());
+ m.unordered_map<char, unordered_map<char, MatrixXd>>::emplace(pair_rkey_row.first,
+ unordered_map<char, MatrixXd>());
for (const auto& pair_ckey_blk : pair_rkey_row.second)
{
- m[pair_rkey_row.first].emplace(pair_ckey_blk.first, - pair_ckey_blk.second);
+ m[pair_rkey_row.first].emplace(pair_ckey_blk.first, -pair_ckey_blk.second);
}
}
return m;
}
-
-wolf::VectorComposite MatrixComposite::operator *(const VectorComposite &_x) const
+wolf::VectorComposite MatrixComposite::operator*(const VectorComposite& _x) const
{
VectorComposite y;
- for (const auto &pair_rkey_row : *this)
+ for (const auto& pair_rkey_row : *this)
{
- const auto &rkey = pair_rkey_row.first;
- const auto &row = pair_rkey_row.second;
+ const auto& rkey = pair_rkey_row.first;
+ const auto& row = pair_rkey_row.second;
- for (const auto &pair_ckey_mat : row)
+ for (const auto& pair_ckey_mat : row)
{
- const auto &ckey = pair_ckey_mat.first;
- const auto &J_r_c = pair_ckey_mat.second;
+ const auto& ckey = pair_ckey_mat.first;
+ const auto& J_r_c = pair_ckey_mat.second;
const auto& it = y.find(rkey);
if (it != y.end())
@@ -188,14 +186,11 @@ wolf::VectorComposite MatrixComposite::operator *(const VectorComposite &_x) con
MatrixComposite MatrixComposite::propagate(const MatrixComposite& _Cov)
{
-
-
// simplify names of operands
const auto& J = *this;
const auto& Q = _Cov;
- MatrixComposite S; // S = J * Q * J.tr
-
+ MatrixComposite S; // S = J * Q * J.tr
/* Covariance propagation
*
@@ -239,23 +234,23 @@ MatrixComposite MatrixComposite::propagate(const MatrixComposite& _Cov)
*/
// Iterate on the output matrix S first, row i, col j.
- for (const auto& pair_i_Si : J) // (1)
+ for (const auto& pair_i_Si : J) // (1)
{
const auto& i = pair_i_Si.first;
const auto& J_i = pair_i_Si.second;
double i_size = J_i.begin()->second.rows();
- for (const auto& pair_j_Sj : J) // (2)
+ for (const auto& pair_j_Sj : J) // (2)
{
- const auto& j = pair_j_Sj.first;
+ const auto& j = pair_j_Sj.first;
const auto& J_j = pair_j_Sj.second;
double j_size = J_j.begin()->second.rows();
- MatrixXd S_ij(MatrixXd::Zero(i_size, j_size)); // (3)
+ MatrixXd S_ij(MatrixXd::Zero(i_size, j_size)); // (3)
- for (const auto& pair_n_Qn : Q) // (4)
+ for (const auto& pair_n_Qn : Q) // (4)
{
const auto& n = pair_n_Qn.first;
const auto& Q_n = pair_n_Qn.second;
@@ -264,33 +259,33 @@ MatrixComposite MatrixComposite::propagate(const MatrixComposite& _Cov)
const auto& J_in = J_i.at(n);
- MatrixXd QJt_nj(MatrixXd::Zero(n_size, j_size)); // (5)
+ MatrixXd QJt_nj(MatrixXd::Zero(n_size, j_size)); // (5)
- for (const auto& pair_k_Qnk : Q_n) // (6)
+ for (const auto& pair_k_Qnk : Q_n) // (6)
{
const auto& k = pair_k_Qnk.first;
const auto& Q_nk = pair_k_Qnk.second;
const auto& J_jk = J_j.at(k);
- QJt_nj += Q_nk * J_jk.transpose(); // (7)
+ QJt_nj += Q_nk * J_jk.transpose(); // (7)
}
- S_ij += J_in * QJt_nj; // (8)
+ S_ij += J_in * QJt_nj; // (8)
}
- S.emplace(i,j,S_ij);
+ S.emplace(i, j, S_ij);
if (j == i)
break; // avoid computing the symmetrical block!
else
- S.emplace(j, i, S_ij.transpose()); // (9)
+ S.emplace(j, i, S_ij.transpose()); // (9)
}
}
return S;
}
-MatrixComposite MatrixComposite::operator * (double scalar_right) const
+MatrixComposite MatrixComposite::operator*(double scalar_right) const
{
MatrixComposite S(*this);
for (const auto& pair_rkey_row : *this)
@@ -305,41 +300,39 @@ MatrixComposite MatrixComposite::operator * (double scalar_right) const
return S;
}
-MatrixComposite operator * (double scalar_left, const MatrixComposite& M)
+MatrixComposite operator*(double scalar_left, const MatrixComposite& M)
{
MatrixComposite S;
S = M * scalar_left;
return S;
}
-MatrixXd MatrixComposite::matrix(const StateKeys &_struct_rows, const StateKeys &_struct_cols) const
+MatrixXd MatrixComposite::matrix(const StateKeys& _struct_rows, const StateKeys& _struct_cols) const
{
-
- std::unordered_map < char, unsigned int> indices_rows;
- std::unordered_map < char, unsigned int> indices_cols;
- unsigned int rows, cols;
+ std::unordered_map<char, unsigned int> indices_rows;
+ std::unordered_map<char, unsigned int> indices_cols;
+ unsigned int rows, cols;
sizeAndIndices(_struct_rows, _struct_cols, indices_rows, indices_cols, rows, cols);
- MatrixXd M ( MatrixXd::Zero(rows, cols) );
+ MatrixXd M(MatrixXd::Zero(rows, cols));
for (const auto& pair_row_rband : (*this))
{
- const auto& row = pair_row_rband.first;
+ const auto& row = pair_row_rband.first;
const auto& rband = pair_row_rband.second;
for (const auto& pair_col_blk : rband)
{
const auto& col = pair_col_blk.first;
const auto& blk = pair_col_blk.second;
- const unsigned int & blk_rows = size_rows_.at(row);
- const unsigned int & blk_cols = size_cols_.at(col);
+ const unsigned int& blk_rows = size_rows_.at(row);
+ const unsigned int& blk_cols = size_cols_.at(col);
assert(blk.rows() == blk_rows && "MatrixComposite block size mismatch! Wrong number of rows.");
assert(blk.cols() == blk_cols && "MatrixComposite block size mismatch! Wrong number of cols.");
M.block(indices_rows.at(row), indices_cols.at(col), blk_rows, blk_cols) = blk;
-
}
}
@@ -349,54 +342,52 @@ MatrixXd MatrixComposite::matrix(const StateKeys &_struct_rows, const StateKeys
unsigned int MatrixComposite::rows() const
{
unsigned int rows = 0;
- for (const auto& pair_row_size : this->size_rows_)
- rows += pair_row_size.second;
+ for (const auto& pair_row_size : this->size_rows_) rows += pair_row_size.second;
return rows;
}
unsigned int MatrixComposite::cols() const
{
unsigned int cols = 0;
- for (const auto& pair_col_size : this->size_rows_)
- cols += pair_col_size.second;
+ for (const auto& pair_col_size : this->size_rows_) cols += pair_col_size.second;
return cols;
}
-void MatrixComposite::sizeAndIndices(const StateKeys &_struct_rows,
- const StateKeys &_struct_cols,
- std::unordered_map<char, unsigned int> &_indices_rows,
- std::unordered_map<char, unsigned int> &_indices_cols,
- unsigned int &_rows,
- unsigned int &_cols) const
+void MatrixComposite::sizeAndIndices(const StateKeys& _struct_rows,
+ const StateKeys& _struct_cols,
+ std::unordered_map<char, unsigned int>& _indices_rows,
+ std::unordered_map<char, unsigned int>& _indices_cols,
+ unsigned int& _rows,
+ unsigned int& _cols) const
{
_rows = 0;
_cols = 0;
for (const auto& row : _struct_rows)
{
- _indices_rows[row] = _rows;
- _rows += size_rows_.at(row);
+ _indices_rows[row] = _rows;
+ _rows += size_rows_.at(row);
}
for (const auto& col : _struct_cols)
{
- _indices_cols[col] = _cols;
- _cols += size_cols_.at(col);
+ _indices_cols[col] = _cols;
+ _cols += size_cols_.at(col);
}
}
-MatrixComposite::MatrixComposite (const StateKeys& _row_structure, const StateKeys& _col_structure)
+MatrixComposite::MatrixComposite(const StateKeys& _row_structure, const StateKeys& _col_structure)
{
for (const auto& rkey : _row_structure)
- for (const auto& ckey : _col_structure)
- this->emplace(rkey, ckey, MatrixXd(0,0));
+ for (const auto& ckey : _col_structure) this->emplace(rkey, ckey, MatrixXd(0, 0));
}
-MatrixComposite::MatrixComposite (const StateKeys& _row_structure,
- const std::list<int>& _row_sizes,
- const StateKeys& _col_structure,
- const std::list<int>& _col_sizes)
+MatrixComposite::MatrixComposite(const StateKeys& _row_structure,
+ const std::list<int>& _row_sizes,
+ const StateKeys& _col_structure,
+ const std::list<int>& _col_sizes)
{
- assert (_row_structure.size() == _row_sizes.size() && "Row structure and sizes have different number of elements!");
- assert (_col_structure.size() == _col_sizes.size() && "Column structure and sizes have different number of elements!");
+ assert(_row_structure.size() == _row_sizes.size() && "Row structure and sizes have different number of elements!");
+ assert(_col_structure.size() == _col_sizes.size() &&
+ "Column structure and sizes have different number of elements!");
auto rsize_it = _row_sizes.begin();
for (const auto& rkey : _row_structure)
@@ -404,30 +395,31 @@ MatrixComposite::MatrixComposite (const StateKeys& _row_structure,
auto csize_it = _col_sizes.begin();
for (const auto& ckey : _col_structure)
{
- this->emplace(rkey, ckey, MatrixXd(*rsize_it, *csize_it)); // caution: blocks non initialized.
+ this->emplace(rkey, ckey, MatrixXd(*rsize_it, *csize_it)); // caution: blocks non initialized.
- csize_it ++;
+ csize_it++;
}
rsize_it++;
}
}
-MatrixComposite::MatrixComposite (const MatrixXd& _m,
- const StateKeys& _row_structure,
- const std::list<int>& _row_sizes,
- const StateKeys& _col_structure,
- const std::list<int>& _col_sizes)
+MatrixComposite::MatrixComposite(const MatrixXd& _m,
+ const StateKeys& _row_structure,
+ const std::list<int>& _row_sizes,
+ const StateKeys& _col_structure,
+ const std::list<int>& _col_sizes)
{
- assert (_row_structure.size() == _row_sizes.size() && "Row structure and sizes have different number of elements!");
- assert (_col_structure.size() == _col_sizes.size() && "Column structure and sizes have different number of elements!");
+ assert(_row_structure.size() == _row_sizes.size() && "Row structure and sizes have different number of elements!");
+ assert(_col_structure.size() == _col_sizes.size() &&
+ "Column structure and sizes have different number of elements!");
- SizeEigen rindex = 0, cindex;
- auto rsize_it = _row_sizes.begin();
+ SizeEigen rindex = 0, cindex;
+ auto rsize_it = _row_sizes.begin();
for (const auto& rkey : _row_structure)
{
assert(_m.rows() >= rindex + *rsize_it && "Provided matrix has insufficient number of rows");
- cindex = 0;
+ cindex = 0;
auto csize_it = _col_sizes.begin();
for (const auto& ckey : _col_structure)
@@ -437,7 +429,7 @@ MatrixComposite::MatrixComposite (const MatrixXd& _m,
this->emplace(rkey, ckey, _m.block(rindex, cindex, *rsize_it, *csize_it));
cindex += *csize_it;
- csize_it ++;
+ csize_it++;
}
assert(_m.cols() == cindex && "Provided matrix has too many columns");
@@ -449,13 +441,16 @@ MatrixComposite::MatrixComposite (const MatrixXd& _m,
assert(_m.rows() == rindex && "Provided matrix has too many rows");
}
-MatrixComposite MatrixComposite::zero (const StateKeys& _row_structure, const std::list<int>& _row_sizes,
- const StateKeys& _col_structure, const std::list<int>& _col_sizes)
+MatrixComposite MatrixComposite::zero(const StateKeys& _row_structure,
+ const std::list<int>& _row_sizes,
+ const StateKeys& _col_structure,
+ const std::list<int>& _col_sizes)
{
MatrixComposite m;
- assert (_row_structure.size() == _row_sizes.size() && "Row structure and sizes have different number of elements!");
- assert (_col_structure.size() == _col_sizes.size() && "Column structure and sizes have different number of elements!");
+ assert(_row_structure.size() == _row_sizes.size() && "Row structure and sizes have different number of elements!");
+ assert(_col_structure.size() == _col_sizes.size() &&
+ "Column structure and sizes have different number of elements!");
auto rsize_it = _row_sizes.begin();
for (const auto& rkey : _row_structure)
@@ -465,34 +460,33 @@ MatrixComposite MatrixComposite::zero (const StateKeys& _row_structure, const st
{
m.emplace(rkey, ckey, MatrixXd::Zero(*rsize_it, *csize_it));
- csize_it ++;
+ csize_it++;
}
rsize_it++;
}
return m;
}
-MatrixComposite MatrixComposite::identity (const StateKeys& _structure, const std::list<int>& _sizes)
+MatrixComposite MatrixComposite::identity(const StateKeys& _structure, const std::list<int>& _sizes)
{
MatrixComposite m;
- assert (_structure.size() == _sizes.size() && "Structure and sizes have different number of elements!");
+ assert(_structure.size() == _sizes.size() && "Structure and sizes have different number of elements!");
auto rsize_it = _sizes.begin();
- auto rkey_it = _structure.begin();
+ auto rkey_it = _structure.begin();
while (rkey_it != _structure.end())
{
+ const auto& rkey = *rkey_it;
+ const auto rsize = *rsize_it;
- const auto& rkey = *rkey_it;
- const auto rsize = *rsize_it;
-
- m.emplace(rkey, rkey, MatrixXd::Identity(rsize,rsize)); // diagonal block
+ m.emplace(rkey, rkey, MatrixXd::Identity(rsize, rsize)); // diagonal block
auto csize_it = rsize_it;
- auto ckey_it = rkey_it;
+ auto ckey_it = rkey_it;
- csize_it ++;
+ csize_it++;
ckey_it++;
while (ckey_it != _structure.end())
@@ -500,20 +494,20 @@ MatrixComposite MatrixComposite::identity (const StateKeys& _structure, const st
const auto& ckey = *ckey_it;
const auto& csize = *csize_it;
- m.emplace(rkey, ckey, MatrixXd::Zero(rsize, csize)); // above-diagonal block
- m.emplace(ckey, rkey, MatrixXd::Zero(csize, rsize)); // symmetric block
+ m.emplace(rkey, ckey, MatrixXd::Zero(rsize, csize)); // above-diagonal block
+ m.emplace(ckey, rkey, MatrixXd::Zero(csize, rsize)); // symmetric block
- csize_it ++;
+ csize_it++;
ckey_it++;
}
- rsize_it ++;
+ rsize_it++;
rkey_it++;
}
return m;
}
-bool MatrixComposite::check ( ) const
+bool MatrixComposite::check() const
{
bool size_ok = true;
@@ -521,7 +515,7 @@ bool MatrixComposite::check ( ) const
for (const auto& pair_rkey_row : *this)
{
const auto& rkey = pair_rkey_row.first;
- const auto& row = pair_rkey_row.second;
+ const auto& row = pair_rkey_row.second;
for (const auto& pair_ckey_blk : row)
{
const auto& ckey = pair_ckey_blk.first;
@@ -529,7 +523,7 @@ bool MatrixComposite::check ( ) const
if (size_rows_.count(rkey) != 0)
{
- if( size_rows_.at(rkey) != blk.rows())
+ if (size_rows_.at(rkey) != blk.rows())
{
WOLF_ERROR("row size for row ", rkey, " has wrong size");
size_ok = false;
@@ -542,7 +536,7 @@ bool MatrixComposite::check ( ) const
}
if (size_cols_.count(ckey) != 0)
{
- if( size_cols_.at(ckey) != blk.cols())
+ if (size_cols_.at(ckey) != blk.cols())
{
WOLF_ERROR("col size for col ", rkey, " has wrong size");
size_ok = false;
@@ -558,15 +552,15 @@ bool MatrixComposite::check ( ) const
return size_ok;
}
-std::ostream& operator <<(std::ostream &_os, const MatrixComposite &_M)
+std::ostream& operator<<(std::ostream& _os, const MatrixComposite& _M)
{
- for (const auto &pair_rkey_row : _M)
+ for (const auto& pair_rkey_row : _M)
{
const auto rkey = pair_rkey_row.first;
- for (const auto &pair_ckey_mat : pair_rkey_row.second)
+ for (const auto& pair_ckey_mat : pair_rkey_row.second)
{
- const auto &ckey = pair_ckey_mat.first;
+ const auto& ckey = pair_ckey_mat.first;
_os << "\n[" << rkey << "," << ckey << "]: \n" << pair_ckey_mat.second;
}
@@ -574,5 +568,4 @@ std::ostream& operator <<(std::ostream &_os, const MatrixComposite &_M)
return _os;
}
-} // namespace wolf
-
+} // namespace wolf
diff --git a/src/composite/spec_state_composite.cpp b/src/composite/spec_state_composite.cpp
index d54698bcb22ec929c17e1a6c419c7f8cacdd51b6..e9d7ed20702fa42a317e6e75282e7257d1668318 100644
--- a/src/composite/spec_state_composite.cpp
+++ b/src/composite/spec_state_composite.cpp
@@ -20,7 +20,6 @@
#include "core/composite/spec_state_composite.h"
#include "core/state_block/factory_state_block.h"
-#include "core/common/params_base.h" // toString()
namespace wolf
{
diff --git a/src/composite/spec_state_sensor_composite.cpp b/src/composite/spec_state_sensor_composite.cpp
index 3f6176af3e33f17bbc555506c272434c957cd95e..4637bf7a6bda1d8058c11f21635701dba4401b89 100644
--- a/src/composite/spec_state_sensor_composite.cpp
+++ b/src/composite/spec_state_sensor_composite.cpp
@@ -20,26 +20,21 @@
#include "core/composite/spec_state_sensor_composite.h"
#include "core/state_block/factory_state_block.h"
-#include "core/common/params_base.h" // toString()
namespace wolf
{
-
SpecStateSensor::SpecStateSensor(const std::string& _type,
const Eigen::VectorXd& _state,
const std::string& _mode,
const Eigen::VectorXd& _noise_std,
bool _dynamic,
- const Eigen::VectorXd& _drift_std) :
- SpecState(_type, _state, _mode, _noise_std),
- dynamic_(_dynamic),
- drift_std_(_drift_std)
+ const Eigen::VectorXd& _drift_std)
+ : SpecState(_type, _state, _mode, _noise_std), dynamic_(_dynamic), drift_std_(_drift_std)
{
check();
}
-SpecStateSensor::SpecStateSensor(const YAML::Node& prior_node) :
- SpecState(prior_node)
+SpecStateSensor::SpecStateSensor(const YAML::Node& prior_node) : SpecState(prior_node)
{
dynamic_ = prior_node["dynamic"].as<bool>();
@@ -55,7 +50,7 @@ void SpecStateSensor::check() const
{
SpecState::check();
- auto sb = FactoryStateBlock::create(type_, state_, mode_ == "fix"); // already tried in SpecState::check()
+ auto sb = FactoryStateBlock::create(type_, state_, mode_ == "fix"); // already tried in SpecState::check()
// check drift sigma size
if (dynamic_ and drift_std_.size() > 0 and drift_std_.size() != sb->getLocalSize())
@@ -65,8 +60,7 @@ void SpecStateSensor::check() const
std::string SpecStateSensor::print(const std::string& _spaces) const
{
- return SpecState::print(_spaces) +
- _spaces + "dynamic: " + toString(dynamic_) + "\n" +
+ return SpecState::print(_spaces) + _spaces + "dynamic: " + toString(dynamic_) + "\n" +
(drift_std_.size() > 0 ? _spaces + "drift_std: " + toString(drift_std_) + "\n" : "");
}
@@ -79,9 +73,8 @@ std::ostream& operator<<(std::ostream& _os, const wolf::SpecStateSensor& _x)
YAML::Node SpecStateSensor::toYaml() const
{
YAML::Node node = SpecState::toYaml();
- node["dynamic"] = dynamic_;
- if (dynamic_ and drift_std_.size() > 0)
- node["drift_std"] = drift_std_;
+ node["dynamic"] = dynamic_;
+ if (dynamic_ and drift_std_.size() > 0) node["drift_std"] = drift_std_;
return node;
}
diff --git a/src/composite/state_composite.cpp b/src/composite/state_composite.cpp
deleted file mode 100644
index d7b26797897522d9ea2d121b4c58fecaf498aef5..0000000000000000000000000000000000000000
--- a/src/composite/state_composite.cpp
+++ /dev/null
@@ -1,273 +0,0 @@
-// WOLF - Copyright (C) 2020,2021,2022,2023
-// Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
-// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
-// Joan Vallvé Navarro (jvallve@iri.upc.edu)
-// All rights reserved.
-//
-// This file is part of WOLF: http://www.iri.upc.edu/wolf
-// WOLF is free software: you can redistribute it and/or modify
-// it under the terms of the GNU Lesser General Public License as published by
-// 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/>.
-
-#include "core/composite/state_composite.h"
-#include "core/state_block/state_block.h"
-
-namespace wolf
-{
-
-const StateBlockMap& StateBlockComposite::getStateBlockMap() const
-{
- return state_block_map_;
-}
-
-void StateBlockComposite::fix()
-{
- for (auto pair_key_sbp : state_block_map_)
- if (pair_key_sbp.second != nullptr) pair_key_sbp.second->fix();
-}
-
-void StateBlockComposite::unfix()
-{
- for (auto pair_key_sbp : state_block_map_)
- if (pair_key_sbp.second != nullptr) pair_key_sbp.second->unfix();
-}
-
-bool StateBlockComposite::isFixed() const
-{
- bool fixed = true;
- for (auto pair_key_sbp : state_block_map_)
- {
- if (pair_key_sbp.second) fixed &= pair_key_sbp.second->isFixed();
- }
- return fixed;
-}
-
-unsigned int StateBlockComposite::remove(const char& _sb_type)
-{
- return state_block_map_.erase(_sb_type);
-}
-
-bool StateBlockComposite::has(const StateBlockPtr& _sb) const
-{
- bool found = false;
- for (const auto& pair_key_sb : state_block_map_)
- {
- found = found || (pair_key_sb.second == _sb);
- }
- return found;
-}
-
-StateBlockPtr StateBlockComposite::at(const char& _sb_type) const
-{
- auto it = state_block_map_.find(_sb_type);
- if (it != state_block_map_.end())
- return it->second;
- else
- return nullptr;
-}
-
-void StateBlockComposite::set(const char& _sb_type, const StateBlockPtr& _sb)
-{
- auto it = state_block_map_.find(_sb_type);
- assert(it != state_block_map_.end() && "Cannot set an inexistent state block! Use addStateBlock instead.");
-
- it->second = _sb;
-}
-
-void StateBlockComposite::set(const char& _key, const VectorXd& _vec)
-{
- auto it = state_block_map_.find(_key);
- assert(it != state_block_map_.end() && "Cannot set an inexistent state block! Use addStateBlock instead.");
-
- assert(it->second->getSize() == _vec.size() && "Provided vector size mismatch with associated state block");
-
- it->second->setState(_vec);
-}
-
-void StateBlockComposite::setVectors(const StateKeys& _structure, const std::list<VectorXd>& _vectors)
-{
- assert(_structure.size() == _vectors.size() && "Sizes of structure and vector list do not match");
-
- auto vec_it = _vectors.begin();
- for (const auto key : _structure)
- {
- set(key, *vec_it);
- vec_it++;
- }
-}
-
-bool StateBlockComposite::key(const StateBlockPtr& _sb, char& _key) const
-{
- const auto& it = this->find(_sb);
-
- bool found = (it != state_block_map_.end());
-
- if (found)
- {
- _key = it->first;
- return true;
- }
- else
- {
- _key = '0';
- return false;
- }
-}
-
-char StateBlockComposite::key(const StateBlockPtr& _sb) const
-{
- const auto& it = this->find(_sb);
-
- bool found = (it != state_block_map_.end());
-
- if (found)
- return it->first;
- else
- return '0';
-}
-
-StateBlockMapCIter StateBlockComposite::find(const StateBlockPtr& _sb) const
-{
- const auto& it = std::find_if(state_block_map_.cbegin(),
- state_block_map_.cend(),
- [_sb](const std::pair<char, StateBlockPtr>& pair) { return pair.second == _sb; });
-
- return it;
-}
-
-StateBlockPtr StateBlockComposite::add(const char& _sb_type, const StateBlockPtr& _sb)
-{
- assert(state_block_map_.count(_sb_type) == 0 &&
- "Trying to add a state block with an existing type! Use setStateBlock instead.");
-
- state_block_map_.emplace(_sb_type, _sb);
-
- return _sb;
-}
-
-void StateBlockComposite::perturb(double amplitude)
-{
- for (const auto& pair_key_sb : state_block_map_)
- {
- auto& sb = pair_key_sb.second;
- if (!sb->isFixed()) sb->perturb(amplitude);
- }
-}
-
-void StateBlockComposite::plus(const VectorComposite& _dx)
-{
- for (const auto& pair_key_sb : state_block_map_)
- {
- const auto& key = pair_key_sb.first;
- const auto& sb = pair_key_sb.second;
- const auto& dv = _dx.at(key);
-
- sb->plus(dv);
- }
-}
-
-VectorComposite StateBlockComposite::getState() const
-{
- VectorComposite v;
- getState(v);
- return v;
-}
-
-bool StateBlockComposite::getState(VectorComposite& _state) const
-{
- for (auto& pair_key_sb : state_block_map_)
- {
- _state.emplace(pair_key_sb.first, pair_key_sb.second->getState());
- }
- return true;
-}
-
-void StateBlockComposite::setState(const VectorComposite& _state)
-{
- for (const auto& pair_key_sb : _state)
- {
- state_block_map_[pair_key_sb.first]->setState(pair_key_sb.second);
- }
-}
-
-void StateBlockComposite::setIdentity(bool _notify)
-{
- for (const auto& pair_key_sb : getStateBlockMap())
- {
- pair_key_sb.second->setIdentity(_notify);
- }
-}
-
-void StateBlockComposite::setZero(bool _notify)
-{
- setIdentity(_notify);
-}
-
-VectorComposite StateBlockComposite::identity() const
-{
- VectorComposite x;
- for (const auto& pair_key_sb : getStateBlockMap()) x.emplace(pair_key_sb.first, pair_key_sb.second->identity());
- return x;
-}
-
-VectorComposite StateBlockComposite::zero() const
-{
- return identity();
-}
-
-SizeEigen StateBlockComposite::stateSize() const
-{
- SizeEigen size = 0;
- for (const auto& pair_key_sb : state_block_map_) size += pair_key_sb.second->getSize();
- return size;
-}
-
-SizeEigen StateBlockComposite::stateSize(const StateKeys& _structure) const
-{
- SizeEigen size = 0;
- for (const auto& key : _structure)
- {
- size += state_block_map_.at(key)->getSize();
- }
- return size;
-}
-
-VectorXd StateBlockComposite::stateVector(const StateKeys& _structure) const
-{
- VectorXd x(stateSize(_structure));
- SizeEigen index = 0;
- SizeEigen size;
- for (const auto& key : _structure)
- {
- const auto& sb = state_block_map_.at(key);
- size = sb->getSize();
- x.segment(index, size) = sb->getState();
- index += size;
- }
- return x;
-}
-
-void StateBlockComposite::stateVector(const StateKeys& _structure, VectorXd& _vector) const
-{
- _vector.resize(stateSize(_structure));
- SizeEigen index = 0;
- SizeEigen size;
- for (const auto& key : _structure)
- {
- const auto& sb = state_block_map_.at(key);
- size = sb->getSize();
- _vector.segment(index, size) = sb->getState();
- index += size;
- }
-}
-
-} // namespace wolf
diff --git a/src/composite/vector_composite.cpp b/src/composite/vector_composite.cpp
index 7156856700259f50287d26a31cef48fe495054a4..68c13b4909330c717bd9c0ac5dead0805e6c2499 100644
--- a/src/composite/vector_composite.cpp
+++ b/src/composite/vector_composite.cpp
@@ -22,7 +22,6 @@
namespace wolf
{
-
VectorComposite::VectorComposite(const StateKeys& _keys, const VectorXd& _v, const std::list<int>& _sizes)
{
assert(_keys.size() == _sizes.size() && "Keys and _sizes list size mismatch");
diff --git a/src/factor/factor_analytic.cpp b/src/factor/factor_analytic.cpp
index 83031d1965af391d0a9787cb726f56aa8138da44..f879376e8e661072d1944eb6021dafeb630bd62b 100644
--- a/src/factor/factor_analytic.cpp
+++ b/src/factor/factor_analytic.cpp
@@ -27,7 +27,7 @@ FactorAnalytic::FactorAnalytic(const std::string& _type,
const FactorTopology& _top,
const Eigen::VectorXd& _measurement,
const Eigen::MatrixXd& _measurement_sqrt_information_upper,
- const NodeStateBlocksPtrList& _node_state_block_list,
+ const NodeStateBlocksPtrList& _node_state_block_list,
const ProcessorBasePtr& _processor_ptr,
const std::vector<StateBlockPtr>& _state_ptrs,
bool _apply_loss_function,
diff --git a/src/feature/feature_base.cpp b/src/feature/feature_base.cpp
index baa1665ccf191e6f134996c968e6bc2c6453fea6..dea59351493161154123f6d16c8c0e70544f50f8 100644
--- a/src/feature/feature_base.cpp
+++ b/src/feature/feature_base.cpp
@@ -175,7 +175,7 @@ void FeatureBase::printHeader(int _depth,
std::string _tabs) const
{
_stream << _tabs << "Ftr" << id() << " trk" << trackId() << " " << getType()
- << ((_depth < 4) ? " -- " + std::to_string(getFactorList().size()) + "fac " : "") << std::endl;
+ << ((_depth < 4) ? " -- " + toString(getFactorList().size()) + "fac " : "") << std::endl;
if (_metric)
_stream << _tabs << " "
<< "m = ( " << std::setprecision(2) << getMeasurement().transpose() << " )" << std::endl;
@@ -194,9 +194,7 @@ void FeatureBase::print(int _depth,
if (c) c->print(_depth, _factored_by, _metric, _state_blocks, _stream, _tabs + " ");
}
-CheckLog FeatureBase::localCheck(bool _verbose,
- std::ostream& _stream,
- std::string _tabs) const
+CheckLog FeatureBase::localCheck(bool _verbose, std::ostream& _stream, std::string _tabs) const
{
CheckLog log;
std::stringstream inconsistency_explanation;
@@ -234,10 +232,7 @@ CheckLog FeatureBase::localCheck(bool _verbose,
return log;
}
-bool FeatureBase::check(CheckLog& _log,
- bool _verbose,
- std::ostream& _stream,
- std::string _tabs) const
+bool FeatureBase::check(CheckLog& _log, bool _verbose, std::ostream& _stream, std::string _tabs) const
{
auto local_log = localCheck(_verbose, _stream, _tabs);
_log.compose(local_log);
diff --git a/src/feature/feature_diff_drive.cpp b/src/feature/feature_diff_drive.cpp
index 4c97a4d33cd91f35ab79643446f36b7bb26e9e46..5703bb078b2809d3fe79402e9a1c98ccdfc89eb5 100644
--- a/src/feature/feature_diff_drive.cpp
+++ b/src/feature/feature_diff_drive.cpp
@@ -22,16 +22,15 @@
namespace wolf
{
-
FeatureDiffDrive::FeatureDiffDrive(const Eigen::VectorXd& _delta_preintegrated,
const Eigen::MatrixXd& _delta_preintegrated_covariance,
const Eigen::VectorXd& _diff_drive_params,
- const Eigen::MatrixXd& _jacobian_diff_drive_params) :
- FeatureMotion("FeatureDiffDrive",
- _delta_preintegrated,
- _delta_preintegrated_covariance,
- _diff_drive_params,
- _jacobian_diff_drive_params)
+ const Eigen::MatrixXd& _jacobian_diff_drive_params)
+ : FeatureMotion("FeatureDiffDrive",
+ _delta_preintegrated,
+ _delta_preintegrated_covariance,
+ _diff_drive_params,
+ _jacobian_diff_drive_params)
{
//
}
diff --git a/src/feature/feature_motion.cpp b/src/feature/feature_motion.cpp
index f5a3ab76e28ec5feb3b9412a633b914cc18e8b17..c677ad335e3cf0a8a48327a69acc82b836b326e0 100644
--- a/src/feature/feature_motion.cpp
+++ b/src/feature/feature_motion.cpp
@@ -17,26 +17,19 @@
//
// 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/>.
-/*
- * feature_motion.cpp
- *
- * Created on: Aug 11, 2017
- * Author: jsola
- */
#include "core/feature/feature_motion.h"
namespace wolf
{
-
FeatureMotion::FeatureMotion(const std::string& _type,
- const VectorXd& _delta_preint,
- const MatrixXd _delta_preint_cov,
- const VectorXd& _calib_preint,
- const MatrixXd& _jacobian_calib) :
- FeatureBase(_type, _delta_preint, _delta_preint_cov),
- calib_preint_(_calib_preint),
- jacobian_calib_(_jacobian_calib)
+ const VectorXd& _delta_preint,
+ const MatrixXd _delta_preint_cov,
+ const VectorXd& _calib_preint,
+ const MatrixXd& _jacobian_calib)
+ : FeatureBase(_type, _delta_preint, _delta_preint_cov),
+ calib_preint_(_calib_preint),
+ jacobian_calib_(_jacobian_calib)
{
//
}
diff --git a/src/feature/feature_odom_2d.cpp b/src/feature/feature_odom_2d.cpp
index f07bb4e6f7cb2ecc2187a86a2e073c1eec1bd94a..dd3a535d5697343299715c35348d792583351f6a 100644
--- a/src/feature/feature_odom_2d.cpp
+++ b/src/feature/feature_odom_2d.cpp
@@ -20,12 +20,11 @@
#include "core/feature/feature_odom_2d.h"
-namespace wolf {
-
-FeatureOdom2d::FeatureOdom2d(const Eigen::VectorXd& _measurement, const Eigen::MatrixXd& _meas_covariance) :
- FeatureBase("FeatureOdom2d", _measurement, _meas_covariance)
+namespace wolf
+{
+FeatureOdom2d::FeatureOdom2d(const Eigen::VectorXd& _measurement, const Eigen::MatrixXd& _meas_covariance)
+ : FeatureBase("FeatureOdom2d", _measurement, _meas_covariance)
{
- //std::cout << "New FeatureOdom2d: measurement " << _measurement.transpose() << std::endl << "covariance" << std::endl << _meas_covariance << std::endl;
}
FeatureOdom2d::~FeatureOdom2d()
@@ -33,4 +32,4 @@ FeatureOdom2d::~FeatureOdom2d()
//
}
-} // namespace wolf
+} // namespace wolf
diff --git a/src/feature/feature_pose.cpp b/src/feature/feature_pose.cpp
index b53ed99b86e118d7a82c8e798805e6c8090a8709..443e08780af75c82dd6d6297f07d6858b29b5c4f 100644
--- a/src/feature/feature_pose.cpp
+++ b/src/feature/feature_pose.cpp
@@ -20,10 +20,10 @@
#include "core/feature/feature_pose.h"
-namespace wolf {
-
-FeaturePose::FeaturePose(const Eigen::VectorXd& _measurement, const Eigen::MatrixXd& _meas_covariance) :
- FeatureBase("FeaturePose", _measurement, _meas_covariance)
+namespace wolf
+{
+FeaturePose::FeaturePose(const Eigen::VectorXd& _measurement, const Eigen::MatrixXd& _meas_covariance)
+ : FeatureBase("FeaturePose", _measurement, _meas_covariance)
{
//
}
@@ -33,4 +33,4 @@ FeaturePose::~FeaturePose()
//
}
-} // namespace wolf
+} // namespace wolf
diff --git a/src/frame/frame_base.cpp b/src/frame/frame_base.cpp
index c60c5c9b60cfe52515cb39f10d23818568d8ad3f..c363b040ba435c7dda0efb9eff9a92fb56f00686 100644
--- a/src/frame/frame_base.cpp
+++ b/src/frame/frame_base.cpp
@@ -29,7 +29,6 @@
namespace wolf
{
-
unsigned int FrameBase::frame_id_count_ = 0;
FrameBase::FrameBase(const TimeStamp& _ts, StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, StateBlockPtr _v_ptr)
@@ -297,7 +296,7 @@ void FrameBase::printHeader(int _depth,
std::string _tabs) const
{
_stream << _tabs << "Frm" << id() << " " << getKeys() << " ts = " << std::setprecision(3) << getTimeStamp()
- << ((_depth < 2) ? " -- " + std::to_string(getCaptureList().size()) + "C " : "");
+ << ((_depth < 2) ? " -- " + toString(getCaptureList().size()) + "C " : "");
if (_factored_by)
{
_stream << " <-- ";
diff --git a/src/hardware/hardware_base.cpp b/src/hardware/hardware_base.cpp
index f41685c7dbcfa128ebbb5b803c984bdbfb277339..1dd3cfe0305fa57276688c6a470788ea6e3dfc1f 100644
--- a/src/hardware/hardware_base.cpp
+++ b/src/hardware/hardware_base.cpp
@@ -23,7 +23,6 @@
namespace wolf
{
-
HardwareBase::HardwareBase() : NodeBase("HARDWARE", "HardwareBase")
{
// std::cout << "constructed H" << std::endl;
@@ -65,7 +64,7 @@ void HardwareBase::printHeader(int _depth,
std::ostream& _stream,
std::string _tabs) const
{
- _stream << _tabs << "Hardware" << ((_depth < 1) ? (" -- " + std::to_string(getSensorList().size()) + "S") : "")
+ _stream << _tabs << "Hardware" << ((_depth < 1) ? (" -- " + toString(getSensorList().size()) + "S") : "")
<< std::endl;
}
void HardwareBase::print(int _depth,
diff --git a/src/landmark/landmark_base.cpp b/src/landmark/landmark_base.cpp
index 037fd2c82e6c33263c6352f7476268b7522b906c..db392e44576e56e9fb0cdfc70f4fc67355350106 100644
--- a/src/landmark/landmark_base.cpp
+++ b/src/landmark/landmark_base.cpp
@@ -147,9 +147,7 @@ void LandmarkBase::print(int _depth,
printHeader(_depth, _factored_by, _metric, _state_blocks, _stream, _tabs);
}
-CheckLog LandmarkBase::localCheck(bool _verbose,
- std::ostream& _stream,
- std::string _tabs) const
+CheckLog LandmarkBase::localCheck(bool _verbose, std::ostream& _stream, std::string _tabs) const
{
CheckLog log;
std::stringstream inconsistency_explanation;
@@ -163,8 +161,8 @@ CheckLog LandmarkBase::localCheck(bool _verbose,
// check problem and map pointers
auto map_ptr = getMap();
- inconsistency_explanation << "Landmarks' problem ptr " << getProblem()
- << " different from Map's problem ptr " << map_ptr->getProblem() << "\n";
+ inconsistency_explanation << "Landmarks' problem ptr " << getProblem() << " different from Map's problem ptr "
+ << map_ptr->getProblem() << "\n";
log.assertTrue((getProblem() == map_ptr->getProblem()), inconsistency_explanation);
@@ -180,10 +178,7 @@ CheckLog LandmarkBase::localCheck(bool _verbose,
return log;
}
-bool LandmarkBase::check(CheckLog& _log,
- bool _verbose,
- std::ostream& _stream,
- std::string _tabs) const
+bool LandmarkBase::check(CheckLog& _log, bool _verbose, std::ostream& _stream, std::string _tabs) const
{
auto local_log = localCheck(_verbose, _stream, _tabs);
_log.compose(local_log);
@@ -194,4 +189,4 @@ bool LandmarkBase::check(CheckLog& _log,
// Register landmark creator
WOLF_REGISTER_LANDMARK(LandmarkBase);
-} // namespace wolf
+} // namespace wolf
diff --git a/src/map/map_base.cpp b/src/map/map_base.cpp
index fe02ce3987987ec5fcf6eb1109366d9f8272a4e6..130f2f220e289f7bf831a7147f5daf6e5f8bc561 100644
--- a/src/map/map_base.cpp
+++ b/src/map/map_base.cpp
@@ -26,24 +26,22 @@
// YAML
#include <yaml-cpp/yaml.h>
-// stl
-#include <fstream>
-#include <ctime>
-#include <iomanip>
-#include <iostream>
-#include <sstream>
-
namespace wolf
{
-
-MapBase::MapBase() : NodeBase("MAP", "Base")
+MapBase::MapBase() : NodeBase("MAP", "MapBase")
{
// std::cout << "constructed M"<< std::endl;
}
-MapBase::MapBase(ParamsMapBasePtr _params, const std::string& _type) : NodeBase("MAP", _type)
+MapBase::MapBase(const YAML::Node& _params) : NodeBase("MAP", _params["type"].as<std::string>())
{
- // std::cout << "constructed M"<< std::endl;
+ if (_params["landmarks"])
+ for (unsigned int i = 0; i < _params["landmarks"].size(); i++)
+ {
+ YAML::Node lmk_node = _params["landmarks"][i];
+ LandmarkBasePtr lmk_ptr = FactoryLandmark::create(lmk_node["type"].as<std::string>(), lmk_node);
+ lmk_ptr->link(shared_from_this());
+ }
}
bool MapBase::hasChildren() const
@@ -145,7 +143,7 @@ void MapBase::printHeader(int _depth,
std::ostream& _stream,
std::string _tabs) const
{
- _stream << _tabs << "Map" << ((_depth < 1) ? (" -- " + std::to_string(getLandmarkList().size()) + "L") : "")
+ _stream << _tabs << "Map" << ((_depth < 1) ? (" -- " + toString(getLandmarkList().size()) + "L") : "")
<< std::endl;
}
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index d998b72e0a568ba1ad9775f0600a4a0c883e4b9e..95f9ef37ad52a9ec5b07ea54162a4bbdecfa482a 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -119,14 +119,14 @@ ProblemPtr Problem::create(const std::string& _frame_structure, SizeEigen _dim)
ProblemPtr Problem::autoSetup(const std::string& _input_yaml_file,
const std::vector<std::string>& _primary_schema_folders)
{
- // schema folders
+ // schema folders
// (optional _primary_schema_folders specify folders where to search schemas before installed ones)
std::vector<std::string> schema_folders = _primary_schema_folders;
schema_folders.push_back(_WOLF_SCHEMA_DIR);
// search plugins to load them temporarily and get the schema folders
std::set<YAML::Node> visited_nodes;
- auto plugins = searchPlugins(YAML::Load(_input_yaml_file), visited_nodes);
+ auto plugins = searchPlugins(YAML::Load(_input_yaml_file), visited_nodes);
for (auto plugin : plugins)
{
// temporary load to automatically register schema folders
@@ -179,11 +179,11 @@ ProblemPtr Problem::autoSetup(YAML::Node _param_node)
std::string tree_manager_type = tree_manager_node["type"].as<std::string>();
if (tree_manager_type != "none" and tree_manager_type != "None" and tree_manager_type != "NONE")
{
- if (not FactoryTreeManager::isCreatorRegistered(tree_manager_type))
+ if (not FactoryTreeManagerNode::isCreatorRegistered(tree_manager_type))
{
problem->loadPlugin(tree_manager_node["plugin"].as<std::string>());
}
- problem->setTreeManager(FactoryTreeManager::create(tree_manager_type, tree_manager_node));
+ problem->setTreeManager(FactoryTreeManagerNode::create(tree_manager_type, tree_manager_node, {}));
}
// First frame
@@ -214,7 +214,7 @@ ProblemPtr Problem::autoSetup(YAML::Node _param_node)
for (auto processor_n : processors_node)
{
auto processor_type = processor_n["type"].as<std::string>();
- if (not FactoryProcessor::isCreatorRegistered(processor_type))
+ if (not FactoryProcessorNode::isCreatorRegistered(processor_type))
{
problem->loadPlugin(processor_n["plugin"].as<std::string>());
}
@@ -231,19 +231,14 @@ ProblemPtr Problem::autoSetup(YAML::Node _param_node)
// load plugin if map is not registered
YAML::Node map_node = _param_node["map"];
auto map_type = map_node["type"].as<std::string>();
- if (not FactoryMap::isCreatorRegistered(map_type))
+ if (not FactoryMapNode::isCreatorRegistered(map_type))
{
problem->loadPlugin(map_node["plugin"].as<std::string>());
}
WOLF_TRACE("Map Type: ", map_type);
- auto map = FactoryMap::create(map_type, map_node);
+ auto map = FactoryMapNode::create(map_type, map_node, {});
map->setProblem(problem);
problem->setMap(map);
- // load map from file (optional)
- if (map_node["filename"])
- {
- problem->loadMap(map_node["filename"].as<std::string>());
- }
// Done
return problem;
@@ -317,9 +312,10 @@ Problem::~Problem()
// WOLF_DEBUG("destructed -P");
}
-SensorBasePtr Problem::installSensor(const YAML::Node& _sensor_node)
+SensorBasePtr Problem::installSensor(const YAML::Node& _sensor_node, const std::vector<std::string>& _folders_schema)
{
- SensorBasePtr sen_ptr = FactorySensorNode::create(_sensor_node["type"].as<std::string>(), _sensor_node);
+ SensorBasePtr sen_ptr =
+ FactorySensorNode::create(_sensor_node["type"].as<std::string>(), _sensor_node, _folders_schema);
sen_ptr->link(getHardware());
return sen_ptr;
}
@@ -333,13 +329,13 @@ SensorBasePtr Problem::installSensor(const std::string& _sen_type,
throw std::runtime_error("Cannot install sensor: provided params YAML file: '" + _params_yaml_filename +
"' does not exist.");
}
- // SensorBasePtr sen_ptr = FactorySensorFile::create(_sen_type, _sen_type, _params_yaml_filename, _folders_schema);
SensorBasePtr sen_ptr = FactorySensorFile::create(_sen_type, _params_yaml_filename, _folders_schema);
sen_ptr->link(getHardware());
return sen_ptr;
}
-ProcessorBasePtr Problem::installProcessor(const YAML::Node& _processor_node)
+ProcessorBasePtr Problem::installProcessor(const YAML::Node& _processor_node,
+ const std::vector<std::string>& _folders_schema)
{
auto corresponding_sensor_name = _processor_node["sensor_name"].as<std::string>();
SensorBasePtr sen_ptr = findSensor(corresponding_sensor_name);
@@ -348,7 +344,7 @@ ProcessorBasePtr Problem::installProcessor(const YAML::Node& _processor_node)
"\" not found. Check sensor name, it must match in sensor and processor!");
auto processor_type = _processor_node["type"].as<std::string>();
- ProcessorBasePtr prc_ptr = FactoryProcessor::create(processor_type, _processor_node);
+ ProcessorBasePtr prc_ptr = FactoryProcessorNode::create(processor_type, _processor_node, _folders_schema);
// Dimension check
int prc_dim = prc_ptr->getDim();
@@ -375,7 +371,7 @@ ProcessorBasePtr Problem::installProcessor(const std::string& _prc_
}
ProcessorBasePtr Problem::installProcessor(const std::string& _prc_type,
- SensorBasePtr& _corresponding_sensor,
+ SensorBasePtr _corresponding_sensor,
const std::string& _params_yaml_filename,
const std::vector<std::string>& _folders_schema)
{
@@ -802,7 +798,7 @@ VectorComposite Problem::stateZero(const StateKeys& _keys) const
return stateZero(getFrameTypes(_keys));
}
-bool Problem::permitKeyFrame(ProcessorBasePtr _processor_ptr) const
+bool Problem::permitKeyFrame(ProcessorBaseConstPtr _processor_ptr) const
{
// This should implement a black list of processors that have forbidden keyframe creation
// This decision is made at problem level, not at processor configuration level.
@@ -1183,7 +1179,7 @@ FrameBasePtr Problem::applyPriorOptions(const TimeStamp& _ts)
// Create first frame with the prior options
FrameBasePtr first_frame = emplaceFrame(_ts, prior_options_);
- first_frame->applyPriors(prior_options_);
+ first_frame->emplacePriors(prior_options_);
// CaptureBasePtr prior_cap(nullptr);
diff --git a/src/processor/motion_buffer.cpp b/src/processor/motion_buffer.cpp
index 30f3958235120c1729f79aaabb312fdff2813831..f5dde074f7c630669f0b87504314efcc80ae8de6 100644
--- a/src/processor/motion_buffer.cpp
+++ b/src/processor/motion_buffer.cpp
@@ -21,51 +21,44 @@
#include "core/processor/motion_buffer.h"
namespace wolf
{
-
Motion::Motion(const TimeStamp& _ts,
- const VectorXd& _data,
- const MatrixXd& _data_cov,
- const VectorXd& _delta,
- const MatrixXd& _delta_cov,
- const VectorXd& _delta_integr,
- const MatrixXd& _delta_integr_cov,
- const MatrixXd& _jac_delta,
- const MatrixXd& _jac_delta_int,
- const MatrixXd& _jac_calib) :
- data_size_(_data.size()),
- delta_size_(_delta.size()),
- cov_size_(_delta_cov.cols()),
- calib_size_(_jac_calib.cols()),
- ts_(_ts),
- data_(_data),
- data_cov_(_data_cov),
- delta_(_delta),
- delta_cov_(_delta_cov),
- delta_integr_(_delta_integr),
- delta_integr_cov_(_delta_integr_cov),
- jacobian_delta_(_jac_delta),
- jacobian_delta_integr_(_jac_delta_int),
- jacobian_calib_(_jac_calib)
+ const VectorXd& _data,
+ const MatrixXd& _data_cov,
+ const VectorXd& _delta,
+ const MatrixXd& _delta_cov,
+ const VectorXd& _delta_integr,
+ const MatrixXd& _delta_integr_cov,
+ const MatrixXd& _jac_delta,
+ const MatrixXd& _jac_delta_int,
+ const MatrixXd& _jac_calib)
+ : data_size_(_data.size()),
+ delta_size_(_delta.size()),
+ cov_size_(_delta_cov.cols()),
+ calib_size_(_jac_calib.cols()),
+ ts_(_ts),
+ data_(_data),
+ data_cov_(_data_cov),
+ delta_(_delta),
+ delta_cov_(_delta_cov),
+ delta_integr_(_delta_integr),
+ delta_integr_cov_(_delta_integr_cov),
+ jacobian_delta_(_jac_delta),
+ jacobian_delta_integr_(_jac_delta_int),
+ jacobian_calib_(_jac_calib)
{
}
Motion::Motion(const TimeStamp& _ts,
- SizeEigen _data_size,
- SizeEigen _delta_size,
- SizeEigen _cov_size,
- SizeEigen _calib_size) :
- data_size_(_data_size),
- delta_size_(_delta_size),
- cov_size_(_cov_size),
- calib_size_(_calib_size),
- ts_(_ts)
+ SizeEigen _data_size,
+ SizeEigen _delta_size,
+ SizeEigen _cov_size,
+ SizeEigen _calib_size)
+ : data_size_(_data_size), delta_size_(_delta_size), cov_size_(_cov_size), calib_size_(_calib_size), ts_(_ts)
{
resize(_data_size, _delta_size, _cov_size, _calib_size);
}
-Motion::~Motion()
-{
-}
+Motion::~Motion() {}
void Motion::resize(SizeEigen _data_s, SizeEigen _delta_s, SizeEigen _delta_cov_s, SizeEigen _calib_s)
{
@@ -89,11 +82,8 @@ MotionBuffer::MotionBuffer()
const Motion& MotionBuffer::getMotion(const TimeStamp& _ts) const
{
- //assert((this->front().ts_ <= _ts) && "Query time stamp out of buffer bounds");
- auto previous = std::find_if(this->rbegin(), this->rend(), [&](const Motion& m)
- {
- return m.ts_ <= _ts;
- });
+ // assert((this->front().ts_ <= _ts) && "Query time stamp out of buffer bounds");
+ auto previous = std::find_if(this->rbegin(), this->rend(), [&](const Motion& m) { return m.ts_ <= _ts; });
if (previous == this->rend())
// The time stamp is older than the buffer's oldest data.
// We could do something here, and throw an error or something, but by now we'll return the first valid data
@@ -104,11 +94,8 @@ const Motion& MotionBuffer::getMotion(const TimeStamp& _ts) const
void MotionBuffer::getMotion(const TimeStamp& _ts, Motion& _motion) const
{
- //assert((this->front().ts_ <= _ts) && "Query time stamp out of buffer bounds");
- auto previous = std::find_if(this->rbegin(), this->rend(), [&](const Motion& m)
- {
- return m.ts_ <= _ts;
- });
+ // assert((this->front().ts_ <= _ts) && "Query time stamp out of buffer bounds");
+ auto previous = std::find_if(this->rbegin(), this->rend(), [&](const Motion& m) { return m.ts_ <= _ts; });
if (previous == this->rend())
// The time stamp is older than the buffer's oldest data.
// We could do something here, but by now we'll return the last valid data
@@ -120,12 +107,9 @@ 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 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");
+ 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)
- {
- return m.ts_ <= _ts;
- });
+ auto previous = std::find_if(this->rbegin(), this->rend(), [&](const Motion& m) { return m.ts_ <= _ts; });
if (previous == this->rend())
{
// The time stamp is more recent than the buffer's most recent data:
@@ -135,14 +119,10 @@ void MotionBuffer::split(const TimeStamp& _ts, MotionBuffer& _buffer_part_before
else
{
// Transfer part of the buffer
- _buffer_part_before_ts.splice(_buffer_part_before_ts.begin(),
- *this,
- this->begin(),
- (previous--).base());
+ _buffer_part_before_ts.splice(_buffer_part_before_ts.begin(), *this, this->begin(), (previous--).base());
}
}
-
void MotionBuffer::print(bool show_data, bool show_delta, bool show_delta_int, bool show_jacs, bool show_covs)
{
using std::cout;
@@ -151,44 +131,38 @@ void MotionBuffer::print(bool show_data, bool show_delta, bool show_delta_int, b
if (!show_data && !show_delta && !show_delta_int && !show_jacs && !show_covs)
{
cout << "Buffer size: " << this->size() << " ; time stamps: <";
- for (Motion mot : *this)
- cout << " " << mot.ts_;
+ for (Motion mot : *this) cout << " " << mot.ts_;
cout << " >" << endl;
}
else
{
- print(0,0,0,0);
+ print(0, 0, 0, 0);
for (Motion mot : *this)
{
cout << "-- Motion (" << mot.ts_ << ")" << endl;
if (show_data)
{
cout << " data: " << mot.data_.transpose() << endl;
- if (show_covs)
- cout << " data cov: \n" << mot.data_cov_ << endl;
+ if (show_covs) cout << " data cov: \n" << mot.data_cov_ << endl;
}
if (show_delta)
{
cout << " delta: " << mot.delta_.transpose() << endl;
- if (show_covs)
- cout << " delta cov: \n" << mot.delta_cov_ << endl;
+ if (show_covs) cout << " delta cov: \n" << mot.delta_cov_ << endl;
}
if (show_delta_int)
{
cout << " delta integrated: " << mot.delta_integr_.transpose() << endl;
- if (show_covs)
- cout << " delta integrated cov: \n" << mot.delta_integr_cov_ << endl;
+ if (show_covs) cout << " delta integrated cov: \n" << mot.delta_integr_cov_ << endl;
}
if (show_jacs)
{
cout << " Jac delta: \n" << mot.jacobian_delta_ << endl;
cout << " Jac delta integr: \n" << mot.jacobian_delta_integr_ << endl;
cout << " Jac calib: \n" << mot.jacobian_calib_ << endl;
-
}
}
}
}
-}
-
+} // namespace wolf
diff --git a/src/processor/motion_provider.cpp b/src/processor/motion_provider.cpp
index 3726ad2c0a6d81396ecf96e27d7c98a531ebc71a..0ad0109bc5d29740f5f18bbaea0510c32d869377 100644
--- a/src/processor/motion_provider.cpp
+++ b/src/processor/motion_provider.cpp
@@ -25,7 +25,9 @@ using namespace wolf;
void MotionProvider::addToProblem(ProblemPtr _prb_ptr, MotionProviderPtr _motion_ptr)
{
- WOLF_DEBUG_COND(not isStateGetter(), "MotionProvider::addToProblem: MotionProvider processor with state_provider=false. Not adding this processor");
+ WOLF_DEBUG_COND(
+ not isStateGetter(),
+ "MotionProvider::addToProblem: MotionProvider processor with state_provider=false. Not adding this processor");
if (isStateGetter())
{
_prb_ptr->addMotionProvider(_motion_ptr);
@@ -38,14 +40,12 @@ void MotionProvider::setOdometry(const VectorComposite& _odom)
std::lock_guard<std::mutex> lock(mut_odometry_);
assert(_odom.has(getStateKeys()) && "MotionProvider::setOdometry(): any key missing in _odom.");
- for (auto key : getStateKeys())
- odometry_[key] = _odom.at(key); //overwrite/insert only keys of the state_types_
+ for (auto key : getStateKeys()) odometry_[key] = _odom.at(key); // overwrite/insert only keys of the state_types_
}
-wolf::VectorComposite MotionProvider::getOdometry ( ) const
+wolf::VectorComposite MotionProvider::getOdometry() const
{
std::lock_guard<std::mutex> lock(mut_odometry_);
return odometry_;
}
-
diff --git a/src/processor/processor_base.cpp b/src/processor/processor_base.cpp
index 49c356d1d6f4a4e5a41c6519fa9fb6cd80fbc360..7dfbc232a9fbceeebfe863fbb0ab8f38cf24e511 100644
--- a/src/processor/processor_base.cpp
+++ b/src/processor/processor_base.cpp
@@ -27,10 +27,10 @@ namespace wolf
{
unsigned int ProcessorBase::processor_id_count_ = 0;
-ProcessorBase::ProcessorBase(const std::string& _type, int _dim, ParamsProcessorBasePtr _params)
- : NodeBase("PROCESSOR", _type, _params->name),
+ProcessorBase::ProcessorBase(const std::string& _type, int _dim, const YAML::Node& _params)
+ : NodeBase("PROCESSOR", _type, _params["name"].as<std::string>()),
processor_id_(++processor_id_count_),
- params_(_params),
+ params_(Clone(_params)),
dim_(_dim),
sensor_ptr_(),
n_capture_callback_(0),
@@ -48,7 +48,7 @@ ProcessorBase::~ProcessorBase()
// WOLF_DEBUG("destructed -p" , id());
}
-bool ProcessorBase::permittedKeyFrame()
+bool ProcessorBase::permittedKeyFrame() const
{
return isVotingActive() && getProblem()->permitKeyFrame(shared_from_this());
}
@@ -147,8 +147,8 @@ void ProcessorBase::setProblem(ProblemPtr _problem)
if (_problem == nullptr or _problem == this->getProblem()) return;
if (dim_ != 0 and dim_ != _problem->getDim())
- throw std::runtime_error("Processor works with " + std::to_string(dim_) + "D but problem is " +
- std::to_string(_problem->getDim()) + "D");
+ throw std::runtime_error("Processor works with " + toString(dim_) + "D but problem is " +
+ toString(_problem->getDim()) + "D");
NodeBase::setProblem(_problem);
@@ -176,6 +176,7 @@ void ProcessorBase::print(int _depth,
{
printHeader(_depth, _factored_by, _metric, _state_blocks, _stream, _tabs);
}
+
CheckLog ProcessorBase::localCheck(bool _verbose, std::ostream& _stream, std::string _tabs) const
{
CheckLog log;
@@ -196,14 +197,15 @@ CheckLog ProcessorBase::localCheck(bool _verbose, std::ostream& _stream, std::st
<< " is different from Sensor problem pointer " << sen_ptr->getProblem() << "\n";
log.assertTrue((getProblem() == sen_ptr->getProblem()), inconsistency_explanation);
- inconsistency_explanation << "Prc" << id() << " @ " << this << " ---> Sen" << sen_ptr->id() << " @ "
- << sen_ptr << " -X-> Prc" << id();
+ inconsistency_explanation << "Prc" << id() << " @ " << this << " ---> Sen" << sen_ptr->id() << " @ " << sen_ptr
+ << " -X-> Prc" << id();
auto sen_prc_list = sen_ptr->getProcessorList();
auto sen_has_prc = std::find(sen_prc_list.begin(), sen_prc_list.end(), shared_from_this());
log.assertTrue(sen_has_prc != sen_prc_list.end(), inconsistency_explanation);
return log;
}
+
bool ProcessorBase::check(CheckLog& _log, bool _verbose, std::ostream& _stream, std::string _tabs) const
{
auto local_log = localCheck(_verbose, _stream, _tabs);
@@ -231,7 +233,7 @@ void ProcessorBase::printProfiling(std::ostream& _stream) const
bool ProcessorBase::checkTimeTolerance(const TimeStamp& _ts1, const TimeStamp& _ts2) const
{
auto dt = std::fabs(_ts1 - _ts2);
- return dt <= params_->time_tolerance;
+ return dt <= this->getTimeTolerance();
}
bool ProcessorBase::checkTimeTolerance(const CaptureBasePtr _cap, const TimeStamp& _ts) const
diff --git a/src/processor/processor_diff_drive.cpp b/src/processor/processor_diff_drive.cpp
index 7e520689cda1d6ce14d3d397cdf12c61d77f8f48..b9d091017be0199ae130dff522c2625cbdc9c9cd 100644
--- a/src/processor/processor_diff_drive.cpp
+++ b/src/processor/processor_diff_drive.cpp
@@ -28,15 +28,14 @@
namespace wolf
{
-
-ProcessorDiffDrive::ProcessorDiffDrive(const ParamsProcessorDiffDrivePtr _params) :
- ProcessorOdom2d(_params),
- params_prc_diff_drv_selfcal_(_params),
- radians_per_tick_(0.0) // This param needs further initialization. See this->configure(sensor).
+ProcessorDiffDrive::ProcessorDiffDrive(const YAML::Node& _params)
+ : ProcessorOdom2d(_params),
+ radians_per_tick_(0.0) // This param needs further initialization. See this->configure(sensor).
{
setType("ProcessorDiffDrive"); // overwrite odom2d setting
- calib_size_ = 3; // overwrite odom2d setting
- unmeasured_perturbation_cov_ = Matrix1d(_params->unmeasured_perturbation_std * _params->unmeasured_perturbation_std); // overwrite odom2d setting
+ calib_size_ = 3; // overwrite odom2d setting
+ unmeasured_perturbation_cov_ =
+ Matrix1d(std::pow(_params["unmeasured_perturbation_std"].as<double>(), 2)); // overwrite odom2d setting
}
ProcessorDiffDrive::~ProcessorDiffDrive()
@@ -44,28 +43,26 @@ ProcessorDiffDrive::~ProcessorDiffDrive()
//
}
-
void ProcessorDiffDrive::configure(SensorBasePtr _sensor)
{
SensorDiffDriveConstPtr sensor_diff_drive = std::dynamic_pointer_cast<SensorDiffDrive>(_sensor);
-
- if ( not _sensor)
- throw std::runtime_error("ProcessorDiffDrive::configure Provided sensor is nullptr or not of type SensorDiffDrive");
+
+ if (not _sensor)
+ throw std::runtime_error(
+ "ProcessorDiffDrive::configure Provided sensor is nullptr or not of type SensorDiffDrive");
radians_per_tick_ = sensor_diff_drive->getRadiansPerTick();
}
-
-
//////////// MOTION INTEGRATION ///////////////
void ProcessorDiffDrive::computeCurrentDelta(const Eigen::VectorXd& _data,
const Eigen::MatrixXd& _data_cov,
const Eigen::VectorXd& _calib,
- const double _dt,
- Eigen::VectorXd& _delta,
- Eigen::MatrixXd& _delta_cov,
- Eigen::MatrixXd& _jacobian_calib) const
+ const double _dt,
+ Eigen::VectorXd& _delta,
+ Eigen::MatrixXd& _delta_cov,
+ Eigen::MatrixXd& _jacobian_calib) const
{
/*
* Differential drive model -----------------------------------------------
@@ -78,40 +75,40 @@ void ProcessorDiffDrive::computeCurrentDelta(const Eigen::VectorXd& _data,
* dth : arc angle [rad]
*/
const double& k = radians_per_tick_;
- const double& r_l = _calib(0); // wheel radii
+ const double& r_l = _calib(0); // wheel radii
const double& r_r = _calib(1);
- const double& d = _calib(2); // wheel separation
- double dl = k * (_data(1) * r_r + _data(0) * r_l) / 2.0;
- double dth = k * (_data(1) * r_r - _data(0) * r_l) / d;
-
+ const double& d = _calib(2); // wheel separation
+ double dl = k * (_data(1) * r_r + _data(0) * r_l) / 2.0;
+ double dth = k * (_data(1) * r_r - _data(0) * r_l) / d;
/// 1. Tangent space: calibrate and go to se2 tangent -----------------------------------------------
- Vector3d tangent(dl, 0, dth); // second value 'ds' is wheel sideways motion, supposed zero. Will have a covariance for allowing skidding.
+ Vector3d tangent(dl, 0, dth); // second value 'ds' is wheel sideways motion, supposed zero. Will have a covariance
+ // for allowing skidding.
Matrix3d J_tangent_calib;
- J_tangent_calib(0,0) = k * _data(0) / 2.0; // d dl / d r_l
- J_tangent_calib(0,1) = k * _data(1) / 2.0; // d dl / d r_r
- J_tangent_calib(0,2) = 0.0; // d dl / d d
- J_tangent_calib(1,0) = 0.0; // d ds / d r_l
- J_tangent_calib(1,1) = 0.0; // d ds / d r_r
- J_tangent_calib(1,2) = 0.0; // d ds / d d
- J_tangent_calib(2,0) = - k * _data(0) / d; // d dth / d r_l
- J_tangent_calib(2,1) = k * _data(1) / d; // d dth / d r_r
- J_tangent_calib(2,2) = - dth / d; // d dth / d d
+ J_tangent_calib(0, 0) = k * _data(0) / 2.0; // d dl / d r_l
+ J_tangent_calib(0, 1) = k * _data(1) / 2.0; // d dl / d r_r
+ J_tangent_calib(0, 2) = 0.0; // d dl / d d
+ J_tangent_calib(1, 0) = 0.0; // d ds / d r_l
+ J_tangent_calib(1, 1) = 0.0; // d ds / d r_r
+ J_tangent_calib(1, 2) = 0.0; // d ds / d d
+ J_tangent_calib(2, 0) = -k * _data(0) / d; // d dth / d r_l
+ J_tangent_calib(2, 1) = k * _data(1) / d; // d dth / d r_r
+ J_tangent_calib(2, 2) = -dth / d; // d dth / d d
Matrix<double, 3, 2> J_tangent_data;
- J_tangent_data(0,0) = k * r_l / 2.0; // d dl / d data(0)
- J_tangent_data(0,1) = k * r_r / 2.0; // d dl / d data(1)
- J_tangent_data(1,0) = 0.0; // d ds / d data(0)
- J_tangent_data(1,1) = 0.0; // d ds / d data(1)
- J_tangent_data(2,0) = - k * r_l / d; // d dth / d data(0)
- J_tangent_data(2,1) = k * r_r / d; // d dth / d data(1)
+ J_tangent_data(0, 0) = k * r_l / 2.0; // d dl / d data(0)
+ J_tangent_data(0, 1) = k * r_r / 2.0; // d dl / d data(1)
+ J_tangent_data(1, 0) = 0.0; // d ds / d data(0)
+ J_tangent_data(1, 1) = 0.0; // d ds / d data(1)
+ J_tangent_data(2, 0) = -k * r_l / d; // d dth / d data(0)
+ J_tangent_data(2, 1) = k * r_r / d; // d dth / d data(1)
Matrix<double, 3, 1> J_tangent_side;
- J_tangent_side(0,0) = 0.0; // d dl / d ds
- J_tangent_side(1,0) = 1.0; // d ds / d ds
- J_tangent_side(2,0) = 0.0; // d dth / d ds
+ J_tangent_side(0, 0) = 0.0; // d dl / d ds
+ J_tangent_side(1, 0) = 1.0; // d ds / d ds
+ J_tangent_side(2, 0) = 0.0; // d dth / d ds
/// 2. Current delta: go to SE2 manifold -----------------------------------------------
@@ -119,7 +116,6 @@ void ProcessorDiffDrive::computeCurrentDelta(const Eigen::VectorXd& _data,
SE2::exp(tangent, _delta, J_delta_tangent);
-
/// 3. delta covariance -----------------------------------------------
// Compose Jacobians -- chain rule
@@ -127,32 +123,26 @@ void ProcessorDiffDrive::computeCurrentDelta(const Eigen::VectorXd& _data,
Matrix<double, 3, 1> J_delta_side = J_delta_tangent * J_tangent_side;
// Propagate covariance
- _delta_cov = J_delta_data * _data_cov * J_delta_data.transpose()
- + J_delta_side * unmeasured_perturbation_cov_ * J_delta_side.transpose();
+ _delta_cov = J_delta_data * _data_cov * J_delta_data.transpose() +
+ J_delta_side * unmeasured_perturbation_cov_ * J_delta_side.transpose();
/// 4. Jacobian of delta wrt calibration params -----------------------------------------------
_jacobian_calib = J_delta_tangent * J_tangent_calib;
-
}
-
-CaptureMotionPtr ProcessorDiffDrive::emplaceCapture(const FrameBasePtr& _frame_own,
- const SensorBasePtr& _sensor,
- const TimeStamp& _ts,
- const VectorXd& _data,
- const MatrixXd& _data_cov,
- const VectorXd& _calib,
- const VectorXd& _calib_preint,
+CaptureMotionPtr ProcessorDiffDrive::emplaceCapture(const FrameBasePtr& _frame_own,
+ const SensorBasePtr& _sensor,
+ const TimeStamp& _ts,
+ const VectorXd& _data,
+ const MatrixXd& _data_cov,
+ const VectorXd& _calib,
+ const VectorXd& _calib_preint,
const CaptureBasePtr& _capture_origin)
{
- auto cap_motion = CaptureBase::emplace<CaptureDiffDrive>(_frame_own,
- _ts,
- _sensor,
- _data,
- _data_cov,
- _capture_origin);
- setCalibration (cap_motion, _calib);
+ auto cap_motion =
+ CaptureBase::emplace<CaptureDiffDrive>(_frame_own, _ts, _sensor, _data, _data_cov, _capture_origin);
+ setCalibration(cap_motion, _calib);
cap_motion->setCalibrationPreint(_calib_preint);
return cap_motion;
@@ -167,15 +157,11 @@ void ProcessorDiffDrive::emplaceFeaturesAndFactors(CaptureBasePtr _capture_origi
_capture_own->getBuffer().back().jacobian_calib_);
auto ftr_motion = std::static_pointer_cast<FeatureMotion>(feature);
- FactorBase::emplace<FactorDiffDrive>(ftr_motion,
- ftr_motion,
- _capture_origin,
- shared_from_this(),
- params_prc_diff_drv_selfcal_->apply_loss_function);
+ FactorBase::emplace<FactorDiffDrive>(
+ ftr_motion, ftr_motion, _capture_origin, shared_from_this(), this->applyLossFunction());
}
// Register in the FactoryProcessor
WOLF_REGISTER_PROCESSOR(ProcessorDiffDrive);
-} // namespace wolf
-
+} // namespace wolf
diff --git a/src/processor/processor_fixed_wing_model.cpp b/src/processor/processor_fixed_wing_model.cpp
index 714e55aa37cb3483f5b1aab6498c99dafee0fb89..582b6e73f54152dc4532687d6870d8362c8530de 100644
--- a/src/processor/processor_fixed_wing_model.cpp
+++ b/src/processor/processor_fixed_wing_model.cpp
@@ -26,51 +26,49 @@
namespace wolf
{
-
-ProcessorFixedWingModel::ProcessorFixedWingModel(ParamsProcessorFixedWingModelPtr _params) :
- ProcessorBase("ProcessorFixedWingModel", 3, _params),
- params_processor_(_params)
+ProcessorFixedWingModel::ProcessorFixedWingModel(const YAML::Node& _params)
+ : ProcessorBase("ProcessorFixedWingModel", 3, _params),
+ velocity_local_(_params["velocity_local"].as<Eigen::Vector3d>()),
+ angle_stdev_(_params["angle_stdev"].as<double>()),
+ min_vel_norm_(_params["min_vel_norm"].as<double>())
{
+ assert(std::abs(velocity_local_.norm() - 1.0) < wolf::Constants::EPS &&
+ "ParamsProcessorFixedWingModel: 'velocity_local' must be normalized");
}
-ProcessorFixedWingModel::~ProcessorFixedWingModel()
-{
-}
+ProcessorFixedWingModel::~ProcessorFixedWingModel() {}
void ProcessorFixedWingModel::configure(SensorBasePtr _sensor)
{
- assert(_sensor->getProblem()->getFrameKeys().find('V') != std::string::npos && "Processor only works with problems with V");
+ assert(_sensor->getProblem()->getFrameKeys().find('V') != std::string::npos &&
+ "Processor only works with problems with V");
}
void ProcessorFixedWingModel::processKeyFrame(FrameBasePtr _keyframe_ptr)
{
- if (_keyframe_ptr->getV()->isFixed())
- return;
+ if (_keyframe_ptr->getV()->isFixed()) return;
- if (not _keyframe_ptr->getFactorsOf(shared_from_this()).empty())
- return;
+ if (not _keyframe_ptr->getFactorsOf(shared_from_this()).empty()) return;
// emplace capture
- auto cap = CaptureBase::emplace<CaptureBase>(_keyframe_ptr, "CaptureBase",
- _keyframe_ptr->getTimeStamp(), getSensor());
-
+ auto cap =
+ CaptureBase::emplace<CaptureBase>(_keyframe_ptr, "CaptureBase", _keyframe_ptr->getTimeStamp(), getSensor());
+
// emplace feature
- auto fea = FeatureBase::emplace<FeatureBase>(cap, "FeatureBase",
- params_processor_->velocity_local,
- Eigen::Matrix1d(params_processor_->angle_stdev * params_processor_->angle_stdev));
-
+ auto fea = FeatureBase::emplace<FeatureBase>(
+ cap, "FeatureBase", velocity_local_, Eigen::Matrix1d(angle_stdev_ * angle_stdev_));
+
// emplace factor
auto fac = FactorBase::emplace<FactorVelocityLocalDirection3d>(fea,
fea->getMeasurement(),
fea->getMeasurementSquareRootInformationUpper(),
_keyframe_ptr,
- params_processor_->min_vel_norm,
+ min_vel_norm_,
shared_from_this(),
- params_processor_->apply_loss_function);
+ applyLossFunction());
}
// Register in the FactoryProcessor
WOLF_REGISTER_PROCESSOR(ProcessorFixedWingModel);
-} // namespace wolf
-
+} // namespace wolf
diff --git a/src/processor/processor_landmark_external.cpp b/src/processor/processor_landmark_external.cpp
index 0142f9b1f0d870cb5bc768c331ac8074d15cbf2f..53021d8cc49b9a08bd29e9b0f4c07559dc56e01b 100644
--- a/src/processor/processor_landmark_external.cpp
+++ b/src/processor/processor_landmark_external.cpp
@@ -36,7 +36,6 @@ using namespace Eigen;
namespace wolf
{
-
void ProcessorLandmarkExternal::preProcess()
{
new_features_incoming_.clear();
@@ -48,7 +47,8 @@ void ProcessorLandmarkExternal::preProcess()
"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();
+ WOLF_DEBUG("ProcessorLandmarkExternal::preprocess: landmark_detections ", landmark_detections.size());
std::set<int> ids;
for (auto detection : landmark_detections)
{
@@ -56,12 +56,12 @@ void ProcessorLandmarkExternal::preProcess()
"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 < filter_quality_th_ or ids.count(detection.id)) continue;
// measure and covariance
VectorXd meas;
MatrixXd cov;
- if (not params_tfle_->use_orientation)
+ if (not use_orientation_)
{
assert(detection.measure.size() >= dim);
assert(detection.covariance.rows() >= dim and detection.covariance.rows() == detection.covariance.cols());
@@ -112,8 +112,7 @@ unsigned int ProcessorLandmarkExternal::processKnown()
while (feat_candidate_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)
+ detectionDistance(feat_last, (*feat_candidate_it), pose_in, pose_out, pose_sen) < match_dist_th_)
{
// grow track
track_matrix_.add(feat_last, *feat_candidate_it);
@@ -204,7 +203,7 @@ bool ProcessorLandmarkExternal::voteForKeyFrame() const
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) >= filter_track_length_th_)
{
n_tracks++;
if (not lmks_ids_origin_.count(track_matrix_.feature(track_id, last_ptr_)->landmarkId())) n_new_tracks++;
@@ -212,38 +211,38 @@ bool ProcessorLandmarkExternal::voteForKeyFrame() const
}
// Necessary condition: active valid tracks
- bool vote_min_features = n_tracks >= params_tfle_->min_features_for_keyframe;
+ bool vote_min_features = n_tracks >= min_features_for_keyframe_;
WOLF_DEBUG("vote_min_features: ",
vote_min_features,
" - Active feature tracks longer than ",
- params_tfle_->filter_track_length_th,
+ filter_track_length_th_,
": ",
n_tracks,
" (should be equal or bigger than ",
- params_tfle_->min_features_for_keyframe,
+ 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;
+ vote_new_features = n_new_tracks >= 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,
+ new_features_for_keyframe_,
")");
// Sufficient condition: time span
- vote_time_span = last_ptr_->getTimeStamp() - origin_ptr_->getTimeStamp() > params_tfle_->time_span;
+ vote_time_span = last_ptr_->getTimeStamp() - origin_ptr_->getTimeStamp() > 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,
+ time_span_,
")");
}
@@ -262,14 +261,14 @@ void ProcessorLandmarkExternal::establishFactors()
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)
+ // (filter_track_length_th_)
FactorBasePtrList fac_list;
auto track_ids_last = track_matrix_.trackIds(last_ptr_);
for (auto track_id : track_ids_last)
{
// check track length
- if (track_matrix_.trackSize(track_id) < params_tfle_->filter_track_length_th) continue;
+ if (track_matrix_.trackSize(track_id) < filter_track_length_th_) continue;
auto feature = track_matrix_.feature(track_id, last_ptr_);
@@ -299,7 +298,7 @@ FactorBasePtr ProcessorLandmarkExternal::emplaceFactor(FeatureBasePtr _feature,
assert(_landmark);
// 2D - Relative Position
- if (getProblem()->getDim() == 2 and (_feature->getMeasurement().size() == 2 or not params_tfle_->use_orientation))
+ if (getProblem()->getDim() == 2 and (_feature->getMeasurement().size() == 2 or not use_orientation_))
{
return FactorBase::emplace<FactorRelativePosition2dWithExtrinsics>(
_feature,
@@ -309,10 +308,10 @@ FactorBasePtr ProcessorLandmarkExternal::emplaceFactor(FeatureBasePtr _feature,
_landmark,
_feature->getCapture()->getSensor(),
shared_from_this(),
- params_tfle_->apply_loss_function);
+ applyLossFunction());
}
// 2D - Relative Pose
- else if (getProblem()->getDim() == 2 and _feature->getMeasurement().size() == 3 and params_tfle_->use_orientation)
+ else if (getProblem()->getDim() == 2 and _feature->getMeasurement().size() == 3 and use_orientation_)
{
return FactorBase::emplace<FactorRelativePose2dWithExtrinsics>(
_feature,
@@ -322,12 +321,11 @@ FactorBasePtr ProcessorLandmarkExternal::emplaceFactor(FeatureBasePtr _feature,
_landmark,
_feature->getCapture()->getSensor(),
shared_from_this(),
- params_tfle_->apply_loss_function,
+ applyLossFunction(),
TOP_LMK);
}
// 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 use_orientation_))
{
return FactorBase::emplace<FactorRelativePosition3dWithExtrinsics>(
_feature,
@@ -337,10 +335,10 @@ FactorBasePtr ProcessorLandmarkExternal::emplaceFactor(FeatureBasePtr _feature,
_landmark,
_feature->getCapture()->getSensor(),
shared_from_this(),
- params_tfle_->apply_loss_function);
+ applyLossFunction());
}
// 3D - Relative Pose
- else if (getProblem()->getDim() == 3 and _feature->getMeasurement().size() == 7 and params_tfle_->use_orientation)
+ else if (getProblem()->getDim() == 3 and _feature->getMeasurement().size() == 7 and use_orientation_)
{
return FactorBase::emplace<FactorRelativePose3dWithExtrinsics>(
_feature,
@@ -350,7 +348,7 @@ FactorBasePtr ProcessorLandmarkExternal::emplaceFactor(FeatureBasePtr _feature,
_landmark,
_feature->getCapture()->getSensor(),
shared_from_this(),
- params_tfle_->apply_loss_function,
+ applyLossFunction(),
TOP_LMK);
}
else
@@ -377,7 +375,7 @@ LandmarkBasePtr ProcessorLandmarkExternal::emplaceLandmark(FeatureBasePtr _featu
LandmarkBasePtr lmk;
// 2D - Relative Position
- if (getProblem()->getDim() == 2 and (_feature->getMeasurement().size() == 2 or not params_tfle_->use_orientation))
+ if (getProblem()->getDim() == 2 and (_feature->getMeasurement().size() == 2 or not use_orientation_))
{
Vector2d frm_p = _feature->getCapture()->getFrame()->getP()->getState();
Vector2d sen_p = _feature->getCapture()->getSensorP()->getState();
@@ -391,7 +389,7 @@ LandmarkBasePtr ProcessorLandmarkExternal::emplaceLandmark(FeatureBasePtr _featu
lmk->setId(_feature->landmarkId());
}
// 2D - Relative Pose
- else if (getProblem()->getDim() == 2 and _feature->getMeasurement().size() == 3 and params_tfle_->use_orientation)
+ else if (getProblem()->getDim() == 2 and _feature->getMeasurement().size() == 3 and use_orientation_)
{
Vector2d frm_p = _feature->getCapture()->getFrame()->getP()->getState();
Vector2d sen_p = _feature->getCapture()->getSensorP()->getState();
@@ -409,8 +407,7 @@ LandmarkBasePtr ProcessorLandmarkExternal::emplaceLandmark(FeatureBasePtr _featu
lmk->setId(_feature->landmarkId());
}
// 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 use_orientation_))
{
Vector3d frm_p = _feature->getCapture()->getFrame()->getP()->getState();
Vector3d sen_p = _feature->getCapture()->getSensorP()->getState();
@@ -424,7 +421,7 @@ LandmarkBasePtr ProcessorLandmarkExternal::emplaceLandmark(FeatureBasePtr _featu
lmk->setId(_feature->landmarkId());
}
// 3D - Relative Pose
- else if (getProblem()->getDim() == 3 and _feature->getMeasurement().size() == 7 and params_tfle_->use_orientation)
+ else if (getProblem()->getDim() == 3 and _feature->getMeasurement().size() == 7 and use_orientation_)
{
Vector3d frm_p = _feature->getCapture()->getFrame()->getP()->getState();
Vector3d sen_p = _feature->getCapture()->getSensorP()->getState();
@@ -458,13 +455,13 @@ void ProcessorLandmarkExternal::modifyLandmark(LandmarkBasePtr _landmark, Featur
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))
+ if (getProblem()->getDim() == 2 and (_feature->getMeasurement().size() == 2 or not use_orientation_))
{
// nothing to do (we assume P already in landmark)
return;
}
// 2D - Relative Pose
- else if (getProblem()->getDim() == 2 and _feature->getMeasurement().size() == 3 and params_tfle_->use_orientation)
+ else if (getProblem()->getDim() == 2 and _feature->getMeasurement().size() == 3 and use_orientation_)
{
// no orientation, add it
if (not _landmark->getO())
@@ -478,14 +475,13 @@ void ProcessorLandmarkExternal::modifyLandmark(LandmarkBasePtr _landmark, Featur
}
}
// 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 use_orientation_))
{
// nothing to do (we assume P already in landmark)
return;
}
// 3D - Relative Pose
- else if (getProblem()->getDim() == 3 and _feature->getMeasurement().size() == 7 and params_tfle_->use_orientation)
+ else if (getProblem()->getDim() == 3 and _feature->getMeasurement().size() == 7 and use_orientation_)
{
// no orientation, add it
if (not _landmark->getO())
diff --git a/src/processor/processor_loop_closure.cpp b/src/processor/processor_loop_closure.cpp
index b2a10b52d8f18e4ac73ba52fdbe377af538e8156..90aaaa8970fbac2dcf6800509e978c6c29a5b8b7 100644
--- a/src/processor/processor_loop_closure.cpp
+++ b/src/processor/processor_loop_closure.cpp
@@ -22,12 +22,8 @@
namespace wolf
{
-
-ProcessorLoopClosure::ProcessorLoopClosure(const std::string& _type,
- int _dim,
- ParamsProcessorLoopClosurePtr _params_loop_closure):
- ProcessorBase(_type, _dim, _params_loop_closure),
- params_loop_closure_(_params_loop_closure)
+ProcessorLoopClosure::ProcessorLoopClosure(const std::string& _type, int _dim, const YAML::Node& _params)
+ : ProcessorBase(_type, _dim, _params)
{
//
}
@@ -37,7 +33,8 @@ void ProcessorLoopClosure::processCapture(CaptureBasePtr _capture)
/* This function has 3 scenarios:
* 1. Capture already linked to a frame (in trajectory) -> process
* 2. Capture has a timestamp compatible with any stored frame -> link + process
- * 3. Otherwise -> store capture (Note that more than one processor can be emplacing frames, so an older frame can arrive later than this one)
+ * 3. Otherwise -> store capture (Note that more than one processor can be emplacing frames, so an older frame can
+ * arrive later than this one)
*/
WOLF_DEBUG("ProcessorLoopClosure::processCapture capture ", _capture->id());
@@ -56,7 +53,7 @@ void ProcessorLoopClosure::processCapture(CaptureBasePtr _capture)
}
// Search for any stored frame within time tolerance of capture
- auto keyframe_from_callback = buffer_frame_.select(_capture->getTimeStamp(), params_->time_tolerance);
+ auto keyframe_from_callback = buffer_frame_.select(_capture->getTimeStamp(), getTimeTolerance());
// CASE 2:
if (_capture->getFrame() == nullptr and keyframe_from_callback)
@@ -103,7 +100,7 @@ void ProcessorLoopClosure::processKeyFrame(FrameBasePtr _keyframe)
}
// Search for any stored capture within time tolerance of frame
- capture = buffer_capture_.select(_keyframe->getTimeStamp(), params_->time_tolerance);
+ capture = buffer_capture_.select(_keyframe->getTimeStamp(), getTimeTolerance());
// CASE 2:
if (capture and not capture->getFrame())
@@ -123,7 +120,7 @@ void ProcessorLoopClosure::processKeyFrame(FrameBasePtr _keyframe)
return;
}
// CASE 3:
- if (buffer_capture_.selectLastAfter(_keyframe->getTimeStamp(), params_->time_tolerance) == nullptr)
+ if (buffer_capture_.selectLastAfter(_keyframe->getTimeStamp(), getTimeTolerance()) == nullptr)
{
WOLF_DEBUG("CASE 3");
@@ -164,11 +161,9 @@ void ProcessorLoopClosure::process(CaptureBasePtr _capture)
emplaceFactors(match_pair.second);
n_loops++;
- if (params_loop_closure_->max_loops > 0 and
- n_loops >= params_loop_closure_->max_loops)
- break;
+ if (getMaxLoops() > 0 and n_loops >= getMaxLoops()) break;
}
}
}
-}// namespace wolf
+} // namespace wolf
diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp
index c7ef59ea54a2a5c1e9458c3fc52130ffa3aaaf04..2da24c1143bbdf130354866e9bb954e715adf147 100644
--- a/src/processor/processor_motion.cpp
+++ b/src/processor/processor_motion.cpp
@@ -23,70 +23,65 @@
namespace wolf
{
-
ProcessorMotion::ProcessorMotion(const std::string& _type,
- TypeComposite _state_types,
- int _dim,
- SizeEigen _state_size,
- SizeEigen _delta_size,
- SizeEigen _delta_cov_size,
- SizeEigen _data_size,
- SizeEigen _calib_size,
- ParamsProcessorMotionPtr _params_motion) :
- ProcessorBase(_type, _dim, _params_motion),
- MotionProvider(_state_types, _params_motion),
- params_motion_(_params_motion),
- processing_step_(RUNNING_WITHOUT_KF),
- bootstrapping_(false),
- x_size_(_state_size),
- data_size_(_data_size),
- delta_size_(_delta_size),
- delta_cov_size_(_delta_cov_size),
- calib_size_(_calib_size),
- origin_ptr_(),
- last_ptr_(),
- incoming_ptr_(),
- last_frame_ptr_(),
- dt_(0.0),
- x_(_state_size),
- delta_(_delta_size),
- delta_cov_(_delta_cov_size, _delta_cov_size),
- delta_integrated_(_delta_size),
- delta_integrated_cov_(_delta_cov_size, _delta_cov_size),
- calib_preint_(_calib_size),
- jacobian_delta_preint_(delta_cov_size_, delta_cov_size_),
- jacobian_delta_(delta_cov_size_, delta_cov_size_),
- jacobian_calib_(delta_cov_size_, calib_size_),
- jacobian_delta_calib_(delta_cov_size_, calib_size_)
-{
- jacobian_delta_preint_ .setIdentity(delta_cov_size_,delta_cov_size_); // dDp'/dDp, dDv'/dDv, all zeros
- jacobian_delta_ .setIdentity(delta_cov_size_,delta_cov_size_); //
- jacobian_calib_ .setZero(delta_cov_size_,calib_size_);
- jacobian_delta_calib_ .setZero(delta_cov_size_,calib_size_);
- unmeasured_perturbation_cov_ =
- params_motion_->unmeasured_perturbation_std
- * params_motion_->unmeasured_perturbation_std
- * MatrixXd::Identity(delta_cov_size_, delta_cov_size_);
+ TypeComposite _state_types,
+ int _dim,
+ SizeEigen _state_size,
+ SizeEigen _delta_size,
+ SizeEigen _delta_cov_size,
+ SizeEigen _data_size,
+ SizeEigen _calib_size,
+ const YAML::Node& _params)
+ : ProcessorBase(_type, _dim, _params),
+ MotionProvider(_state_types, _params),
+ processing_step_(RUNNING_WITHOUT_KF),
+ bootstrapping_(false),
+ x_size_(_state_size),
+ data_size_(_data_size),
+ delta_size_(_delta_size),
+ delta_cov_size_(_delta_cov_size),
+ calib_size_(_calib_size),
+ origin_ptr_(),
+ last_ptr_(),
+ incoming_ptr_(),
+ last_frame_ptr_(),
+ dt_(0.0),
+ x_(_state_size),
+ delta_(_delta_size),
+ delta_cov_(_delta_cov_size, _delta_cov_size),
+ delta_integrated_(_delta_size),
+ delta_integrated_cov_(_delta_cov_size, _delta_cov_size),
+ calib_preint_(_calib_size),
+ jacobian_delta_preint_(delta_cov_size_, delta_cov_size_),
+ jacobian_delta_(delta_cov_size_, delta_cov_size_),
+ jacobian_calib_(delta_cov_size_, calib_size_),
+ jacobian_delta_calib_(delta_cov_size_, calib_size_)
+{
+ jacobian_delta_preint_.setIdentity(delta_cov_size_, delta_cov_size_); // dDp'/dDp, dDv'/dDv, all zeros
+ jacobian_delta_.setIdentity(delta_cov_size_, delta_cov_size_); //
+ jacobian_calib_.setZero(delta_cov_size_, calib_size_);
+ jacobian_delta_calib_.setZero(delta_cov_size_, calib_size_);
+ unmeasured_perturbation_cov_ = pow(params_["unmeasured_perturbation_std"].as<double>(), 2) *
+ MatrixXd::Identity(delta_cov_size_, delta_cov_size_);
}
ProcessorMotion::~ProcessorMotion()
{
-// std::cout << "destructed -p-Mot" << id() << std::endl;
+ // std::cout << "destructed -p-Mot" << id() << std::endl;
}
-void ProcessorMotion::mergeCaptures(CaptureMotionPtr cap_prev,
- CaptureMotionPtr cap_target)
+void ProcessorMotion::mergeCaptures(CaptureMotionPtr cap_prev, CaptureMotionPtr cap_target)
{
- assertsCaptureMotion(cap_prev, "ProcessorMotion::mergeCaptures: cap_prev");
+ 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");
+ 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();
- cap_target->getBuffer().insert(cap_target->getBuffer().begin(),
- cap_prev->getBuffer().begin(),
- cap_prev->getBuffer().end());
+ cap_target->getBuffer().insert(
+ cap_target->getBuffer().begin(), cap_prev->getBuffer().begin(), cap_prev->getBuffer().end());
// change origin
cap_target->setOriginCapture(cap_prev->getOriginCapture());
@@ -103,7 +98,7 @@ void ProcessorMotion::mergeCaptures(CaptureMotionPtr cap_prev,
// remove feature and factor (automatically)
cap_target->getFeatureList().back()->remove();
- assert(cap_target->getFeatureList().empty());// there should be one feature only
+ assert(cap_target->getFeatureList().empty()); // there should be one feature only
}
// emplace new feature and factor (if origin has frame)
@@ -114,7 +109,7 @@ void ProcessorMotion::mergeCaptures(CaptureMotionPtr cap_prev,
}
void ProcessorMotion::splitBuffer(const CaptureMotionPtr& _capture_source,
- const TimeStamp _ts_split,
+ const TimeStamp _ts_split,
const CaptureMotionPtr& _capture_target) const
{
/** we are doing this:
@@ -142,7 +137,9 @@ void ProcessorMotion::splitBuffer(const CaptureMotionPtr& _capture_source,
}
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!");
+ WOLF_ERROR(
+ "ProcessorMotion::splitBuffer: _capture_source->getOriginCapture().ts == ts_split within tolerance, "
+ "shouldn't split!");
}
if (_capture_source->getBuffer().front().ts_ >= _ts_split)
{
@@ -158,16 +155,18 @@ void ProcessorMotion::splitBuffer(const CaptureMotionPtr& _capture_source,
}
// 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();
+ 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
_capture_source->getBuffer().split(_ts_split, _capture_target->getBuffer());
// start with empty motion
- TimeStamp t_zero = _capture_target->getBuffer().back().ts_; // last tick of previous buffer is zero tick of current buffer
+ TimeStamp t_zero =
+ _capture_target->getBuffer().back().ts_; // last tick of previous buffer is zero tick of current buffer
_capture_source->getBuffer().push_front(motionZero(t_zero));
// Update the existing capture
@@ -198,69 +197,62 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
}
incoming_ptr_ = std::dynamic_pointer_cast<CaptureMotion>(_incoming_ptr);
- assert(incoming_ptr_ != nullptr && ("Capture type mismatch. Processor " + getName() + " can only process captures of type CaptureMotion").c_str());
+ assert(incoming_ptr_ != nullptr &&
+ ("Capture type mismatch. Processor " + getName() + " can only process captures of type CaptureMotion")
+ .c_str());
// if origin_ was removed reset (let it die)
if (origin_ptr_ and origin_ptr_->isRemoving())
{
origin_ptr_ = nullptr;
- if (last_ptr_)
- last_ptr_->remove();
+ if (last_ptr_) last_ptr_->remove();
}
- preProcess(); // Derived class operations
+ preProcess(); // Derived class operations
FrameBasePtr keyframe_from_callback = computeProcessingStep();
- if (keyframe_from_callback)
- buffer_frame_.removeUpTo( keyframe_from_callback->getTimeStamp() );
-
- switch(processing_step_) // Things to do before integrating motion data
+ if (keyframe_from_callback) buffer_frame_.removeUpTo(keyframe_from_callback->getTimeStamp());
+
+ switch (processing_step_) // Things to do before integrating motion data
{
- case FIRST_TIME_WITHOUT_KF :
- {
+ case FIRST_TIME_WITHOUT_KF: {
// There is no KF: create own origin
setOrigin(getProblem()->stateZero(getStateKeys()), _incoming_ptr->getTimeStamp());
break;
}
- case FIRST_TIME_WITH_KF_BEFORE_INCOMING :
- {
+ case FIRST_TIME_WITH_KF_BEFORE_INCOMING: {
// cannot join to the KF: create own origin
setOrigin(getProblem()->getState(getStateKeys()), _incoming_ptr->getTimeStamp());
break;
}
- case FIRST_TIME_WITH_KF_ON_INCOMING :
- {
+ case FIRST_TIME_WITH_KF_ON_INCOMING: {
// can join to the KF
setOrigin(keyframe_from_callback);
break;
}
- case FIRST_TIME_WITH_KF_AFTER_INCOMING :
- {
+ case FIRST_TIME_WITH_KF_AFTER_INCOMING: {
// cannot join to the KF: create own origin
setOrigin(getProblem()->getState(getStateKeys()), _incoming_ptr->getTimeStamp());
break;
}
- case RUNNING_WITHOUT_KF :
- case RUNNING_WITH_KF_ON_ORIGIN :
+ case RUNNING_WITHOUT_KF:
+ case RUNNING_WITH_KF_ON_ORIGIN:
break;
- default :
+ default:
break;
}
-
// integrate motion data
- // Done at this place because setPrior() needs
+ // Done at this place because setPrior() needs
integrateOneStep();
// perform bootstrap steps (usually only IMU requires this)
if (bootstrapping_) bootstrap();
- switch (processing_step_) // Things to do after integrating motion data
+ switch (processing_step_) // Things to do after integrating motion data
{
- case RUNNING_WITH_KF_BEFORE_ORIGIN :
- {
-
+ case RUNNING_WITH_KF_BEFORE_ORIGIN: {
/*
* Legend:
* * : any frame or keyframe
@@ -292,11 +284,14 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
TimeStamp timestamp_from_callback = keyframe_from_callback->getTimeStamp();
// find the capture whose buffer is affected by the new keyframe
- auto capture_existing = findCaptureContainingTimeStamp(timestamp_from_callback);
+ auto capture_existing = findCaptureContainingTimeStamp(timestamp_from_callback);
if (not capture_existing)
{
- WOLF_WARN(getName(), ": Cannot join KF. The received KF (TS = ", timestamp_from_callback, ") is older than the first motion capture.");
+ WOLF_WARN(getName(),
+ ": Cannot join KF. The received KF (TS = ",
+ timestamp_from_callback,
+ ") is older than the first motion capture.");
break;
}
@@ -304,32 +299,35 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
auto proc_state = getState(timestamp_from_callback);
for (auto pair_key_vec : proc_state)
if (!keyframe_from_callback->has(pair_key_vec.first))
- keyframe_from_callback->addStateBlock(pair_key_vec.first,
- FactoryStateBlock::create(std::string(1, pair_key_vec.first),
- pair_key_vec.second,
- false));
+ keyframe_from_callback->addStateBlock(
+ pair_key_vec.first,
+ FactoryStateBlock::create(std::string(1, pair_key_vec.first), pair_key_vec.second, false));
keyframe_from_callback->setState(proc_state);
// Find the capture acting as the buffer's origin
- auto capture_origin = capture_existing->getOriginCapture();
+ 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.");
+ 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);
+ VectorXd calib_origin = getCalibration(capture_origin);
// emplace a new motion capture to the new keyframe
- auto capture_for_keyframe_callback = emplaceCapture(keyframe_from_callback, // j
- getSensor(),
- timestamp_from_callback,
- Eigen::VectorXd::Zero(data_size_),
- getSensor()->computeNoiseCov(Eigen::VectorXd::Zero(data_size_)),
- calib_origin,
- calib_origin,
- capture_origin);
+ auto capture_for_keyframe_callback =
+ emplaceCapture(keyframe_from_callback, // j
+ getSensor(),
+ timestamp_from_callback,
+ Eigen::VectorXd::Zero(data_size_),
+ getSensor()->computeNoiseCov(Eigen::VectorXd::Zero(data_size_)),
+ calib_origin,
+ calib_origin,
+ capture_origin);
// split the buffer
// and give the part of the buffer before the new keyframe to the capture for the KF callback
@@ -342,10 +340,11 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
// modify existing feature and factor (if they exist in the existing capture)
if (!capture_existing->getFeatureList().empty())
{
- // setMeasurement is not allowed: feature (and factor) will be removed and a new feature and factor will be emplaced
- capture_existing->getFeatureList().back()->remove(); // factor is removed automatically
+ // setMeasurement is not allowed: feature (and factor) will be removed and a new feature and factor
+ // will be emplaced
+ capture_existing->getFeatureList().back()->remove(); // factor is removed automatically
- assert(capture_existing->getFeatureList().empty());// there was only one feature!
+ assert(capture_existing->getFeatureList().empty()); // there was only one feature!
emplaceFeaturesAndFactors(capture_for_keyframe_callback, capture_existing);
}
@@ -353,8 +352,7 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
break;
}
- case RUNNING_WITH_KF_AFTER_ORIGIN :
- {
+ case RUNNING_WITH_KF_AFTER_ORIGIN: {
/*
* Legend:
* * : any frame or keyframe
@@ -395,29 +393,30 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
auto proc_state = this->getState(timestamp_from_callback);
for (auto pair_key_vector : proc_state)
if (!keyframe_from_callback->has(pair_key_vector.first))
- keyframe_from_callback->addStateBlock(pair_key_vector.first,
- FactoryStateBlock::create(std::string(1, pair_key_vector.first),
- pair_key_vector.second,
- false));
+ keyframe_from_callback->addStateBlock(
+ pair_key_vector.first,
+ FactoryStateBlock::create(
+ std::string(1, pair_key_vector.first), pair_key_vector.second, false));
keyframe_from_callback->setState(proc_state);
- auto & capture_existing = last_ptr_;
+ auto& capture_existing = last_ptr_;
// Find the capture acting as the buffer's origin
- auto & capture_origin = origin_ptr_;
+ auto& capture_origin = origin_ptr_;
// Get calibration params for preintegration from origin, since it has chances to have optimized values
- VectorXd calib_origin = getCalibration(capture_origin);
+ VectorXd calib_origin = getCalibration(capture_origin);
// emplace a new motion capture to the new keyframe
- auto capture_for_keyframe_callback = emplaceCapture(keyframe_from_callback,
- getSensor(),
- timestamp_from_callback,
- Eigen::VectorXd::Zero(data_size_),
- getSensor()->computeNoiseCov(Eigen::VectorXd::Zero(data_size_)),
- calib_origin,
- calib_origin,
- capture_origin);
+ auto capture_for_keyframe_callback =
+ emplaceCapture(keyframe_from_callback,
+ getSensor(),
+ timestamp_from_callback,
+ Eigen::VectorXd::Zero(data_size_),
+ getSensor()->computeNoiseCov(Eigen::VectorXd::Zero(data_size_)),
+ calib_origin,
+ calib_origin,
+ capture_origin);
// split the buffer
// and give the part of the buffer before the new keyframe to the capture for the KF callback
@@ -433,23 +432,21 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
break;
}
- default :
+ default:
break;
}
-
// Update state and time stamps
auto ts = getTimeStamp();
- last_ptr_->setTimeStamp( ts );
- last_ptr_->getFrame()->setTimeStamp( ts );
- VectorComposite state_propa = getState( ts );
- for (auto & pair_key_vec : state_propa)
+ last_ptr_->setTimeStamp(ts);
+ last_ptr_->getFrame()->setTimeStamp(ts);
+ VectorComposite state_propa = getState(ts);
+ for (auto& pair_key_vec : state_propa)
if (!last_ptr_->getFrame()->has(pair_key_vec.first))
- last_ptr_->getFrame()->addStateBlock(pair_key_vec.first,
- FactoryStateBlock::create(std::string(1, pair_key_vec.first),
- pair_key_vec.second,
- false));
- last_ptr_->getFrame()->setState( state_propa );
+ last_ptr_->getFrame()->addStateBlock(
+ pair_key_vec.first,
+ FactoryStateBlock::create(std::string(1, pair_key_vec.first), pair_key_vec.second, false));
+ last_ptr_->getFrame()->setState(state_propa);
if (permittedKeyFrame() && voteForKeyFrame())
{
@@ -488,28 +485,26 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
setCalibration(last_ptr_, getCalibration(origin_ptr_));
// Set the frame of last_ptr as key
- auto keyframe = last_ptr_->getFrame();
- keyframe ->link(getProblem());
+ auto keyframe = last_ptr_->getFrame();
+ keyframe->link(getProblem());
// create motion feature and add it to the key_capture
// create motion factor and link it to parent feature and other frame (which is origin's frame)
emplaceFeaturesAndFactors(origin_ptr_, last_ptr_);
// create a new frame
- auto frame_new = std::make_shared<FrameBase>(getTimeStamp(),
- getStateTypes(),
- getProblem()->getState());
+ auto frame_new = std::make_shared<FrameBase>(getTimeStamp(), getStateTypes(), getProblem()->getState());
// create a new capture
- auto capture_new = emplaceCapture(frame_new,
- getSensor(),
- keyframe->getTimeStamp(),
- Eigen::VectorXd::Zero(data_size_),
- getSensor()->computeNoiseCov(Eigen::VectorXd::Zero(data_size_)),
- getCalibration(origin_ptr_),
- getCalibration(origin_ptr_),
- last_ptr_);
+ auto capture_new = emplaceCapture(frame_new,
+ getSensor(),
+ keyframe->getTimeStamp(),
+ Eigen::VectorXd::Zero(data_size_),
+ getSensor()->computeNoiseCov(Eigen::VectorXd::Zero(data_size_)),
+ getCalibration(origin_ptr_),
+ getCalibration(origin_ptr_),
+ last_ptr_);
// reset the new buffer
- capture_new->getBuffer().push_back( motionZero(keyframe->getTimeStamp()) ) ;
+ capture_new->getBuffer().push_back(motionZero(keyframe->getTimeStamp()));
// reset derived things
resetDerived();
@@ -521,32 +516,27 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
// callback to other processors
getProblem()->keyFrameCallback(keyframe, shared_from_this());
-
}
// clear incoming just in case
- incoming_ptr_ = nullptr; // This line is not really needed, but it makes things clearer.
+ incoming_ptr_ = nullptr; // This line is not really needed, but it makes things clearer.
postProcess();
-
}
VectorComposite ProcessorMotion::getState(const StateKeys& _keys) const
{
const StateKeys& keys = (_keys == "" ? getStateKeys() : _keys);
-
- if (origin_ptr_ == nullptr or
- origin_ptr_->isRemoving() or
- last_ptr_ == nullptr or
- last_ptr_->getFrame() == nullptr) // We do not have any info of where to find a valid state
- // Further checking here for origin_ptr is redundant: if last=null, then origin=null too.
+ if (origin_ptr_ == nullptr or origin_ptr_->isRemoving() or last_ptr_ == nullptr or
+ last_ptr_->getFrame() ==
+ nullptr) // We do not have any info of where to find a valid state
+ // Further checking here for origin_ptr is redundant: if last=null, then origin=null too.
{
WOLF_DEBUG("Processor has no state. Returning an empty VectorComposite with no blocks");
- return VectorComposite(); // return empty state
+ return VectorComposite(); // return empty state
}
-
// From here on, we do have info to compute a valid state
// if buffer is empty --> we did not advance from origin!
@@ -582,7 +572,7 @@ VectorComposite ProcessorMotion::getState(const StateKeys& _keys) const
auto calib_preint = last_ptr_->getCalibrationPreint();
VectorComposite state;
- if ( hasCalibration())
+ if (hasCalibration())
{
// Get current calibration -- from origin capture
auto calib = getCalibration(origin_ptr_);
@@ -617,15 +607,12 @@ VectorComposite ProcessorMotion::getState(const StateKeys& _keys) const
pair_key_vec_it = state.erase(pair_key_vec_it);
else
- pair_key_vec_it ++;
-
+ pair_key_vec_it++;
}
return state;
}
}
-
-
// _x needs to have the size of the processor state
VectorComposite ProcessorMotion::getState(const TimeStamp& _ts, const StateKeys& _keys) const
{
@@ -636,14 +623,13 @@ VectorComposite ProcessorMotion::getState(const TimeStamp& _ts, const StateKeys&
// We need to search for the capture containing a motion buffer with the queried time stamp
auto capture_motion = findCaptureContainingTimeStamp(_ts);
- if (capture_motion == nullptr) // we do not have any info of where to find a valid state
+ if (capture_motion == nullptr) // we do not have any info of where to find a valid state
{
WOLF_DEBUG("Processor has no state. Returning an empty VectorComposite with no blocks");
- return VectorComposite(); // return empty state
+ return VectorComposite(); // return empty state
}
-
- else // We found a CaptureMotion whose buffer contains the time stamp
+ else // We found a CaptureMotion whose buffer contains the time stamp
{
// if buffer is empty --> we did not advance from origin!
// this may happen when in the very first frame where the capture has no motion info --> empty buffer
@@ -666,7 +652,7 @@ VectorComposite ProcessorMotion::getState(const TimeStamp& _ts, const StateKeys&
*/
// Get state of origin
- auto cap_orig = capture_motion->getOriginCapture();
+ auto cap_orig = capture_motion->getOriginCapture();
auto x_origin = cap_orig->getFrame()->getState(getStateKeys());
// Get motion at time stamp
@@ -680,7 +666,7 @@ VectorComposite ProcessorMotion::getState(const TimeStamp& _ts, const StateKeys&
VectorComposite state;
- if ( hasCalibration() )
+ if (hasCalibration())
{
// Get current calibration -- from origin capture
auto calib = getCalibration(cap_orig);
@@ -715,21 +701,17 @@ VectorComposite ProcessorMotion::getState(const TimeStamp& _ts, const StateKeys&
pair_key_vec_it = state.erase(pair_key_vec_it);
else
- pair_key_vec_it ++;
-
+ pair_key_vec_it++;
}
return state;
}
-
}
}
FrameBasePtr ProcessorMotion::setOrigin(const VectorComposite& _x_origin, const TimeStamp& _ts_origin)
{
- FrameBasePtr key_frame_ptr = FrameBase::emplace<FrameBase>(getProblem()->getTrajectory(),
- _ts_origin,
- getStateTypes(),
- _x_origin);
+ FrameBasePtr key_frame_ptr =
+ FrameBase::emplace<FrameBase>(getProblem()->getTrajectory(), _ts_origin, getStateTypes(), _x_origin);
setOrigin(key_frame_ptr);
return key_frame_ptr;
@@ -738,8 +720,8 @@ FrameBasePtr ProcessorMotion::setOrigin(const VectorComposite& _x_origin, const
void ProcessorMotion::setOrigin(FrameBasePtr _origin_frame)
{
assert(_origin_frame && "ProcessorMotion::setOrigin: Provided frame pointer is nullptr.");
- assert(_origin_frame->getTrajectory() != nullptr
- && "ProcessorMotion::setOrigin: origin frame must be in the trajectory.");
+ assert(_origin_frame->getTrajectory() != nullptr &&
+ "ProcessorMotion::setOrigin: origin frame must be in the trajectory.");
TimeStamp origin_ts = _origin_frame->getTimeStamp();
@@ -759,10 +741,8 @@ void ProcessorMotion::setOrigin(FrameBasePtr _origin_frame)
// ---------- LAST ----------
// Make non-key-frame for last Capture
- last_frame_ptr_ = std::make_shared<FrameBase>(origin_ts,
- getStateTypes(),
- _origin_frame->getState());
-
+ last_frame_ptr_ = std::make_shared<FrameBase>(origin_ts, getStateTypes(), _origin_frame->getState());
+
// emplace (emtpy) last Capture
last_ptr_ = emplaceCapture(last_frame_ptr_,
getSensor(),
@@ -788,8 +768,7 @@ void ProcessorMotion::integrateOneStep()
// Set dt
dt_ = updateDt();
WOLF_WARN_COND(dt_ < 0, "ProcessorMotion::integrateOneStep: dt < 0, skipping integration");
- if (dt_ < 0)
- return;
+ if (dt_ < 0) return;
assert(dt_ >= 0 && "Time interval _dt is negative!");
// get vector of parameters to calibrate
@@ -805,25 +784,21 @@ void ProcessorMotion::integrateOneStep()
jacobian_delta_calib_);
// integrate the current delta to pre-integrated measurements, and get Jacobians
- deltaPlusDelta(getBuffer().back().delta_integr_,
- delta_,
- dt_,
- delta_integrated_,
- jacobian_delta_preint_,
- jacobian_delta_);
+ deltaPlusDelta(
+ getBuffer().back().delta_integr_, delta_, dt_, delta_integrated_, jacobian_delta_preint_, jacobian_delta_);
// integrate Jacobian wrt calib
- if ( hasCalibration() ) // if no calibration, matrices can have mismatching sizes, and this computation is nevertheless pointless
+ if (hasCalibration()) // if no calibration, matrices can have mismatching sizes, and this computation is
+ // nevertheless pointless
{
- jacobian_calib_.noalias()
- = jacobian_delta_preint_ * getBuffer().back().jacobian_calib_
- + jacobian_delta_ * jacobian_delta_calib_;
+ jacobian_calib_.noalias() =
+ jacobian_delta_preint_ * getBuffer().back().jacobian_calib_ + jacobian_delta_ * jacobian_delta_calib_;
}
// Integrate covariance
- delta_integrated_cov_.noalias()
- = jacobian_delta_preint_ * getBuffer().back().delta_integr_cov_ * jacobian_delta_preint_.transpose()
- + jacobian_delta_ * delta_cov_ * jacobian_delta_.transpose();
+ delta_integrated_cov_.noalias() =
+ jacobian_delta_preint_ * getBuffer().back().delta_integr_cov_ * jacobian_delta_preint_.transpose() +
+ jacobian_delta_ * delta_cov_ * jacobian_delta_.transpose();
// push all into buffer
getBuffer().emplace_back(incoming_ptr_->getTimeStamp(),
@@ -848,12 +823,12 @@ 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();
+ VectorXd calib = _capture_ptr->getCalibrationPreint();
// some temporaries for integration
- delta_integrated_ =deltaZero();
+ delta_integrated_ = deltaZero();
delta_integrated_cov_.setZero();
- jacobian_calib_ .setZero();
+ jacobian_calib_.setZero();
// Iterate all the buffer
auto motion_it = _capture_ptr->getBuffer().begin();
@@ -882,18 +857,19 @@ void ProcessorMotion::reintegrateBuffer(CaptureMotionPtr _capture_ptr) const
motion_it->jacobian_delta_);
// integrate Jacobian wrt calib
- if ( hasCalibration() )
- motion_it->jacobian_calib_ = motion_it->jacobian_delta_integr_ * jacobian_calib_
- + motion_it->jacobian_delta_ * jacobian_delta_calib_;
+ if (hasCalibration())
+ motion_it->jacobian_calib_ = motion_it->jacobian_delta_integr_ * jacobian_calib_ +
+ motion_it->jacobian_delta_ * jacobian_delta_calib_;
// Integrate covariance
- motion_it->delta_integr_cov_ = motion_it->jacobian_delta_integr_ * delta_integrated_cov_ * motion_it->jacobian_delta_integr_.transpose()
- + motion_it->jacobian_delta_ * motion_it->delta_cov_ * motion_it->jacobian_delta_.transpose();
+ motion_it->delta_integr_cov_ =
+ motion_it->jacobian_delta_integr_ * delta_integrated_cov_ * motion_it->jacobian_delta_integr_.transpose() +
+ motion_it->jacobian_delta_ * motion_it->delta_cov_ * motion_it->jacobian_delta_.transpose();
// update temporaries
- delta_integrated_ = motion_it->delta_integr_;
- delta_integrated_cov_ = motion_it->delta_integr_cov_;
- jacobian_calib_ = motion_it->jacobian_calib_;
+ delta_integrated_ = motion_it->delta_integr_;
+ delta_integrated_cov_ = motion_it->delta_integr_cov_;
+ jacobian_calib_ = motion_it->jacobian_calib_;
// advance in buffer
motion_it++;
@@ -906,13 +882,11 @@ CaptureMotionConstPtr ProcessorMotion::findCaptureContainingTimeStamp(const Time
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;
+ // 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
- if (last_ptr_->containsTimeStamp(_ts, this->params_->time_tolerance))
- return last_ptr_;
-
+ if (last_ptr_->containsTimeStamp(_ts, this->getTimeTolerance())) return last_ptr_;
// Then look in the Wolf tree...
// -----------------------------
@@ -923,24 +897,22 @@ CaptureMotionConstPtr ProcessorMotion::findCaptureContainingTimeStamp(const Time
// 1. See that the KF contains a CaptureMotion
// 2. See that the TS is smaller than the KF's TS
// 3. See that the TS is bigger than the first Motion in the CaptureMotion's buffer (if any)
- FrameBaseConstPtr frame = nullptr;
- CaptureBaseConstPtr capture = nullptr;
- CaptureMotionConstPtr capture_motion = nullptr;
- auto frame_map = getProblem()->getTrajectory()->getFrameMap();
- for (auto frame_rev_iter = frame_map.rbegin();
- frame_rev_iter != frame_map.rend();
- ++frame_rev_iter)
+ FrameBaseConstPtr frame = nullptr;
+ CaptureBaseConstPtr capture = nullptr;
+ CaptureMotionConstPtr capture_motion = nullptr;
+ auto frame_map = getProblem()->getTrajectory()->getFrameMap();
+ for (auto frame_rev_iter = frame_map.rbegin(); frame_rev_iter != frame_map.rend(); ++frame_rev_iter)
{
- frame = frame_rev_iter->second;
+ frame = frame_rev_iter->second;
auto sensor = getSensor();
- capture = frame->getCaptureOf(sensor);
+ capture = frame->getCaptureOf(sensor);
if (capture != nullptr)
{
assert(std::dynamic_pointer_cast<const CaptureMotion>(capture) != nullptr);
// Rule 1 satisfied! We found a Capture belonging to this processor's Sensor ==> it is a CaptureMotion
capture_motion = std::static_pointer_cast<const CaptureMotion>(capture);
- if (capture_motion->containsTimeStamp(_ts, params_->time_tolerance))
+ if (capture_motion->containsTimeStamp(_ts, getTimeTolerance()))
{
// Found time stamp satisfying rule 3 above !! ==> break for loop
break;
@@ -956,12 +928,10 @@ CaptureMotionPtr ProcessorMotion::findCaptureContainingTimeStamp(const TimeStamp
{
assert(_ts.ok());
- if(last_ptr_ == nullptr) return nullptr;
+ if (last_ptr_ == nullptr) return nullptr;
// First check if last_ptr is the one we are looking for
- if (last_ptr_->containsTimeStamp(_ts, this->params_->time_tolerance))
- return last_ptr_;
-
+ if (last_ptr_->containsTimeStamp(_ts, this->getTimeTolerance())) return last_ptr_;
// Then look in the Wolf tree...
// -----------------------------
@@ -972,24 +942,22 @@ CaptureMotionPtr ProcessorMotion::findCaptureContainingTimeStamp(const TimeStamp
// 1. See that the KF contains a CaptureMotion
// 2. See that the TS is smaller than the KF's TS
// 3. See that the TS is bigger than the first Motion in the CaptureMotion's buffer (if any)
- FrameBasePtr frame = nullptr;
- CaptureBasePtr capture = nullptr;
- CaptureMotionPtr capture_motion = nullptr;
- auto frame_map = getProblem()->getTrajectory()->getFrameMap();
- for (auto frame_rev_iter = frame_map.rbegin();
- frame_rev_iter != frame_map.rend();
- ++frame_rev_iter)
+ FrameBasePtr frame = nullptr;
+ CaptureBasePtr capture = nullptr;
+ CaptureMotionPtr capture_motion = nullptr;
+ auto frame_map = getProblem()->getTrajectory()->getFrameMap();
+ for (auto frame_rev_iter = frame_map.rbegin(); frame_rev_iter != frame_map.rend(); ++frame_rev_iter)
{
- frame = frame_rev_iter->second;
+ frame = frame_rev_iter->second;
auto sensor = getSensor();
- capture = frame->getCaptureOf(sensor);
+ capture = frame->getCaptureOf(sensor);
if (capture != nullptr)
{
assert(std::dynamic_pointer_cast<CaptureMotion>(capture) != nullptr);
// Rule 1 satisfied! We found a Capture belonging to this processor's Sensor ==> it is a CaptureMotion
capture_motion = std::static_pointer_cast<CaptureMotion>(capture);
- if (capture_motion->containsTimeStamp(_ts, params_->time_tolerance))
+ if (capture_motion->containsTimeStamp(_ts, getTimeTolerance()))
{
// Found time stamp satisfying rule 3 above !! ==> break for loop
break;
@@ -1006,30 +974,39 @@ FrameBasePtr ProcessorMotion::computeProcessingStep()
// Origin not set
if (!origin_ptr_)
{
- FrameBasePtr keyframe_from_callback = buffer_frame_.selectFirstBefore(incoming_ptr_->getTimeStamp(), params_motion_->time_tolerance);
+ FrameBasePtr keyframe_from_callback =
+ buffer_frame_.selectFirstBefore(incoming_ptr_->getTimeStamp(), this->getTimeTolerance());
if (keyframe_from_callback)
{
- if (checkTimeTolerance(keyframe_from_callback,
- incoming_ptr_))
+ if (checkTimeTolerance(keyframe_from_callback, incoming_ptr_))
{
WOLF_DEBUG("PM ", getName(), ": First time with a KF compatible.")
processing_step_ = FIRST_TIME_WITH_KF_ON_INCOMING;
}
else if (keyframe_from_callback->getTimeStamp() < incoming_ptr_->getTimeStamp())
{
- WOLF_DEBUG("PM ", getName(), ": First time with a KF too old. It seems the prior has been set before receiving the first capture of this processor.")
+ WOLF_DEBUG("PM ",
+ getName(),
+ ": First time with a KF too old. It seems the prior has been set before receiving the "
+ "first capture of this processor.")
processing_step_ = FIRST_TIME_WITH_KF_BEFORE_INCOMING;
}
else
{
- WOLF_DEBUG("PM ", getName(), ": First time with a KF newer than the first capture. It only can happen if prior mode is 'nothing'")
+ WOLF_DEBUG("PM ",
+ getName(),
+ ": First time with a KF newer than the first capture. It only can happen if prior mode is "
+ "'nothing'")
processing_step_ = FIRST_TIME_WITH_KF_AFTER_INCOMING;
}
}
else
{
- WOLF_DEBUG("PM ", getName(), ": First time with a KF newer than the first capture. It only can happen if prior mode is 'nothing'")
+ WOLF_DEBUG(
+ "PM ",
+ getName(),
+ ": First time with a KF newer than the first capture. It only can happen if prior mode is 'nothing'")
processing_step_ = FIRST_TIME_WITHOUT_KF;
}
@@ -1037,7 +1014,8 @@ FrameBasePtr ProcessorMotion::computeProcessingStep()
}
else
{
- FrameBasePtr keyframe_from_callback = buffer_frame_.selectFirstBefore(last_ptr_->getTimeStamp(), params_motion_->time_tolerance);
+ FrameBasePtr keyframe_from_callback =
+ buffer_frame_.selectFirstBefore(last_ptr_->getTimeStamp(), this->getTimeTolerance());
// ignore "future" KF to avoid MotionBuffer::split() error
if (keyframe_from_callback && keyframe_from_callback->getTimeStamp() > last_ptr_->getBuffer().back().ts_)
@@ -1045,8 +1023,7 @@ FrameBasePtr ProcessorMotion::computeProcessingStep()
if (keyframe_from_callback)
{
- if (checkTimeTolerance(keyframe_from_callback,
- origin_ptr_))
+ if (checkTimeTolerance(keyframe_from_callback, origin_ptr_))
processing_step_ = RUNNING_WITH_KF_ON_ORIGIN;
@@ -1057,7 +1034,6 @@ FrameBasePtr ProcessorMotion::computeProcessingStep()
else
processing_step_ = RUNNING_WITH_KF_AFTER_ORIGIN;
-
}
else
processing_step_ = RUNNING_WITHOUT_KF;
@@ -1073,39 +1049,46 @@ void ProcessorMotion::assertsCaptureMotion(CaptureMotionPtr _capture, std::strin
{
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());
+ 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
+TimeStamp ProcessorMotion::getTimeStamp() const
{
- if (not origin_ptr_ or
- origin_ptr_->isRemoving() or
- not last_ptr_)
+ if (not origin_ptr_ or origin_ptr_->isRemoving() or not last_ptr_)
{
WOLF_DEBUG("PM proc \"", getName(), "\" has no time stamp. Returning a non-valid timestamp equal to 0");
return TimeStamp::Invalid();
}
- if (getBuffer().empty())
- return last_ptr_->getTimeStamp();
+ if (getBuffer().empty()) return last_ptr_->getTimeStamp();
return getBuffer().back().ts_;
}
-void ProcessorMotion::printHeader(int _depth, bool _factored_by, bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const
+void ProcessorMotion::printHeader(int _depth,
+ bool _factored_by,
+ bool _metric,
+ bool _state_blocks,
+ std::ostream& _stream,
+ std::string _tabs) const
{
_stream << _tabs << "PrcM" << id() << " " << getType() << " \"" << getName() << "\"" << std::endl;
if (getOrigin())
- _stream << _tabs << " " << "o: Cap" << getOrigin()->id() << " - " << " Frm"
- << getOrigin()->getFrame()->id() << std::endl;
+ _stream << _tabs << " "
+ << "o: Cap" << getOrigin()->id() << " - "
+ << " Frm" << getOrigin()->getFrame()->id() << std::endl;
if (getLast())
- _stream << _tabs << " " << "l: Cap" << getLast()->id() << " - " << " Frm"
- << getLast()->getFrame()->id() << std::endl;
+ _stream << _tabs << " "
+ << "l: Cap" << getLast()->id() << " - "
+ << " Frm" << getLast()->getFrame()->id() << std::endl;
if (getIncoming())
- _stream << _tabs << " " << "i: Cap" << getIncoming()->id() << std::endl;
-
+ _stream << _tabs << " "
+ << "i: Cap" << getIncoming()->id() << std::endl;
}
-} // namespace wolf
+} // namespace wolf
diff --git a/src/processor/processor_odom_2d.cpp b/src/processor/processor_odom_2d.cpp
index 634a832e7faa4d1dda3891bdb7c278ba1c8757e7..0a8117a34db7a8da9067eb88d3c862b0fb213394 100644
--- a/src/processor/processor_odom_2d.cpp
+++ b/src/processor/processor_odom_2d.cpp
@@ -26,10 +26,8 @@
namespace wolf
{
-
-ProcessorOdom2d::ProcessorOdom2d(ParamsProcessorOdom2dPtr _params)
- : ProcessorMotion("ProcessorOdom2d", {{'P', "StatePoint2d"}, {'O', "StateAngle"}}, 2, 3, 3, 3, 2, 0, _params),
- params_odom_2d_(_params)
+ProcessorOdom2d::ProcessorOdom2d(const YAML::Node& _params)
+ : ProcessorMotion("ProcessorOdom2d", {{'P', "StatePoint2d"}, {'O', "StateAngle"}}, 2, 3, 3, 3, 2, 0, _params)
{
//
}
@@ -128,25 +126,25 @@ void ProcessorOdom2d::statePlusDelta(const VectorComposite& _x,
bool ProcessorOdom2d::voteForKeyFrame() const
{
// Distance criterion
- if (getBuffer().back().delta_integr_.head<2>().norm() > params_odom_2d_->dist_traveled)
+ if (getBuffer().back().delta_integr_.head<2>().norm() > getMaxDistTraveled())
{
WOLF_DEBUG("PM", id(), " ", getType(), " votes per distance");
return true;
}
// Buffer length
- if (getBuffer().back().delta_integr_.tail<1>().norm() > params_odom_2d_->angle_turned)
+ if (getBuffer().back().delta_integr_.tail<1>().norm() > getMaxAngleTurned())
{
WOLF_DEBUG("PM", id(), " ", getType(), " votes per angle");
return true;
}
// Uncertainty criterion
- if (getBuffer().back().delta_integr_cov_.determinant() > params_odom_2d_->cov_det)
+ if (getBuffer().back().delta_integr_cov_.determinant() > getCovDet())
{
WOLF_DEBUG("PM", id(), " ", getType(), " votes per state covariance");
return true;
}
// Time criterion
- if (getBuffer().back().ts_.get() - origin_ptr_->getFrame()->getTimeStamp().get() > params_odom_2d_->max_time_span)
+ if (getBuffer().back().ts_.get() - origin_ptr_->getFrame()->getTimeStamp().get() > getMaxTimeSpan())
{
WOLF_DEBUG("PM", id(), " ", getType(), " votes per time");
return true;
@@ -185,7 +183,7 @@ void ProcessorOdom2d::emplaceFeaturesAndFactors(CaptureBasePtr _capture_origin,
_capture_origin->getFrame(),
_capture_own->getFrame(),
shared_from_this(),
- params_->apply_loss_function,
+ applyLossFunction(),
TOP_MOTION);
}
diff --git a/src/processor/processor_odom_3d.cpp b/src/processor/processor_odom_3d.cpp
index b8803908aec1e11e3903bace6edbb39fcd5dc2ad..28900f34f84959c6ec29c50faa974f5d20f8589d 100644
--- a/src/processor/processor_odom_3d.cpp
+++ b/src/processor/processor_odom_3d.cpp
@@ -24,10 +24,8 @@
namespace wolf
{
-
-ProcessorOdom3d::ProcessorOdom3d(ParamsProcessorOdom3dPtr _params)
- : ProcessorMotion("ProcessorOdom3d", {{'P', "StatePoint3d"}, {'O', "StateQuaternion"}}, 3, 7, 7, 6, 6, 0, _params),
- params_odom_3d_(_params)
+ProcessorOdom3d::ProcessorOdom3d(const YAML::Node& _params)
+ : ProcessorMotion("ProcessorOdom3d", {{'P', "StatePoint3d"}, {'O', "StateQuaternion"}}, 3, 7, 7, 6, 6, 0, _params)
{
//
}
@@ -168,20 +166,20 @@ void ProcessorOdom3d::statePlusDelta(const VectorComposite& _x,
bool ProcessorOdom3d::voteForKeyFrame() const
{
// time span
- if (getBuffer().back().ts_ - getBuffer().front().ts_ > params_odom_3d_->max_time_span)
+ if (getBuffer().back().ts_ - getBuffer().front().ts_ > getMaxTimeSpan())
{
WOLF_DEBUG("PM: vote: time span");
return true;
}
// buffer length
- if (getBuffer().size() > params_odom_3d_->max_buff_length)
+ if (getBuffer().size() > getMaxBuffLength())
{
WOLF_DEBUG("PM: vote: buffer size");
return true;
}
// distance traveled
double dist = getMotion().delta_integr_.head(3).norm();
- if (dist > params_odom_3d_->dist_traveled)
+ if (dist > getMaxDistTraveled())
{
WOLF_DEBUG("PM: vote: distance traveled");
return true;
@@ -189,7 +187,7 @@ bool ProcessorOdom3d::voteForKeyFrame() const
// angle turned
double angle =
q2v(Quaterniond(getMotion().delta_integr_.data() + 3)).norm(); // 2.0 * acos(getMotion().delta_integr_(6));
- if (angle > params_odom_3d_->angle_turned)
+ if (angle > getMaxAngleTurned())
{
WOLF_DEBUG("PM: vote: angle turned");
return true;
@@ -234,7 +232,7 @@ void ProcessorOdom3d::emplaceFeaturesAndFactors(CaptureBasePtr _capture_origin,
_capture_origin->getFrame(),
_capture_own->getFrame(),
shared_from_this(),
- params_->apply_loss_function,
+ applyLossFunction(),
TOP_MOTION);
}
diff --git a/src/processor/processor_tracker.cpp b/src/processor/processor_tracker.cpp
index a4e8280188965d8b94f359a913b2ea21358ab65d..bc093a4e338c9b73f2eb2313ce78c286c1d92356 100644
--- a/src/processor/processor_tracker.cpp
+++ b/src/processor/processor_tracker.cpp
@@ -17,12 +17,6 @@
//
// 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/>.
-/*
- * ProcessorTracker.cpp
- *
- * Created on: Apr 7, 2016
- * Author: jvallve
- */
// wolf
#include "core/processor/processor_tracker.h"
@@ -32,19 +26,19 @@
namespace wolf
{
-
ProcessorTracker::ProcessorTracker(const std::string& _type,
- const StateKeys& _state_keys,
- int _dim,
- ParamsProcessorTrackerPtr _params_tracker) :
- ProcessorBase(_type, _dim, _params_tracker),
- params_tracker_(_params_tracker),
- processing_step_(FIRST_TIME_WITHOUT_KEYFRAME),
- origin_ptr_(nullptr),
- last_ptr_(nullptr),
- incoming_ptr_(nullptr),
- last_frame_ptr_(nullptr),
- state_keys_(_state_keys)
+ const StateKeys& _state_keys,
+ int _dim,
+ const YAML::Node& _params)
+ : ProcessorBase(_type, _dim, _params),
+ processing_step_(FIRST_TIME_WITHOUT_KEYFRAME),
+ origin_ptr_(nullptr),
+ last_ptr_(nullptr),
+ incoming_ptr_(nullptr),
+ last_frame_ptr_(nullptr),
+ state_keys_(_state_keys),
+ min_features_for_keyframe_(_params["keyframe_vote"]["min_features_for_keyframe"].as<unsigned int>()),
+ max_new_features_(_params["max_new_features"].as<int>())
{
//
}
@@ -66,40 +60,43 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
incoming_ptr_ = _incoming_ptr;
-
- preProcess(); // Derived class operations
+ preProcess(); // Derived class operations
computeProcessingStep();
- switch(processing_step_)
+ switch (processing_step_)
{
- case FIRST_TIME_WITH_KEYFRAME :
- {
- FrameBasePtr keyframe_from_callback = buffer_frame_.select( incoming_ptr_->getTimeStamp(), params_tracker_->time_tolerance);
- buffer_frame_.removeUpTo( keyframe_from_callback->getTimeStamp() );
-
- WOLF_DEBUG( "PT ", getName(), " FIRST_TIME_WITH_KEYFRAME: KF" , keyframe_from_callback->id() , " callback unpacked with ts= " , keyframe_from_callback->getTimeStamp() );
+ case FIRST_TIME_WITH_KEYFRAME: {
+ FrameBasePtr keyframe_from_callback =
+ buffer_frame_.select(incoming_ptr_->getTimeStamp(), getTimeTolerance());
+ buffer_frame_.removeUpTo(keyframe_from_callback->getTimeStamp());
+
+ 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);
// 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.
+ // 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();
// Reset this
resetDerived();
// Update pointers
- origin_ptr_ = incoming_ptr_;
- last_ptr_ = incoming_ptr_;
- incoming_ptr_ = nullptr;
+ origin_ptr_ = incoming_ptr_;
+ last_ptr_ = incoming_ptr_;
+ incoming_ptr_ = nullptr;
break;
}
- case FIRST_TIME_WITHOUT_KEYFRAME :
- {
- WOLF_DEBUG( "PT ", getName(), " FIRST_TIME_WITHOUT_KEYFRAME" );
+ case FIRST_TIME_WITHOUT_KEYFRAME: {
+ WOLF_DEBUG("PT ", getName(), " FIRST_TIME_WITHOUT_KEYFRAME");
// make a new KF at incoming
FrameBasePtr keyframe = getProblem()->emplaceFrame(incoming_ptr_->getTimeStamp());
@@ -108,10 +105,9 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
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();
+ // 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();
// Issue KF callback with new KF
getProblem()->keyFrameCallback(keyframe, shared_from_this());
@@ -119,46 +115,48 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
// Reset this
resetDerived();
// Update pointers
- origin_ptr_ = incoming_ptr_;
- last_ptr_ = incoming_ptr_;
+ origin_ptr_ = incoming_ptr_;
+ last_ptr_ = incoming_ptr_;
incoming_ptr_ = nullptr;
break;
}
- case SECOND_TIME_WITH_KEYFRAME :
- {
- // No-break case only for debug. Next case will be executed too.
- FrameBasePtr keyframe_from_callback = buffer_frame_.select( last_ptr_->getTimeStamp(),
- params_tracker_->time_tolerance);
+ case SECOND_TIME_WITH_KEYFRAME: {
+ // No-break case only for debug. Next case will be executed too.
+ FrameBasePtr keyframe_from_callback = buffer_frame_.select(last_ptr_->getTimeStamp(), getTimeTolerance());
// This received KF is discarded since it is most likely the same KF we createed in FIRST_TIME, ...
// ... only that in FIRST_TIME we checked for incominig, and now we checked for last.
- // Such KF however should have been removed from the buffer of keyframes with the call to buffer_frame_.removeUpTo()
-
+ // Such KF however should have been removed from the buffer of keyframes with the call to
+ // buffer_frame_.removeUpTo()
+
// The debug line is here to check if this is really the same KF
// or it is rather a new KF created by some other processor,
// which happens to be within tolerance of timestamps.
// In this case we discard it anyway because we already have a KF in last
// and we can't link a capture to two KFs.
- WOLF_DEBUG( "PT ", getName(), " SECOND_TIME_WITH_KEYFRAME: KF" , keyframe_from_callback->id() , " callback unpacked with ts= " , keyframe_from_callback->getTimeStamp() );
+ WOLF_DEBUG("PT ",
+ getName(),
+ " SECOND_TIME_WITH_KEYFRAME: KF",
+ keyframe_from_callback->id(),
+ " callback unpacked with ts= ",
+ keyframe_from_callback->getTimeStamp());
}
// Fall through
- case SECOND_TIME_WITHOUT_KEYFRAME :
- {
- WOLF_DEBUG( "PT ", getName(), " SECOND_TIME_WITHOUT_KEYFRAME" );
+ case SECOND_TIME_WITHOUT_KEYFRAME: {
+ WOLF_DEBUG("PT ", getName(), " SECOND_TIME_WITHOUT_KEYFRAME");
// Make a NON KEY Frame to hold incoming capture
- FrameBasePtr keyframe = std::make_shared<FrameBase>(incoming_ptr_->getTimeStamp(),
- getProblem()->getFrameTypes(),
- getProblem()->getState());
+ FrameBasePtr keyframe = std::make_shared<FrameBase>(
+ incoming_ptr_->getTimeStamp(), getProblem()->getFrameTypes(), getProblem()->getState());
incoming_ptr_->link(keyframe);
// Process info
// If we have known landmarks or features. Process them.
if (not getProblem()->getMap()->getLandmarkList().empty() or not known_features_last_.empty())
processKnown();
-
+
// Both Trackers: We have a last_ Capture with not enough features, so populate it.
- processNew(params_tracker_->max_new_features);
+ processNew(max_new_features_);
// Establish factors
establishFactors();
@@ -166,20 +164,23 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
// Reset this
resetDerived();
// Update pointers
- origin_ptr_ = last_ptr_;
- last_ptr_ = incoming_ptr_;
+ origin_ptr_ = last_ptr_;
+ last_ptr_ = incoming_ptr_;
last_frame_ptr_ = keyframe;
- incoming_ptr_ = nullptr;
+ incoming_ptr_ = nullptr;
break;
}
- case RUNNING_WITH_KEYFRAME :
- {
- FrameBasePtr keyframe_from_callback = buffer_frame_.select( last_ptr_->getTimeStamp() ,
- params_tracker_->time_tolerance);
- buffer_frame_.removeUpTo( keyframe_from_callback->getTimeStamp() );
+ case RUNNING_WITH_KEYFRAME: {
+ FrameBasePtr keyframe_from_callback = buffer_frame_.select(last_ptr_->getTimeStamp(), getTimeTolerance());
+ buffer_frame_.removeUpTo(keyframe_from_callback->getTimeStamp());
- WOLF_DEBUG( "PT ", getName(), " RUNNING_WITH_KEYFRAME: KF" , keyframe_from_callback->id() , " callback unpacked with ts= " , keyframe_from_callback->getTimeStamp() );
+ WOLF_DEBUG("PT ",
+ getName(),
+ " RUNNING_WITH_KEYFRAME: KF",
+ keyframe_from_callback->id(),
+ " callback unpacked with ts= ",
+ keyframe_from_callback->getTimeStamp());
processKnown();
@@ -189,13 +190,12 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
last_old_frame->remove();
// Create new NON KEY frame for incoming
- FrameBasePtr frame = std::make_shared<FrameBase>(incoming_ptr_->getTimeStamp(),
- getProblem()->getFrameTypes(),
- getProblem()->getState());
+ FrameBasePtr frame = std::make_shared<FrameBase>(
+ incoming_ptr_->getTimeStamp(), getProblem()->getFrameTypes(), getProblem()->getState());
incoming_ptr_->link(frame);
// Detect new Features, initialize Landmarks, ...
- processNew(params_tracker_->max_new_features);
+ processNew(max_new_features_);
// Establish factors
establishFactors();
@@ -210,16 +210,15 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
break;
}
- case RUNNING_WITHOUT_KEYFRAME :
- {
- WOLF_DEBUG( "PT ", getName(), " RUNNING_WITHOUT_KEYFRAME");
+ case RUNNING_WITHOUT_KEYFRAME: {
+ WOLF_DEBUG("PT ", getName(), " RUNNING_WITHOUT_KEYFRAME");
processKnown();
if (voteForKeyFrame() && permittedKeyFrame())
{
// process
- processNew(params_tracker_->max_new_features);
+ processNew(max_new_features_);
// We create a KF
// set KF on last
@@ -232,12 +231,12 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
// Call the new keyframe callback in order to let the other processors to establish their factors
getProblem()->keyFrameCallback(last_ptr_->getFrame(), shared_from_this());
-
// make NON KEY frame; append incoming to new frame
- FrameBasePtr frame = std::make_shared<FrameBase>(incoming_ptr_->getTimeStamp(),
- getProblem()->getFrameTypes(),
- getProblem()->getState(incoming_ptr_->getTimeStamp()));
- incoming_ptr_ ->link(frame);
+ FrameBasePtr frame =
+ std::make_shared<FrameBase>(incoming_ptr_->getTimeStamp(),
+ getProblem()->getFrameTypes(),
+ getProblem()->getState(incoming_ptr_->getTimeStamp()));
+ incoming_ptr_->link(frame);
// Reset this
resetDerived();
@@ -246,18 +245,19 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
last_ptr_ = incoming_ptr_;
last_frame_ptr_ = frame;
incoming_ptr_ = nullptr;
-
}
else
{
// We do not create a KF
// Replace last frame for a new NON KEY frame in incoming
- FrameBasePtr frame = std::make_shared<FrameBase>(incoming_ptr_->getTimeStamp(),
- getProblem()->getFrameTypes(),
- getProblem()->getState(incoming_ptr_->getTimeStamp()));
+ FrameBasePtr frame =
+ std::make_shared<FrameBase>(incoming_ptr_->getTimeStamp(),
+ getProblem()->getFrameTypes(),
+ 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
+ last_ptr_->unlink(); // unlink last (destroying the frame) instead of frame destruction that would
+ // implicitly destroy last
// Advance this
advanceDerived();
@@ -268,7 +268,7 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
}
break;
}
- default :
+ default:
break;
}
@@ -278,7 +278,12 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
void ProcessorTracker::computeProcessingStep()
{
// First determine the processing phase by checking the status of the tracker pointers
- enum {FIRST_TIME, SECOND_TIME, RUNNING} step;
+ enum
+ {
+ FIRST_TIME,
+ SECOND_TIME,
+ RUNNING
+ } step;
if (origin_ptr_ == nullptr && last_ptr_ == nullptr)
step = FIRST_TIME;
else if (origin_ptr_ == last_ptr_)
@@ -289,26 +294,26 @@ void ProcessorTracker::computeProcessingStep()
// Then combine with the existence (or not) of a keyframe callback pack
switch (step)
{
- case FIRST_TIME : // We check for KF in incoming
+ case FIRST_TIME: // We check for KF in incoming
- if (buffer_frame_.select(incoming_ptr_->getTimeStamp(), params_tracker_->time_tolerance))
+ if (buffer_frame_.select(incoming_ptr_->getTimeStamp(), getTimeTolerance()))
processing_step_ = FIRST_TIME_WITH_KEYFRAME;
- else // ! last && ! pack(incoming)
+ else // ! last && ! pack(incoming)
processing_step_ = FIRST_TIME_WITHOUT_KEYFRAME;
- break;
+ break;
- case SECOND_TIME : // We check for KF in last
+ case SECOND_TIME: // We check for KF in last
- if (buffer_frame_.select(last_ptr_->getTimeStamp(), params_tracker_->time_tolerance))
+ if (buffer_frame_.select(last_ptr_->getTimeStamp(), getTimeTolerance()))
processing_step_ = SECOND_TIME_WITH_KEYFRAME;
else
processing_step_ = SECOND_TIME_WITHOUT_KEYFRAME;
break;
- case RUNNING : // We check for KF in last
- default :
+ case RUNNING: // We check for KF in last
+ default:
- if (buffer_frame_.select(last_ptr_->getTimeStamp(), params_tracker_->time_tolerance))
+ if (buffer_frame_.select(last_ptr_->getTimeStamp(), getTimeTolerance()))
{
if (last_ptr_->getFrame()->getProblem())
{
@@ -328,18 +333,25 @@ void ProcessorTracker::computeProcessingStep()
}
}
-void ProcessorTracker::printHeader(int _depth, bool _factored_by, bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const
+void ProcessorTracker::printHeader(int _depth,
+ bool _factored_by,
+ bool _metric,
+ bool _state_blocks,
+ std::ostream& _stream,
+ std::string _tabs) const
{
_stream << _tabs << "PrcT" << id() << " " << getType() << " \"" << getName() << "\"" << std::endl;
if (getOrigin())
- _stream << _tabs << " " << "o: Cap" << getOrigin()->id() << " - " << " Frm"
- << getOrigin()->getFrame()->id() << std::endl;
+ _stream << _tabs << " "
+ << "o: Cap" << getOrigin()->id() << " - "
+ << " Frm" << getOrigin()->getFrame()->id() << std::endl;
if (getLast())
- _stream << _tabs << " " << "l: Cap" << getLast()->id() << " - " << " Frm"
- << getLast()->getFrame()->id() << std::endl;
+ _stream << _tabs << " "
+ << "l: Cap" << getLast()->id() << " - "
+ << " Frm" << getLast()->getFrame()->id() << std::endl;
if (getIncoming())
- _stream << _tabs << " " << "i: Cap" << getIncoming()->id() << std::endl;
+ _stream << _tabs << " "
+ << "i: Cap" << getIncoming()->id() << std::endl;
}
-} // namespace wolf
-
+} // namespace wolf
diff --git a/src/processor/processor_tracker_feature.cpp b/src/processor/processor_tracker_feature.cpp
index 56d96092c80875fb83f225304aff5d865938e8e4..5abd3844ff71aacf2f64af0d8271b24f32c63304 100644
--- a/src/processor/processor_tracker_feature.cpp
+++ b/src/processor/processor_tracker_feature.cpp
@@ -17,30 +17,20 @@
//
// 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/>.
-/*
- * \processor_tracker_feature.cpp
- *
- * Created on: 27/02/2016
- * \author: jsola
- */
#include "core/processor/processor_tracker_feature.h"
namespace wolf
{
-
ProcessorTrackerFeature::ProcessorTrackerFeature(const std::string& _type,
- const StateKeys& _structure,
- int _dim,
- ParamsProcessorTrackerFeaturePtr _params_tracker_feature) :
- ProcessorTracker(_type, _structure, _dim, _params_tracker_feature),
- params_tracker_feature_(_params_tracker_feature)
+ const StateKeys& _structure,
+ int _dim,
+ const YAML::Node& _params)
+ : ProcessorTracker(_type, _structure, _dim, _params)
{
}
-ProcessorTrackerFeature::~ProcessorTrackerFeature()
-{
-}
+ProcessorTrackerFeature::~ProcessorTrackerFeature() {}
unsigned int ProcessorTrackerFeature::processNew(const int& _max_new_features)
{
@@ -57,29 +47,30 @@ unsigned int ProcessorTrackerFeature::processNew(const int& _max_new_features)
matches_last_from_incoming_.clear();
// Populate the last Capture with new Features. The result is in new_features_last_.
- unsigned int n = detectNewFeatures(_max_new_features,
- last_ptr_,
- new_features_last_);
+ unsigned int n = detectNewFeatures(_max_new_features, last_ptr_, new_features_last_);
// check all features have been emplaced
- assert(std::all_of(new_features_last_.begin(), new_features_last_.end(), [](FeatureBasePtr f){return f->getCapture() != nullptr;}) &&
+ assert(std::all_of(new_features_last_.begin(),
+ new_features_last_.end(),
+ [](FeatureBasePtr f) { return f->getCapture() != nullptr; }) &&
"some features not linked after returning from detectNewFeatures()");
// fill the track matrix
for (auto ftr : new_features_last_)
{
- assert(std::find(known_features_last_.begin(), known_features_last_.end(), ftr) == known_features_last_.end() && "detectNewFeatures() provided a new feature that is already in known_features_last_`");
+ assert(std::find(known_features_last_.begin(), known_features_last_.end(), ftr) ==
+ known_features_last_.end() &&
+ "detectNewFeatures() provided a new feature that is already in known_features_last_`");
track_matrix_.newTrack(ftr);
}
// Track new features from last to incoming. This will append new correspondences to matches_last_incoming
- trackFeatures(new_features_last_,
- incoming_ptr_,
- new_features_incoming_,
- matches_last_from_incoming_);
+ trackFeatures(new_features_last_, incoming_ptr_, new_features_incoming_, matches_last_from_incoming_);
// check all features have been emplaced
- assert(std::all_of(new_features_incoming_.begin(), new_features_incoming_.end(), [](FeatureBasePtr f){return f->getCapture() != nullptr;}) &&
+ assert(std::all_of(new_features_incoming_.begin(),
+ new_features_incoming_.end(),
+ [](FeatureBasePtr f) { return f->getCapture() != nullptr; }) &&
"any not linked feature returned by trackFeatures()");
// fill the track matrix
@@ -95,8 +86,9 @@ unsigned int ProcessorTrackerFeature::processNew(const int& _max_new_features)
unsigned int ProcessorTrackerFeature::processKnown()
{
- //assert(incoming_ptr_->getFeatureList().size() == 0
- // && "In ProcessorTrackerFeature::processKnown(): incoming_ptr_ feature list must be empty before processKnown()");
+ // assert(incoming_ptr_->getFeatureList().size() == 0
+ // && "In ProcessorTrackerFeature::processKnown(): incoming_ptr_ feature list must be empty before
+ // processKnown()");
// clear list of known features
matches_last_from_incoming_.clear();
@@ -115,13 +107,12 @@ unsigned int ProcessorTrackerFeature::processKnown()
}
// Track features from last_ptr_ to incoming_ptr_
- trackFeatures(known_features_last_,
- incoming_ptr_,
- known_features_incoming_,
- matches_last_from_incoming_);
+ trackFeatures(known_features_last_, incoming_ptr_, known_features_incoming_, matches_last_from_incoming_);
// 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()");
// fill the track matrix
@@ -143,11 +134,11 @@ unsigned int ProcessorTrackerFeature::processKnown()
if (!correctFeatureDrift(feature_in_origin, feature_in_last, (*ftr_inc_it)))
{
// Remove this feature from many places:
- assert(matches_last_from_incoming_ .count(*ftr_inc_it));
- matches_last_from_incoming_ .erase (*ftr_inc_it); // remove match
- track_matrix_ .remove(*ftr_inc_it); // remove from track matrix
- (*ftr_inc_it) ->remove(); // remove from wolf tree
- ftr_inc_it = known_features_incoming_.erase(ftr_inc_it); // remove from known features list
+ assert(matches_last_from_incoming_.count(*ftr_inc_it));
+ matches_last_from_incoming_.erase(*ftr_inc_it); // remove match
+ track_matrix_.remove(*ftr_inc_it); // remove from track matrix
+ (*ftr_inc_it)->remove(); // remove from wolf tree
+ ftr_inc_it = known_features_incoming_.erase(ftr_inc_it); // remove from known features list
}
else
ftr_inc_it++;
@@ -162,8 +153,8 @@ void ProcessorTrackerFeature::advanceDerived()
// Reset here the list of correspondences.
matches_last_from_incoming_.clear();
known_features_last_ = std::move(known_features_incoming_);
- //new_features_incoming should be zero!
- //known_features_last_.splice(new_features_incoming_);
+ // new_features_incoming should be zero!
+ // known_features_last_.splice(new_features_incoming_);
// // remove last from track matrix in case you want to have only KF in the track matrix
// track_matrix_.remove(last_ptr_);
@@ -177,33 +168,37 @@ void ProcessorTrackerFeature::resetDerived()
known_features_last_.splice(known_features_last_.end(), new_features_incoming_);
// Debug
- //for (auto const & pair_trkid_pair : track_matrix_.matches(origin_ptr_, last_ptr_))
+ // for (auto const & pair_trkid_pair : track_matrix_.matches(origin_ptr_, last_ptr_))
//{
- // WOLF_DEBUG("Matches reset: track: ", pair_trkid_pair.first, " origin: ", pair_trkid_pair.second.first->id(), " last: ", pair_trkid_pair.second.second->id());
+ // WOLF_DEBUG("Matches reset: track: ", pair_trkid_pair.first, " origin: ", pair_trkid_pair.second.first->id(),
+ // " last: ", pair_trkid_pair.second.second->id());
//}
}
void ProcessorTrackerFeature::establishFactors()
{
- if (origin_ptr_ == last_ptr_)
- return;
+ if (origin_ptr_ == last_ptr_) return;
TrackMatches matches_origin_last = track_matrix_.matches(origin_ptr_, last_ptr_);
- for (auto const & pair_trkid_pair : matches_origin_last)
+ for (auto const& pair_trkid_pair : matches_origin_last)
{
FeatureBasePtr feature_in_origin = pair_trkid_pair.second.first;
FeatureBasePtr feature_in_last = pair_trkid_pair.second.second;
+ auto fac_ptr = emplaceFactor(feature_in_last, feature_in_origin);
- auto fac_ptr = emplaceFactor(feature_in_last, feature_in_origin);
-
- assert((fac_ptr == nullptr or fac_ptr->getFeature() != nullptr) && "not emplaced factor returned by emplaceFactor()");
+ assert((fac_ptr == nullptr or fac_ptr->getFeature() != nullptr) &&
+ "not emplaced factor returned by emplaceFactor()");
- WOLF_DEBUG_COND(fac_ptr, "New factor: track: " , feature_in_last->trackId(),
- " origin: " , feature_in_origin->id() ,
- " from last: " , feature_in_last->id() );
+ WOLF_DEBUG_COND(fac_ptr,
+ "New factor: track: ",
+ feature_in_last->trackId(),
+ " origin: ",
+ feature_in_origin->id(),
+ " from last: ",
+ feature_in_last->id());
}
}
-} // namespace wolf
+} // namespace wolf
diff --git a/src/processor/processor_tracker_landmark.cpp b/src/processor/processor_tracker_landmark.cpp
index f879944e30821d68bcdf9e15aa0d08ffecb15a7c..e6703dec0cd288e7a3547fd4c39ca344f3a6123d 100644
--- a/src/processor/processor_tracker_landmark.cpp
+++ b/src/processor/processor_tracker_landmark.cpp
@@ -25,15 +25,10 @@
namespace wolf
{
-
ProcessorTrackerLandmark::ProcessorTrackerLandmark(const std::string& _type,
- const StateKeys& _structure,
- ParamsProcessorTrackerLandmarkPtr _params_tracker_landmark) :
- ProcessorTracker(_type,
- _structure,
- 0,
- _params_tracker_landmark),
- params_tracker_landmark_(_params_tracker_landmark)
+ const StateKeys& _structure,
+ const YAML::Node& _params)
+ : ProcessorTracker(_type, _structure, 0, _params)
{
//
}
@@ -46,8 +41,8 @@ ProcessorTrackerLandmark::~ProcessorTrackerLandmark()
void ProcessorTrackerLandmark::advanceDerived()
{
matches_landmark_from_last_ = std::move(matches_landmark_from_incoming_);
- new_features_last_ = std::move(new_features_incoming_);
- known_features_last_ = std::move(known_features_incoming_);
+ new_features_last_ = std::move(new_features_incoming_);
+ known_features_last_ = std::move(known_features_incoming_);
known_features_last_.splice(known_features_last_.end(), new_features_incoming_);
matches_landmark_from_incoming_.clear();
@@ -58,8 +53,8 @@ void ProcessorTrackerLandmark::advanceDerived()
void ProcessorTrackerLandmark::resetDerived()
{
matches_landmark_from_last_ = std::move(matches_landmark_from_incoming_);
- new_features_last_ = std::move(new_features_incoming_);
- known_features_last_ = std::move(known_features_incoming_);
+ new_features_last_ = std::move(new_features_incoming_);
+ known_features_last_ = std::move(known_features_incoming_);
known_features_last_.splice(known_features_last_.end(), new_features_incoming_);
matches_landmark_from_incoming_.clear();
@@ -79,7 +74,9 @@ unsigned int ProcessorTrackerLandmark::processKnown()
matches_landmark_from_incoming_);
// 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 findLandmarks()");
return n;
@@ -95,17 +92,17 @@ unsigned int ProcessorTrackerLandmark::processNew(const int& _max_features)
*/
// new lists cleared
- new_features_last_.clear();
+ new_features_last_.clear();
new_features_incoming_.clear();
new_landmarks_.clear();
// We first need to populate the \b last Capture with new Features
- unsigned int n = detectNewFeatures(_max_features,
- last_ptr_,
- new_features_last_);
+ unsigned int n = detectNewFeatures(_max_features, last_ptr_, new_features_last_);
// check all features have been emplaced
- assert(std::all_of(new_features_last_.begin(), new_features_last_.end(), [](FeatureBasePtr f){return f->getCapture() != nullptr;}) &&
+ assert(std::all_of(new_features_last_.begin(),
+ new_features_last_.end(),
+ [](FeatureBasePtr f) { return f->getCapture() != nullptr; }) &&
"any not linked feature returned by detectNewFeatures()");
// create new landmarks with the new features discovered
@@ -114,13 +111,12 @@ unsigned int ProcessorTrackerLandmark::processNew(const int& _max_features)
// Find the new landmarks in incoming_ptr_ (if it's not nullptr)
if (incoming_ptr_ != nullptr)
{
- findLandmarks(new_landmarks_,
- incoming_ptr_,
- new_features_incoming_,
- matches_landmark_from_incoming_);
+ findLandmarks(new_landmarks_, incoming_ptr_, new_features_incoming_, matches_landmark_from_incoming_);
// check all features have been emplaced
- assert(std::all_of(new_features_incoming_.begin(), new_features_incoming_.end(), [](FeatureBasePtr f){return f->getCapture() != nullptr;}) &&
+ assert(std::all_of(new_features_incoming_.begin(),
+ new_features_incoming_.end(),
+ [](FeatureBasePtr f) { return f->getCapture() != nullptr; }) &&
"some features not linked after returning from detectNewFeatures()");
}
@@ -144,7 +140,7 @@ void ProcessorTrackerLandmark::emplaceNewLandmarks()
new_landmarks_.push_back(new_lmk_ptr);
// create new correspondence
- matches_landmark_from_last_[new_feature_ptr] = std::make_shared<LandmarkMatch>(new_lmk_ptr, 1); // max score
+ matches_landmark_from_last_[new_feature_ptr] = std::make_shared<LandmarkMatch>(new_lmk_ptr, 1); // max score
}
}
@@ -157,10 +153,11 @@ void ProcessorTrackerLandmark::establishFactors()
{
assert(matches_landmark_from_last_.count(last_feature) == 1);
- FactorBasePtr fac_ptr = emplaceFactor(last_feature, matches_landmark_from_last_[last_feature]->landmark_ptr_);
+ FactorBasePtr fac_ptr =
+ emplaceFactor(last_feature, matches_landmark_from_last_[last_feature]->landmark_ptr_);
assert(fac_ptr->getFeature() != nullptr && "not linked factor returned by emplaceFactor()");
}
}
-} // namespace wolf
+} // namespace wolf
diff --git a/src/processor/track_matrix.cpp b/src/processor/track_matrix.cpp
index e57e0adfac75d6fdf000b7ba02be551694893aa3..37d32b7dc38c3e2cac981957b5b5d39900721238 100644
--- a/src/processor/track_matrix.cpp
+++ b/src/processor/track_matrix.cpp
@@ -22,7 +22,6 @@
namespace wolf
{
-
SizeStd TrackMatrix::track_id_count_ = 0;
TrackMatrix::TrackMatrix()
@@ -39,9 +38,8 @@ TrackConst TrackMatrix::track(const SizeStd& _track_id) const
{
TrackConst track_const;
if (tracks_.count(_track_id) > 0)
- for (auto&& pair : tracks_.at(_track_id))
- track_const[pair.first]=pair.second;
-
+ for (auto&& pair : tracks_.at(_track_id)) track_const[pair.first] = pair.second;
+
return track_const;
}
@@ -55,21 +53,16 @@ Track TrackMatrix::track(const SizeStd& _track_id)
SnapshotConst TrackMatrix::snapshot(CaptureBaseConstPtr _capture) const
{
- const auto& it = std::find_if(snapshots_.cbegin(),
- snapshots_.cend(),
- [_capture](const std::pair<CaptureBasePtr, Snapshot>& pair)
- {
- return pair.first == _capture;
- }
- );
+ const auto& it = std::find_if(
+ snapshots_.cbegin(), snapshots_.cend(), [_capture](const std::pair<CaptureBasePtr, Snapshot>& pair) {
+ return pair.first == _capture;
+ });
- if (it == snapshots_.cend())
- return SnapshotConst();
+ if (it == snapshots_.cend()) return SnapshotConst();
SnapshotConst snapshot_const;
- for (auto&& pair : it->second)
- snapshot_const[pair.first]=pair.second;
-
+ for (auto&& pair : it->second) snapshot_const[pair.first] = pair.second;
+
return snapshot_const;
}
@@ -83,21 +76,22 @@ Snapshot TrackMatrix::snapshot(CaptureBasePtr _capture)
void TrackMatrix::newTrack(FeatureBasePtr _ftr)
{
- track_id_count_ ++;
+ track_id_count_++;
add(track_id_count_, _ftr);
}
void TrackMatrix::add(const SizeStd& _track_id, const FeatureBasePtr& _ftr)
{
- assert(( tracks_.count(_track_id) != 0 || _track_id == track_id_count_) && "Provided track ID does not exist. Use newTrack() instead.");
- assert( _ftr->getCapture() != nullptr && "adding a feature not linked to any capture");
+ assert((tracks_.count(_track_id) != 0 || _track_id == track_id_count_) &&
+ "Provided track ID does not exist. Use newTrack() instead.");
+ assert(_ftr->getCapture() != nullptr && "adding a feature not linked to any capture");
_ftr->setTrackId(_track_id);
tracks_[_track_id].emplace(_ftr->getCapture()->getTimeStamp(), _ftr);
- snapshots_[_ftr->getCapture()].emplace(_track_id, _ftr); // will create new snapshot if _cap_id is not present
+ snapshots_[_ftr->getCapture()].emplace(_track_id, _ftr); // will create new snapshot if _cap_id is not present
}
-void TrackMatrix::add(const FeatureBasePtr& _ftr_existing,const FeatureBasePtr& _ftr_new)
+void TrackMatrix::add(const FeatureBasePtr& _ftr_existing, const FeatureBasePtr& _ftr_new)
{
add(_ftr_existing->trackId(), _ftr_new);
}
@@ -111,8 +105,7 @@ void TrackMatrix::remove(const SizeStd& _track_id)
{
CaptureBasePtr cap = pair_time_ftr.second->getCapture();
snapshots_.at(cap).erase(_track_id);
- if (snapshots_.at(cap).empty())
- snapshots_.erase(cap);
+ if (snapshots_.at(cap).empty()) snapshots_.erase(cap);
}
// Remove track
@@ -130,8 +123,7 @@ void TrackMatrix::remove(CaptureBasePtr _cap)
{
SizeStd trk_id = pair_trkid_ftr.first;
tracks_.at(trk_id).erase(ts);
- if (tracks_.at(trk_id).empty())
- tracks_.erase(trk_id);
+ if (tracks_.at(trk_id).empty()) tracks_.erase(trk_id);
}
// remove snapshot
@@ -146,17 +138,15 @@ void TrackMatrix::remove(FeatureBasePtr _ftr)
{
if (auto cap = _ftr->getCapture())
{
- if(tracks_.count(_ftr->trackId()) && tracks_.at(_ftr->trackId()).count(cap->getTimeStamp()))
+ if (tracks_.count(_ftr->trackId()) && tracks_.at(_ftr->trackId()).count(cap->getTimeStamp()))
{
- tracks_ .at(_ftr->trackId()).erase(cap->getTimeStamp());
- if (tracks_.at(_ftr->trackId()).empty())
- tracks_.erase(_ftr->trackId());
+ tracks_.at(_ftr->trackId()).erase(cap->getTimeStamp());
+ if (tracks_.at(_ftr->trackId()).empty()) tracks_.erase(_ftr->trackId());
}
- if(snapshots_.count(cap) && snapshots_.at(cap).count(_ftr->trackId()))
+ if (snapshots_.count(cap) && snapshots_.at(cap).count(_ftr->trackId()))
{
- snapshots_ .at(cap).erase(_ftr->trackId());
- if (snapshots_.at(cap).empty())
- snapshots_.erase(cap);
+ snapshots_.at(cap).erase(_ftr->trackId());
+ if (snapshots_.at(cap).empty()) snapshots_.erase(cap);
}
}
}
@@ -210,8 +200,7 @@ vector<FeatureBaseConstPtr> TrackMatrix::trackAsVector(const SizeStd& _track_id)
if (tracks_.count(_track_id))
{
vec.reserve(trackSize(_track_id));
- for (auto const& pair_time_ftr : tracks_.at(_track_id))
- vec.push_back(pair_time_ftr.second);
+ for (auto const& pair_time_ftr : tracks_.at(_track_id)) vec.push_back(pair_time_ftr.second);
}
return vec;
}
@@ -222,8 +211,7 @@ vector<FeatureBasePtr> TrackMatrix::trackAsVector(const SizeStd& _track_id)
if (tracks_.count(_track_id))
{
vec.reserve(trackSize(_track_id));
- for (auto const& pair_time_ftr : tracks_.at(_track_id))
- vec.push_back(pair_time_ftr.second);
+ for (auto const& pair_time_ftr : tracks_.at(_track_id)) vec.push_back(pair_time_ftr.second);
}
return vec;
}
@@ -234,8 +222,7 @@ FeatureBaseConstPtrList TrackMatrix::snapshotAsList(CaptureBaseConstPtr _cap) co
FeatureBaseConstPtrList lst;
- for (auto const& pair_trkid_ftr : snapshot_const)
- lst.push_back(pair_trkid_ftr.second);
+ for (auto const& pair_trkid_ftr : snapshot_const) lst.push_back(pair_trkid_ftr.second);
return lst;
}
@@ -244,8 +231,7 @@ FeatureBasePtrList TrackMatrix::snapshotAsList(CaptureBasePtr _cap)
{
FeatureBasePtrList lst;
if (snapshots_.count(_cap))
- for (auto const& pair_trkid_ftr : snapshots_.at(_cap))
- lst.push_back(pair_trkid_ftr.second);
+ for (auto const& pair_trkid_ftr : snapshots_.at(_cap)) lst.push_back(pair_trkid_ftr.second);
return lst;
}
@@ -253,8 +239,8 @@ TrackMatchesConst TrackMatrix::matches(CaptureBaseConstPtr _cap_1, CaptureBaseCo
{
TrackMatchesConst pairs;
- auto s_1 = snapshot(_cap_1);
- auto s_2 = snapshot(_cap_2);
+ auto s_1 = snapshot(_cap_1);
+ auto s_2 = snapshot(_cap_2);
auto s_short = s_1;
auto s_long = s_2;
if (s_1.size() > s_2.size())
@@ -263,7 +249,7 @@ TrackMatchesConst TrackMatrix::matches(CaptureBaseConstPtr _cap_1, CaptureBaseCo
s_short = s_2;
}
- for (auto const & pair_trkid_ftr : s_short)
+ for (auto const& pair_trkid_ftr : s_short)
{
SizeStd trk_id = pair_trkid_ftr.first;
if (s_long.count(trk_id))
@@ -277,8 +263,8 @@ TrackMatches TrackMatrix::matches(CaptureBasePtr _cap_1, CaptureBasePtr _cap_2)
{
TrackMatches pairs;
- auto s_1 = snapshot(_cap_1);
- auto s_2 = snapshot(_cap_2);
+ auto s_1 = snapshot(_cap_1);
+ auto s_2 = snapshot(_cap_2);
auto s_short = s_1;
auto s_long = s_2;
if (s_1.size() > s_2.size())
@@ -287,11 +273,10 @@ TrackMatches TrackMatrix::matches(CaptureBasePtr _cap_1, CaptureBasePtr _cap_2)
s_short = s_2;
}
- for (auto const & pair_trkid_ftr : s_short)
+ for (auto const& pair_trkid_ftr : s_short)
{
SizeStd trk_id = pair_trkid_ftr.first;
- if (s_long.count(trk_id))
- pairs[trk_id] = pair<FeatureBasePtr, FeatureBasePtr>(s_1.at(trk_id), s_2.at(trk_id));
+ if (s_long.count(trk_id)) pairs[trk_id] = pair<FeatureBasePtr, FeatureBasePtr>(s_1.at(trk_id), s_2.at(trk_id));
}
return pairs;
@@ -333,7 +318,8 @@ TrackConst TrackMatrix::trackAtKeyframes(const SizeStd& _track_id) const
{
auto& ts = pair_ts_ftr.first;
auto& ftr = pair_ts_ftr.second;
- if (ftr && ftr->getCapture() && ftr->getCapture()->getFrame() && ftr->getCapture()->getFrame()->getProblem())
+ if (ftr && ftr->getCapture() && ftr->getCapture()->getFrame() &&
+ ftr->getCapture()->getFrame()->getProblem())
track_kf[ts] = ftr;
}
return track_kf;
@@ -352,7 +338,8 @@ Track TrackMatrix::trackAtKeyframes(const SizeStd& _track_id)
{
auto& ts = pair_ts_ftr.first;
auto& ftr = pair_ts_ftr.second;
- if (ftr && ftr->getCapture() && ftr->getCapture()->getFrame() && ftr->getCapture()->getFrame()->getProblem())
+ if (ftr && ftr->getCapture() && ftr->getCapture()->getFrame() &&
+ ftr->getCapture()->getFrame()->getProblem())
track_kf[ts] = ftr;
}
return track_kf;
@@ -364,25 +351,20 @@ Track TrackMatrix::trackAtKeyframes(const SizeStd& _track_id)
list<SizeStd> TrackMatrix::trackIds(CaptureBaseConstPtr _capture) const
{
list<SizeStd> track_ids;
-
+
if (not _capture)
- for (auto track_pair : tracks_)
- track_ids.push_back(track_pair.first);
- else
+ for (auto track_pair : tracks_) track_ids.push_back(track_pair.first);
+ else
{
- auto it = std::find_if(snapshots_.cbegin(),
- snapshots_.cend(),
- [_capture](const std::pair<CaptureBasePtr, Snapshot>& pair)
- {
- return pair.first == _capture;
- }
- );
+ auto it = std::find_if(
+ snapshots_.cbegin(), snapshots_.cend(), [_capture](const std::pair<CaptureBasePtr, Snapshot>& pair) {
+ return pair.first == _capture;
+ });
if (it != snapshots_.cend())
- for (auto track_pair : it->second)
- track_ids.push_back(track_pair.first);
+ for (auto track_pair : it->second) track_ids.push_back(track_pair.first);
}
return track_ids;
}
-}
+} // namespace wolf
diff --git a/src/sensor/sensor_base.cpp b/src/sensor/sensor_base.cpp
index 85f627cd6fa2fdf3b3a376b1edb3f0b6d33c92cb..b8a861fcf51efbf0eff61dad9b7134e27525fe91 100644
--- a/src/sensor/sensor_base.cpp
+++ b/src/sensor/sensor_base.cpp
@@ -30,34 +30,24 @@ namespace wolf
{
unsigned int SensorBase::sensor_id_count_ = 0;
-SensorBase::SensorBase(const std::string& _type,
- const int& _dim_compatible,
- ParamsSensorBasePtr _params,
- const SpecStateSensorComposite& _priors)
- : NodeStateBlocks("SENSOR", _type, _priors(_params->getStatesKeys()).cast<SpecStateComposite>()),
+SensorBase::SensorBase(const std::string& _type, const int& _dim_compatible, const YAML::Node& _params)
+ : NodeStateBlocks("SENSOR", _type, SpecStateComposite(_params["states"])),
hardware_ptr_(),
sensor_id_(++sensor_id_count_), // simple ID factory
- params_(_params),
state_block_dynamic_(),
last_capture_(nullptr),
+ params_(Clone(_params)),
dim_compatible_(_dim_compatible)
{
- setName(_params->name);
-
- // WARN if more keys are provided than the StatesKeys:
- WOLF_WARN_COND(_priors.getKeys().size() > _params->getStatesKeys().size(),
- "SensorBase: provided 'prior' with more keys than required. Provided: ",
- _priors.getKeys(),
- ", only required: ",
- _params->getStatesKeys());
+ setName(_params["name"].as<std::string>());
// Drift covariance rates
- WOLF_DEBUG("SensorBase state keys: ", _params->getStatesKeys());
+ auto priors_sensor = SpecStateSensorComposite(_params["states"]);
for (auto state_pair : getStateBlockMap())
{
- state_block_dynamic_.emplace(state_pair.first, _priors.at(state_pair.first).isDynamic());
- if (_priors.at(state_pair.first).isDynamic())
- setDriftStd(state_pair.first, _priors.at(state_pair.first).getDriftStd());
+ state_block_dynamic_.emplace(state_pair.first, priors_sensor.at(state_pair.first).isDynamic());
+ if (priors_sensor.at(state_pair.first).isDynamic())
+ setDriftStd(state_pair.first, priors_sensor.at(state_pair.first).getDriftStd());
}
}
diff --git a/src/sensor/sensor_diff_drive.cpp b/src/sensor/sensor_diff_drive.cpp
index 9fc3710fa53b726c6fbba0cccfa054c9f6bfa486..6956f98185b9240a0478ecd31e4f155ab8762b0a 100644
--- a/src/sensor/sensor_diff_drive.cpp
+++ b/src/sensor/sensor_diff_drive.cpp
@@ -24,20 +24,16 @@
namespace wolf
{
-
-SensorDiffDrive::SensorDiffDrive(ParamsSensorDiffDrivePtr _params,
- const SpecStateSensorComposite& _priors) :
- SensorBase("SensorDiffDrive",
- 2,
- _params,
- _priors),
- params_diff_drive_(_params)
+SensorDiffDrive::SensorDiffDrive(const YAML::Node& _params)
+ : SensorBase("SensorDiffDrive", 2, _params),
+ ticks_per_wheel_revolution_(_params["ticks_per_wheel_revolution"].as<double>()),
+ ticks_std_factor_(_params["ticks_std_factor"].as<double>())
{
}
-Eigen::MatrixXd SensorDiffDrive::computeNoiseCov(const Eigen::VectorXd & _data) const
+Eigen::MatrixXd SensorDiffDrive::computeNoiseCov(const Eigen::VectorXd& _data) const
{
- return (params_diff_drive_->ticks_std_factor * _data).cwiseAbs2().asDiagonal();
+ return (ticks_std_factor_ * _data).cwiseAbs2().asDiagonal();
}
// Register in the FactorySensorNode
diff --git a/src/sensor/sensor_motion_model.cpp b/src/sensor/sensor_motion_model.cpp
index cf2b149665dbca4349f482b74be9308b0f166ff5..6cd04a1515086d849d4b195ef5e72047c82ec3d9 100644
--- a/src/sensor/sensor_motion_model.cpp
+++ b/src/sensor/sensor_motion_model.cpp
@@ -20,28 +20,19 @@
#include "core/sensor/sensor_motion_model.h"
-namespace wolf {
-
-SensorMotionModel::SensorMotionModel(ParamsSensorMotionModelPtr _params,
- const SpecStateSensorComposite& _priors) :
- SensorBase("SensorMotionModel",
- -1,
- _params,
- _priors)
+namespace wolf
{
-}
+SensorMotionModel::SensorMotionModel(const YAML::Node& _params) : SensorBase("SensorMotionModel", -1, _params) {}
-SensorMotionModel::~SensorMotionModel()
-{
-}
+SensorMotionModel::~SensorMotionModel() {}
-Eigen::MatrixXd SensorMotionModel::computeNoiseCov(const Eigen::VectorXd & _data) const
+Eigen::MatrixXd SensorMotionModel::computeNoiseCov(const Eigen::VectorXd& _data) const
{
throw std::runtime_error("SensorMotionModel::computeNoiseCov not implemented!");
- return Eigen::MatrixXd(0,0);
+ return Eigen::MatrixXd(0, 0);
}
// Register in the FactorySensorNode
WOLF_REGISTER_SENSOR(SensorMotionModel);
-} // namespace wolf
+} // namespace wolf
diff --git a/src/solver/solver_manager.cpp b/src/solver/solver_manager.cpp
index d0b295cc6e790073589fe7f98b69018b4517dbc4..033888518733feaad298ef22836d251ff49f68c2 100644
--- a/src/solver/solver_manager.cpp
+++ b/src/solver/solver_manager.cpp
@@ -23,54 +23,54 @@
#include "core/map/map_base.h"
#include "core/landmark/landmark_base.h"
-namespace wolf {
-
-SolverManager::SolverManager(const ProblemPtr& _problem) :
- SolverManager(_problem, std::make_shared<ParamsSolver>())
-{
-}
-
-SolverManager::SolverManager(const ProblemPtr& _problem,
- const ParamsSolverPtr& _params) :
- n_solve_(0),
- n_cov_(0),
- acc_duration_total_(0),
- acc_duration_solver_(0),
- acc_duration_update_(0),
- acc_duration_state_(0),
- acc_duration_cov_(0),
- max_duration_total_(0),
- max_duration_solver_(0),
- max_duration_update_(0),
- max_duration_state_(0),
- max_duration_cov_(0),
- wolf_problem_(_problem),
- params_(_params)
+namespace wolf
+{
+SolverManager::SolverManager(const ProblemPtr& _problem, const YAML::Node& _params)
+ : n_solve_(0),
+ n_cov_(0),
+ acc_duration_total_(0),
+ acc_duration_solver_(0),
+ acc_duration_update_(0),
+ acc_duration_state_(0),
+ acc_duration_cov_(0),
+ max_duration_total_(0),
+ max_duration_solver_(0),
+ max_duration_update_(0),
+ max_duration_state_(0),
+ max_duration_cov_(0),
+ wolf_problem_(_problem),
+ params_(_params)
{
assert(_problem != nullptr && "Passed a nullptr ProblemPtr.");
-}
-SolverManager::~SolverManager()
-{
+ period_ = _params["period"].as<double>();
+ verbose_ = (SolverManager::ReportVerbosity)_params["verbose"].as<int>();
+ compute_cov_ = _params["compute_cov"].as<bool>();
+ if (compute_cov_)
+ {
+ cov_enum_ = (SolverManager::CovarianceBlocksToBeComputed)_params["cov_enum"].as<int>();
+ cov_period_ = _params["cov_period"].as<double>();
+ }
}
+SolverManager::~SolverManager() {}
+
void SolverManager::update()
{
// lock mutex to prevent Problem::transform() during this update()
- std::lock_guard<std::mutex> lock_transform (wolf_problem_->mut_transform_);
+ std::lock_guard<std::mutex> lock_transform(wolf_problem_->mut_transform_);
// Consume notification maps
- std::map<StateBlockPtr,Notification> sb_notification_map;
- std::map<FactorBasePtr,Notification> fac_notification_map;
+ std::map<StateBlockPtr, Notification> sb_notification_map;
+ std::map<FactorBasePtr, Notification> fac_notification_map;
wolf_problem_->consumeNotifications(sb_notification_map, fac_notification_map);
- #ifdef _WOLF_DEBUG
- assert(check("before update()"));
- #endif
+#ifdef _WOLF_DEBUG
+ assert(check("before update()"));
+#endif
// REMOVE FACTORS
- for (auto fac_notification_it = fac_notification_map.begin();
- fac_notification_it != fac_notification_map.end();
+ for (auto fac_notification_it = fac_notification_map.begin(); fac_notification_it != fac_notification_map.end();
/* nothing, next is handled within the for */)
{
if (fac_notification_it->second == REMOVE)
@@ -83,11 +83,10 @@ void SolverManager::update()
}
// ADD/REMOVE STATE BLOCS
- while ( !sb_notification_map.empty() )
+ while (!sb_notification_map.empty())
{
// add
- if (sb_notification_map.begin()->second == ADD)
- addStateBlock(sb_notification_map.begin()->first);
+ if (sb_notification_map.begin()->second == ADD) addStateBlock(sb_notification_map.begin()->first);
// remove
else
@@ -100,7 +99,8 @@ void SolverManager::update()
// ADD FACTORS
while (!fac_notification_map.empty())
{
- assert(fac_notification_map.begin()->second == ADD && "unexpected factor notification value after all REMOVE have been processed, this should be ADD");
+ assert(fac_notification_map.begin()->second == ADD &&
+ "unexpected factor notification value after all REMOVE have been processed, this should be ADD");
// add factor
addFactor(fac_notification_map.begin()->first);
@@ -118,31 +118,31 @@ void SolverManager::update()
// Check for "floating" state blocks (not involved in any factor -> not observable problem)
if (state_blocks_2_factors_.at(state_ptr).empty())
{
- WOLF_DEBUG("SolverManager::update(): StateBlock ", state_ptr, " is 'Floating' (not involved in any factor). Storing it apart.");
+ WOLF_DEBUG("SolverManager::update(): StateBlock ",
+ state_ptr,
+ " is 'Floating' (not involved in any factor). Storing it apart.");
new_floating_state_blocks.insert(state_ptr);
continue;
}
// state update
- if (state_ptr->stateUpdated())
- updateStateBlockState(state_ptr);
+ if (state_ptr->stateUpdated()) updateStateBlockState(state_ptr);
// fix update
- if (state_ptr->fixUpdated())
- updateStateBlockStatus(state_ptr);
+ if (state_ptr->fixUpdated()) updateStateBlockStatus(state_ptr);
// local parameterization update
- if (state_ptr->localParamUpdated())
- updateStateBlockLocalParametrization(state_ptr);
+ if (state_ptr->localParamUpdated()) updateStateBlockLocalParametrization(state_ptr);
}
// REMOVE NEWLY DETECTED "floating" STATE BLOCKS (will be added next update() call)
for (auto state_ptr : new_floating_state_blocks)
{
removeStateBlock(state_ptr);
- floating_state_blocks_.insert(state_ptr); // This line must be AFTER removeStateBlock()!
+ floating_state_blocks_.insert(state_ptr); // This line must be AFTER removeStateBlock()!
}
- // RESET FLAGS for floating state blocks: meaning "solver handled this change" (state, fix and local param will be set in addStateBlock)
+ // RESET FLAGS for floating state blocks: meaning "solver handled this change" (state, fix and local param will be
+ // set in addStateBlock)
for (auto state_ptr : floating_state_blocks_)
{
state_ptr->resetStateUpdated();
@@ -152,8 +152,8 @@ void SolverManager::update()
wolf_problem_->resetTransformed();
#ifdef _WOLF_DEBUG
- assert(check("after update()"));
- #endif
+ assert(check("after update()"));
+#endif
}
wolf::ProblemPtr SolverManager::getProblem()
@@ -163,7 +163,7 @@ wolf::ProblemPtr SolverManager::getProblem()
std::string SolverManager::solve()
{
- return solve(params_->verbose);
+ return solve(verbose_);
}
std::string SolverManager::solve(const ReportVerbosity report_level)
@@ -174,16 +174,18 @@ std::string SolverManager::solve(const ReportVerbosity report_level)
// update problem
auto start_update = std::chrono::high_resolution_clock::now();
update();
- auto duration_update = std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::high_resolution_clock::now() - start_update);
+ auto duration_update = std::chrono::duration_cast<std::chrono::microseconds>(
+ std::chrono::high_resolution_clock::now() - start_update);
acc_duration_update_ += duration_update;
- max_duration_update_ = std::max(max_duration_update_,duration_update);
+ max_duration_update_ = std::max(max_duration_update_, duration_update);
// call derived solver
- auto start_solver = std::chrono::high_resolution_clock::now();
- std::string report = solveDerived(report_level);
- auto duration_solver = std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::high_resolution_clock::now() - start_solver);
+ auto start_solver = std::chrono::high_resolution_clock::now();
+ std::string report = solveDerived(report_level);
+ auto duration_solver = std::chrono::duration_cast<std::chrono::microseconds>(
+ std::chrono::high_resolution_clock::now() - start_solver);
acc_duration_solver_ += duration_solver;
- max_duration_solver_ = std::max(max_duration_solver_,duration_solver);
+ max_duration_solver_ = std::max(max_duration_solver_, duration_solver);
// update StateBlocks with optimized state value.
auto start_state = std::chrono::high_resolution_clock::now();
@@ -201,20 +203,22 @@ std::string SolverManager::solve(const ReportVerbosity report_level)
stateblock_statevector.first->transform(wolf_problem_->getTransformation());
}
}
- auto duration_state = std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::high_resolution_clock::now() - start_state);
+ auto duration_state =
+ std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::high_resolution_clock::now() - start_state);
acc_duration_state_ += duration_state;
- max_duration_state_ = std::max(max_duration_state_,duration_state);
+ max_duration_state_ = std::max(max_duration_state_, duration_state);
- auto duration_total = std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::high_resolution_clock::now() - start_total);
+ auto duration_total =
+ std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::high_resolution_clock::now() - start_total);
acc_duration_total_ += duration_total;
- max_duration_total_ = std::max(max_duration_total_,duration_total);
+ max_duration_total_ = std::max(max_duration_total_, duration_total);
return report;
}
bool SolverManager::computeCovariances()
{
- return computeCovariances(params_->cov_enum);
+ return computeCovariances(cov_enum_);
}
bool SolverManager::computeCovariances(const CovarianceBlocksToBeComputed blocks)
@@ -224,9 +228,10 @@ bool SolverManager::computeCovariances(const CovarianceBlocksToBeComputed blocks
auto ret = computeCovariancesDerived(blocks);
- auto duration_cov = std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::high_resolution_clock::now() - start_cov);
+ auto duration_cov =
+ std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::high_resolution_clock::now() - start_cov);
acc_duration_cov_ += duration_cov;
- max_duration_cov_ = std::max(max_duration_cov_,duration_cov);
+ max_duration_cov_ = std::max(max_duration_cov_, duration_cov);
return ret;
}
@@ -238,9 +243,10 @@ bool SolverManager::computeCovariances(const std::vector<StateBlockPtr>& st_list
auto ret = computeCovariancesDerived(st_list);
- auto duration_cov = std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::high_resolution_clock::now() - start_cov);
+ auto duration_cov =
+ std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::high_resolution_clock::now() - start_cov);
acc_duration_cov_ += duration_cov;
- max_duration_cov_ = std::max(max_duration_cov_,duration_cov);
+ max_duration_cov_ = std::max(max_duration_cov_, duration_cov);
return ret;
}
@@ -267,13 +273,21 @@ void SolverManager::addFactor(const FactorBasePtr& fac_ptr)
// Check if it was stored as a 'floating' state block
if (floating_state_blocks_.count(st) == 1)
{
- WOLF_DEBUG("SolverManager::addFactor(): Factor ", fac_ptr->id(), " involves state block ", st, " stored as 'floating'. Adding the state block to the solver.");
- floating_state_blocks_.erase(st); // This line must be BEFORE addStateBlock()!
+ WOLF_DEBUG("SolverManager::addFactor(): Factor ",
+ fac_ptr->id(),
+ " involves state block ",
+ st,
+ " stored as 'floating'. Adding the state block to the solver.");
+ floating_state_blocks_.erase(st); // This line must be BEFORE addStateBlock()!
addStateBlock(st);
}
else
{
- WOLF_WARN("SolverManager::addFactor(): Factor ", fac_ptr->id(), " is notified to ADD but the involved state block ", st, " is not. Skipping, will be added later.");
+ WOLF_WARN("SolverManager::addFactor(): Factor ",
+ fac_ptr->id(),
+ " is notified to ADD but the involved state block ",
+ st,
+ " is not. Skipping, will be added later.");
// put back notification in problem (will be added next update() call) and do nothing
wolf_problem_->notifyFactor(fac_ptr, ADD);
return;
@@ -287,7 +301,8 @@ void SolverManager::addFactor(const FactorBasePtr& fac_ptr)
for (auto st : fac_ptr->getStateBlockPtrVector())
{
assert(state_blocks_.count(st) != 0 && "SolverManager::addFactor before adding any involved state block");
- assert(state_blocks_2_factors_.count(st) != 0 && "SolverManager::addFactor before adding any involved state block");
+ assert(state_blocks_2_factors_.count(st) != 0 &&
+ "SolverManager::addFactor before adding any involved state block");
state_blocks_2_factors_.at(st).push_back(fac_ptr);
}
@@ -314,7 +329,8 @@ void SolverManager::removeFactor(const FactorBasePtr& fac_ptr)
for (auto st : fac_ptr->getStateBlockPtrVector())
{
assert(state_blocks_.count(st) != 0 && "SolverManager::removeFactor missing any involved state block");
- assert(state_blocks_2_factors_.count(st) != 0 && "SolverManager::removeFactor missing any involved state block");
+ assert(state_blocks_2_factors_.count(st) != 0 &&
+ "SolverManager::removeFactor missing any involved state block");
state_blocks_2_factors_.at(st).remove(fac_ptr);
}
}
@@ -336,7 +352,8 @@ void SolverManager::addStateBlock(const StateBlockPtr& state_ptr)
assert(state_blocks_.count(state_ptr) == 0 && "SolverManager::addStateBlock state block already added");
assert(state_blocks_2_factors_.count(state_ptr) == 0 && "SolverManager::addStateBlock state block already added");
- assert(state_ptr->isValid() && "SolverManager::addStateBlock state block state not valid (local parameterization)");
+ assert(state_ptr->isValid() &&
+ "SolverManager::addStateBlock state block state not valid (local parameterization)");
// stateblock maps
state_blocks_.emplace(state_ptr, state_ptr->getState());
@@ -345,7 +362,8 @@ void SolverManager::addStateBlock(const StateBlockPtr& state_ptr)
// derived
addStateBlockDerived(state_ptr);
- // A state_block is added with its last state_ptr, status and local_param, thus, no update is needed for any of those things -> reset flags
+ // A state_block is added with its last state_ptr, status and local_param, thus, no update is needed for any of
+ // those things -> reset flags
state_ptr->resetStateUpdated();
state_ptr->resetFixUpdated();
state_ptr->resetLocalParamUpdated();
@@ -378,15 +396,19 @@ void SolverManager::removeStateBlock(const StateBlockPtr& state_ptr)
*/
if (!state_blocks_2_factors_.at(state_ptr).empty())
{
- WOLF_WARN("SolverManager::removeStateBlock(): StateBlock ", state_ptr, " is notified to REMOVE but ", state_blocks_2_factors_.at(state_ptr).size(), " involved factors still not removed. Skipping, will be removed later.");
+ WOLF_WARN("SolverManager::removeStateBlock(): StateBlock ",
+ state_ptr,
+ " is notified to REMOVE but ",
+ state_blocks_2_factors_.at(state_ptr).size(),
+ " involved factors still not removed. Skipping, will be removed later.");
// put back notification in problem (will be removed next update() call) and do nothing
- for (auto fac : state_blocks_2_factors_.at(state_ptr))
- WOLF_INFO(fac->id());
+ for (auto fac : state_blocks_2_factors_.at(state_ptr)) WOLF_INFO(fac->id());
wolf_problem_->notifyStateBlock(state_ptr, REMOVE);
return;
}
- assert(state_blocks_2_factors_.at(state_ptr).empty() && "SolverManager::removeStateBlock removing state block before removing all factors involved");
+ assert(state_blocks_2_factors_.at(state_ptr).empty() &&
+ "SolverManager::removeStateBlock removing state block before removing all factors involved");
// derived
removeStateBlockDerived(state_ptr);
@@ -409,11 +431,12 @@ void SolverManager::updateStateBlockState(const StateBlockPtr& state_ptr)
{
assert(state_ptr && "SolverManager::updateStateBlockState null state block");
assert(state_blocks_.count(state_ptr) == 1 && "SolverManager::updateStateBlockState unregistered state block");
- assert(state_ptr->isValid() && "SolverManager::updateStateBlockState state block state not valid (local parameterization)");
+ assert(state_ptr->isValid() &&
+ "SolverManager::updateStateBlockState state block state not valid (local parameterization)");
assert(state_ptr->getState().size() == getAssociatedMemBlock(state_ptr).size());
Eigen::VectorXd new_state = state_ptr->getState();
- std::copy(new_state.data(),new_state.data()+new_state.size(),getAssociatedMemBlockPtr(state_ptr));
+ std::copy(new_state.data(), new_state.data() + new_state.size(), getAssociatedMemBlockPtr(state_ptr));
// reset flag
state_ptr->resetStateUpdated();
}
@@ -465,12 +488,13 @@ double* SolverManager::getAssociatedMemBlockPtr(const StateBlockPtr& state_ptr)
SolverManager::ReportVerbosity SolverManager::getVerbosity() const
{
- return params_->verbose;
+ return verbose_;
}
bool SolverManager::isStateBlockRegistered(const StateBlockPtr& state_ptr) const
{
- return isStateBlockFloating(state_ptr) or (state_blocks_.count(state_ptr) == 1 and isStateBlockRegisteredDerived(state_ptr));
+ return isStateBlockFloating(state_ptr) or
+ (state_blocks_.count(state_ptr) == 1 and isStateBlockRegisteredDerived(state_ptr));
}
bool SolverManager::isStateBlockFloating(const StateBlockPtr& state_ptr) const
@@ -485,33 +509,28 @@ bool SolverManager::isFactorRegistered(const FactorBasePtr& fac_ptr) const
bool SolverManager::isStateBlockFixed(const StateBlockPtr& st)
{
- if (!isStateBlockRegistered(st))
- return false;
+ if (!isStateBlockRegistered(st)) return false;
- if (isStateBlockFloating(st))
- return st->isFixed();
+ if (isStateBlockFloating(st)) return st->isFixed();
return isStateBlockFixedDerived(st);
}
-bool SolverManager::hasThisLocalParametrization(const StateBlockPtr& st, const LocalParametrizationBasePtr& local_param)
+bool SolverManager::hasThisLocalParametrization(const StateBlockPtr& st,
+ const LocalParametrizationBasePtr& local_param)
{
- if (!isStateBlockRegistered(st))
- return false;
+ if (!isStateBlockRegistered(st)) return false;
- if (isStateBlockFloating(st))
- return st->getLocalParametrization() == local_param;
+ if (isStateBlockFloating(st)) return st->getLocalParametrization() == local_param;
return hasThisLocalParametrizationDerived(st, local_param);
};
bool SolverManager::hasLocalParametrization(const StateBlockPtr& st) const
{
- if (!isStateBlockRegistered(st))
- return false;
+ if (!isStateBlockRegistered(st)) return false;
- if (isStateBlockFloating(st))
- return st->hasLocalParametrization();
+ if (isStateBlockFloating(st)) return st->hasLocalParametrization();
return hasLocalParametrizationDerived(st);
};
@@ -531,19 +550,19 @@ int SolverManager::numStateBlocksFloating() const
return floating_state_blocks_.size();
}
-ParamsSolverPtr SolverManager::getParams() const
+YAML::Node SolverManager::getParams() const
{
- return params_;
+ return Clone(params_);
}
double SolverManager::getPeriod() const
{
- return params_->period;
+ return period_;
}
double SolverManager::getCovPeriod() const
{
- return params_->cov_period;
+ return cov_period_;
}
bool SolverManager::check(std::string prefix) const
@@ -553,7 +572,9 @@ bool SolverManager::check(std::string prefix) const
// state blocks
if (state_blocks_.size() != state_blocks_2_factors_.size())
{
- WOLF_ERROR("SolverManager::check: mismatching number of state blocks (state_blocks_, state_blocks_2_factors_) - in ", prefix);
+ WOLF_ERROR(
+ "SolverManager::check: mismatching number of state blocks (state_blocks_, state_blocks_2_factors_) - in ",
+ prefix);
ok = false;
}
auto sb_vec_it = state_blocks_.begin();
@@ -563,14 +584,22 @@ bool SolverManager::check(std::string prefix) const
// same state block in both maps
if (sb_vec_it->first != sb_fac_it->first)
{
- WOLF_ERROR("SolverManager::check: mismatching state blocks ", sb_vec_it->first, " and ", sb_fac_it->first, " - in ", prefix);
+ WOLF_ERROR("SolverManager::check: mismatching state blocks ",
+ sb_vec_it->first,
+ " and ",
+ sb_fac_it->first,
+ " - in ",
+ prefix);
ok = false;
}
// no factors involving state block
if (sb_fac_it->second.empty())
{
- WOLF_ERROR("SolverManager::check: state block ", sb_fac_it->first, " is in state_blocks_ but not involved in any factor - in ", prefix);
+ WOLF_ERROR("SolverManager::check: state block ",
+ sb_fac_it->first,
+ " is in state_blocks_ but not involved in any factor - in ",
+ prefix);
ok = false;
}
else
@@ -580,7 +609,12 @@ bool SolverManager::check(std::string prefix) const
{
if (factors_.count(fac) == 0)
{
- WOLF_ERROR("SolverManager::check: factor ", fac->id(), " (involved in sb ", sb_fac_it->first, ") missing in factors_ map - in ", prefix);
+ WOLF_ERROR("SolverManager::check: factor ",
+ fac->id(),
+ " (involved in sb ",
+ sb_fac_it->first,
+ ") missing in factors_ map - in ",
+ prefix);
ok = false;
}
}
@@ -589,7 +623,10 @@ bool SolverManager::check(std::string prefix) const
// can't be in both state_blocks_ and floating_state_blocks_
if (floating_state_blocks_.count(sb_fac_it->first) == 1)
{
- WOLF_ERROR("SolverManager::check: state block ", sb_fac_it->first, " is both in state_blocks_ and floating_state_blocks_ - in ", prefix);
+ WOLF_ERROR("SolverManager::check: state block ",
+ sb_fac_it->first,
+ " is both in state_blocks_ and floating_state_blocks_ - in ",
+ prefix);
ok = false;
}
@@ -605,7 +642,12 @@ bool SolverManager::check(std::string prefix) const
{
if (state_blocks_.count(sb) == 0)
{
- WOLF_ERROR("SolverManager::check: state block ", sb, " inolved in factor ", fac->id(), " missing in state_blocks_ map - in ", prefix);
+ WOLF_ERROR("SolverManager::check: state block ",
+ sb,
+ " inolved in factor ",
+ fac->id(),
+ " missing in state_blocks_ map - in ",
+ prefix);
ok = false;
}
}
@@ -619,7 +661,10 @@ bool SolverManager::check(std::string prefix) const
for (auto sb : state_blocks_)
if (!isStateBlockRegisteredDerived(sb.first))
{
- WOLF_ERROR("SolverManager::check: state block ", sb.first, " is in state_blocks_ but not registered in derived solver - in ", prefix);
+ WOLF_ERROR("SolverManager::check: state block ",
+ sb.first,
+ " is in state_blocks_ but not registered in derived solver - in ",
+ prefix);
ok = false;
}
@@ -627,19 +672,32 @@ bool SolverManager::check(std::string prefix) const
for (auto fac : factors_)
if (!isFactorRegisteredDerived(fac))
{
- WOLF_ERROR("SolverManager::check: factor ", fac->id(), " is in factors_ but not registered in derived solver - in ", prefix);
+ WOLF_ERROR("SolverManager::check: factor ",
+ fac->id(),
+ " is in factors_ but not registered in derived solver - in ",
+ prefix);
ok = false;
}
// numbers
if (numStateBlocksDerived() != state_blocks_.size())
{
- WOLF_ERROR("SolverManager::check: numStateBlocksDerived() = ", numStateBlocksDerived(), " DIFFERENT THAN state_blocks_.size() = ", state_blocks_.size(), " - in ", prefix);
+ WOLF_ERROR("SolverManager::check: numStateBlocksDerived() = ",
+ numStateBlocksDerived(),
+ " DIFFERENT THAN state_blocks_.size() = ",
+ state_blocks_.size(),
+ " - in ",
+ prefix);
ok = false;
}
if (numFactorsDerived() != numFactors())
{
- WOLF_ERROR("SolverManager::check: numFactorsDerived() = ", numFactorsDerived(), " DIFFERENT THAN numFactors() = ", numFactors(), " - in ", prefix);
+ WOLF_ERROR("SolverManager::check: numFactorsDerived() = ",
+ numFactorsDerived(),
+ " DIFFERENT THAN numFactors() = ",
+ numFactors(),
+ " - in ",
+ prefix);
ok = false;
}
@@ -648,27 +706,30 @@ bool SolverManager::check(std::string prefix) const
void SolverManager::printProfiling(std::ostream& _stream) const
{
- _stream <<"\nSolverManager:"
- <<"\nTotal values:"
- << "\n\tSolving state: " << 1e-6*acc_duration_total_.count() << " s - executions: " << n_solve_
- << "\n\t\tUpdate problem: " << 1e-6*acc_duration_update_.count() << " s" << " (" << 100.0 * acc_duration_update_.count() / acc_duration_total_.count() << " %)"
- << "\n\t\tSolver: " << 1e-6*acc_duration_solver_.count() << " s" << " (" << 100.0 * acc_duration_solver_.count() / acc_duration_total_.count() << " %)"
- << "\n\t\tUpdate state: " << 1e-6*acc_duration_state_.count() << " s" << " (" << 100.0 * acc_duration_state_.count() / acc_duration_total_.count() << " %)"
- << "\n\tSolving covariance: " << 1e-6*acc_duration_cov_.count() << " s - executions: " << n_cov_
- <<"\nAverage:"
- << "\n\tSolving state: " << 1e-3*acc_duration_total_.count() / n_solve_ << " ms"
- << "\n\t\tUpdate problem: " << 1e-3*acc_duration_update_.count() / n_solve_ << " ms"
- << "\n\t\tSolver: " << 1e-3*acc_duration_solver_.count() / n_solve_ << " ms"
- << "\n\t\tUpdate state: " << 1e-3*acc_duration_state_.count() / n_solve_ << " ms"
- << "\n\tSolving covariance: " << 1e-3*acc_duration_cov_.count() / n_cov_ << " ms"
- <<"\nMax values:"
- << "\n\tSolving state: " << 1e-3*max_duration_total_.count() << " ms"
- << "\n\t\tUpdate problem: " << 1e-3*max_duration_update_.count() << " ms"
- << "\n\t\tSolver: " << 1e-3*max_duration_solver_.count() << " ms"
- << "\n\t\tUpdate state: " << 1e-3*max_duration_state_.count() << " ms"
- << "\n\tSolving covariance: " << 1e-3*max_duration_cov_.count() << " ms" << std::endl;
+ _stream << "\nSolverManager:"
+ << "\nTotal values:"
+ << "\n\tSolving state: " << 1e-6 * acc_duration_total_.count() << " s - executions: " << n_solve_
+ << "\n\t\tUpdate problem: " << 1e-6 * acc_duration_update_.count() << " s"
+ << " (" << 100.0 * acc_duration_update_.count() / acc_duration_total_.count() << " %)"
+ << "\n\t\tSolver: " << 1e-6 * acc_duration_solver_.count() << " s"
+ << " (" << 100.0 * acc_duration_solver_.count() / acc_duration_total_.count() << " %)"
+ << "\n\t\tUpdate state: " << 1e-6 * acc_duration_state_.count() << " s"
+ << " (" << 100.0 * acc_duration_state_.count() / acc_duration_total_.count() << " %)"
+ << "\n\tSolving covariance: " << 1e-6 * acc_duration_cov_.count() << " s - executions: " << n_cov_
+ << "\nAverage:"
+ << "\n\tSolving state: " << 1e-3 * acc_duration_total_.count() / n_solve_ << " ms"
+ << "\n\t\tUpdate problem: " << 1e-3 * acc_duration_update_.count() / n_solve_ << " ms"
+ << "\n\t\tSolver: " << 1e-3 * acc_duration_solver_.count() / n_solve_ << " ms"
+ << "\n\t\tUpdate state: " << 1e-3 * acc_duration_state_.count() / n_solve_ << " ms"
+ << "\n\tSolving covariance: " << 1e-3 * acc_duration_cov_.count() / n_cov_ << " ms"
+ << "\nMax values:"
+ << "\n\tSolving state: " << 1e-3 * max_duration_total_.count() << " ms"
+ << "\n\t\tUpdate problem: " << 1e-3 * max_duration_update_.count() << " ms"
+ << "\n\t\tSolver: " << 1e-3 * max_duration_solver_.count() << " ms"
+ << "\n\t\tUpdate state: " << 1e-3 * max_duration_state_.count() << " ms"
+ << "\n\tSolving covariance: " << 1e-3 * max_duration_cov_.count() << " ms" << std::endl;
printProfilingDerived(_stream);
}
-} // namespace wolf
+} // namespace wolf
diff --git a/src/solver_suitesparse/solver_manager.cpp b/src/solver_suitesparse/solver_manager.cpp
index 4b2a57342976d31e547ace486092392b96f503b9..56c3bc896d4a7229defcff172a38242239c141bd 100644
--- a/src/solver_suitesparse/solver_manager.cpp
+++ b/src/solver_suitesparse/solver_manager.cpp
@@ -20,73 +20,66 @@
#include "core/ceres_wrapper/solver_ceres.h"
-SolverManager::SolverManager()
-{
-
-}
+SolverManager::SolverManager() {}
SolverManager::~SolverManager()
{
- removeAllStateUnits();
+ removeAllStateUnits();
}
-void SolverManager::solve()
-{
-
-}
+void SolverManager::solve() {}
-//void SolverManager::computeCovariances(WolfProblemPtr _problem_ptr)
+// void SolverManager::computeCovariances(WolfProblemPtr _problem_ptr)
//{
//}
void SolverManager::update(const WolfProblemPtr _problem_ptr)
{
- // IF REALLOCATION OF STATE, REMOVE EVERYTHING AND BUILD THE PROBLEM AGAIN
- if (_problem_ptr->isReallocated())
- {
- // todo: reallocate x
- }
- else
- {
- // ADD/UPDATE STATE BLOKS
- for(auto state_block : _problem_ptr->getStateList())
- {
- if (state_block->getPendingStatus() == ADD_PENDING)
- addStateUnit(state_block);
-
- else if(state_block->getPendingStatus() == UPDATE_PENDING)
- updateStateUnitStatus(state_block);
- }
- //std::cout << "state units updated!" << std::endl;
-
- // REMOVE STATE UNITS
- while (!_problem_ptr->getRemovedStateList().empty())
- {
- // TODO: remove state unit
- //_problem_ptr->getRemovedStateList().pop_front();
- }
- //std::cout << "state units removed!" << std::endl;
-
- // ADD CONSTRAINTS
- FactorBasePtrList fac_list;
- _problem_ptr->getTrajectory()->getFactorList(fac_list);
- //std::cout << "fac_list.size() = " << fac_list.size() << std::endl;
- for(auto fac_it = fac_list.begin(); fac_it!=fac_list.end(); fac_it++)
- if ((*fac_it)->getPendingStatus() == ADD_PENDING)
- addFactor(*fac_it);
-
- //std::cout << "factors updated!" << std::endl;
- }
+ // IF REALLOCATION OF STATE, REMOVE EVERYTHING AND BUILD THE PROBLEM AGAIN
+ if (_problem_ptr->isReallocated())
+ {
+ // todo: reallocate x
+ }
+ else
+ {
+ // ADD/UPDATE STATE BLOKS
+ for (auto state_block : _problem_ptr->getStateList())
+ {
+ if (state_block->getPendingStatus() == ADD_PENDING)
+ addStateUnit(state_block);
+
+ else if (state_block->getPendingStatus() == UPDATE_PENDING)
+ updateStateUnitStatus(state_block);
+ }
+ // std::cout << "state units updated!" << std::endl;
+
+ // REMOVE STATE UNITS
+ while (!_problem_ptr->getRemovedStateList().empty())
+ {
+ // TODO: remove state unit
+ //_problem_ptr->getRemovedStateList().pop_front();
+ }
+ // std::cout << "state units removed!" << std::endl;
+
+ // ADD CONSTRAINTS
+ FactorBasePtrList fac_list;
+ _problem_ptr->getTrajectory()->getFactorList(fac_list);
+ // std::cout << "fac_list.size() = " << fac_list.size() << std::endl;
+ for (auto fac_it = fac_list.begin(); fac_it != fac_list.end(); fac_it++)
+ if ((*fac_it)->getPendingStatus() == ADD_PENDING) addFactor(*fac_it);
+
+ // std::cout << "factors updated!" << std::endl;
+ }
}
void SolverManager::addFactor(FactorBasePtr _corr_ptr)
{
- //TODO MatrixXd J; Vector e;
+ // TODO MatrixXd J; Vector e;
// getResidualsAndJacobian(_corr_ptr, J, e);
// solverQR->addFactor(_corr_ptr, J, e);
-// factor_map_[_corr_ptr->id()] = blockIdx;
- _corr_ptr->setPendingStatus(NOT_PENDING);
+ // factor_map_[_corr_ptr->id()] = blockIdx;
+ _corr_ptr->setPendingStatus(NOT_PENDING);
}
void SolverManager::removeFactor(const unsigned int& _corr_idx)
@@ -96,170 +89,160 @@ void SolverManager::removeFactor(const unsigned int& _corr_idx)
void SolverManager::addStateUnit(StateBlockPtr _st_ptr)
{
- //std::cout << "Adding State Unit " << _st_ptr->id() << std::endl;
- //_st_ptr->print();
-
- switch (_st_ptr->getStateType())
- {
- case ST_COMPLEX_ANGLE:
- {
- // TODO
- //std::cout << "Adding Complex angle Local Parametrization to the List... " << std::endl;
- //ceres_problem_->AddParameterBlock(_st_ptr->get(), ((StateComplexAngle*)_st_ptr)->BLOCK_SIZE, new ComplexAngleParameterization);
- break;
- }
- case ST_THETA:
- {
- //std::cout << "No Local Parametrization to be added" << std::endl;
- ceres_problem_->AddParameterBlock(_st_ptr->get(), ((StateBlockPtr)_st_ptr)->BLOCK_SIZE, nullptr);
- break;
- }
- case ST_POINT_1d:
- {
- //std::cout << "No Local Parametrization to be added" << std::endl;
- ceres_problem_->AddParameterBlock(_st_ptr->get(), ((StatePoint1d*)_st_ptr)->BLOCK_SIZE, nullptr);
- break;
- }
- case ST_VECTOR:
- {
- //std::cout << "No Local Parametrization to be added" << std::endl;
- ceres_problem_->AddParameterBlock(_st_ptr->get(), ((StateBlockPtr)_st_ptr)->BLOCK_SIZE, nullptr);
- break;
- }
- case ST_POINT_3d:
- {
- //std::cout << "No Local Parametrization to be added" << std::endl;
- ceres_problem_->AddParameterBlock(_st_ptr->get(), ((StateBlockPtr)_st_ptr)->BLOCK_SIZE, nullptr);
- break;
- }
- default:
- std::cout << "Unknown Local Parametrization type!" << std::endl;
- }
- if (_st_ptr->isFixed())
- updateStateUnitStatus(_st_ptr);
-
- _st_ptr->setPendingStatus(NOT_PENDING);
+ // std::cout << "Adding State Unit " << _st_ptr->id() << std::endl;
+ //_st_ptr->print();
+
+ switch (_st_ptr->getStateType())
+ {
+ case ST_COMPLEX_ANGLE: {
+ // TODO
+ // std::cout << "Adding Complex angle Local Parametrization to the List... " << std::endl;
+ // ceres_problem_->AddParameterBlock(_st_ptr->get(), ((StateComplexAngle*)_st_ptr)->BLOCK_SIZE, new
+ // ComplexAngleParameterization);
+ break;
+ }
+ case ST_THETA: {
+ // std::cout << "No Local Parametrization to be added" << std::endl;
+ ceres_problem_->AddParameterBlock(_st_ptr->get(), ((StateBlockPtr)_st_ptr)->BLOCK_SIZE, nullptr);
+ break;
+ }
+ case ST_POINT_1d: {
+ // std::cout << "No Local Parametrization to be added" << std::endl;
+ ceres_problem_->AddParameterBlock(_st_ptr->get(), ((StatePoint1d*)_st_ptr)->BLOCK_SIZE, nullptr);
+ break;
+ }
+ case ST_VECTOR: {
+ // std::cout << "No Local Parametrization to be added" << std::endl;
+ ceres_problem_->AddParameterBlock(_st_ptr->get(), ((StateBlockPtr)_st_ptr)->BLOCK_SIZE, nullptr);
+ break;
+ }
+ case ST_POINT_3d: {
+ // std::cout << "No Local Parametrization to be added" << std::endl;
+ ceres_problem_->AddParameterBlock(_st_ptr->get(), ((StateBlockPtr)_st_ptr)->BLOCK_SIZE, nullptr);
+ break;
+ }
+ default:
+ std::cout << "Unknown Local Parametrization type!" << std::endl;
+ }
+ if (_st_ptr->isFixed()) updateStateUnitStatus(_st_ptr);
+
+ _st_ptr->setPendingStatus(NOT_PENDING);
}
void SolverManager::removeAllStateUnits()
{
- std::vector<double*> parameter_blocks;
+ std::vector<double*> parameter_blocks;
- ceres_problem_->GetParameterBlocks(¶meter_blocks);
+ ceres_problem_->GetParameterBlocks(¶meter_blocks);
- for (unsigned int i = 0; i< parameter_blocks.size(); i++)
- ceres_problem_->RemoveParameterBlock(parameter_blocks[i]);
+ for (unsigned int i = 0; i < parameter_blocks.size(); i++)
+ ceres_problem_->RemoveParameterBlock(parameter_blocks[i]);
}
void SolverManager::updateStateUnitStatus(StateBlockPtr _st_ptr)
{
// TODO
-// if (!_st_ptr->isFixed())
-// ceres_problem_->SetParameterBlockVariable(_st_ptr->get());
-// else if (_st_ptr->isFixed())
-// ceres_problem_->SetParameterBlockConstant(_st_ptr->get());
-// else
-// printf("\nERROR: Update state unit status with unknown status");
-//
-// _st_ptr->setPendingStatus(NOT_PENDING);
+ // if (!_st_ptr->isFixed())
+ // ceres_problem_->SetParameterBlockVariable(_st_ptr->get());
+ // else if (_st_ptr->isFixed())
+ // ceres_problem_->SetParameterBlockConstant(_st_ptr->get());
+ // else
+ // printf("\nERROR: Update state unit status with unknown status");
+ //
+ // _st_ptr->setPendingStatus(NOT_PENDING);
}
ceres::CostFunction* SolverManager::createCostFunction(FactorBasePtr _corrPtr)
{
- //std::cout << "adding ctr " << _corrPtr->id() << std::endl;
- //_corrPtr->print();
-
- switch (_corrPtr->getFactorType())
- {
- case FAC_GPS_FIX_2d:
- {
- FactorGPS2d* specific_ptr = (FactorGPS2d*)(_corrPtr);
- return new ceres::AutoDiffCostFunction<FactorGPS2d,
- specific_ptr->residualSize,
- specific_ptr->block0Size,
- specific_ptr->block1Size,
- specific_ptr->block2Size,
- specific_ptr->block3Size,
- specific_ptr->block4Size,
- specific_ptr->block5Size,
- specific_ptr->block6Size,
- specific_ptr->block7Size,
- specific_ptr->block8Size,
- specific_ptr->block9Size>(specific_ptr);
- break;
- }
- case FAC_ODOM_2d_COMPLEX_ANGLE:
- {
- FactorOdom2dComplexAngle* specific_ptr = (FactorOdom2dComplexAngle*)(_corrPtr);
- return new ceres::AutoDiffCostFunction<FactorOdom2dComplexAngle,
- specific_ptr->residualSize,
- specific_ptr->block0Size,
- specific_ptr->block1Size,
- specific_ptr->block2Size,
- specific_ptr->block3Size,
- specific_ptr->block4Size,
- specific_ptr->block5Size,
- specific_ptr->block6Size,
- specific_ptr->block7Size,
- specific_ptr->block8Size,
- specific_ptr->block9Size>(specific_ptr);
- break;
- }
- case FAC_ODOM_2d:
- {
- FactorOdom2d* specific_ptr = (FactorOdom2d*)(_corrPtr);
- return new ceres::AutoDiffCostFunction<FactorOdom2d,
- specific_ptr->residualSize,
- specific_ptr->block0Size,
- specific_ptr->block1Size,
- specific_ptr->block2Size,
- specific_ptr->block3Size,
- specific_ptr->block4Size,
- specific_ptr->block5Size,
- specific_ptr->block6Size,
- specific_ptr->block7Size,
- specific_ptr->block8Size,
- specific_ptr->block9Size>(specific_ptr);
- break;
- }
- case FAC_CORNER_2d:
- {
- FactorCorner2d* specific_ptr = (FactorCorner2d*)(_corrPtr);
- return new ceres::AutoDiffCostFunction<FactorCorner2d,
- specific_ptr->residualSize,
- specific_ptr->block0Size,
- specific_ptr->block1Size,
- specific_ptr->block2Size,
- specific_ptr->block3Size,
- specific_ptr->block4Size,
- specific_ptr->block5Size,
- specific_ptr->block6Size,
- specific_ptr->block7Size,
- specific_ptr->block8Size,
- specific_ptr->block9Size>(specific_ptr);
- break;
- }
- case FAC_IMU:
- {
- FactorIMU* specific_ptr = (FactorIMU*)(_corrPtr);
- return new ceres::AutoDiffCostFunction<FactorIMU,
- specific_ptr->residualSize,
- specific_ptr->block0Size,
- specific_ptr->block1Size,
- specific_ptr->block2Size,
- specific_ptr->block3Size,
- specific_ptr->block4Size,
- specific_ptr->block5Size,
- specific_ptr->block6Size,
- specific_ptr->block7Size,
- specific_ptr->block8Size,
- specific_ptr->block9Size>(specific_ptr);
- break;
- }
- default:
- std::cout << "Unknown factor type! Please add it in the CeresWrapper::createCostFunction()" << std::endl;
-
- return nullptr;
- }
+ // std::cout << "adding ctr " << _corrPtr->id() << std::endl;
+ //_corrPtr->print();
+
+ switch (_corrPtr->getFactorType())
+ {
+ case FAC_GPS_FIX_2d: {
+ FactorGPS2d* specific_ptr = (FactorGPS2d*)(_corrPtr);
+ return new ceres::AutoDiffCostFunction<FactorGPS2d,
+ specific_ptr->residualSize,
+ specific_ptr->block0Size,
+ specific_ptr->block1Size,
+ specific_ptr->block2Size,
+ specific_ptr->block3Size,
+ specific_ptr->block4Size,
+ specific_ptr->block5Size,
+ specific_ptr->block6Size,
+ specific_ptr->block7Size,
+ specific_ptr->block8Size,
+ specific_ptr->block9Size>(specific_ptr);
+ break;
+ }
+ case FAC_ODOM_2d_COMPLEX_ANGLE: {
+ FactorOdom2dComplexAngle* specific_ptr = (FactorOdom2dComplexAngle*)(_corrPtr);
+ return new ceres::AutoDiffCostFunction<FactorOdom2dComplexAngle,
+ specific_ptr->residualSize,
+ specific_ptr->block0Size,
+ specific_ptr->block1Size,
+ specific_ptr->block2Size,
+ specific_ptr->block3Size,
+ specific_ptr->block4Size,
+ specific_ptr->block5Size,
+ specific_ptr->block6Size,
+ specific_ptr->block7Size,
+ specific_ptr->block8Size,
+ specific_ptr->block9Size>(specific_ptr);
+ break;
+ }
+ case FAC_ODOM_2d: {
+ FactorOdom2d* specific_ptr = (FactorOdom2d*)(_corrPtr);
+ return new ceres::AutoDiffCostFunction<FactorOdom2d,
+ specific_ptr->residualSize,
+ specific_ptr->block0Size,
+ specific_ptr->block1Size,
+ specific_ptr->block2Size,
+ specific_ptr->block3Size,
+ specific_ptr->block4Size,
+ specific_ptr->block5Size,
+ specific_ptr->block6Size,
+ specific_ptr->block7Size,
+ specific_ptr->block8Size,
+ specific_ptr->block9Size>(specific_ptr);
+ break;
+ }
+ case FAC_CORNER_2d: {
+ FactorCorner2d* specific_ptr = (FactorCorner2d*)(_corrPtr);
+ return new ceres::AutoDiffCostFunction<FactorCorner2d,
+ specific_ptr->residualSize,
+ specific_ptr->block0Size,
+ specific_ptr->block1Size,
+ specific_ptr->block2Size,
+ specific_ptr->block3Size,
+ specific_ptr->block4Size,
+ specific_ptr->block5Size,
+ specific_ptr->block6Size,
+ specific_ptr->block7Size,
+ specific_ptr->block8Size,
+ specific_ptr->block9Size>(specific_ptr);
+ break;
+ }
+ case FAC_IMU: {
+ FactorIMU* specific_ptr = (FactorIMU*)(_corrPtr);
+ return new ceres::AutoDiffCostFunction<FactorIMU,
+ specific_ptr->residualSize,
+ specific_ptr->block0Size,
+ specific_ptr->block1Size,
+ specific_ptr->block2Size,
+ specific_ptr->block3Size,
+ specific_ptr->block4Size,
+ specific_ptr->block5Size,
+ specific_ptr->block6Size,
+ specific_ptr->block7Size,
+ specific_ptr->block8Size,
+ specific_ptr->block9Size>(specific_ptr);
+ break;
+ }
+ default:
+ std::cout << "Unknown factor type! Please add it in the CeresWrapper::createCostFunction()" << std::endl;
+
+ return nullptr;
+ }
}
diff --git a/src/state_block/local_parametrization_base.cpp b/src/state_block/local_parametrization_base.cpp
index 9c5b8b92ca5d10e56779482fa3a2a1cff52819ba..ac00ac4943755ce6d8ba960631936945a85d8445 100644
--- a/src/state_block/local_parametrization_base.cpp
+++ b/src/state_block/local_parametrization_base.cpp
@@ -20,17 +20,15 @@
#include "core/state_block/local_parametrization_base.h"
-namespace wolf {
-
-LocalParametrizationBase::LocalParametrizationBase(unsigned int _global_size, unsigned int _local_size) :
- global_size_(_global_size), local_size_(_local_size)
+namespace wolf
{
-}
-
-LocalParametrizationBase::~LocalParametrizationBase()
+LocalParametrizationBase::LocalParametrizationBase(unsigned int _global_size, unsigned int _local_size)
+ : global_size_(_global_size), local_size_(_local_size)
{
}
+LocalParametrizationBase::~LocalParametrizationBase() {}
+
unsigned int LocalParametrizationBase::getLocalSize() const
{
return local_size_;
@@ -41,15 +39,15 @@ unsigned int LocalParametrizationBase::getGlobalSize() const
return global_size_;
}
-} // namespace wolf
+} // namespace wolf
bool wolf::LocalParametrizationBase::plus(const Eigen::VectorXd &_x,
const Eigen::VectorXd &_delta,
- Eigen::VectorXd &_x_plus_delta) const
+ Eigen::VectorXd & _x_plus_delta) const
{
Eigen::Map<const Eigen::VectorXd> x(_x.data(), _x.size());
Eigen::Map<const Eigen::VectorXd> delta(_delta.data(), _delta.size());
- Eigen::Map<Eigen::VectorXd> x_plus_delta(_x_plus_delta.data(), _x_plus_delta.size());
+ Eigen::Map<Eigen::VectorXd> x_plus_delta(_x_plus_delta.data(), _x_plus_delta.size());
return plus(x, delta, x_plus_delta);
}
diff --git a/src/state_block/local_parametrization_homogeneous.cpp b/src/state_block/local_parametrization_homogeneous.cpp
index 30b9eb046bce356967437d2fb7f314354ad50648..3e01a193e5e0a5935426191a7d3c40bbe42c68de 100644
--- a/src/state_block/local_parametrization_homogeneous.cpp
+++ b/src/state_block/local_parametrization_homogeneous.cpp
@@ -20,12 +20,11 @@
#include "core/state_block/local_parametrization_homogeneous.h"
#include "iostream"
-#include "core/math/rotations.h" // we use quaternion algebra here
+#include "core/math/rotations.h" // we use quaternion algebra here
-namespace wolf {
-
-LocalParametrizationHomogeneous::LocalParametrizationHomogeneous() :
- LocalParametrizationBase(4, 3)
+namespace wolf
+{
+LocalParametrizationHomogeneous::LocalParametrizationHomogeneous() : LocalParametrizationBase(4, 3)
{
//
}
@@ -37,7 +36,7 @@ LocalParametrizationHomogeneous::~LocalParametrizationHomogeneous()
bool LocalParametrizationHomogeneous::plus(Eigen::Map<const Eigen::VectorXd>& _h,
Eigen::Map<const Eigen::VectorXd>& _delta,
- Eigen::Map<Eigen::VectorXd>& _h_plus_delta) const
+ Eigen::Map<Eigen::VectorXd>& _h_plus_delta) const
{
assert(_h.size() == global_size_ && "Wrong size of input homogeneous point.");
assert(_delta.size() == local_size_ && "Wrong size of input delta.");
@@ -45,28 +44,28 @@ bool LocalParametrizationHomogeneous::plus(Eigen::Map<const Eigen::VectorXd>& _h
using namespace Eigen;
- _h_plus_delta = ( exp_q(_delta) * Quaterniond(_h.data()) ).coeffs();
+ _h_plus_delta = (exp_q(_delta) * Quaterniond(_h.data())).coeffs();
return true;
}
bool LocalParametrizationHomogeneous::computeJacobian(Eigen::Map<const Eigen::VectorXd>& _h,
- Eigen::Map<Eigen::MatrixRowXd>& _jacobian) const
+ Eigen::Map<Eigen::MatrixRowXd>& _jacobian) const
{
assert(_h.size() == global_size_ && "Wrong size of input quaternion.");
assert(_jacobian.rows() == global_size_ && _jacobian.cols() == local_size_ && "Wrong size of Jacobian matrix.");
- Eigen::Vector4d hh = _h/2;
- _jacobian << hh(3), hh(2), -hh(1),
- -hh(2), hh(3), hh(0),
- hh(1), -hh(0), hh(3),
- -hh(0), -hh(1), -hh(2) ;
+ Eigen::Vector4d hh = _h / 2;
+ _jacobian << hh(3), hh(2), -hh(1), //
+ -hh(2), hh(3), hh(0), //
+ hh(1), -hh(0), hh(3), //
+ -hh(0), -hh(1), -hh(2);
return true;
}
bool LocalParametrizationHomogeneous::minus(Eigen::Map<const Eigen::VectorXd>& _h1,
Eigen::Map<const Eigen::VectorXd>& _h2,
- Eigen::Map<Eigen::VectorXd>& _h2_minus_h1)
+ Eigen::Map<Eigen::VectorXd>& _h2_minus_h1)
{
using Eigen::Quaterniond;
_h2_minus_h1 = log_q(Quaterniond(_h2.data()) * Quaterniond(_h1.data()).conjugate());
@@ -77,5 +76,4 @@ bool LocalParametrizationHomogeneous::isValid(const Eigen::VectorXd& _state, dou
{
return _state.size() == global_size_;
}
-} // namespace wolf
-
+} // namespace wolf
diff --git a/src/state_block/local_parametrization_quaternion.cpp b/src/state_block/local_parametrization_quaternion.cpp
index ae2a948f62a68cc9b541d3e4236ca76436f782cf..e240ea02e18b4176ec5b112dd0719cc25a48e35d 100644
--- a/src/state_block/local_parametrization_quaternion.cpp
+++ b/src/state_block/local_parametrization_quaternion.cpp
@@ -22,16 +22,15 @@
#include "core/math/rotations.h"
#include <iostream>
-namespace wolf {
-
+namespace wolf
+{
////////// LOCAL PERTURBATION //////////////
template <>
bool LocalParametrizationQuaternion<DQ_LOCAL>::plus(Eigen::Map<const Eigen::VectorXd>& _q,
Eigen::Map<const Eigen::VectorXd>& _delta_theta,
- Eigen::Map<Eigen::VectorXd>& _q_plus_delta_theta) const
+ Eigen::Map<Eigen::VectorXd>& _q_plus_delta_theta) const
{
-
assert(_q.size() == global_size_ && "Wrong size of input quaternion.");
assert(_delta_theta.size() == local_size_ && "Wrong size of input delta_theta.");
assert(_q_plus_delta_theta.size() == global_size_ && "Wrong size of output quaternion.");
@@ -40,23 +39,23 @@ bool LocalParametrizationQuaternion<DQ_LOCAL>::plus(Eigen::Map<const Eigen::Vect
using namespace Eigen;
- _q_plus_delta_theta = ( Quaterniond(_q.data()) * exp_q(_delta_theta) ).coeffs();
+ _q_plus_delta_theta = (Quaterniond(_q.data()) * exp_q(_delta_theta)).coeffs();
return true;
}
template <>
bool LocalParametrizationQuaternion<DQ_LOCAL>::computeJacobian(Eigen::Map<const Eigen::VectorXd>& _q,
- Eigen::Map<Eigen::MatrixRowXd>& _jacobian) const
+ Eigen::Map<Eigen::MatrixRowXd>& _jacobian) const
{
assert(_q.size() == global_size_ && "Wrong size of input quaternion.");
assert(_jacobian.rows() == global_size_ && _jacobian.cols() == local_size_ && "Wrong size of Jacobian matrix.");
- Eigen::Vector4d qq = _q/2;
- _jacobian << qq(3), -qq(2), qq(1),
- qq(2), qq(3), -qq(0),
- -qq(1), qq(0), qq(3),
- -qq(0), -qq(1), -qq(2) ;
+ Eigen::Vector4d qq = _q / 2;
+ _jacobian << qq(3), -qq(2), qq(1), //
+ qq(2), qq(3), -qq(0), //
+ -qq(1), qq(0), qq(3), //
+ -qq(0), -qq(1), -qq(2);
return true;
}
@@ -64,7 +63,7 @@ bool LocalParametrizationQuaternion<DQ_LOCAL>::computeJacobian(Eigen::Map<const
template <>
bool LocalParametrizationQuaternion<DQ_LOCAL>::minus(Eigen::Map<const Eigen::VectorXd>& _q1,
Eigen::Map<const Eigen::VectorXd>& _q2,
- Eigen::Map<Eigen::VectorXd>& _q2_minus_q1)
+ Eigen::Map<Eigen::VectorXd>& _q2_minus_q1)
{
assert(_q1.size() == global_size_ && "Wrong size of input quaternion.");
assert(_q2.size() == global_size_ && "Wrong size of input quaternion.");
@@ -81,9 +80,8 @@ bool LocalParametrizationQuaternion<DQ_LOCAL>::minus(Eigen::Map<const Eigen::Vec
template <>
bool LocalParametrizationQuaternion<DQ_GLOBAL>::plus(Eigen::Map<const Eigen::VectorXd>& _q,
Eigen::Map<const Eigen::VectorXd>& _delta_theta,
- Eigen::Map<Eigen::VectorXd>& _q_plus_delta_theta) const
+ Eigen::Map<Eigen::VectorXd>& _q_plus_delta_theta) const
{
-
assert(_q.size() == global_size_ && "Wrong size of input quaternion.");
assert(_delta_theta.size() == local_size_ && "Wrong size of input delta_theta.");
assert(_q_plus_delta_theta.size() == global_size_ && "Wrong size of output quaternion.");
@@ -92,23 +90,23 @@ bool LocalParametrizationQuaternion<DQ_GLOBAL>::plus(Eigen::Map<const Eigen::Vec
using namespace Eigen;
- _q_plus_delta_theta = ( exp_q(_delta_theta) * Quaterniond(_q.data()) ).coeffs();
+ _q_plus_delta_theta = (exp_q(_delta_theta) * Quaterniond(_q.data())).coeffs();
return true;
}
template <>
bool LocalParametrizationQuaternion<DQ_GLOBAL>::computeJacobian(Eigen::Map<const Eigen::VectorXd>& _q,
- Eigen::Map<Eigen::MatrixRowXd>& _jacobian) const
+ Eigen::Map<Eigen::MatrixRowXd>& _jacobian) const
{
assert(_q.size() == global_size_ && "Wrong size of input quaternion.");
assert(_jacobian.rows() == global_size_ && _jacobian.cols() == local_size_ && "Wrong size of Jacobian matrix.");
- Eigen::Vector4d qq = _q/2;
- _jacobian << qq(3), qq(2), -qq(1),
- -qq(2), qq(3), qq(0),
- qq(1), -qq(0), qq(3),
- -qq(0), -qq(1), -qq(2);
+ Eigen::Vector4d qq = _q / 2;
+ _jacobian << qq(3), qq(2), -qq(1), //
+ -qq(2), qq(3), qq(0), //
+ qq(1), -qq(0), qq(3), //
+ -qq(0), -qq(1), -qq(2);
return true;
}
@@ -116,7 +114,7 @@ bool LocalParametrizationQuaternion<DQ_GLOBAL>::computeJacobian(Eigen::Map<const
template <>
bool LocalParametrizationQuaternion<DQ_GLOBAL>::minus(Eigen::Map<const Eigen::VectorXd>& _q1,
Eigen::Map<const Eigen::VectorXd>& _q2,
- Eigen::Map<Eigen::VectorXd>& _q2_minus_q1)
+ Eigen::Map<Eigen::VectorXd>& _q2_minus_q1)
{
assert(_q1.size() == global_size_ && "Wrong size of input quaternion.");
assert(_q2.size() == global_size_ && "Wrong size of input quaternion.");
@@ -128,4 +126,4 @@ bool LocalParametrizationQuaternion<DQ_GLOBAL>::minus(Eigen::Map<const Eigen::Ve
return true;
}
-} // namespace wolf
+} // namespace wolf
diff --git a/src/state_block/state_block.cpp b/src/state_block/state_block.cpp
index 9765c619e6c146ce3e6a9c4774003b6edfffeaaa..c6642a3925d32b3a775eabb73020a0ca10419d4f 100644
--- a/src/state_block/state_block.cpp
+++ b/src/state_block/state_block.cpp
@@ -21,26 +21,23 @@
#include "core/state_block/state_block.h"
namespace wolf
{
-
void StateBlock::setState(const Eigen::VectorXd& _state, const bool _notify)
{
assert(_state.size() == state_.size());
{
std::lock_guard<std::mutex> lock(mut_state_);
- state_ = _state;
+ state_ = _state;
state_size_ = state_.size();
}
// Flag
- if (_notify)
- state_updated_.store(true);
+ if (_notify) state_updated_.store(true);
}
void StateBlock::setFixed(bool _fixed)
{
// Flag
- if (fixed_.load() != _fixed)
- fix_updated_.store(true);
+ if (fixed_.load() != _fixed) fix_updated_.store(true);
// set
fixed_.store(_fixed);
@@ -53,7 +50,7 @@ void StateBlock::perturb(double amplitude)
this->plus(perturbation);
}
-void StateBlock::plus(const Eigen::VectorXd &_dv)
+void StateBlock::plus(const Eigen::VectorXd& _dv)
{
if (local_param_ptr_ == nullptr)
setState(getState() + _dv);
@@ -65,15 +62,16 @@ void StateBlock::plus(const Eigen::VectorXd &_dv)
}
}
-}
+} // namespace wolf
#include "core/state_block/factory_state_block.h"
#include "core/state_block/state_quaternion.h"
#include "core/state_block/state_angle.h"
#include "core/state_block/state_homogeneous_3d.h"
-namespace wolf{
+namespace wolf
+{
WOLF_REGISTER_STATEBLOCK(StateQuaternion);
WOLF_REGISTER_STATEBLOCK(StateAngle);
WOLF_REGISTER_STATEBLOCK(StateHomogeneous3d);
-}
+} // namespace wolf
diff --git a/src/state_block/state_block_derived.cpp b/src/state_block/state_block_derived.cpp
index e2c3ff10e2a4b8f41aeb35904b35fd710426f5f3..99e8f61cd283ba3771b261ea243e37873dd8bf81 100644
--- a/src/state_block/state_block_derived.cpp
+++ b/src/state_block/state_block_derived.cpp
@@ -23,7 +23,6 @@
namespace wolf
{
-
WOLF_REGISTER_STATEBLOCK(StatePoint2d);
WOLF_REGISTER_STATEBLOCK(StateVector2d);
WOLF_REGISTER_STATEBLOCK(StatePoint3d);
diff --git a/src/trajectory/trajectory_base.cpp b/src/trajectory/trajectory_base.cpp
index 5b9d96728e4588b9cd00651895752bfb839277bb..aa42661a252c37ecd9a5955afcbfac2dc1224b07 100644
--- a/src/trajectory/trajectory_base.cpp
+++ b/src/trajectory/trajectory_base.cpp
@@ -22,13 +22,9 @@
#include "core/frame/frame_base.h"
#include <iterator>
-namespace wolf {
-
-TrajectoryBase::TrajectoryBase() :
- NodeBase("TRAJECTORY", "TrajectoryBase"),
- frame_map_()
+namespace wolf
{
-}
+TrajectoryBase::TrajectoryBase() : NodeBase("TRAJECTORY", "TrajectoryBase"), frame_map_() {}
TrajectoryBase::~TrajectoryBase()
{
@@ -38,7 +34,8 @@ TrajectoryBase::~TrajectoryBase()
FrameBasePtr TrajectoryBase::addFrame(FrameBasePtr _frame_ptr)
{
// add to list
- assert(frame_map_.count(_frame_ptr->getTimeStamp()) == 0 && "Trying to add a keyframe with the same timestamp of an existing one");
+ assert(frame_map_.count(_frame_ptr->getTimeStamp()) == 0 &&
+ "Trying to add a keyframe with the same timestamp of an existing one");
frame_map_.emplace(_frame_ptr->getTimeStamp(), _frame_ptr);
@@ -50,44 +47,39 @@ void TrajectoryBase::removeFrame(FrameBasePtr _frame_ptr)
frame_map_.erase(_frame_ptr->getTimeStamp());
}
-void TrajectoryBase::getFactorList(FactorBaseConstPtrList & _fac_list) const
+void TrajectoryBase::getFactorList(FactorBaseConstPtrList& _fac_list) const
{
- for(auto fr_pair: frame_map_)
- if (not fr_pair.second->isRemoving())
- fr_pair.second->getFactorList(_fac_list);
+ for (auto fr_pair : frame_map_)
+ if (not fr_pair.second->isRemoving()) fr_pair.second->getFactorList(_fac_list);
}
-void TrajectoryBase::getFactorList(FactorBasePtrList & _fac_list)
+void TrajectoryBase::getFactorList(FactorBasePtrList& _fac_list)
{
- for(auto fr_pair: frame_map_)
- if (not fr_pair.second->isRemoving())
- fr_pair.second->getFactorList(_fac_list);
+ for (auto fr_pair : frame_map_)
+ if (not fr_pair.second->isRemoving()) fr_pair.second->getFactorList(_fac_list);
}
TimeStamp TrajectoryBase::closestTimeStampToTimeStamp(const TimeStamp& _ts) const
{
- //If frame_map_ is empty then closestFrameToTimeStamp is meaningless
- if(frame_map_.empty())
- return TimeStamp::Invalid();
+ // If frame_map_ is empty then closestFrameToTimeStamp is meaningless
+ if (frame_map_.empty()) return TimeStamp::Invalid();
// Lower bound provides the first iterator that does not go before ts (maybe equal or greater)
auto lower_bound = frame_map_.lower_bound(_ts);
// If first frame does not go before ts, it is the closest one
- if ( lower_bound == frame_map_.begin())
- return frame_map_.begin()->first;
+ if (lower_bound == frame_map_.begin()) return frame_map_.begin()->first;
// If last frame goes before ts, it is the closest one as well
- if ( lower_bound == frame_map_.end())
- return frame_map_.rbegin()->first;
+ if (lower_bound == frame_map_.end()) return frame_map_.rbegin()->first;
// Otherwise we have to compare lb and its previous
auto upper_bound = lower_bound;
- lower_bound = std::prev(lower_bound);
-
+ lower_bound = std::prev(lower_bound);
+
auto lb_dist = fabs(lower_bound->first - _ts);
auto ub_dist = fabs(upper_bound->first - _ts);
- if(lb_dist < ub_dist)
+ if (lb_dist < ub_dist)
return lower_bound->first;
else
return upper_bound->first;
@@ -102,8 +94,7 @@ FrameBaseConstPtr TrajectoryBase::closestFrameToTimeStamp(const TimeStamp& _ts)
assert(not closest_ts.ok() or frame_map_.count(closest_ts) != 0);
- if (closest_ts.ok())
- return frame_map_.at(closest_ts);
+ if (closest_ts.ok()) return frame_map_.at(closest_ts);
return nullptr;
}
@@ -114,8 +105,7 @@ FrameBasePtr TrajectoryBase::closestFrameToTimeStamp(const TimeStamp& _ts)
assert(not closest_ts.ok() or frame_map_.count(closest_ts) != 0);
- if (closest_ts.ok())
- return frame_map_.at(closest_ts);
+ if (closest_ts.ok()) return frame_map_.at(closest_ts);
return nullptr;
}
@@ -124,57 +114,59 @@ FrameBaseConstPtr TrajectoryBase::getNextFrame(FrameBaseConstPtr _frame, const u
{
auto frame_it = frame_map_.find(_frame->getTimeStamp());
WOLF_WARN_COND(frame_it == frame_map_.end(), "Frame not found in the frame map!");
-
- if (frame_it == frame_map_.end() or
- std::distance(frame_it, frame_map_.end()) <= i)
- return nullptr;
- return std::next(frame_it,i)->second;
+ if (frame_it == frame_map_.end() or std::distance(frame_it, frame_map_.end()) <= i) return nullptr;
+
+ return std::next(frame_it, i)->second;
}
FrameBasePtr TrajectoryBase::getNextFrame(FrameBasePtr _frame, const unsigned int& i)
{
auto frame_it = frame_map_.find(_frame->getTimeStamp());
WOLF_WARN_COND(frame_it == frame_map_.end(), "Frame not found in the frame map!");
-
- if (frame_it == frame_map_.end() or
- std::distance(frame_it, frame_map_.end()) <= i)
- return nullptr;
- return std::next(frame_it,i)->second;
+ if (frame_it == frame_map_.end() or std::distance(frame_it, frame_map_.end()) <= i) return nullptr;
+
+ return std::next(frame_it, i)->second;
}
FrameBaseConstPtr TrajectoryBase::getPreviousFrame(FrameBaseConstPtr _frame, const unsigned int& i) const
{
auto frame_it = frame_map_.find(_frame->getTimeStamp());
WOLF_WARN_COND(frame_it == frame_map_.end(), "Frame not found in the frame map!");
-
- if (frame_it == frame_map_.end() or
- std::distance(frame_map_.begin(), frame_it) < i)
- return nullptr;
- return std::prev(frame_it,i)->second;
+ if (frame_it == frame_map_.end() or std::distance(frame_map_.begin(), frame_it) < i) return nullptr;
+
+ return std::prev(frame_it, i)->second;
}
FrameBasePtr TrajectoryBase::getPreviousFrame(FrameBasePtr _frame, const unsigned int& i)
{
auto frame_it = frame_map_.find(_frame->getTimeStamp());
WOLF_WARN_COND(frame_it == frame_map_.end(), "Frame not found in the frame map!");
-
- if (frame_it == frame_map_.end() or
- std::distance(frame_map_.begin(), frame_it) < i)
- return nullptr;
- return std::prev(frame_it,i)->second;
-}
+ if (frame_it == frame_map_.end() or std::distance(frame_map_.begin(), frame_it) < i) return nullptr;
+ return std::prev(frame_it, i)->second;
+}
-void TrajectoryBase::printHeader(int _depth, bool _metric, bool _state_blocks, bool _factored_by, std::ostream& _stream, std::string _tabs) const
+void TrajectoryBase::printHeader(int _depth,
+ bool _metric,
+ bool _state_blocks,
+ bool _factored_by,
+ std::ostream& _stream,
+ std::string _tabs) const
{
- _stream << _tabs << "Trajectory" << ((_depth < 1) ? (" -- " + std::to_string(getFrameMap().size()) + "F") : "") << std::endl;
+ _stream << _tabs << "Trajectory" << ((_depth < 1) ? (" -- " + toString(getFrameMap().size()) + "F") : "")
+ << std::endl;
}
-void TrajectoryBase::print(int _depth, bool _factored_by, bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const
+void TrajectoryBase::print(int _depth,
+ bool _factored_by,
+ bool _metric,
+ bool _state_blocks,
+ std::ostream& _stream,
+ std::string _tabs) const
{
printHeader(_depth, _factored_by, _metric, _state_blocks, _stream, _tabs);
if (_depth >= 1)
@@ -185,7 +177,7 @@ void TrajectoryBase::print(int _depth, bool _factored_by, bool _metric, bool _st
CheckLog TrajectoryBase::localCheck(bool _verbose, std::ostream& _stream, std::string _tabs) const
{
- CheckLog log;
+ CheckLog log;
std::stringstream inconsistency_explanation;
if (_verbose)
@@ -194,8 +186,9 @@ CheckLog TrajectoryBase::localCheck(bool _verbose, std::ostream& _stream, std::s
}
// check pointer to Problem
- inconsistency_explanation << "Trj->getProblem() [" << getProblem()
- << "] -> " << " Prb->getTrajectory() [" << getProblem()->getTrajectory() << "] -> Trj [" << this << "] Mismatch!\n";
+ inconsistency_explanation << "Trj->getProblem() [" << getProblem() << "] -> "
+ << " Prb->getTrajectory() [" << getProblem()->getTrajectory() << "] -> Trj [" << this
+ << "] Mismatch!\n";
log.assertTrue((getProblem()->getTrajectory() == shared_from_this()), inconsistency_explanation);
return log;
}
@@ -205,8 +198,7 @@ bool TrajectoryBase::check(CheckLog& _log, bool _verbose, std::ostream& _stream,
auto local_log = localCheck(_verbose, _stream, _tabs);
_log.compose(local_log);
for (auto F_pair : getFrameMap())
- if (F_pair.second)
- F_pair.second->check(_log, _verbose, _stream, _tabs + " ");
+ if (F_pair.second) F_pair.second->check(_log, _verbose, _stream, _tabs + " ");
return _log.is_consistent_;
}
-} // namespace wolf
+} // namespace wolf
diff --git a/src/tree_manager/tree_manager_sliding_window.cpp b/src/tree_manager/tree_manager_sliding_window.cpp
index 1f64403e997db99abc927e3b1ba5115e822f841f..e00e3e1f478af5d13c7017a91bdeb7fc81656910 100644
--- a/src/tree_manager/tree_manager_sliding_window.cpp
+++ b/src/tree_manager/tree_manager_sliding_window.cpp
@@ -22,25 +22,32 @@
namespace wolf
{
+TreeManagerSlidingWindow::TreeManagerSlidingWindow(const YAML::Node& _params)
+ : TreeManagerBase("TreeManagerSlidingWindow", _params)
+{
+ n_frames_ = _params["n_frames"].as<unsigned int>();
+ n_fix_first_frames_ = _params["n_fix_first_frames"].as<unsigned int>();
+ viral_remove_parent_without_children_ = _params["viral_remove_parent_without_children"].as<bool>();
+ if (n_frames_ <= n_fix_first_frames_)
+ throw std::runtime_error(
+ "TreeManagerSlidingWindow: Wrong parameter value. 'n_fix_first_frames' should be lower than "
+ "'n_frames'!");
+}
void TreeManagerSlidingWindow::keyFrameCallback(FrameBasePtr _frame)
{
- int n_f = getProblem()->getTrajectory()->size();
- bool remove_first = (n_f > params_sw_->n_frames);
- int n_fix = (n_f >= params_sw_->n_frames ?
- params_sw_->n_fix_first_frames :
- n_f - (params_sw_->n_frames - params_sw_->n_fix_first_frames));
+ int n_f = getProblem()->getTrajectory()->size();
+ bool remove_first = (n_f > n_frames_);
+ int n_fix = (n_f >= n_frames_ ? n_fix_first_frames_ : n_f - (n_frames_ - n_fix_first_frames_));
- auto frame = (remove_first ?
- getProblem()->getTrajectory()->getFirstFrame()->getNextFrame() :
- getProblem()->getTrajectory()->getFirstFrame());
- int fixed_frames = 0;
+ auto frame = (remove_first ? getProblem()->getTrajectory()->getFirstFrame()->getNextFrame()
+ : getProblem()->getTrajectory()->getFirstFrame());
+ int fixed_frames = 0;
// Fix n_fix first frames
while (fixed_frames < n_fix)
{
- if (not frame)
- break;
+ if (not frame) break;
if (not frame->isFixed())
{
WOLF_DEBUG("TreeManagerSlidingWindow fixing frame ", frame->id());
@@ -54,12 +61,11 @@ void TreeManagerSlidingWindow::keyFrameCallback(FrameBasePtr _frame)
if (remove_first)
{
WOLF_DEBUG("TreeManagerSlidingWindow removing first frame");
- getProblem()->getTrajectory()->getFirstFrame()->remove(params_sw_->viral_remove_parent_without_children);
+ getProblem()->getTrajectory()->getFirstFrame()->remove(viral_remove_parent_without_children_);
}
}
// Register in the FactoryTreeManager
WOLF_REGISTER_TREE_MANAGER(TreeManagerSlidingWindow);
-} // namespace wolf
-
+} // namespace wolf
diff --git a/src/tree_manager/tree_manager_sliding_window_dual_rate.cpp b/src/tree_manager/tree_manager_sliding_window_dual_rate.cpp
index eb8567761c07658a6289260f16907553ffc4bed1..8d3bf82a6c887e5671a47a8ab92c76201c3449be 100644
--- a/src/tree_manager/tree_manager_sliding_window_dual_rate.cpp
+++ b/src/tree_manager/tree_manager_sliding_window_dual_rate.cpp
@@ -24,29 +24,34 @@
namespace wolf
{
-
-TreeManagerSlidingWindowDualRate::TreeManagerSlidingWindowDualRate(ParamsTreeManagerSlidingWindowDualRatePtr _params) :
- TreeManagerSlidingWindow(_params),
- params_swdr_(_params),
- count_frames_(0)
+TreeManagerSlidingWindowDualRate::TreeManagerSlidingWindowDualRate(const YAML::Node& _params)
+ : TreeManagerSlidingWindow(_params), count_frames_(0)
{
NodeBase::setType("TreeManagerSlidingWindowDualRate");
+
+ n_frames_recent_ = _params["n_frames_recent"].as<unsigned int>();
+ rate_old_frames_ = _params["rate_old_frames"].as<unsigned int>();
+ if (n_frames_recent_ >= n_frames_)
+ {
+ throw std::runtime_error(
+ "ParamsTreeManagerSlidingWindowDualRate: 'n_frames_recent' should be smaller than 'n_frames'");
+ }
}
void TreeManagerSlidingWindowDualRate::keyFrameCallback(FrameBasePtr _frame)
{
- int n_f = getProblem()->getTrajectory()->size(); // in trajectory there are only key frames
+ int n_f = getProblem()->getTrajectory()->size(); // in trajectory there are only key frames
// recent segment not complete
- if (n_f <= params_swdr_->n_frames_recent)
- return;
+ if (n_f <= n_frames_recent_) return;
// REMOVE FIRST RECENT FRAME: all recent frames except one of each rate_old_frames
if (count_frames_ != 0)
{
WOLF_DEBUG("TreeManagerSlidingWindow removing the oldest of recent frames");
- FrameBasePtr remove_recent_frame = getProblem()->getTrajectory()->getLastFrame()->getPreviousFrame(params_swdr_->n_frames_recent);
- FrameBasePtr keep_recent_frame = remove_recent_frame->getNextFrame();
+ FrameBasePtr remove_recent_frame =
+ getProblem()->getTrajectory()->getLastFrame()->getPreviousFrame(n_frames_recent_);
+ FrameBasePtr keep_recent_frame = remove_recent_frame->getNextFrame();
// compose motion captures for all processors motion
for (auto motion_provider_pair : getProblem()->getMotionProviderMap())
@@ -61,8 +66,10 @@ void TreeManagerSlidingWindowDualRate::keyFrameCallback(FrameBasePtr _frame)
continue;
}
- auto cap_prev = std::dynamic_pointer_cast<CaptureMotion>(remove_recent_frame->getCaptureOf(proc_motion->getSensor()));
- auto cap_next = std::dynamic_pointer_cast<CaptureMotion>(keep_recent_frame->getCaptureOf(proc_motion->getSensor()));
+ auto cap_prev =
+ std::dynamic_pointer_cast<CaptureMotion>(remove_recent_frame->getCaptureOf(proc_motion->getSensor()));
+ auto cap_next =
+ std::dynamic_pointer_cast<CaptureMotion>(keep_recent_frame->getCaptureOf(proc_motion->getSensor()));
// merge captures (if exist)
if (cap_prev and cap_next)
@@ -74,7 +81,7 @@ void TreeManagerSlidingWindowDualRate::keyFrameCallback(FrameBasePtr _frame)
}
// remove frame
- remove_recent_frame->remove(params_swdr_->viral_remove_parent_without_children);
+ remove_recent_frame->remove(viral_remove_parent_without_children_);
}
// Call tree manager sliding window
// It will remove oldest frame if tfirst recent frame has been kept
@@ -82,12 +89,10 @@ void TreeManagerSlidingWindowDualRate::keyFrameCallback(FrameBasePtr _frame)
// iterate counter
count_frames_++;
- if (count_frames_ == params_swdr_->rate_old_frames)
- count_frames_ = 0;
+ if (count_frames_ == rate_old_frames_) count_frames_ = 0;
}
// Register in the FactoryTreeManager
WOLF_REGISTER_TREE_MANAGER(TreeManagerSlidingWindowDualRate);
-} // namespace wolf
-
+} // namespace wolf
diff --git a/src/utils/graph_search.cpp b/src/utils/graph_search.cpp
index cf48d302a5688d907721f493b99562e630bdb506..448d00138b94e6fdcf74678c077701a1511f09ef 100644
--- a/src/utils/graph_search.cpp
+++ b/src/utils/graph_search.cpp
@@ -22,50 +22,43 @@
using namespace wolf;
-GraphSearch::GraphSearch() :
- parents_()
-{
-
-}
+GraphSearch::GraphSearch() : parents_() {}
-GraphSearch::~GraphSearch()
-{
-
-}
+GraphSearch::~GraphSearch() {}
FactorBasePtrList GraphSearch::computeShortestPath(NodeStateBlocksPtr node1,
NodeStateBlocksPtr node2,
const unsigned int max_graph_dist)
{
- //WOLF_INFO("GraphSearch::computeShortestPath: from frame ", node1->id(), " to frame ", node2->id());
+ // WOLF_INFO("GraphSearch::computeShortestPath: from frame ", node1->id(), " to frame ", node2->id());
std::set<NodeStateBlocksPtr> node_neigs({node1});
- parents_[node1] = std::pair<FactorBasePtr,NodeStateBlocksPtr>(nullptr, nullptr);
+ parents_[node1] = std::pair<FactorBasePtr, NodeStateBlocksPtr>(nullptr, nullptr);
unsigned int depth = 0;
- //WOLF_INFO(node1->id());
+ // WOLF_INFO(node1->id());
while (not node_neigs.empty())
{
node_neigs = getNeighborFrames(node_neigs);
depth++;
- //if (not node_neigs.empty())
+ // if (not node_neigs.empty())
//{
// std::string node_neigs_str(depth, '.');
// for (auto node : node_neigs)
- // node_neigs_str += std::to_string(node->id()) + std::string(" ");
+ // node_neigs_str += toString(node->id()) + std::string(" ");
// WOLF_INFO(node_neigs_str);
//}
// finish
if (node_neigs.count(node2) != 0)
{
- //WOLF_INFO("Frame ", node2->id(), " found!");
+ // WOLF_INFO("Frame ", node2->id(), " found!");
assert(parents_.count(node2) != 0);
FactorBasePtrList factor_path;
- auto node_it = node2;
+ auto node_it = node2;
while (node_it != node1)
{
@@ -77,10 +70,9 @@ FactorBasePtrList GraphSearch::computeShortestPath(NodeStateBlocksPtr node1,
}
// stop
- if (max_graph_dist > 0 and depth == max_graph_dist)
- break;
+ if (max_graph_dist > 0 and depth == max_graph_dist) break;
}
- //WOLF_INFO("Path to frame ", node2->id(), " NOT found!");
+ // WOLF_INFO("Path to frame ", node2->id(), " NOT found!");
return FactorBasePtrList();
}
@@ -97,18 +89,17 @@ std::set<NodeStateBlocksPtr> GraphSearch::getNeighborFrames(const std::set<NodeS
// Iterate over all factors_by
for (auto fac_by : facs_by)
{
- //WOLF_INFO_COND(fac_by, "fac_by: ", fac_by->id());
- //WOLF_INFO_COND(fac_by->getFrame(), "fac_by->getFrame(): ", fac_by->getFrame()->id());
+ // WOLF_INFO_COND(fac_by, "fac_by: ", fac_by->id());
+ // WOLF_INFO_COND(fac_by->getFrame(), "fac_by->getFrame(): ", fac_by->getFrame()->id());
for (auto node_other_w : fac_by->getNodesFactored())
{
auto node_other = node_other_w.lock();
- //WOLF_INFO_COND(node_other, "node_other ", node_other->id());
- if (node_other != node and
- parents_.count(node_other) == 0)
+ // WOLF_INFO_COND(node_other, "node_other ", node_other->id());
+ if (node_other != node and parents_.count(node_other) == 0)
{
- //WOLF_INFO("registering");
+ // WOLF_INFO("registering");
node_neigs.insert(node_other);
- parents_[node_other] = std::pair<FactorBasePtr,NodeStateBlocksPtr>(fac_by, node);
+ parents_[node_other] = std::pair<FactorBasePtr, NodeStateBlocksPtr>(fac_by, node);
}
}
}
@@ -116,4 +107,3 @@ std::set<NodeStateBlocksPtr> GraphSearch::getNeighborFrames(const std::set<NodeS
return node_neigs;
}
-
diff --git a/src/utils/loader.cpp b/src/utils/loader.cpp
index 56f251465804a2af71b7737e5e3222281f8fe49b..44dfffcd359d897f7d15ce7c9db38bf3efb289ae 100644
--- a/src/utils/loader.cpp
+++ b/src/utils/loader.cpp
@@ -23,7 +23,6 @@
namespace wolf
{
-
Loader::Loader(int _flags)
{
flags_ = _flags;
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index de3072f9d751a07a750669ccae41ffa694e822ff..457c8fe19a67549514bd08c17da48bab3d21b07d 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -16,7 +16,7 @@ target_link_libraries(dummy ${PLUGIN_NAME})
# Create a specific test executable for gtest_example #
wolf_add_gtest(gtest_example gtest_example.cpp) #
# #
-# OPTIONAL: if the gtest depends on the dummy library #
+# if the gtest depends on the dummy library #
# (used for implementing pure virtual base classes), #
# add a line #
# target_link_libraries(gtest_example dummy) #
@@ -29,7 +29,7 @@ wolf_add_gtest(gtest_example gtest_example.cpp) #
# #
##########################################################
-# ------- First Base classes ----------
+# ------------------ First Base classes ------------------
# BufferFrame
wolf_add_gtest(gtest_buffer_frame gtest_buffer_frame.cpp)
@@ -68,6 +68,7 @@ wolf_add_gtest(gtest_graph_search gtest_graph_search.cpp)
# MotionProvider classes test
wolf_add_gtest(gtest_motion_provider gtest_motion_provider.cpp)
+target_link_libraries(gtest_motion_provider PUBLIC dummy)
# LocalParametrizationXxx classes test
wolf_add_gtest(gtest_local_param gtest_local_param.cpp)
@@ -116,9 +117,6 @@ wolf_add_gtest(gtest_solver_manager_multithread gtest_solver_manager_multithread
# StateBlock class test
wolf_add_gtest(gtest_state_block gtest_state_block.cpp)
-# # StateBlock class test DEPRECATED CLASS
-# wolf_add_gtest(gtest_state_composite gtest_state_composite.cpp)
-
# TimeStamp class test
wolf_add_gtest(gtest_time_stamp gtest_time_stamp.cpp)
@@ -131,9 +129,9 @@ wolf_add_gtest(gtest_trajectory gtest_trajectory.cpp)
# TreeManager class test
wolf_add_gtest(gtest_tree_manager gtest_tree_manager.cpp)
-# # ------- Now Derived classes ----------
+# ----------------- Now Derived classes ------------------
-# FactorAbs(P/O/V) classes test
+# FactorAbsolute (P/O/V) test
wolf_add_gtest(gtest_factor_absolute gtest_factor_absolute.cpp)
# FactorDistance3d test
@@ -145,9 +143,6 @@ wolf_add_gtest(gtest_factor_block_difference gtest_factor_block_difference.cpp)
# FactorDiffDrive class test
wolf_add_gtest(gtest_factor_diff_drive gtest_factor_diff_drive.cpp)
-# FactorOdom2dAutodiff class test
-wolf_add_gtest(gtest_factor_odom_2d_autodiff gtest_factor_odom_2d_autodiff.cpp)
-
# FactorPose2d class test
wolf_add_gtest(gtest_factor_pose_2d gtest_factor_pose_2d.cpp)
@@ -163,6 +158,9 @@ wolf_add_gtest(gtest_factor_pose_3d_with_extrinsics gtest_factor_pose_3d_with_ex
# FactorRelativePose2d class test
wolf_add_gtest(gtest_factor_relative_pose_2d gtest_factor_relative_pose_2d.cpp)
+# FactorRelativePose2dAutodiff class test
+wolf_add_gtest(gtest_factor_relative_pose_2d_autodiff gtest_factor_relative_pose_2d_autodiff.cpp)
+
# FactorRelativePose2dWithExtrinsics class test
wolf_add_gtest(gtest_factor_relative_pose_2d_with_extrinsics gtest_factor_relative_pose_2d_with_extrinsics.cpp)
@@ -208,14 +206,14 @@ wolf_add_gtest(gtest_processor_landmark_external gtest_processor_landmark_extern
# ProcessorLoopClosure class test
wolf_add_gtest(gtest_processor_loop_closure gtest_processor_loop_closure.cpp)
-# ProcessorMotion in 2d
-wolf_add_gtest(gtest_odom_2d gtest_odom_2d.cpp)
+# ProcessorOdom2d class test
+wolf_add_gtest(gtest_processor_odom_2d gtest_processor_odom_2d.cpp)
# ProcessorOdom3d class test
wolf_add_gtest(gtest_processor_odom_3d gtest_processor_odom_3d.cpp)
-# Processor and FactorPose3dWithExtrinsics class test
-wolf_add_gtest(gtest_processor_and_factor_pose_3d_with_extrinsics gtest_processor_and_factor_pose_3d_with_extrinsics.cpp)
+# ProcessorPose3d class test
+wolf_add_gtest(gtest_processor_pose_3d gtest_processor_pose_3d.cpp)
# ProcessorTrackerFeatureDummy class test
wolf_add_gtest(gtest_processor_tracker_feature_dummy gtest_processor_tracker_feature_dummy.cpp)
diff --git a/test/dummy/ProcessorDiffDriveMock.schema b/test/dummy/ProcessorDiffDriveMock.schema
new file mode 100644
index 0000000000000000000000000000000000000000..72676f65afb63f1fbbae652f922d2a72ecfa4afa
--- /dev/null
+++ b/test/dummy/ProcessorDiffDriveMock.schema
@@ -0,0 +1 @@
+follow: ProcessorDiffDrive.schema
\ No newline at end of file
diff --git a/test/dummy/ProcessorLoopClosureDummy.schema b/test/dummy/ProcessorLoopClosureDummy.schema
new file mode 100644
index 0000000000000000000000000000000000000000..d6725645a9efa45908f3dfc72eaf07a40cf7b745
--- /dev/null
+++ b/test/dummy/ProcessorLoopClosureDummy.schema
@@ -0,0 +1 @@
+follow: ProcessorLoopClosure.schema
\ No newline at end of file
diff --git a/test/dummy/ProcessorMotionProviderDummy.schema b/test/dummy/ProcessorMotionProviderDummy.schema
new file mode 100644
index 0000000000000000000000000000000000000000..8888e242817fb17908315f079ecb960abc4b737c
--- /dev/null
+++ b/test/dummy/ProcessorMotionProviderDummy.schema
@@ -0,0 +1,2 @@
+follow: ProcessorBase.schema
+follow: MotionProvider.schema
\ No newline at end of file
diff --git a/test/dummy/ProcessorMotionProviderDummyPOV.schema b/test/dummy/ProcessorMotionProviderDummyPOV.schema
new file mode 100644
index 0000000000000000000000000000000000000000..8888e242817fb17908315f079ecb960abc4b737c
--- /dev/null
+++ b/test/dummy/ProcessorMotionProviderDummyPOV.schema
@@ -0,0 +1,2 @@
+follow: ProcessorBase.schema
+follow: MotionProvider.schema
\ No newline at end of file
diff --git a/test/dummy/ProcessorTrackerLandmarkDummy.schema b/test/dummy/ProcessorTrackerLandmarkDummy.schema
new file mode 100644
index 0000000000000000000000000000000000000000..f5662ec2cd2899716b80ee7ff6c7c1a59bc530cd
--- /dev/null
+++ b/test/dummy/ProcessorTrackerLandmarkDummy.schema
@@ -0,0 +1,5 @@
+follow: ProcessorTrackerLandmark.schema
+n_landmarks_lost:
+ _mandatory: true
+ _type: unsigned int
+ _doc: dummy parameter.
\ No newline at end of file
diff --git a/test/dummy/SensorDummy2d.schema b/test/dummy/SensorDummy2d.schema
index 4e62225fe5e1779d36e79d282dcb33a3005d037e..c733dc45ddd7e385c2fd7d24e2ba2a9dc5e7c4c7 100644
--- a/test/dummy/SensorDummy2d.schema
+++ b/test/dummy/SensorDummy2d.schema
@@ -1,5 +1,10 @@
follow: SensorBase.schema
states:
+ keys:
+ _mandatory: false
+ _type: string
+ _value: PO
+ _doc: States' keys of sensor
P:
follow: SpecStateSensorP2d.schema
O:
diff --git a/test/dummy/SensorDummy3d.schema b/test/dummy/SensorDummy3d.schema
index 6e090b85aeca710ae6e3bf83dbf09c388780655c..c656bcfcfce9272dc55347b82a5697ab6e994ce3 100644
--- a/test/dummy/SensorDummy3d.schema
+++ b/test/dummy/SensorDummy3d.schema
@@ -1,5 +1,10 @@
follow: SensorBase.schema
states:
+ keys:
+ _mandatory: false
+ _type: string
+ _value: PO
+ _doc: States' keys of sensor
P:
follow: SpecStateSensorP3d.schema
O:
diff --git a/test/dummy/SensorDummyPoia3d.schema b/test/dummy/SensorDummyPoia3d.schema
index fb89410ab07c696bc3530c0cf8d3b0cd4bbd55b2..ee3e760498daee7ac226ac7a86c6055ef28285a7 100644
--- a/test/dummy/SensorDummyPoia3d.schema
+++ b/test/dummy/SensorDummyPoia3d.schema
@@ -1,5 +1,10 @@
follow: SensorBase.schema
states:
+ keys:
+ _mandatory: false
+ _type: string
+ _value: POIA
+ _doc: states' keys of the sensor
P:
follow: SpecStateSensorP3d.schema
O:
@@ -9,25 +14,14 @@ states:
type:
_type: string
_mandatory: false
- _default: StateParams5
- _options: [StateParams5]
+ _value: StateParams5
_doc: The derived type of the state in 'I'
state:
_type: Vector5d
_mandatory: true
_doc: A vector containing the state 'I' values
A:
- follow: SpecStateSensor.schema
- type:
- _type: string
- _mandatory: false
- _default: StateQuaternion
- _options: [StateQuaternion]
- _doc: The derived type of the state in 'A'
- state:
- _type: Vector4d
- _mandatory: true
- _doc: A vector containing the state 'A' values
+ follow: SpecStateSensorO3d.schema
noise_p_std:
_mandatory: true
_type: double
diff --git a/test/dummy/SolverDummy.schema b/test/dummy/SolverDummy.schema
new file mode 100644
index 0000000000000000000000000000000000000000..dba4fce7c0626913aa7ef141253d2b103c591bd3
--- /dev/null
+++ b/test/dummy/SolverDummy.schema
@@ -0,0 +1 @@
+follow: SolverManager.schema
\ No newline at end of file
diff --git a/test/dummy/dummy_object.h b/test/dummy/dummy_object.h
index 7dbb1d6b6571153018df9e09aeb4df78f9da2f15..07a2983dc04f2ab460cf676d20006928c0d84aa3 100644
--- a/test/dummy/dummy_object.h
+++ b/test/dummy/dummy_object.h
@@ -26,7 +26,8 @@
#include "core/common/wolf.h"
#include "factory_dummy_object.h"
-namespace wolf {
+namespace wolf
+{
WOLF_PTR_TYPEDEFS(DummyObject);
/*
@@ -41,36 +42,34 @@ WOLF_PTR_TYPEDEFS(DummyObject);
* DummyObjectClass(const std::string& _unique_name,
* const ParamsServer& _server);
*/
-#define WOLF_DUMMY_OBJECT_CREATE(DummyObjectClass) \
-static DummyObjectPtr create(const std::string& _unique_name) \
-{ \
- DummyObjectPtr sub = std::make_shared<DummyObjectClass>(_unique_name); \
- return sub; \
-} \
+#define WOLF_DUMMY_OBJECT_CREATE(DummyObjectClass) \
+ static DummyObjectPtr create(const std::string& _unique_name) \
+ { \
+ DummyObjectPtr sub = std::make_shared<DummyObjectClass>(_unique_name); \
+ return sub; \
+ }
class DummyObject
{
- protected:
- //wolf
- std::string prefix_;
- std::string name_;
- std::string topic_;
+ protected:
+ // wolf
+ std::string prefix_;
+ std::string name_;
+ std::string topic_;
- public:
- DummyObject(const std::string& _unique_name) :
- prefix_("ROS DummyObject/" + _unique_name),
- name_(_unique_name)
- {
- //topic_ = _server.getParam<std::string>(prefix_ + "/topic");
- }
+ public:
+ DummyObject(const std::string& _unique_name) : prefix_("ROS DummyObject/" + _unique_name), name_(_unique_name)
+ {
+ // topic_ = _server.getParam<std::string>(prefix_ + "/topic");
+ }
- virtual ~DummyObject(){};
+ virtual ~DummyObject(){};
- //std::string getTopic() const;
+ // std::string getTopic() const;
- std::string getName() const;
+ std::string getName() const;
- virtual void print() const = 0;
+ virtual void print() const = 0;
};
inline std::string DummyObject::getName() const
@@ -78,4 +77,4 @@ inline std::string DummyObject::getName() const
return name_;
}
-}
+} // namespace wolf
diff --git a/test/dummy/dummy_object_derived.cpp b/test/dummy/dummy_object_derived.cpp
index 02c690931496ad38ddd3e1099cb7a236d601b3aa..f5b3e10c0e481da5c81bc881b2badda7ffee734b 100644
--- a/test/dummy/dummy_object_derived.cpp
+++ b/test/dummy/dummy_object_derived.cpp
@@ -22,9 +22,9 @@
namespace wolf
{
-
-DummyObjectDerived::DummyObjectDerived(const std::string& _unique_name) ://,
- DummyObject(_unique_name)
+DummyObjectDerived::DummyObjectDerived(const std::string& _unique_name)
+ : //,
+ DummyObject(_unique_name)
{
}
@@ -35,4 +35,4 @@ void DummyObjectDerived::print() const
WOLF_REGISTER_DUMMY_OBJECT(DummyObjectDerived)
-}
+} // namespace wolf
diff --git a/test/dummy/dummy_object_derived.h b/test/dummy/dummy_object_derived.h
index 3be8a35d9a7ce0c41b4721e956c4ff48a21a9abf..90640c4ecdc5a50c099ab435ad1c1a38beda127a 100644
--- a/test/dummy/dummy_object_derived.h
+++ b/test/dummy/dummy_object_derived.h
@@ -27,17 +27,17 @@
#include "dummy_object.h"
-namespace wolf {
-
+namespace wolf
+{
class DummyObjectDerived : public DummyObject
{
- public:
- DummyObjectDerived(const std::string& _unique_name);
- WOLF_DUMMY_OBJECT_CREATE(DummyObjectDerived);
-
- void print() const override;
+ public:
+ DummyObjectDerived(const std::string& _unique_name);
+ WOLF_DUMMY_OBJECT_CREATE(DummyObjectDerived);
+
+ void print() const override;
- virtual ~DummyObjectDerived(){};
+ virtual ~DummyObjectDerived(){};
};
-}
+} // namespace wolf
diff --git a/test/dummy/dummy_object_derived_derived.cpp b/test/dummy/dummy_object_derived_derived.cpp
index 004176952d00d9e3f4481aa5ad356b9b24823922..516bf6b60a20b32a49cd1c4f7cd09e62c2a44145 100644
--- a/test/dummy/dummy_object_derived_derived.cpp
+++ b/test/dummy/dummy_object_derived_derived.cpp
@@ -22,9 +22,8 @@
namespace wolf
{
-
-DummyObjectDerivedDerived::DummyObjectDerivedDerived(const std::string& _unique_name) :
- DummyObjectDerived(_unique_name)
+DummyObjectDerivedDerived::DummyObjectDerivedDerived(const std::string& _unique_name)
+ : DummyObjectDerived(_unique_name)
{
}
@@ -34,4 +33,4 @@ void DummyObjectDerivedDerived::print() const
}
WOLF_REGISTER_DUMMY_OBJECT(DummyObjectDerivedDerived)
-}
+} // namespace wolf
diff --git a/test/dummy/dummy_object_derived_derived.h b/test/dummy/dummy_object_derived_derived.h
index ba58696fc91f8d9905c052673cd78c028e9e30f5..8ba7a5a7f11af32988be590240436644f2cd93d9 100644
--- a/test/dummy/dummy_object_derived_derived.h
+++ b/test/dummy/dummy_object_derived_derived.h
@@ -27,17 +27,17 @@
#include "dummy_object_derived.h"
-namespace wolf {
-
+namespace wolf
+{
class DummyObjectDerivedDerived : public DummyObjectDerived
{
- public:
- DummyObjectDerivedDerived(const std::string& _unique_name);
- WOLF_DUMMY_OBJECT_CREATE(DummyObjectDerivedDerived);
-
- void print() const override;
+ public:
+ DummyObjectDerivedDerived(const std::string& _unique_name);
+ WOLF_DUMMY_OBJECT_CREATE(DummyObjectDerivedDerived);
+
+ void print() const override;
- virtual ~DummyObjectDerivedDerived(){};
+ virtual ~DummyObjectDerivedDerived(){};
};
-}
+} // namespace wolf
diff --git a/test/dummy/factor_dummy_zero_1.h b/test/dummy/factor_dummy_zero_1.h
index 4bd1853b0812a1f1fc33ed36ff26485c0070939a..033d694dd9835168187e43be89a228fbfec064da 100644
--- a/test/dummy/factor_dummy_zero_1.h
+++ b/test/dummy/factor_dummy_zero_1.h
@@ -20,44 +20,41 @@
#pragma once
-//Wolf includes
+// Wolf includes
#include "core/factor/factor_autodiff.h"
//#include "ceres/jet.h"
-namespace wolf {
-
+namespace wolf
+{
WOLF_PTR_TYPEDEFS(FactorDummyZero1);
-//class
+// class
class FactorDummyZero1 : public FactorAutodiff<FactorDummyZero1, 1, 1>
{
- public:
- FactorDummyZero1(const FeatureBasePtr& _ftr_ptr,
- const NodeStateBlocksPtr& _node,
- const StateBlockPtr& _sb_ptr) :
- FactorAutodiff("FactorDummy1",
- TOP_OTHER,
- _ftr_ptr->getMeasurement(),
- _ftr_ptr->getMeasurementSquareRootInformationUpper(),
- {_node},
- nullptr,
- {_sb_ptr},
- false,
- FAC_ACTIVE)
- {
- //
- }
-
- ~FactorDummyZero1() override = default;
-
- template<typename T>
- bool operator ()(const T* const _st1,
- T* _residuals) const
- {
- _residuals[0] = _st1[0];
- return true;
- }
+ public:
+ FactorDummyZero1(const FeatureBasePtr& _ftr_ptr, const NodeStateBlocksPtr& _node, const StateBlockPtr& _sb_ptr)
+ : FactorAutodiff("FactorDummy1",
+ TOP_OTHER,
+ _ftr_ptr->getMeasurement(),
+ _ftr_ptr->getMeasurementSquareRootInformationUpper(),
+ {_node},
+ nullptr,
+ {_sb_ptr},
+ false,
+ FAC_ACTIVE)
+ {
+ //
+ }
+
+ ~FactorDummyZero1() override = default;
+
+ template <typename T>
+ bool operator()(const T* const _st1, T* _residuals) const
+ {
+ _residuals[0] = _st1[0];
+ return true;
+ }
};
-} // namespace wolf
+} // namespace wolf
diff --git a/test/dummy/factor_dummy_zero_15.h b/test/dummy/factor_dummy_zero_15.h
index 0b029e9d094b9a94f1dddd6d55def527374f942c..14cb3bc08cefa1cae5dcfcf5ad4265a8de966da2 100644
--- a/test/dummy/factor_dummy_zero_15.h
+++ b/test/dummy/factor_dummy_zero_15.h
@@ -25,7 +25,6 @@
namespace wolf
{
-
// class
template <unsigned int ID>
class FactorDummyZero15
diff --git a/test/dummy/factor_feature_dummy.h b/test/dummy/factor_feature_dummy.h
index f6352318721a410107831cb1ee6163395b05c98a..44c5a0007cd045cfbb9ccaf9cced29b12bde2527 100644
--- a/test/dummy/factor_feature_dummy.h
+++ b/test/dummy/factor_feature_dummy.h
@@ -24,7 +24,6 @@
namespace wolf
{
-
WOLF_PTR_TYPEDEFS(FactorFeatureDummy);
class FactorFeatureDummy : public FactorBase
diff --git a/test/dummy/factor_landmark_dummy.h b/test/dummy/factor_landmark_dummy.h
index 45220b0a10e0c777f17bac593418021dd469e8c8..4909085cf8cefdca8c1a8c217b6b400372163c8c 100644
--- a/test/dummy/factor_landmark_dummy.h
+++ b/test/dummy/factor_landmark_dummy.h
@@ -24,7 +24,6 @@
namespace wolf
{
-
WOLF_PTR_TYPEDEFS(FactorLandmarkDummy);
class FactorLandmarkDummy : public FactorBase
@@ -34,7 +33,7 @@ class FactorLandmarkDummy : public FactorBase
const LandmarkBasePtr& _landmark_other_ptr,
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function,
- FactorStatus _status = FAC_ACTIVE);
+ FactorStatus _status = FAC_ACTIVE);
~FactorLandmarkDummy() override = default;
diff --git a/test/dummy/factor_odom_2d_autodiff.h b/test/dummy/factor_relative_pose_2d_autodiff.h
similarity index 79%
rename from test/dummy/factor_odom_2d_autodiff.h
rename to test/dummy/factor_relative_pose_2d_autodiff.h
index 0f5424af8916e2644ee4009431a40b2a42475308..f8f9ec07fa042643a1f40f618c37bb21c23ff0ed 100644
--- a/test/dummy/factor_odom_2d_autodiff.h
+++ b/test/dummy/factor_relative_pose_2d_autodiff.h
@@ -29,17 +29,17 @@
namespace wolf
{
-WOLF_PTR_TYPEDEFS(FactorOdom2dAutodiff);
+WOLF_PTR_TYPEDEFS(FactorRelativePose2dAutodiff);
// class
-class FactorOdom2dAutodiff : public FactorAutodiff<FactorOdom2dAutodiff, 3, 2, 1, 2, 1>
+class FactorRelativePose2dAutodiff : public FactorAutodiff<FactorRelativePose2dAutodiff, 3, 2, 1, 2, 1>
{
public:
- FactorOdom2dAutodiff(const FeatureBasePtr& _ftr_ptr,
- const FrameBasePtr& _frame_other_ptr,
- const ProcessorBasePtr& _processor_ptr,
- bool _apply_loss_function,
- FactorStatus _status = FAC_ACTIVE)
+ FactorRelativePose2dAutodiff(const FeatureBasePtr& _ftr_ptr,
+ const FrameBasePtr& _frame_other_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status = FAC_ACTIVE)
: FactorAutodiff("FactorOdom2dAutodiff",
TOP_OTHER,
_ftr_ptr->getMeasurement(),
@@ -56,7 +56,7 @@ class FactorOdom2dAutodiff : public FactorAutodiff<FactorOdom2dAutodiff, 3, 2, 1
//
}
- ~FactorOdom2dAutodiff() override = default;
+ ~FactorRelativePose2dAutodiff() override = default;
template <typename T>
bool operator()(const T* const _p1, const T* const _o1, const T* const _p2, const T* const _o2, T* _residuals)
@@ -64,11 +64,11 @@ class FactorOdom2dAutodiff : public FactorAutodiff<FactorOdom2dAutodiff, 3, 2, 1
};
template <typename T>
-inline bool FactorOdom2dAutodiff::operator()(const T* const _p1,
- const T* const _o1,
- const T* const _p2,
- const T* const _o2,
- T* _residuals) const
+inline bool FactorRelativePose2dAutodiff::operator()(const T* const _p1,
+ const T* const _o1,
+ const T* const _p2,
+ const T* const _o2,
+ T* _residuals) const
{
// MAPS
Eigen::Map<Eigen::Matrix<T, 3, 1> > res(_residuals);
diff --git a/test/dummy/factory_dummy_object.h b/test/dummy/factory_dummy_object.h
index eaad26a9439ea32c2a9b58e9ac4c9b7cf44a5d5d..12d2b3e40a4dec836e4fc64edc8447e6ee47b4ad 100644
--- a/test/dummy/factory_dummy_object.h
+++ b/test/dummy/factory_dummy_object.h
@@ -25,19 +25,19 @@
namespace wolf
{
-
class DummyObject;
-typedef Factory<std::shared_ptr<DummyObject>,
- const std::string&> FactoryDummyObject;
-template<>
+typedef Factory<std::shared_ptr<DummyObject>, const std::string&> FactoryDummyObject;
+template <>
inline std::string FactoryDummyObject::getClass() const
{
- return "FactoryDummyObject";
+ return "FactoryDummyObject";
}
-
-#define WOLF_REGISTER_DUMMY_OBJECT(DummyObjectType) \
- namespace{ const bool WOLF_UNUSED DummyObjectType##Registered = \
- wolf::FactoryDummyObject::registerCreator(#DummyObjectType, DummyObjectType::create); } \
+#define WOLF_REGISTER_DUMMY_OBJECT(DummyObjectType) \
+ namespace \
+ { \
+ const bool WOLF_UNUSED DummyObjectType##Registered = \
+ wolf::FactoryDummyObject::registerCreator(#DummyObjectType, DummyObjectType::create); \
+ }
} /* namespace wolf */
diff --git a/test/dummy/landmark_dummy.h b/test/dummy/landmark_dummy.h
index e7d8228e4d7568bf603ef0a9cfc64a37358e192a..f17f4585ce7b2a9d7f3edf7973a829a7b647d471 100644
--- a/test/dummy/landmark_dummy.h
+++ b/test/dummy/landmark_dummy.h
@@ -20,7 +20,7 @@
#pragma once
-//Wolf includes
+// Wolf includes
#include "core/landmark/landmark_base.h"
#include "core/landmark/factory_landmark.h"
@@ -28,44 +28,42 @@
// Std includes
-
-namespace wolf {
-
+namespace wolf
+{
WOLF_PTR_TYPEDEFS(LandmarkDummy);
-
-//class LandmarkDummy
+
+// class LandmarkDummy
class LandmarkDummy : public LandmarkBase
{
- public:
- LandmarkDummy(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, const int& _int_param, const double& _double_param);
- LandmarkDummy(const YAML::Node& _node);
- WOLF_LANDMARK_CREATE(LandmarkDummy);
-
- ~LandmarkDummy() override = default;
-
- int getIntParam() const;
- double getDoubleParam() const;
-
- YAML::Node toYaml() const override;
-
- private:
- const int int_param_;
- const double double_param_;
-
-};
+ public:
+ LandmarkDummy(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, const int& _int_param, const double& _double_param);
+ LandmarkDummy(const YAML::Node& _node);
+ WOLF_LANDMARK_CREATE(LandmarkDummy);
+ ~LandmarkDummy() override = default;
+
+ int getIntParam() const;
+ double getDoubleParam() const;
+
+ YAML::Node toYaml() const override;
+
+ private:
+ const int int_param_;
+ const double double_param_;
+};
-inline LandmarkDummy::LandmarkDummy(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, const int& _int_param, const double& _double_param) :
- LandmarkBase("LandmarkDummy", _p_ptr, _o_ptr),
- int_param_(_int_param),
- double_param_(_double_param)
+inline LandmarkDummy::LandmarkDummy(StateBlockPtr _p_ptr,
+ StateBlockPtr _o_ptr,
+ const int& _int_param,
+ const double& _double_param)
+ : LandmarkBase("LandmarkDummy", _p_ptr, _o_ptr), int_param_(_int_param), double_param_(_double_param)
{
}
-inline LandmarkDummy::LandmarkDummy(const YAML::Node& _node) :
- LandmarkBase("LandmarkDummy", _node),
- int_param_(_node["int_param"].as<int>()),
- double_param_(_node["double_param"].as<double>())
+inline LandmarkDummy::LandmarkDummy(const YAML::Node& _node)
+ : LandmarkBase("LandmarkDummy", _node),
+ int_param_(_node["int_param"].as<int>()),
+ double_param_(_node["double_param"].as<double>())
{
}
@@ -83,7 +81,7 @@ inline YAML::Node LandmarkDummy::toYaml() const
{
YAML::Node node = LandmarkBase::toYaml();
- node["int_param"] = int_param_;
+ node["int_param"] = int_param_;
node["double_param"] = double_param_;
return node;
@@ -91,4 +89,4 @@ inline YAML::Node LandmarkDummy::toYaml() const
WOLF_REGISTER_LANDMARK(LandmarkDummy);
-} // namespace wolf
+} // namespace wolf
diff --git a/test/dummy/processor_diff_drive_mock.h b/test/dummy/processor_diff_drive_mock.h
new file mode 100644
index 0000000000000000000000000000000000000000..d85674d25c8526154326554174325a6ce5f652fe
--- /dev/null
+++ b/test/dummy/processor_diff_drive_mock.h
@@ -0,0 +1,118 @@
+// WOLF - Copyright (C) 2020,2021,2022,2023
+// Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
+// Joan Vallvé Navarro (jvallve@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF: http://www.iri.upc.edu/wolf
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// 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/>.
+
+#pragma once
+
+/**************************
+ * WOLF includes *
+ **************************/
+#include "core/processor/processor_diff_drive.h"
+
+namespace wolf
+{
+WOLF_PTR_TYPEDEFS(ProcessorDiffDriveMock);
+
+class ProcessorDiffDriveMock : public ProcessorDiffDrive
+{
+ using Base = ProcessorDiffDrive;
+
+ public:
+ ProcessorDiffDriveMock(const YAML::Node& _params) : ProcessorDiffDrive(_params)
+ {
+ this->setType("ProcessorDiffDriveMock");
+ }
+ void configure(SensorBasePtr _sensor) override
+ {
+ Base::configure(_sensor);
+ }
+ void computeCurrentDelta(const Eigen::VectorXd& _data,
+ const Eigen::MatrixXd& _data_cov,
+ const Eigen::VectorXd& _calib,
+ const double _dt,
+ Eigen::VectorXd& _delta,
+ Eigen::MatrixXd& _delta_cov,
+ Eigen::MatrixXd& _jacobian_calib) const override
+ {
+ Base::computeCurrentDelta(_data, _data_cov, _calib, _dt, _delta, _delta_cov, _jacobian_calib);
+ }
+
+ void deltaPlusDelta(const Eigen::VectorXd& _delta1,
+ const Eigen::VectorXd& _delta2,
+ const double _dt2,
+ Eigen::VectorXd& _delta1_plus_delta2) const override
+ {
+ Base::deltaPlusDelta(_delta1, _delta2, _dt2, _delta1_plus_delta2);
+ }
+
+ void deltaPlusDelta(const Eigen::VectorXd& _delta1,
+ const Eigen::VectorXd& _delta2,
+ const double _dt2,
+ Eigen::VectorXd& _delta1_plus_delta2,
+ Eigen::MatrixXd& _jacobian1,
+ Eigen::MatrixXd& _jacobian2) const override
+ {
+ Base::deltaPlusDelta(_delta1, _delta2, _dt2, _delta1_plus_delta2, _jacobian1, _jacobian2);
+ }
+
+ void statePlusDelta(const VectorComposite& _x,
+ const Eigen::VectorXd& _delta,
+ const double _dt,
+ VectorComposite& _x_plus_delta) const override
+ {
+ Base::statePlusDelta(_x, _delta, _dt, _x_plus_delta);
+ }
+
+ Eigen::VectorXd deltaZero() const override
+ {
+ return Base::deltaZero();
+ }
+
+ VectorXd getCalibration(const CaptureBaseConstPtr _capture) const override
+ {
+ return Base::getCalibration(_capture);
+ }
+
+ CaptureMotionPtr emplaceCapture(const FrameBasePtr& _frame_own,
+ const SensorBasePtr& _sensor,
+ const TimeStamp& _ts,
+ const VectorXd& _data,
+ const MatrixXd& _data_cov,
+ const VectorXd& _calib,
+ const VectorXd& _calib_preint,
+ const CaptureBasePtr& _capture_origin) override
+ {
+ return Base::emplaceCapture(
+ _frame_own, _sensor, _ts, _data, _data_cov, _calib, _calib_preint, _capture_origin);
+ }
+
+ double getRadPerTick()
+ {
+ return radians_per_tick_;
+ }
+};
+
+} // namespace wolf
+
+// Register in the FactorySensorNode
+#include "core/sensor/factory_sensor.h"
+namespace wolf
+{
+WOLF_REGISTER_PROCESSOR(ProcessorDiffDriveMock);
+} // namespace wolf
diff --git a/test/dummy/processor_loop_closure_dummy.h b/test/dummy/processor_loop_closure_dummy.h
index e045036c6c2ac8337a1dbd3e090818eb77f79d8e..e1829caf2b26fb9c9afbce9d2849d6e4b27220b5 100644
--- a/test/dummy/processor_loop_closure_dummy.h
+++ b/test/dummy/processor_loop_closure_dummy.h
@@ -22,19 +22,19 @@
#include "core/processor/processor_loop_closure.h"
-using namespace wolf;
-using namespace Eigen;
-
+namespace wolf
+{
WOLF_PTR_TYPEDEFS(ProcessorLoopClosureDummy);
// dummy class:
class ProcessorLoopClosureDummy : public ProcessorLoopClosure
{
public:
- ProcessorLoopClosureDummy(ParamsProcessorLoopClosurePtr _params)
+ ProcessorLoopClosureDummy(const YAML::Node& _params)
: ProcessorLoopClosure("ProcessorLoopClosureDummy", 2, _params)
{
}
+ WOLF_PROCESSOR_CREATE(ProcessorLoopClosureDummy);
protected:
bool voteFindLoopClosures(CaptureBasePtr cap) override
@@ -113,3 +113,12 @@ class ProcessorLoopClosureDummy : public ProcessorLoopClosure
return buffer_capture_.getContainer().size();
}
};
+
+} /* namespace wolf */
+
+// Register in the FactoryProcessor
+#include "core/processor/factory_processor.h"
+namespace wolf
+{
+WOLF_REGISTER_PROCESSOR(ProcessorLoopClosureDummy);
+} // namespace wolf
diff --git a/test/dummy/processor_motion_provider_dummy.h b/test/dummy/processor_motion_provider_dummy.h
index c32d0ce2b6d2f51215bcecdcc225d27bda31a42f..5eff684007d31a708bc65b0d71a751d34d62978e 100644
--- a/test/dummy/processor_motion_provider_dummy.h
+++ b/test/dummy/processor_motion_provider_dummy.h
@@ -25,61 +25,65 @@
namespace wolf
{
-
-WOLF_STRUCT_PTR_TYPEDEFS(ParamsMotionProviderDummy);
-
-struct ParamsMotionProviderDummy : public ParamsProcessorBase, public ParamsMotionProvider
-{
- TypeComposite state_types = {{'P',"StatePoint2d"},{'O',"StateAngle"}};
-
- ParamsMotionProviderDummy() = default;
- ParamsMotionProviderDummy(const YAML::Node& _n):
- ParamsProcessorBase(_n),
- ParamsMotionProvider(_n)
- {
-
- };
-};
-
WOLF_PTR_TYPEDEFS(MotionProviderDummy);
-class MotionProviderDummy : public ProcessorBase, public MotionProvider
+class ProcessorMotionProviderDummy : public ProcessorBase, public MotionProvider
{
- public:
- MotionProviderDummy(ParamsMotionProviderDummyPtr _params) :
- ProcessorBase("MotionProviderDummy", 2, _params),
- MotionProvider(_params->state_types, _params)
- {}
- ~MotionProviderDummy(){};
+ public:
+ ProcessorMotionProviderDummy(const YAML::Node& _params)
+ : ProcessorBase("ProcessorMotionProviderDummy", 2, _params),
+ MotionProvider({{'P', "StatePoint2d"}, {'O', "StateAngle"}}, _params)
+ {
+ }
+ ~ProcessorMotionProviderDummy(){};
- // Factory method for high level API
- WOLF_PROCESSOR_CREATE(MotionProviderDummy, ParamsMotionProviderDummy);
+ // Factory method for high level API
+ WOLF_PROCESSOR_CREATE(ProcessorMotionProviderDummy);
- void configure(SensorBasePtr _sensor) override {};
- void processCapture(CaptureBasePtr) override {};
- void processKeyFrame(FrameBasePtr _keyframe_ptr) override {};
- bool triggerInCapture(CaptureBasePtr) const override { return false; };
- bool triggerInKeyFrame(FrameBasePtr _keyframe_ptr) const override { return false; };
- bool storeKeyFrame(FrameBasePtr) override { return false; };
- bool storeCapture(CaptureBasePtr) override { return false; };
- bool voteForKeyFrame() const override { return false; };
- TimeStamp getTimeStamp() const override {return TimeStamp(0);};
+ void configure(SensorBasePtr _sensor) override{};
+ void processCapture(CaptureBasePtr) override{};
+ void processKeyFrame(FrameBasePtr _keyframe_ptr) override{};
+ bool triggerInCapture(CaptureBasePtr) const override
+ {
+ return false;
+ };
+ bool triggerInKeyFrame(FrameBasePtr _keyframe_ptr) const override
+ {
+ return false;
+ };
+ bool storeKeyFrame(FrameBasePtr) override
+ {
+ return false;
+ };
+ bool storeCapture(CaptureBasePtr) override
+ {
+ return false;
+ };
+ bool voteForKeyFrame() const override
+ {
+ return false;
+ };
+ TimeStamp getTimeStamp() const override
+ {
+ return TimeStamp(0);
+ };
- VectorComposite getState(const StateKeys& _structure = "") const override
- {
- return getOdometry();
- };
+ VectorComposite getState(const StateKeys& _structure = "") const override
+ {
+ return getOdometry();
+ };
- VectorComposite getState(const TimeStamp& _ts, const StateKeys& _structure = "") const override
- {
- return getOdometry();
- };
+ VectorComposite getState(const TimeStamp& _ts, const StateKeys& _structure = "") const override
+ {
+ return getOdometry();
+ };
};
} /* namespace wolf */
// Register in the FactoryProcessor
#include "core/processor/factory_processor.h"
-namespace wolf {
-WOLF_REGISTER_PROCESSOR(MotionProviderDummy);
-} // namespace wolf
+namespace wolf
+{
+WOLF_REGISTER_PROCESSOR(ProcessorMotionProviderDummy);
+} // namespace wolf
diff --git a/test/dummy/processor_motion_provider_dummy_pov.h b/test/dummy/processor_motion_provider_dummy_pov.h
new file mode 100644
index 0000000000000000000000000000000000000000..0ce065488a85e0bc10ae2358939d96e4e78d7264
--- /dev/null
+++ b/test/dummy/processor_motion_provider_dummy_pov.h
@@ -0,0 +1,89 @@
+// WOLF - Copyright (C) 2020,2021,2022,2023
+// Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
+// Joan Vallvé Navarro (jvallve@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF: http://www.iri.upc.edu/wolf
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// 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/>.
+
+#pragma once
+
+#include <core/processor/motion_provider.h>
+#include "core/processor/processor_base.h"
+
+namespace wolf
+{
+WOLF_PTR_TYPEDEFS(MotionProviderDummy);
+
+class ProcessorMotionProviderDummyPOV : public ProcessorBase, public MotionProvider
+{
+ public:
+ ProcessorMotionProviderDummyPOV(const YAML::Node& _params)
+ : ProcessorBase("ProcessorMotionProviderDummyPOV", 2, _params),
+ MotionProvider({{'P', "StatePoint2d"}, {'O', "StateAngle"}, {'V', "StateVector2d"}}, _params)
+ {
+ }
+ ~ProcessorMotionProviderDummyPOV(){};
+
+ // Factory method for high level API
+ WOLF_PROCESSOR_CREATE(ProcessorMotionProviderDummyPOV);
+
+ void configure(SensorBasePtr _sensor) override{};
+ void processCapture(CaptureBasePtr) override{};
+ void processKeyFrame(FrameBasePtr _keyframe_ptr) override{};
+ bool triggerInCapture(CaptureBasePtr) const override
+ {
+ return false;
+ };
+ bool triggerInKeyFrame(FrameBasePtr _keyframe_ptr) const override
+ {
+ return false;
+ };
+ bool storeKeyFrame(FrameBasePtr) override
+ {
+ return false;
+ };
+ bool storeCapture(CaptureBasePtr) override
+ {
+ return false;
+ };
+ bool voteForKeyFrame() const override
+ {
+ return false;
+ };
+ TimeStamp getTimeStamp() const override
+ {
+ return TimeStamp(0);
+ };
+
+ VectorComposite getState(const StateKeys& _structure = "") const override
+ {
+ return getOdometry();
+ };
+
+ VectorComposite getState(const TimeStamp& _ts, const StateKeys& _structure = "") const override
+ {
+ return getOdometry();
+ };
+};
+
+} /* namespace wolf */
+
+// Register in the FactoryProcessor
+#include "core/processor/factory_processor.h"
+namespace wolf
+{
+WOLF_REGISTER_PROCESSOR(ProcessorMotionProviderDummyPOV);
+} // namespace wolf
diff --git a/test/dummy/processor_tracker_feature_dummy.cpp b/test/dummy/processor_tracker_feature_dummy.cpp
index 3c4f87893cb69b8579066e3249ce2cbd7b23e1d0..4d763733b5913ace2d5582717efddf96a1a1a085 100644
--- a/test/dummy/processor_tracker_feature_dummy.cpp
+++ b/test/dummy/processor_tracker_feature_dummy.cpp
@@ -23,7 +23,6 @@
namespace wolf
{
-
unsigned int ProcessorTrackerFeatureDummy::trackFeatures(const FeatureBasePtrList& _features_in,
const CaptureBasePtr& _capture,
FeatureBasePtrList& _features_out,
@@ -34,7 +33,7 @@ unsigned int ProcessorTrackerFeatureDummy::trackFeatures(const FeatureBasePtrLis
auto count = 0;
for (auto feat_in : _features_in)
{
- if (count < params_tracker_feature_dummy_->n_tracks_lost) // lose first ntrackslost tracks
+ if (count < n_tracks_lost_) // lose first ntrackslost tracks
{
WOLF_INFO("track: ", feat_in->trackId(), " feature: ", feat_in->id(), " lost!");
count++;
@@ -58,11 +57,11 @@ bool ProcessorTrackerFeatureDummy::voteForKeyFrame() const
{
WOLF_INFO("Nbr. of active feature tracks: ", incoming_ptr_->getFeatureList().size());
- bool vote = incoming_ptr_->getFeatureList().size() < params_tracker_feature_->min_features_for_keyframe;
+ bool vote = incoming_ptr_->getFeatureList().size() < min_features_for_keyframe_;
WOLF_INFO((vote ? "Vote " : "Do not vote "), "for KF");
- return incoming_ptr_->getFeatureList().size() < params_tracker_feature_->min_features_for_keyframe;
+ return incoming_ptr_->getFeatureList().size() < min_features_for_keyframe_;
}
unsigned int ProcessorTrackerFeatureDummy::detectNewFeatures(const int& _max_new_features,
@@ -104,11 +103,8 @@ FactorBasePtr ProcessorTrackerFeatureDummy::emplaceFactor(FeatureBasePtr _featur
" with origin feature ",
_feature_other_ptr->id());
- return FactorBase::emplace<FactorFeatureDummy>(_feature_ptr,
- _feature_ptr,
- _feature_other_ptr,
- shared_from_this(),
- params_tracker_feature_dummy_->apply_loss_function);
+ return FactorBase::emplace<FactorFeatureDummy>(
+ _feature_ptr, _feature_ptr, _feature_other_ptr, shared_from_this(), applyLossFunction());
}
} // namespace wolf
diff --git a/test/dummy/processor_tracker_feature_dummy.h b/test/dummy/processor_tracker_feature_dummy.h
index b69ae02de12abfeeea4f79eb1bf2bb0da84e7bf5..ed35b621f5ec1b2c849f116e77d11779322eaeb7 100644
--- a/test/dummy/processor_tracker_feature_dummy.h
+++ b/test/dummy/processor_tracker_feature_dummy.h
@@ -26,39 +26,23 @@
namespace wolf
{
-
-WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorTrackerFeatureDummy);
-
-struct ParamsProcessorTrackerFeatureDummy : public ParamsProcessorTrackerFeature
-{
- unsigned int n_tracks_lost; ///< number of tracks lost each time track is called (the first ones)
-
- ParamsProcessorTrackerFeatureDummy() = default;
- ParamsProcessorTrackerFeatureDummy(const YAML::Node& _n) : ParamsProcessorTrackerFeature(_n)
- {
- WOLF_INFO("ParamsProcessorTrackerFeatureDummy input node:\n", _n);
- n_tracks_lost = _n["n_tracks_lost"].as<unsigned int>();
- WOLF_INFO("ParamsProcessorTrackerFeatureDummy loaded");
- }
-};
-
WOLF_PTR_TYPEDEFS(ProcessorTrackerFeatureDummy);
// Class
class ProcessorTrackerFeatureDummy : public ProcessorTrackerFeature
{
+ protected:
+ unsigned int n_tracks_lost_; ///< number of tracks lost each time track is called (the first ones)
+
public:
- ProcessorTrackerFeatureDummy(ParamsProcessorTrackerFeatureDummyPtr _params_tracker_feature);
+ ProcessorTrackerFeatureDummy(const YAML::Node& _params);
~ProcessorTrackerFeatureDummy() override;
// Factory method for high level API
- WOLF_PROCESSOR_CREATE(ProcessorTrackerFeatureDummy, ParamsProcessorTrackerFeatureDummy);
+ WOLF_PROCESSOR_CREATE(ProcessorTrackerFeatureDummy);
void configure(SensorBasePtr _sensor) override{};
- protected:
- ParamsProcessorTrackerFeatureDummyPtr params_tracker_feature_dummy_;
-
/** \brief Track provided features in \b _capture
* \param _features_in input list of features in \b last to track
* \param _capture the capture in which the _features_in should be searched
@@ -117,12 +101,74 @@ class ProcessorTrackerFeatureDummy : public ProcessorTrackerFeature
* This function emplaces a factor of the appropriate type for the derived processor.
*/
FactorBasePtr emplaceFactor(FeatureBasePtr _feature_ptr, FeatureBasePtr _feature_other_ptr) override;
+
+ // PUBLIC METHODS FOR TESTS:
+ 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;
+ };
};
-inline ProcessorTrackerFeatureDummy::ProcessorTrackerFeatureDummy(
- ParamsProcessorTrackerFeatureDummyPtr _params_tracker_feature_dummy)
- : ProcessorTrackerFeature("ProcessorTrackerFeatureDummy", "PO", 0, _params_tracker_feature_dummy),
- params_tracker_feature_dummy_(_params_tracker_feature_dummy)
+inline ProcessorTrackerFeatureDummy::ProcessorTrackerFeatureDummy(const YAML::Node& _params)
+ : ProcessorTrackerFeature("ProcessorTrackerFeatureDummy", "PO", 0, _params),
+ n_tracks_lost_(_params["n_tracks_lost"].as<unsigned int>())
{
//
}
diff --git a/test/dummy/processor_tracker_landmark_dummy.cpp b/test/dummy/processor_tracker_landmark_dummy.cpp
index f9531cab4726548c2965ce6285ad995ed633e8f8..e273083950f12b424625e8b9deacd29b3cf48e29 100644
--- a/test/dummy/processor_tracker_landmark_dummy.cpp
+++ b/test/dummy/processor_tracker_landmark_dummy.cpp
@@ -26,11 +26,9 @@
namespace wolf
{
-
-ProcessorTrackerLandmarkDummy::ProcessorTrackerLandmarkDummy(
- ParamsProcessorTrackerLandmarkDummyPtr _params_tracker_landmark_dummy)
- : ProcessorTrackerLandmark("ProcessorTrackerLandmarkDummy", "PO", _params_tracker_landmark_dummy),
- params_tracker_landmark_dummy_(_params_tracker_landmark_dummy)
+ProcessorTrackerLandmarkDummy::ProcessorTrackerLandmarkDummy(const YAML::Node& _params)
+ : ProcessorTrackerLandmark("ProcessorTrackerLandmarkDummy", "PO", _params),
+ n_landmarks_lost_(_params["n_landmarks_lost"].as<unsigned int>())
{
//
}
@@ -54,8 +52,7 @@ unsigned int ProcessorTrackerLandmarkDummy::findLandmarks(const LandmarkBasePtrL
auto count = 0;
for (auto landmark_in_ptr : _landmarks_in)
{
- if (count <
- prev_found - params_tracker_landmark_dummy_->n_landmarks_lost) // lose last n_landmarks_lost landmarks
+ if (count < prev_found - n_landmarks_lost_) // lose last n_landmarks_lost landmarks
{
FeatureBasePtr ftr = FeatureBase::emplace<FeatureBase>(
_capture, "DUMMY FEATURE", Eigen::Vector1d::Ones(), Eigen::MatrixXd::Ones(1, 1));
@@ -75,7 +72,7 @@ unsigned int ProcessorTrackerLandmarkDummy::findLandmarks(const LandmarkBasePtrL
bool ProcessorTrackerLandmarkDummy::voteForKeyFrame() const
{
std::cout << "N features: " << incoming_ptr_->getFeatureList().size() << std::endl;
- bool vote = incoming_ptr_->getFeatureList().size() < params_tracker_landmark_dummy_->min_features_for_keyframe;
+ bool vote = incoming_ptr_->getFeatureList().size() < min_features_for_keyframe_;
std::cout << (vote ? "Vote " : "Not vote ") << "for KF" << std::endl;
return vote;
}
@@ -124,11 +121,17 @@ FactorBasePtr ProcessorTrackerLandmarkDummy::emplaceFactor(FeatureBasePtr _featu
std::cout << "\tProcessorTrackerLandmarkDummy::emplaceFactor" << std::endl;
std::cout << "\t\tfeature " << _feature_ptr->id() << std::endl;
std::cout << "\t\tlandmark " << _landmark_ptr->id() << std::endl;
- return FactorBase::emplace<FactorLandmarkDummy>(_feature_ptr,
- _feature_ptr,
- _landmark_ptr,
- shared_from_this(),
- params_tracker_landmark_dummy_->apply_loss_function);
+ return FactorBase::emplace<FactorLandmarkDummy>(
+ _feature_ptr, _feature_ptr, _landmark_ptr, shared_from_this(), applyLossFunction());
+}
+
+void ProcessorTrackerLandmarkDummy::setLast(CaptureBasePtr _last_ptr)
+{
+ last_ptr_ = _last_ptr;
+}
+void ProcessorTrackerLandmarkDummy::setInc(CaptureBasePtr _incoming_ptr)
+{
+ incoming_ptr_ = _incoming_ptr;
}
} // namespace wolf
diff --git a/test/dummy/processor_tracker_landmark_dummy.h b/test/dummy/processor_tracker_landmark_dummy.h
index b24265709342ae20d0b7f32bc83397fda5244cbc..ebe439379f0f4e88dd097649551c227f3b1113f9 100644
--- a/test/dummy/processor_tracker_landmark_dummy.h
+++ b/test/dummy/processor_tracker_landmark_dummy.h
@@ -24,95 +24,152 @@
namespace wolf
{
+WOLF_PTR_TYPEDEFS(ProcessorTrackerLandmarkDummy);
-WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorTrackerLandmarkDummy);
-
-struct ParamsProcessorTrackerLandmarkDummy : public ParamsProcessorTrackerLandmark
+class ProcessorTrackerLandmarkDummy : public ProcessorTrackerLandmark
{
- unsigned int n_landmarks_lost; ///< number of landmarks lost each time findLandmarks is called (the last ones)
+ public:
+ ProcessorTrackerLandmarkDummy(const YAML::Node& _params);
+ ~ProcessorTrackerLandmarkDummy() override;
+
+ // Factory method for high level API
+ WOLF_PROCESSOR_CREATE(ProcessorTrackerLandmarkDummy);
+
+ void configure(SensorBasePtr _sensor) override{};
+
+ protected:
+ unsigned int n_landmarks_lost_; ///< number of landmarks lost each time findLandmarks is called (the last ones)
+
+ /** \brief Find provided landmarks as features in the provided capture
+ * \param _landmarks_in input list of landmarks to be found
+ * \param _capture the capture in which the _landmarks_in should be searched
+ * \param _features_out returned list of features found in \b _capture corresponding to a landmark of _landmarks_in
+ * \param _feature_landmark_correspondences returned map of landmark correspondences: _feature_landmark_correspondences[_feature_out_ptr] = landmark_in_ptr
+ *
+ * IMPORTANT: The features in _features_out should be emplaced. Don't use `make_shared`, use `FeatureBase::emplace`
+ * instead. Then, they will be already linked to the _capture.
+ *
+ * \return the number of landmarks found
+ */
+ unsigned int findLandmarks(const LandmarkBasePtrList& _landmarks_in,
+ const CaptureBasePtr& _capture,
+ FeatureBasePtrList& _features_out,
+ LandmarkMatchMap& _feature_landmark_correspondences) override;
+
+ /** \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 Detect new Features
+ * \param _max_features maximum number of features detected (-1: unlimited. 0: none)
+ * \param _capture The capture in which the new features should be detected.
+ * \param _features_out The list of detected Features in _capture.
+ * \return The number of detected Features.
+ *
+ * This function detects Features that do not correspond to known Features/Landmarks in the system.
+ *
+ * IMPORTANT: The features in _features_out should be emplaced. Don't use `make_shared`, use `FeatureBase::emplace`
+ * instead. Then, they will be already linked to the _capture.
+ *
+ * The function is called in ProcessorTrackerLandmark::processNew() to set the member new_features_last_,
+ * the list of newly detected features of the capture last_ptr_.
+ */
+ unsigned int detectNewFeatures(const int& _max_new_features,
+ const CaptureBasePtr& _capture,
+ FeatureBasePtrList& _features_out) override;
+
+ /** \brief Emplace one landmark
+ *
+ * Implement in derived classes to build the type of landmark you need for this tracker.
+ */
+ LandmarkBasePtr emplaceLandmark(FeatureBasePtr _feature_ptr) override;
+
+ /** \brief Emplace a new factor
+ * \param _feature_ptr pointer to the Feature to constrain
+ * \param _landmark_ptr LandmarkBase pointer to the Landmark constrained.
+ *
+ * Implement this method in derived classes.
+ */
+ FactorBasePtr emplaceFactor(FeatureBasePtr _feature_ptr, LandmarkBasePtr _landmark_ptr) override;
+
+ // PUBLIC METHODS FOR TESTS:
+ public:
+ void setLast(CaptureBasePtr _last_ptr);
+ void setInc(CaptureBasePtr _incoming_ptr);
+
+ unsigned int callProcessKnown()
+ {
+ return this->processKnown();
+ }
- ParamsProcessorTrackerLandmarkDummy() = default;
- ParamsProcessorTrackerLandmarkDummy(const YAML::Node& _n):
- ParamsProcessorTrackerLandmark(_n)
+ unsigned int callProcessNew(const int& _max_new_features)
{
- n_landmarks_lost = _n["n_landmarks_lost"].as<unsigned int>();
+ return this->processNew(_max_new_features);
}
-};
-WOLF_PTR_TYPEDEFS(ProcessorTrackerLandmarkDummy);
+ unsigned int callDetectNewFeatures(const int& _max_features,
+ const CaptureBasePtr& _capture,
+ FeatureBasePtrList& _features_out)
+ {
+ return this->detectNewFeatures(_max_features, _capture, _features_out);
+ }
-class ProcessorTrackerLandmarkDummy : public ProcessorTrackerLandmark
-{
- public:
- ProcessorTrackerLandmarkDummy(ParamsProcessorTrackerLandmarkDummyPtr _params_tracker_landmark_dummy);
- ~ProcessorTrackerLandmarkDummy() override;
-
- // Factory method for high level API
- WOLF_PROCESSOR_CREATE(ProcessorTrackerLandmarkDummy, ParamsProcessorTrackerLandmarkDummy);
-
- void configure(SensorBasePtr _sensor) override { };
-
- protected:
-
- ParamsProcessorTrackerLandmarkDummyPtr params_tracker_landmark_dummy_;
-
- /** \brief Find provided landmarks as features in the provided capture
- * \param _landmarks_in input list of landmarks to be found
- * \param _capture the capture in which the _landmarks_in should be searched
- * \param _features_out returned list of features found in \b _capture corresponding to a landmark of _landmarks_in
- * \param _feature_landmark_correspondences returned map of landmark correspondences: _feature_landmark_correspondences[_feature_out_ptr] = landmark_in_ptr
- *
- * IMPORTANT: The features in _features_out should be emplaced. Don't use `make_shared`, use `FeatureBase::emplace` instead.
- * Then, they will be already linked to the _capture.
- *
- * \return the number of landmarks found
- */
- unsigned int findLandmarks(const LandmarkBasePtrList& _landmarks_in,
- const CaptureBasePtr& _capture,
- FeatureBasePtrList& _features_out,
- LandmarkMatchMap& _feature_landmark_correspondences) override;
-
- /** \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 Detect new Features
- * \param _max_features maximum number of features detected (-1: unlimited. 0: none)
- * \param _capture The capture in which the new features should be detected.
- * \param _features_out The list of detected Features in _capture.
- * \return The number of detected Features.
- *
- * This function detects Features that do not correspond to known Features/Landmarks in the system.
- *
- * IMPORTANT: The features in _features_out should be emplaced. Don't use `make_shared`, use `FeatureBase::emplace` instead.
- * Then, they will be already linked to the _capture.
- *
- * The function is called in ProcessorTrackerLandmark::processNew() to set the member new_features_last_,
- * the list of newly detected features of the capture last_ptr_.
- */
- unsigned int detectNewFeatures(const int& _max_new_features,
- const CaptureBasePtr& _capture,
- FeatureBasePtrList& _features_out) override;
-
- /** \brief Emplace one landmark
- *
- * Implement in derived classes to build the type of landmark you need for this tracker.
- */
- LandmarkBasePtr emplaceLandmark(FeatureBasePtr _feature_ptr) override;
-
- /** \brief Emplace a new factor
- * \param _feature_ptr pointer to the Feature to constrain
- * \param _landmark_ptr LandmarkBase pointer to the Landmark constrained.
- *
- * Implement this method in derived classes.
- */
- FactorBasePtr emplaceFactor(FeatureBasePtr _feature_ptr,
- LandmarkBasePtr _landmark_ptr) override;
+ unsigned int callFindLandmarks(const LandmarkBasePtrList& _landmarks_in,
+ const CaptureBasePtr& _capture,
+ FeatureBasePtrList& _features_out,
+ LandmarkMatchMap& _feature_landmark_correspondences)
+ {
+ return this->findLandmarks(_landmarks_in, _capture, _features_out, _feature_landmark_correspondences);
+ }
+
+ LandmarkBasePtr callEmplaceLandmark(FeatureBasePtr _feature_ptr)
+ {
+ return this->emplaceLandmark(_feature_ptr);
+ }
+ void callEmplaceNewLandmarks()
+ {
+ this->emplaceNewLandmarks();
+ }
+ FactorBasePtr callEmplaceFactor(FeatureBasePtr _feature_ptr, LandmarkBasePtr _landmark_ptr)
+ {
+ return this->emplaceFactor(_feature_ptr, _landmark_ptr);
+ }
+
+ void callEstablishFactors()
+ {
+ this->establishFactors();
+ }
+
+ void setNewFeaturesLast(FeatureBasePtrList& _new_features_list)
+ {
+ new_features_last_ = _new_features_list;
+ }
+
+ LandmarkMatchMap getMatchesLandmarkFromIncoming()
+ {
+ return matches_landmark_from_incoming_;
+ }
+ LandmarkMatchMap getMatchesLandmarkFromLast()
+ {
+ return matches_landmark_from_last_;
+ }
+ LandmarkBasePtrList getNewLandmarks()
+ {
+ return new_landmarks_;
+ }
+
+ void callReset()
+ {
+ this->resetDerived();
+ origin_ptr_ = last_ptr_;
+ last_ptr_ = incoming_ptr_;
+ incoming_ptr_ = nullptr;
+ };
};
-} // namespace wolf
+} // namespace wolf
diff --git a/test/dummy/sensor_dummy.h b/test/dummy/sensor_dummy.h
index 7d42756ec20099a9065dcebc0a1dd5d3b4a91246..c8b677de82e80b2de5837edca3921feba362bd90 100644
--- a/test/dummy/sensor_dummy.h
+++ b/test/dummy/sensor_dummy.h
@@ -27,111 +27,34 @@
namespace wolf
{
-
-WOLF_STRUCT_PTR_TYPEDEFS(ParamsSensorDummy);
-
-struct ParamsSensorDummy : public ParamsSensorBase
-{
- double noise_p_std;
- double noise_o_std;
-
- ParamsSensorDummy() = default;
- ParamsSensorDummy(const YAML::Node& _n) : ParamsSensorBase(_n)
- {
- noise_p_std = _n["noise_p_std"].as<double>();
- noise_o_std = _n["noise_o_std"].as<double>();
- }
-
- std::string getStatesKeys() const override
- {
- return "PO";
- }
-
- ~ParamsSensorDummy() override = default;
- std::string print() const override
- {
- return ParamsSensorBase::print() + "\n" + "noise_p_std: " + toString(noise_p_std) + "\n" +
- "noise_o_std: " + toString(noise_o_std) + "\n";
- }
-};
-
template <unsigned int DIM>
class SensorDummy : public SensorBase
{
private:
- ParamsSensorDummyPtr params_dummy_;
+ double noise_p_std_;
+ double noise_o_std_;
public:
- SensorDummy(ParamsSensorDummyPtr _params, const SpecStateSensorComposite& _priors)
- : SensorBase("SensorDummy" + toString(DIM) + "d", DIM, _params, _priors), params_dummy_(_params)
+ SensorDummy(const YAML::Node& _params)
+ : SensorBase("SensorDummy" + toString(DIM) + "d", DIM, _params),
+ noise_p_std_(_params["noise_p_std"].as<double>()),
+ noise_o_std_(_params["noise_o_std"].as<double>())
{
static_assert(DIM == 2 or DIM == 3);
}
- WOLF_SENSOR_TEMPLATE_DIM_CREATE(SensorDummy, DIM, ParamsSensorDummy);
+ WOLF_SENSOR_TEMPLATE_DIM_CREATE(SensorDummy, DIM);
virtual ~SensorDummy() = default;
Eigen::MatrixXd computeNoiseCov(const Eigen::VectorXd& _data) const override
{
- return (Eigen::Vector2d() << params_dummy_->noise_p_std, params_dummy_->noise_o_std)
- .finished()
- .cwiseAbs2()
- .asDiagonal();
- }
-};
-
-WOLF_STRUCT_PTR_TYPEDEFS(ParamsSensorDummyPoia);
-
-struct ParamsSensorDummyPoia : public ParamsSensorDummy
-{
- ParamsSensorDummyPoia() = default;
- ParamsSensorDummyPoia(const YAML::Node& _n) : ParamsSensorDummy(_n) {}
-
- std::string getStatesKeys() const override
- {
- return "POIA";
- }
-
- ~ParamsSensorDummyPoia() override = default;
- std::string print() const override
- {
- return ParamsSensorDummy::print() + "\n";
- }
-};
-
-template <unsigned int DIM>
-class SensorDummyPoia : public SensorBase
-{
- private:
- ParamsSensorDummyPoiaPtr params_dummy_poia_;
-
- public:
- SensorDummyPoia(ParamsSensorDummyPoiaPtr _params, const SpecStateSensorComposite& _priors)
- : SensorBase("SensorDummyPoia" + toString(DIM) + "d", DIM, _params, _priors), params_dummy_poia_(_params)
- {
- static_assert(DIM == 2 or DIM == 3);
- }
- WOLF_SENSOR_TEMPLATE_DIM_CREATE(SensorDummyPoia, DIM, ParamsSensorDummyPoia);
-
- virtual ~SensorDummyPoia() = default;
-
- Eigen::MatrixXd computeNoiseCov(const Eigen::VectorXd& _data) const override
- {
- return (Eigen::Vector2d() << params_dummy_poia_->noise_p_std, params_dummy_poia_->noise_o_std)
- .finished()
- .cwiseAbs2()
- .asDiagonal();
+ return (Eigen::Vector2d() << noise_p_std_, noise_o_std_).finished().cwiseAbs2().asDiagonal();
}
};
-
-typedef SensorDummy<2> SensorDummy2d;
-typedef SensorDummy<3> SensorDummy3d;
-typedef SensorDummyPoia<2> SensorDummyPoia2d;
-typedef SensorDummyPoia<3> SensorDummyPoia3d;
+typedef SensorDummy<2> SensorDummy2d;
+typedef SensorDummy<3> SensorDummy3d;
WOLF_DECLARED_PTR_TYPEDEFS(SensorDummy2d);
WOLF_DECLARED_PTR_TYPEDEFS(SensorDummy3d);
-WOLF_DECLARED_PTR_TYPEDEFS(SensorDummyPoia2d);
-WOLF_DECLARED_PTR_TYPEDEFS(SensorDummyPoia3d);
} // namespace wolf
@@ -141,6 +64,4 @@ namespace wolf
{
WOLF_REGISTER_SENSOR(SensorDummy2d);
WOLF_REGISTER_SENSOR(SensorDummy3d);
-WOLF_REGISTER_SENSOR(SensorDummyPoia2d);
-WOLF_REGISTER_SENSOR(SensorDummyPoia3d);
} // namespace wolf
diff --git a/test/dummy/sensor_dummy_poia.h b/test/dummy/sensor_dummy_poia.h
new file mode 100644
index 0000000000000000000000000000000000000000..3801ba02b34caa34df0b08a358a91fc6fc604b46
--- /dev/null
+++ b/test/dummy/sensor_dummy_poia.h
@@ -0,0 +1,67 @@
+// WOLF - Copyright (C) 2020,2021,2022,2023
+// Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
+// Joan Vallvé Navarro (jvallve@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF: http://www.iri.upc.edu/wolf
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// 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/>.
+
+#pragma once
+
+/**************************
+ * WOLF includes *
+ **************************/
+#include "core/sensor/sensor_base.h"
+
+namespace wolf
+{
+template <unsigned int DIM>
+class SensorDummyPoia : public SensorBase
+{
+ private:
+ double noise_p_std_;
+ double noise_o_std_;
+
+ public:
+ SensorDummyPoia(const YAML::Node& _params)
+ : SensorBase("SensorDummyPoia" + toString(DIM) + "d", DIM, _params),
+ noise_p_std_(_params["noise_p_std"].as<double>()),
+ noise_o_std_(_params["noise_o_std"].as<double>())
+ {
+ static_assert(DIM == 2 or DIM == 3);
+ }
+ WOLF_SENSOR_TEMPLATE_DIM_CREATE(SensorDummyPoia, DIM);
+
+ virtual ~SensorDummyPoia() = default;
+
+ Eigen::MatrixXd computeNoiseCov(const Eigen::VectorXd& _data) const override
+ {
+ return (Eigen::Vector2d() << noise_p_std_, noise_o_std_).finished().cwiseAbs2().asDiagonal();
+ }
+};
+typedef SensorDummyPoia<2> SensorDummyPoia2d;
+typedef SensorDummyPoia<3> SensorDummyPoia3d;
+WOLF_DECLARED_PTR_TYPEDEFS(SensorDummyPoia2d);
+WOLF_DECLARED_PTR_TYPEDEFS(SensorDummyPoia3d);
+
+} // namespace wolf
+
+// Register in the FactorySensorNode
+#include "core/sensor/factory_sensor.h"
+namespace wolf
+{
+WOLF_REGISTER_SENSOR(SensorDummyPoia2d);
+WOLF_REGISTER_SENSOR(SensorDummyPoia3d);
+} // namespace wolf
diff --git a/test/dummy/solver_dummy.h b/test/dummy/solver_dummy.h
new file mode 100644
index 0000000000000000000000000000000000000000..895436557a5ec65ef8f93e1bf6585e2dd466c194
--- /dev/null
+++ b/test/dummy/solver_dummy.h
@@ -0,0 +1,147 @@
+// WOLF - Copyright (C) 2020,2021,2022,2023
+// Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
+// Joan Vallvé Navarro (jvallve@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF: http://www.iri.upc.edu/wolf
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// 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/>.
+
+#pragma once
+
+#include "core/solver/solver_manager.h"
+
+namespace wolf
+{
+WOLF_PTR_TYPEDEFS(SolverDummy);
+class SolverDummy : public SolverManager
+{
+ public:
+ std::set<FactorBasePtr> factors_derived_;
+ std::map<StateBlockPtr, bool> state_block_fixed_;
+ std::map<StateBlockPtr, LocalParametrizationBasePtr> state_block_local_param_;
+
+ SolverDummy(const ProblemPtr& wolf_problem, const YAML::Node params) : SolverManager(wolf_problem, params){};
+ WOLF_SOLVER_CREATE(SolverDummy);
+
+ bool isStateBlockFixedDerived(const StateBlockPtr& st) override
+ {
+ return state_block_fixed_.at(st);
+ };
+
+ bool hasThisLocalParametrizationDerived(const StateBlockPtr& st,
+ const LocalParametrizationBasePtr& local_param) override
+ {
+ return state_block_local_param_.at(st) == local_param;
+ };
+
+ bool hasLocalParametrizationDerived(const StateBlockPtr& st) const override
+ {
+ return state_block_local_param_.at(st) != nullptr;
+ };
+
+ virtual int numStateBlocksDerived() const override
+ {
+ return state_block_fixed_.size();
+ }
+
+ virtual int numFactorsDerived() const override
+ {
+ return factors_derived_.size();
+ };
+
+ bool computeCovariancesDerived(const CovarianceBlocksToBeComputed blocks) override
+ {
+ return false;
+ };
+ bool computeCovariancesDerived(const std::vector<StateBlockPtr>& st_list) override
+ {
+ return false;
+ };
+
+ // The following are dummy implementations
+ bool hasConverged() override
+ {
+ return true;
+ }
+ bool wasStopped() override
+ {
+ return false;
+ }
+ unsigned int iterations() override
+ {
+ return 1;
+ }
+ double initialCost() override
+ {
+ return double(1);
+ }
+ double finalCost() override
+ {
+ return double(0);
+ }
+ double totalTime() override
+ {
+ return double(0);
+ }
+ void printProfilingDerived(std::ostream& _stream) const override {}
+
+ protected:
+ bool checkDerived(std::string prefix = "") const override
+ {
+ return true;
+ }
+ std::string solveDerived(const ReportVerbosity report_level) override
+ {
+ return std::string("");
+ };
+ void addFactorDerived(const FactorBasePtr& fac_ptr) override
+ {
+ factors_derived_.insert(fac_ptr);
+ };
+ void removeFactorDerived(const FactorBasePtr& fac_ptr) override
+ {
+ factors_derived_.erase(fac_ptr);
+ };
+ void addStateBlockDerived(const StateBlockPtr& state_ptr) override
+ {
+ state_block_fixed_[state_ptr] = state_ptr->isFixed();
+ state_block_local_param_[state_ptr] = state_ptr->getLocalParametrization();
+ };
+ void removeStateBlockDerived(const StateBlockPtr& state_ptr) override
+ {
+ state_block_fixed_.erase(state_ptr);
+ state_block_local_param_.erase(state_ptr);
+ };
+ void updateStateBlockStatusDerived(const StateBlockPtr& state_ptr) override
+ {
+ state_block_fixed_[state_ptr] = state_ptr->isFixed();
+ };
+ void updateStateBlockLocalParametrizationDerived(const StateBlockPtr& state_ptr) override
+ {
+ state_block_local_param_[state_ptr] = state_ptr->getLocalParametrization();
+ };
+ bool isStateBlockRegisteredDerived(const StateBlockPtr& state_ptr) const override
+ {
+ return state_block_fixed_.count(state_ptr) == 1 and state_block_local_param_.count(state_ptr) == 1;
+ };
+
+ bool isFactorRegisteredDerived(const FactorBasePtr& fac_ptr) const override
+ {
+ return factors_derived_.count(fac_ptr) == 1;
+ };
+};
+
+WOLF_REGISTER_SOLVER(SolverDummy)
+} // namespace wolf
diff --git a/test/dummy/solver_manager_dummy.h b/test/dummy/solver_manager_dummy.h
deleted file mode 100644
index 0b368f64c254c4d9b30c78f81ca66bc21258729f..0000000000000000000000000000000000000000
--- a/test/dummy/solver_manager_dummy.h
+++ /dev/null
@@ -1,120 +0,0 @@
-// WOLF - Copyright (C) 2020,2021,2022,2023
-// Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
-// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
-// Joan Vallvé Navarro (jvallve@iri.upc.edu)
-// All rights reserved.
-//
-// This file is part of WOLF: http://www.iri.upc.edu/wolf
-// WOLF is free software: you can redistribute it and/or modify
-// it under the terms of the GNU Lesser General Public License as published by
-// 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/>.
-
-#pragma once
-
-#include "core/solver/solver_manager.h"
-
-namespace wolf
-{
-
-WOLF_PTR_TYPEDEFS(SolverManagerDummy);
-class SolverManagerDummy : public SolverManager
-{
- public:
- std::set<FactorBasePtr> factors_derived_;
- std::map<StateBlockPtr,bool> state_block_fixed_;
- std::map<StateBlockPtr,LocalParametrizationBasePtr> state_block_local_param_;
-
- SolverManagerDummy(const ProblemPtr& wolf_problem) :
- SolverManager(wolf_problem)
- {
- };
-
- bool isStateBlockFixedDerived(const StateBlockPtr& st) override
- {
- return state_block_fixed_.at(st);
- };
-
- bool hasThisLocalParametrizationDerived(const StateBlockPtr& st, const LocalParametrizationBasePtr& local_param) override
- {
- return state_block_local_param_.at(st) == local_param;
- };
-
- bool hasLocalParametrizationDerived(const StateBlockPtr& st) const override
- {
- return state_block_local_param_.at(st) != nullptr;
- };
-
- virtual int numStateBlocksDerived() const override
- {
- return state_block_fixed_.size();
- }
-
- virtual int numFactorsDerived() const override
- {
- return factors_derived_.size();
- };
-
- bool computeCovariancesDerived(const CovarianceBlocksToBeComputed blocks) override {return false;};
- bool computeCovariancesDerived(const std::vector<StateBlockPtr>& st_list) override {return false;};
-
-
- // The following are dummy implementations
- bool hasConverged() override { return true; }
- bool wasStopped() override { return false; }
- unsigned int iterations() override { return 1; }
- double initialCost() override { return double(1); }
- double finalCost() override { return double(0); }
- double totalTime() override { return double(0); }
- void printProfilingDerived(std::ostream& _stream) const override {}
-
- protected:
-
- bool checkDerived(std::string prefix="") const override {return true;}
- std::string solveDerived(const ReportVerbosity report_level) override { return std::string("");};
- void addFactorDerived(const FactorBasePtr& fac_ptr) override
- {
- factors_derived_.insert(fac_ptr);
- };
- void removeFactorDerived(const FactorBasePtr& fac_ptr) override
- {
- factors_derived_.erase(fac_ptr);
- };
- void addStateBlockDerived(const StateBlockPtr& state_ptr) override
- {
- state_block_fixed_[state_ptr] = state_ptr->isFixed();
- state_block_local_param_[state_ptr] = state_ptr->getLocalParametrization();
- };
- void removeStateBlockDerived(const StateBlockPtr& state_ptr) override
- {
- state_block_fixed_.erase(state_ptr);
- state_block_local_param_.erase(state_ptr);
- };
- void updateStateBlockStatusDerived(const StateBlockPtr& state_ptr) override
- {
- state_block_fixed_[state_ptr] = state_ptr->isFixed();
- };
- void updateStateBlockLocalParametrizationDerived(const StateBlockPtr& state_ptr) override
- {
- state_block_local_param_[state_ptr] = state_ptr->getLocalParametrization();
- };
- bool isStateBlockRegisteredDerived(const StateBlockPtr& state_ptr) const override
- {
- return state_block_fixed_.count(state_ptr) == 1 and state_block_local_param_.count(state_ptr) == 1;
- };
-
- bool isFactorRegisteredDerived(const FactorBasePtr& fac_ptr) const override
- {
- return factors_derived_.count(fac_ptr) == 1;
- };
-};
-
-}
diff --git a/test/dummy/tree_manager_dummy.h b/test/dummy/tree_manager_dummy.h
index c1c939022234be04ec4552fd94692103d7286120..49b3d8a2561712316bf091ff8ccd25e216fa5177 100644
--- a/test/dummy/tree_manager_dummy.h
+++ b/test/dummy/tree_manager_dummy.h
@@ -24,53 +24,27 @@
namespace wolf
{
-
-WOLF_STRUCT_PTR_TYPEDEFS(ParamsTreeManagerDummy)
-struct ParamsTreeManagerDummy : public ParamsTreeManagerBase
-{
- ParamsTreeManagerDummy() = default;
- ParamsTreeManagerDummy(const YAML::Node& _n):
- ParamsTreeManagerBase(_n)
- {
- toy_param = _n["toy_param"].as<double>();
- }
-
- ~ParamsTreeManagerDummy() override = default;
-
- bool toy_param;
-
- std::string print() const override
- {
- return ParamsTreeManagerBase::print() + "\n"
- + "toy_param: " + toString(toy_param) + "\n";
- }
-};
-
WOLF_PTR_TYPEDEFS(TreeManagerDummy)
class TreeManagerDummy : public TreeManagerBase
{
- public:
- TreeManagerDummy(ParamsTreeManagerBasePtr _params) :
- TreeManagerBase("TreeManagerDummy", _params),
- n_KF_(0)
- {};
- WOLF_TREE_MANAGER_CREATE(TreeManagerDummy, ParamsTreeManagerBase)
+ public:
+ TreeManagerDummy(const YAML::Node& _params) : TreeManagerBase("TreeManagerDummy", _params), n_KF_(0)
+ {
+ toy_param_ = _params["toy_param"].as<double>();
+ };
+ WOLF_TREE_MANAGER_CREATE(TreeManagerDummy)
- ~TreeManagerDummy() override{}
+ ~TreeManagerDummy() override {}
- void keyFrameCallback(FrameBasePtr _KF) override
- {
- n_KF_++;
- };
+ void keyFrameCallback(FrameBasePtr _KF) override
+ {
+ n_KF_++;
+ };
- int n_KF_;
+ int n_KF_;
+ double toy_param_;
};
-} /* namespace wolf */
-
-// Register in the FactoryTreeManager
-#include "core/tree_manager/factory_tree_manager.h"
-namespace wolf {
WOLF_REGISTER_TREE_MANAGER(TreeManagerDummy)
} /* namespace wolf */
diff --git a/test/gtest_SE2.cpp b/test/gtest_SE2.cpp
index 84733273d55c042b3c58b4a83951efb051cbed45..87164bebf9cba2fb9b07111391e9f570700687e5 100644
--- a/test/gtest_SE2.cpp
+++ b/test/gtest_SE2.cpp
@@ -27,7 +27,7 @@ using namespace SE2;
TEST(SE2, exp)
{
- for (auto i = 1; i<10; i++)
+ for (auto i = 1; i < 10; i++)
{
double o = Vector1d::Random()(0) * M_PI;
ASSERT_MATRIX_APPROX(SO2::exp(o), Rotation2Dd(o).matrix(), 1e-8);
@@ -37,14 +37,14 @@ TEST(SE2, exp)
TEST(SE2, invBlocks)
{
Vector2d p, ip;
- double o, io;
+ double o, io;
// zero
p = Vector2d::Zero();
o = 0;
- SE2::inverse(p,o,ip,io);
-
+ SE2::inverse(p, o, ip, io);
+
ASSERT_MATRIX_APPROX(ip, Vector2d::Zero(), 1e-8);
ASSERT_NEAR(io, 0, 1e-8);
@@ -52,8 +52,8 @@ TEST(SE2, invBlocks)
p = Vector2d::Random();
o = 0;
- SE2::inverse(p,o,ip,io);
-
+ SE2::inverse(p, o, ip, io);
+
ASSERT_MATRIX_APPROX(ip, -p, 1e-8);
ASSERT_NEAR(io, 0, 1e-8);
@@ -61,8 +61,8 @@ TEST(SE2, invBlocks)
p = Vector2d::Zero();
o = Vector1d::Random()(0) * M_PI;
- SE2::inverse(p,o,ip,io);
-
+ SE2::inverse(p, o, ip, io);
+
ASSERT_MATRIX_APPROX(ip, Vector2d::Zero(), 1e-8);
ASSERT_NEAR(io, -o, 1e-8);
@@ -72,8 +72,8 @@ TEST(SE2, invBlocks)
p = Vector2d::Random();
o = Vector1d::Random()(0) * M_PI;
- SE2::inverse(p,o,ip,io);
-
+ SE2::inverse(p, o, ip, io);
+
ASSERT_MATRIX_APPROX(ip, Rotation2Dd(-o).matrix() * -p, 1e-8);
ASSERT_NEAR(io, -o, 1e-8);
}
@@ -86,25 +86,25 @@ TEST(SE2, invVector)
// zero
d = Vector3d::Zero();
- SE2::inverse(d,id);
-
+ SE2::inverse(d, id);
+
ASSERT_MATRIX_APPROX(id, Vector3d::Zero(), 1e-8);
// zero angle
- d = Vector3d::Random();
+ d = Vector3d::Random();
d(2) = 0;
- SE2::inverse(d,id);
-
+ SE2::inverse(d, id);
+
ASSERT_MATRIX_APPROX(id.head<2>(), -d.head<2>(), 1e-8);
ASSERT_NEAR(id(2), 0, 1e-8);
// zero translation
- d = Vector3d::Zero();
+ d = Vector3d::Zero();
d(2) = Vector1d::Random()(0) * M_PI;
- SE2::inverse(d,id);
-
+ SE2::inverse(d, id);
+
ASSERT_MATRIX_APPROX(id.head<2>(), Vector2d::Zero(), 1e-8);
ASSERT_NEAR(id(2), -d(2), 1e-8);
@@ -114,8 +114,8 @@ TEST(SE2, invVector)
d = Vector3d::Random();
d(2) *= M_PI;
- SE2::inverse(d,id);
-
+ SE2::inverse(d, id);
+
ASSERT_MATRIX_APPROX(id.head<2>(), Rotation2Dd(-d(2)) * -d.head<2>(), 1e-8);
ASSERT_NEAR(id(2), -d(2), 1e-8);
}
@@ -124,7 +124,7 @@ TEST(SE2, invVector)
TEST(SE2, betweenBlocks)
{
Vector2d p1, p2, pd;
- double o1, o2, od;
+ double o1, o2, od;
// both origin: relative pose zero
p1 = Vector2d::Zero();
@@ -153,11 +153,11 @@ TEST(SE2, betweenBlocks)
// random relative pose
for (auto i = 0; i < 10; i++)
{
- p1 = Vector2d::Random();
- o1 = Vector1d::Random()(0) * M_PI;
+ p1 = Vector2d::Random();
+ o1 = Vector1d::Random()(0) * M_PI;
Vector2d pd_gt = Vector2d::Random();
- double od_gt = Vector1d::Random()(0) * M_PI;
-
+ double od_gt = Vector1d::Random()(0) * M_PI;
+
p2 = p1 + Rotation2Dd(o1) * pd_gt;
o2 = o1 + od_gt;
@@ -198,9 +198,9 @@ TEST(SE2, betweenVectors)
p1(2) *= M_PI;
Vector3d pd_gt = Vector3d::Random();
pd_gt(2) *= M_PI;
-
+
p2.head<2>() = p1.head<2>() + Rotation2Dd(p1(2)) * pd_gt.head<2>();
- p2(2) = p1(2) + pd_gt(2);
+ p2(2) = p1(2) + pd_gt(2);
between(p1, p2, pd);
@@ -238,9 +238,9 @@ TEST(SE2, betweenVectorsReturn)
p1(2) *= M_PI;
Vector3d pd_gt = Vector3d::Random();
pd_gt(2) *= M_PI;
-
+
p2.head<2>() = p1.head<2>() + Rotation2Dd(p1(2)) * pd_gt.head<2>();
- p2(2) = p1(2) + pd_gt(2);
+ p2(2) = p1(2) + pd_gt(2);
pd = between(p1, p2);
@@ -250,7 +250,6 @@ TEST(SE2, betweenVectorsReturn)
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_SE3.cpp b/test/gtest_SE3.cpp
index 700f6787f7992fb888fb56d84207849b929b9788..ca50261cc069537a01bfbcd16f0af8e48108ae56 100644
--- a/test/gtest_SE3.cpp
+++ b/test/gtest_SE3.cpp
@@ -28,14 +28,16 @@ using namespace SE3;
TEST(SE3, exp_0)
{
Vector6d tau = Vector6d::Zero();
- Vector7d pose; pose << 0,0,0, 0,0,0,1;
+ Vector7d pose;
+ pose << 0, 0, 0, 0, 0, 0, 1;
ASSERT_POSE3d_APPROX(pose, exp(tau), 1e-8);
}
TEST(SE3, log_I)
{
- Vector7d pose; pose << 0,0,0, 0,0,0,1;
+ Vector7d pose;
+ pose << 0, 0, 0, 0, 0, 0, 1;
Vector6d tau = Vector6d::Zero();
ASSERT_MATRIX_APPROX(tau, log(pose), 1e-8);
@@ -43,7 +45,7 @@ TEST(SE3, log_I)
TEST(SE3, exp_log)
{
- Vector6d tau = Vector6d::Random() / 5.0;
+ Vector6d tau = Vector6d::Random() / 5.0;
Vector7d pose = exp(tau);
ASSERT_MATRIX_APPROX(tau, log(exp(tau)), 1e-8);
@@ -56,8 +58,8 @@ TEST(SE3, exp_of_multiple)
Vector7d pose, pose2, pose3;
tau = Vector6d::Random() / 5;
pose << exp(tau);
- tau2 = 2*tau;
- tau3 = 3*tau;
+ tau2 = 2 * tau;
+ tau3 = 3 * tau;
pose2 = compose(pose, pose);
pose3 = compose(pose2, pose);
@@ -77,8 +79,8 @@ TEST(SE3, log_of_power)
Vector7d pose, pose2, pose3;
tau = Vector6d::Random() / 5;
pose << exp(tau);
- tau2 = 2*tau;
- tau3 = 3*tau;
+ tau2 = 2 * tau;
+ tau3 = 3 * tau;
pose2 = compose(pose, pose);
pose3 = compose(pose2, pose);
@@ -94,15 +96,17 @@ TEST(SE3, log_of_power)
TEST(SE3, inverse)
{
- Vector3d p; p.setRandom();
- Vector4d qvec; qvec.setRandom().normalized();
+ Vector3d p;
+ p.setRandom();
+ Vector4d qvec;
+ qvec.setRandom().normalized();
Quaterniond q(qvec);
- Vector3d pi_true = -(q.conjugate() * p);
+ Vector3d pi_true = -(q.conjugate() * p);
Quaterniond qi_true = q.conjugate();
// separate API
- Vector3d pi_out;
+ Vector3d pi_out;
Quaterniond qi_out;
inverse(p, q, pi_out, qi_out);
@@ -110,8 +114,10 @@ TEST(SE3, inverse)
ASSERT_MATRIX_APPROX(qi_out.coeffs(), qi_true.coeffs(), 1e-8);
// Vector7d API
- Vector7d pose; pose << p, q.coeffs();
- Vector7d posei_true; posei_true << pi_true, qi_true.coeffs();
+ Vector7d pose;
+ pose << p, q.coeffs();
+ Vector7d posei_true;
+ posei_true << pi_true, qi_true.coeffs();
Vector7d posei_out;
inverse(pose, posei_out);
ASSERT_MATRIX_APPROX(posei_out, posei_true, 1e-8);
@@ -122,13 +128,15 @@ TEST(SE3, inverse)
TEST(SE3, inverseComposite)
{
- Vector3d p; p.setRandom();
- Vector4d qvec; qvec.setRandom().normalized();
+ Vector3d p;
+ p.setRandom();
+ Vector4d qvec;
+ qvec.setRandom().normalized();
VectorComposite pose_vc("PO", {p, qvec});
- Quaterniond q(qvec);
+ Quaterniond q(qvec);
// ground truth
- Vector3d pi_true = -(q.conjugate() * p);
+ Vector3d pi_true = -(q.conjugate() * p);
Quaterniond qi_true = q.conjugate();
VectorComposite pose_vc_out("PO", Vector7d::Zero(), {3, 4});
@@ -143,48 +151,51 @@ TEST(SE3, inverseComposite)
TEST(SE3, composeBlocks)
{
- Vector3d p1, p2, pc;
+ Vector3d p1, p2, pc;
Quaterniond q1, q2, qc;
p1.setRandom();
- q1 = exp_q(Vector3d::Random()*100);
+ q1 = exp_q(Vector3d::Random() * 100);
p2.setRandom();
- q2 = exp_q(Vector3d::Random()*100);
+ q2 = exp_q(Vector3d::Random() * 100);
compose(p1, q1, p2, q2, pc, qc);
- ASSERT_MATRIX_APPROX(pc, p1 + q1*p2, 1e-8);
- ASSERT_QUATERNION_APPROX(qc, q1*q2, 1e-8);
+ ASSERT_MATRIX_APPROX(pc, p1 + q1 * p2, 1e-8);
+ ASSERT_QUATERNION_APPROX(qc, q1 * q2, 1e-8);
}
TEST(SE3, composeVectorBlocks)
{
- Vector3d p1, p2, pc;
+ Vector3d p1, p2, pc;
Quaterniond q1, q2, qc;
p1.setRandom();
- q1 = exp_q(Vector3d::Random()*100);
+ q1 = exp_q(Vector3d::Random() * 100);
p2.setRandom();
- q2 = exp_q(Vector3d::Random()*100);
+ q2 = exp_q(Vector3d::Random() * 100);
compose(p1, q1.coeffs(), p2, q2.coeffs(), pc, qc.coeffs());
- ASSERT_MATRIX_APPROX(pc, p1 + q1*p2, 1e-8);
- ASSERT_QUATERNION_APPROX(qc, q1*q2, 1e-8);
+ ASSERT_MATRIX_APPROX(pc, p1 + q1 * p2, 1e-8);
+ ASSERT_QUATERNION_APPROX(qc, q1 * q2, 1e-8);
}
TEST(SE3, composeEigenVectors)
{
- Vector3d p1, p2, pc;
+ Vector3d p1, p2, pc;
Quaterniond q1, q2, qc;
p1.setRandom();
- q1 = exp_q(Vector3d::Random()*100);
+ q1 = exp_q(Vector3d::Random() * 100);
p2.setRandom();
- q2 = exp_q(Vector3d::Random()*100);
+ q2 = exp_q(Vector3d::Random() * 100);
- compose(p1, q1, p2, q2, pc, qc); // tested in composeVectorBlocks
+ compose(p1, q1, p2, q2, pc, qc); // tested in composeVectorBlocks
- Vector7d x1; x1 << p1, q1.coeffs();
- Vector7d x2; x2 << p2, q2.coeffs();
- Vector7d xc, xc_true; xc_true << pc, qc.coeffs();
+ Vector7d x1;
+ x1 << p1, q1.coeffs();
+ Vector7d x2;
+ x2 << p2, q2.coeffs();
+ Vector7d xc, xc_true;
+ xc_true << pc, qc.coeffs();
compose(x1, x2, xc);
@@ -193,16 +204,16 @@ TEST(SE3, composeEigenVectors)
TEST(SE3, composeVectorComposite)
{
- Vector3d p1, p2, pc;
+ Vector3d p1, p2, pc;
Quaterniond q1, q2, qc;
p1.setRandom();
- q1 = exp_q(Vector3d::Random()*100);
+ q1 = exp_q(Vector3d::Random() * 100);
p2.setRandom();
- q2 = exp_q(Vector3d::Random()*100);
+ q2 = exp_q(Vector3d::Random() * 100);
- compose(p1, q1, p2, q2, pc, qc); // tested in composeVectorBlocks
+ compose(p1, q1, p2, q2, pc, qc); // tested in composeVectorBlocks
- VectorComposite x1, x2, xc("PO", Vector7d::Zero(), {3,4});
+ VectorComposite x1, x2, xc("PO", Vector7d::Zero(), {3, 4});
x1.emplace('P', p1);
x1.emplace('O', q1.coeffs());
@@ -224,16 +235,16 @@ TEST(SE3, composeVectorComposite_Jacobians)
Vector3d p1, p2, pc;
Quaterniond q1, q2, qc;
p1.setRandom();
- q1 = exp_q(Vector3d::Random()*100);
+ q1 = exp_q(Vector3d::Random() * 100);
p2.setRandom();
- q2 = exp_q(Vector3d::Random()*100);
+ q2 = exp_q(Vector3d::Random() * 100);
Matrix3d R1 = q1.matrix();
Matrix3d R2 = q2.matrix();
- compose(p1, q1, p2, q2, pc, qc); // tested in composeVectorBlocks
+ compose(p1, q1, p2, q2, pc, qc); // tested in composeVectorBlocks
- VectorComposite x1, x2, xc("PO", Vector7d::Zero(), {3,4});
+ VectorComposite x1, x2, xc("PO", Vector7d::Zero(), {3, 4});
MatrixComposite J_xc_x1, J_xc_x2;
x1.emplace('P', p1);
@@ -247,23 +258,25 @@ TEST(SE3, composeVectorComposite_Jacobians)
ASSERT_MATRIX_APPROX(xc.at('P'), pc, 1e-8);
ASSERT_QUATERNION_VECTOR_APPROX(xc.at('O'), qc.coeffs(), 1e-8);
- ASSERT_MATRIX_APPROX(J_xc_x1.at('P', 'P'), Matrix3d::Identity() , 1e-8);
- ASSERT_MATRIX_APPROX(J_xc_x1.at('P', 'O'), - R1 * skew(p2) , 1e-8);
- ASSERT_MATRIX_APPROX(J_xc_x1.at('O', 'P'), Matrix3d::Zero() , 1e-8);
- ASSERT_MATRIX_APPROX(J_xc_x1.at('O', 'O'), R2.transpose() , 1e-8);
+ ASSERT_MATRIX_APPROX(J_xc_x1.at('P', 'P'), Matrix3d::Identity(), 1e-8);
+ ASSERT_MATRIX_APPROX(J_xc_x1.at('P', 'O'), -R1 * skew(p2), 1e-8);
+ ASSERT_MATRIX_APPROX(J_xc_x1.at('O', 'P'), Matrix3d::Zero(), 1e-8);
+ ASSERT_MATRIX_APPROX(J_xc_x1.at('O', 'O'), R2.transpose(), 1e-8);
- ASSERT_MATRIX_APPROX(J_xc_x2.at('P', 'P'), R1 , 1e-8);
- ASSERT_MATRIX_APPROX(J_xc_x2.at('P', 'O'), Matrix3d::Zero() , 1e-8);
- ASSERT_MATRIX_APPROX(J_xc_x2.at('O', 'P'), Matrix3d::Zero() , 1e-8);
- ASSERT_MATRIX_APPROX(J_xc_x2.at('O', 'O'), Matrix3d::Identity() , 1e-8);
+ ASSERT_MATRIX_APPROX(J_xc_x2.at('P', 'P'), R1, 1e-8);
+ ASSERT_MATRIX_APPROX(J_xc_x2.at('P', 'O'), Matrix3d::Zero(), 1e-8);
+ ASSERT_MATRIX_APPROX(J_xc_x2.at('O', 'P'), Matrix3d::Zero(), 1e-8);
+ ASSERT_MATRIX_APPROX(J_xc_x2.at('O', 'O'), Matrix3d::Identity(), 1e-8);
}
TEST(SE3, exp_0_Composite)
{
// we check that exp(zero) = identity
- Vector3d p; p.setZero();
- Vector3d theta; theta.setZero();
+ Vector3d p;
+ p.setZero();
+ Vector3d theta;
+ theta.setZero();
VectorComposite tau;
@@ -274,22 +287,25 @@ TEST(SE3, exp_0_Composite)
ASSERT_MATRIX_APPROX(x.at('P'), Vector3d::Zero(), 1e-8);
ASSERT_QUATERNION_VECTOR_APPROX(x.at('O'), Quaterniond::Identity().coeffs(), 1e-8);
-
}
TEST(SE3, plus_0_Composite)
{
// we check that x plus zero = x
- Vector3d p; p.setRandom();
- Vector4d q; q.setRandom().normalized();
+ Vector3d p;
+ p.setRandom();
+ Vector4d q;
+ q.setRandom().normalized();
VectorComposite x;
x.emplace('P', p);
x.emplace('O', q);
- Vector3d dp; dp.setZero();
- Vector3d dtheta; dtheta.setZero();
+ Vector3d dp;
+ dp.setZero();
+ Vector3d dtheta;
+ dtheta.setZero();
VectorComposite tau;
tau.emplace('P', dp);
@@ -305,13 +321,14 @@ TEST(SE3, interpolate_I_xyz)
{
Vector7d p1, p2, p_pre;
- p1 << 0,0,0, 0,0,0,1;
- p2 << 1,2,3, 0,0,0,1;
+ p1 << 0, 0, 0, 0, 0, 0, 1;
+ p2 << 1, 2, 3, 0, 0, 0, 1;
double t = 0.2;
interpolate(p1, p2, t, p_pre);
- Vector7d p_gt; p_gt << 0.2,0.4,0.6, 0,0,0,1;
+ Vector7d p_gt;
+ p_gt << 0.2, 0.4, 0.6, 0, 0, 0, 1;
ASSERT_POSE3d_APPROX(p_pre, p_gt, 1e-8);
}
@@ -320,13 +337,14 @@ TEST(SE3, interpolate_xyz_xyz)
{
Vector7d p1, p2, p_pre;
- p1 << 1,2,3, 0,0,0,1;
- p2 << 2,4,6, 0,0,0,1;
+ p1 << 1, 2, 3, 0, 0, 0, 1;
+ p2 << 2, 4, 6, 0, 0, 0, 1;
double t = 0.2;
interpolate(p1, p2, t, p_pre);
- Vector7d p_gt; p_gt << 1.2,2.4,3.6, 0,0,0,1;
+ Vector7d p_gt;
+ p_gt << 1.2, 2.4, 3.6, 0, 0, 0, 1;
ASSERT_POSE3d_APPROX(p_pre, p_gt, 1e-8);
}
@@ -335,13 +353,14 @@ TEST(SE3, interpolate_I_qx)
{
Vector7d p1, p2, p_pre;
- p1 << 0,0,0, 0,0,0,1;
- p2 << 0,0,0, 1,0,0,0;
+ p1 << 0, 0, 0, 0, 0, 0, 1;
+ p2 << 0, 0, 0, 1, 0, 0, 0;
double t = 0.5;
interpolate(p1, p2, t, p_pre);
- Vector7d p_gt; p_gt << 0,0,0, sqrt(2)/2,0,0,sqrt(2)/2;
+ Vector7d p_gt;
+ p_gt << 0, 0, 0, sqrt(2) / 2, 0, 0, sqrt(2) / 2;
ASSERT_POSE3d_APPROX(p_pre, p_gt, 1e-8);
}
@@ -350,13 +369,14 @@ TEST(SE3, interpolate_I_qy)
{
Vector7d p1, p2, p_pre;
- p1 << 0,0,0, 0,0,0,1;
- p2 << 0,0,0, 0,1,0,0;
+ p1 << 0, 0, 0, 0, 0, 0, 1;
+ p2 << 0, 0, 0, 0, 1, 0, 0;
double t = 0.5;
interpolate(p1, p2, t, p_pre);
- Vector7d p_gt; p_gt << 0,0,0, 0,sqrt(2)/2,0,sqrt(2)/2;
+ Vector7d p_gt;
+ p_gt << 0, 0, 0, 0, sqrt(2) / 2, 0, sqrt(2) / 2;
ASSERT_POSE3d_APPROX(p_pre, p_gt, 1e-8);
}
@@ -365,13 +385,14 @@ TEST(SE3, interpolate_I_qz)
{
Vector7d p1, p2, p_pre;
- p1 << 0,0,0, 0,0,0,1;
- p2 << 0,0,0, 0,0,1,0;
+ p1 << 0, 0, 0, 0, 0, 0, 1;
+ p2 << 0, 0, 0, 0, 0, 1, 0;
double t = 0.5;
interpolate(p1, p2, t, p_pre);
- Vector7d p_gt; p_gt << 0,0,0, 0,0,sqrt(2)/2,sqrt(2)/2;
+ Vector7d p_gt;
+ p_gt << 0, 0, 0, 0, 0, sqrt(2) / 2, sqrt(2) / 2;
ASSERT_POSE3d_APPROX(p_pre, p_gt, 1e-8);
}
@@ -381,12 +402,13 @@ TEST(SE3, interpolate_I_qxyz)
Vector7d p1, p2, dp, p_pre, p_gt;
Vector3d da;
- da.setRandom(); da /= 5;
+ da.setRandom();
+ da /= 5;
- p1 << 0,0,0, 0,0,0,1;
- dp << 0,0,0, exp_q(da).coeffs();
- p_gt = compose(p1, dp);
- p2 = compose(p_gt, dp);
+ p1 << 0, 0, 0, 0, 0, 0, 1;
+ dp << 0, 0, 0, exp_q(da).coeffs();
+ p_gt = compose(p1, dp);
+ p2 = compose(p_gt, dp);
double t = 0.5;
interpolate(p1, p2, t, p_pre);
@@ -399,10 +421,12 @@ TEST(SE3, interpolate_half)
Vector7d p1, p2, dp, p_pre, p_gt;
Vector6d da;
- p1.setRandom(); p1.tail(4).normalize();
+ p1.setRandom();
+ p1.tail(4).normalize();
- da.setRandom(); da /= 5; // small rotation
- dp << exp(da);
+ da.setRandom();
+ da /= 5; // small rotation
+ dp << exp(da);
// compose double, then interpolate 1/2
@@ -420,10 +444,12 @@ TEST(SE3, interpolate_third)
Vector7d p1, p2, dp, dp2, dp3, p_pre, p_gt;
Vector6d da;
- p1.setRandom(); p1.tail(4).normalize();
+ p1.setRandom();
+ p1.tail(4).normalize();
- da.setRandom(); da /= 5; // small rotation
- dp << exp(da);
+ da.setRandom();
+ da /= 5; // small rotation
+ dp << exp(da);
dp2 = compose(dp, dp);
dp3 = compose(dp2, dp);
@@ -432,16 +458,16 @@ TEST(SE3, interpolate_third)
p_gt = compose(p1, dp);
p2 = compose(p1, dp3);
- double t = 1.0/3.0;
+ double t = 1.0 / 3.0;
interpolate(p1, p2, t, p_pre);
ASSERT_POSE3d_APPROX(p_pre, p_gt, 1e-8);
}
-
TEST(SE3, toSE3_I)
{
- Vector7d pose; pose << 0,0,0, 0,0,0,1;
+ Vector7d pose;
+ pose << 0, 0, 0, 0, 0, 0, 1;
Matrix4d R;
toSE3(pose, R);
@@ -450,7 +476,6 @@ TEST(SE3, toSE3_I)
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_buffer_frame.cpp b/test/gtest_buffer_frame.cpp
index a373155132d40049e6ef3991ff392cc058191522..54e12a7f3aab55343117a5c9e7190bb5d9fedd69 100644
--- a/test/gtest_buffer_frame.cpp
+++ b/test/gtest_buffer_frame.cpp
@@ -33,26 +33,25 @@ using namespace Eigen;
class BufferFrameTest : public testing::Test
{
- public:
-
- BufferFrame buffer_kf;
- FrameBasePtr f10, f20, f21, f28;
- double tt10, tt20, tt21, tt28;
- TimeStamp timestamp;
- double timetolerance;
-
- void SetUp(void) override
- {
- f10 = std::make_shared<FrameBase>(TimeStamp(10),nullptr,nullptr,nullptr);
- f20 = std::make_shared<FrameBase>(TimeStamp(20),nullptr,nullptr,nullptr);
- f21 = std::make_shared<FrameBase>(TimeStamp(21),nullptr,nullptr,nullptr);
- f28 = std::make_shared<FrameBase>(TimeStamp(28),nullptr,nullptr,nullptr);
-
- tt10 = 1.0;
- tt20 = 1.0;
- tt21 = 1.0;
- tt28 = 1.0;
- };
+ public:
+ BufferFrame buffer_kf;
+ FrameBasePtr f10, f20, f21, f28;
+ double tt10, tt20, tt21, tt28;
+ TimeStamp timestamp;
+ double timetolerance;
+
+ void SetUp(void) override
+ {
+ f10 = std::make_shared<FrameBase>(TimeStamp(10), nullptr, nullptr, nullptr);
+ f20 = std::make_shared<FrameBase>(TimeStamp(20), nullptr, nullptr, nullptr);
+ f21 = std::make_shared<FrameBase>(TimeStamp(21), nullptr, nullptr, nullptr);
+ f28 = std::make_shared<FrameBase>(TimeStamp(28), nullptr, nullptr, nullptr);
+
+ tt10 = 1.0;
+ tt20 = 1.0;
+ tt21 = 1.0;
+ tt28 = 1.0;
+ };
};
TEST_F(BufferFrameTest, empty)
@@ -63,32 +62,32 @@ TEST_F(BufferFrameTest, empty)
TEST_F(BufferFrameTest, emplace)
{
buffer_kf.emplace(10, f10);
- ASSERT_EQ(buffer_kf.size(), (unsigned int) 1);
+ ASSERT_EQ(buffer_kf.size(), (unsigned int)1);
buffer_kf.emplace(20, f20);
- ASSERT_EQ(buffer_kf.size(), (unsigned int) 2);
+ ASSERT_EQ(buffer_kf.size(), (unsigned int)2);
}
TEST_F(BufferFrameTest, clear)
{
buffer_kf.emplace(10, f10);
buffer_kf.emplace(20, f20);
- ASSERT_EQ(buffer_kf.size(), (unsigned int) 2);
+ ASSERT_EQ(buffer_kf.size(), (unsigned int)2);
buffer_kf.clear();
ASSERT_TRUE(buffer_kf.empty());
}
TEST_F(BufferFrameTest, doubleCheckTimeTolerance)
{
- buffer_kf.clear();
- buffer_kf.emplace(10, f10);
- buffer_kf.emplace(20, f20);
- // min time tolerance > diff between time stamps. It should return true
- ASSERT_TRUE(buffer_kf.doubleCheckTimeTolerance(TimeStamp(10), 20, TimeStamp(20), 20));
- // min time tolerance < diff between time stamps. It should return true
- ASSERT_FALSE(buffer_kf.doubleCheckTimeTolerance(TimeStamp(10), 1, TimeStamp(20), 20));
+ buffer_kf.clear();
+ buffer_kf.emplace(10, f10);
+ buffer_kf.emplace(20, f20);
+ // min time tolerance > diff between time stamps. It should return true
+ ASSERT_TRUE(buffer_kf.doubleCheckTimeTolerance(TimeStamp(10), 20, TimeStamp(20), 20));
+ // min time tolerance < diff between time stamps. It should return true
+ ASSERT_FALSE(buffer_kf.doubleCheckTimeTolerance(TimeStamp(10), 1, TimeStamp(20), 20));
}
-//TEST_F(BufferFrameTest, select)
+// TEST_F(BufferFrameTest, select)
//{
// // Evaluation using two packs (p1,p2)
// // with different time tolerances (tp1,tp2)
@@ -149,64 +148,63 @@ TEST_F(BufferFrameTest, doubleCheckTimeTolerance)
TEST_F(BufferFrameTest, selectFirstBefore)
{
- buffer_kf.clear();
-
- buffer_kf.emplace(10, f10);
- buffer_kf.emplace(20, f20);
- buffer_kf.emplace(21, f21);
-
- // input time stamps
- std::vector<TimeStamp> q_ts = {9.5, 9.995, 10.005, 19.5, 20.5, 21.5};
- double tt = 0.01;
-
- // Solution matrix
- // q_ts | pack
- //=================
- // first time
- //-----------------
- // 9.5 nullptr
- // 9.995 10
- // 10,005 10
- // 19.5 10
- // 20.5 10
- // 21.5 10
- // second time
- //-----------------
- // 9.5 nullptr
- // 9.995 null
- // 10,005 null
- // 19.5 null
- // 20.5 20
- // 21.5 20
- // third time
- //-----------------
- // 9.5 nullptr
- // 9.995 null
- // 10,005 null
- // 19.5 null
- // 20.5 null
- // 21.5 21
-
- Eigen::MatrixXd truth(3,6), res(3,6);
- truth << 0,10,10,10,10,10, 0,0,0,0,20,20, 0,0,0,0,0,21;
- res.setZero();
-
- for (int i=0; i<3; i++)
- {
- FrameBasePtr packQ;
- int j = 0;
- for (auto ts : q_ts)
- {
- packQ = buffer_kf.selectFirstBefore(ts, tt);
- if (packQ)
- res(i,j) = packQ->getTimeStamp().get();
-
- j++;
- }
- buffer_kf.removeUpTo(packQ->getTimeStamp());
- }
-
- ASSERT_MATRIX_APPROX(res, truth, 1e-6);
+ buffer_kf.clear();
+
+ buffer_kf.emplace(10, f10);
+ buffer_kf.emplace(20, f20);
+ buffer_kf.emplace(21, f21);
+
+ // input time stamps
+ std::vector<TimeStamp> q_ts = {9.5, 9.995, 10.005, 19.5, 20.5, 21.5};
+ double tt = 0.01;
+
+ // Solution matrix
+ // q_ts | pack
+ //=================
+ // first time
+ //-----------------
+ // 9.5 nullptr
+ // 9.995 10
+ // 10,005 10
+ // 19.5 10
+ // 20.5 10
+ // 21.5 10
+ // second time
+ //-----------------
+ // 9.5 nullptr
+ // 9.995 null
+ // 10,005 null
+ // 19.5 null
+ // 20.5 20
+ // 21.5 20
+ // third time
+ //-----------------
+ // 9.5 nullptr
+ // 9.995 null
+ // 10,005 null
+ // 19.5 null
+ // 20.5 null
+ // 21.5 21
+
+ Eigen::MatrixXd truth(3, 6), res(3, 6);
+ truth << 0, 10, 10, 10, 10, 10, 0, 0, 0, 0, 20, 20, 0, 0, 0, 0, 0, 21;
+ res.setZero();
+
+ for (int i = 0; i < 3; i++)
+ {
+ FrameBasePtr packQ;
+ int j = 0;
+ for (auto ts : q_ts)
+ {
+ packQ = buffer_kf.selectFirstBefore(ts, tt);
+ if (packQ) res(i, j) = packQ->getTimeStamp().get();
+
+ j++;
+ }
+ buffer_kf.removeUpTo(packQ->getTimeStamp());
+ }
+
+ ASSERT_MATRIX_APPROX(res, truth, 1e-6);
}
TEST_F(BufferFrameTest, removeUpTo)
@@ -220,27 +218,26 @@ TEST_F(BufferFrameTest, removeUpTo)
// it should remove f20 and f10, thus size should be 1 after removal
// Specifically, only f21 should remain
- buffer_kf.removeUpTo( f20->getTimeStamp() );
- ASSERT_EQ(buffer_kf.size(), (unsigned int) 1);
- ASSERT_TRUE(buffer_kf.select(f10->getTimeStamp(),tt)==nullptr);
- ASSERT_TRUE(buffer_kf.select(f20->getTimeStamp(),tt)==nullptr);
- ASSERT_TRUE(buffer_kf.select(f21->getTimeStamp(),tt)!=nullptr);
+ buffer_kf.removeUpTo(f20->getTimeStamp());
+ ASSERT_EQ(buffer_kf.size(), (unsigned int)1);
+ ASSERT_TRUE(buffer_kf.select(f10->getTimeStamp(), tt) == nullptr);
+ ASSERT_TRUE(buffer_kf.select(f20->getTimeStamp(), tt) == nullptr);
+ ASSERT_TRUE(buffer_kf.select(f21->getTimeStamp(), tt) != nullptr);
// Chech removal of an imprecise time stamp
// Specifically, only f28 should remain
buffer_kf.emplace(28, f28);
- ASSERT_EQ(buffer_kf.size(), (unsigned int) 2);
- FrameBasePtr f22 = std::make_shared<FrameBase>(TimeStamp(22),nullptr,nullptr,nullptr);
+ ASSERT_EQ(buffer_kf.size(), (unsigned int)2);
+ FrameBasePtr f22 = std::make_shared<FrameBase>(TimeStamp(22), nullptr, nullptr, nullptr);
// PackKeyFramePtr pack22 = std::make_shared<PackKeyFrame>(f22,5);
- buffer_kf.removeUpTo( f22->getTimeStamp() );
- ASSERT_EQ(buffer_kf.size(), (unsigned int) 1);
- ASSERT_TRUE(buffer_kf.select(f21->getTimeStamp(),tt)==nullptr);
- ASSERT_TRUE(buffer_kf.select(f28->getTimeStamp(),tt)!=nullptr);
+ buffer_kf.removeUpTo(f22->getTimeStamp());
+ ASSERT_EQ(buffer_kf.size(), (unsigned int)1);
+ ASSERT_TRUE(buffer_kf.select(f21->getTimeStamp(), tt) == nullptr);
+ ASSERT_TRUE(buffer_kf.select(f28->getTimeStamp(), tt) != nullptr);
}
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_capture_base.cpp b/test/gtest_capture_base.cpp
index ed25b7d02a5d4eb11baf016cf947ed6886fc2470..4bfbe7b5a28055c57cedcca15e7033218d3b3fb4 100644
--- a/test/gtest_capture_base.cpp
+++ b/test/gtest_capture_base.cpp
@@ -32,7 +32,7 @@ using namespace Eigen;
TEST(CaptureBase, ConstructorNoSensor)
{
- CaptureBasePtr C(std::make_shared<CaptureBase>("DUMMY", 1.2)); // timestamp = 1.2
+ CaptureBasePtr C(std::make_shared<CaptureBase>("DUMMY", 1.2)); // timestamp = 1.2
ASSERT_EQ(C->getTimeStamp(), 1.2);
ASSERT_FALSE(C->getFrame());
@@ -42,8 +42,9 @@ TEST(CaptureBase, ConstructorNoSensor)
TEST(CaptureBase, ConstructorWithSensor)
{
- SensorBasePtr S = FactorySensorFile::create("SensorOdom2d", wolf_dir + "/test/yaml/sensor_odom_2d.yaml", {wolf_dir});
- CaptureBasePtr C(std::make_shared<CaptureBase>("DUMMY", 1.5, S)); // timestamp = 1.5
+ SensorBasePtr S =
+ FactorySensorFile::create("SensorOdom2d", wolf_dir + "/test/yaml/sensor_odom_2d.yaml", {wolf_dir});
+ CaptureBasePtr C(std::make_shared<CaptureBase>("DUMMY", 1.5, S)); // timestamp = 1.5
ASSERT_EQ(C->getTimeStamp(), 1.5);
ASSERT_FALSE(C->getFrame());
ASSERT_FALSE(C->getProblem());
@@ -52,8 +53,9 @@ TEST(CaptureBase, ConstructorWithSensor)
TEST(CaptureBase, Static_sensor_params)
{
- SensorBasePtr S = FactorySensorFile::create("SensorOdom2d", wolf_dir + "/test/yaml/sensor_odom_2d.yaml", {wolf_dir});
- CaptureBasePtr C(std::make_shared<CaptureBase>("DUMMY", 1.5, S)); // timestamp = 1.5
+ SensorBasePtr S =
+ FactorySensorFile::create("SensorOdom2d", wolf_dir + "/test/yaml/sensor_odom_2d.yaml", {wolf_dir});
+ CaptureBasePtr C(std::make_shared<CaptureBase>("DUMMY", 1.5, S)); // timestamp = 1.5
// query capture blocks
ASSERT_EQ(C->getSensorP(), S->getP());
@@ -63,11 +65,12 @@ TEST(CaptureBase, Static_sensor_params)
TEST(CaptureBase, Dynamic_sensor_params)
{
- SensorBasePtr S = FactorySensorFile::create("SensorDiffDrive", wolf_dir + "/test/yaml/sensor_diff_drive_dynamic.yaml", {wolf_dir});
- StateBlockPtr p(std::make_shared<StatePoint2d>(Vector2d::Zero()));
- StateBlockPtr o(std::make_shared<StateAngle>(0) );
- StateBlockPtr i(std::make_shared<StateParams2>(Vector2d::Zero()));
- CaptureBasePtr C(std::make_shared<CaptureBase>("DUMMY", 1.5, S, p, o, i)); // timestamp = 1.5
+ SensorBasePtr S = FactorySensorFile::create(
+ "SensorDiffDrive", wolf_dir + "/test/yaml/sensor_diff_drive_dynamic.yaml", {wolf_dir});
+ StateBlockPtr p(std::make_shared<StatePoint2d>(Vector2d::Zero()));
+ StateBlockPtr o(std::make_shared<StateAngle>(0));
+ StateBlockPtr i(std::make_shared<StateParams2>(Vector2d::Zero()));
+ CaptureBasePtr C(std::make_shared<CaptureBase>("DUMMY", 1.5, S, p, o, i)); // timestamp = 1.5
// query capture blocks
ASSERT_EQ(C->getSensorP(), p);
@@ -77,40 +80,41 @@ TEST(CaptureBase, Dynamic_sensor_params)
TEST(CaptureBase, addFeature)
{
- CaptureBasePtr C(std::make_shared<CaptureBase>("DUMMY", 1.2)); // timestamp = 1.2
-
+ CaptureBasePtr C(std::make_shared<CaptureBase>("DUMMY", 1.2)); // timestamp = 1.2
+
auto f = FeatureBase::emplace<FeatureBase>(C, "DUMMY", Vector2d::Zero(), Matrix2d::Identity());
-
+
ASSERT_FALSE(C->getFeatureList().empty());
ASSERT_EQ(C->getFeatureList().front(), f);
}
TEST(CaptureBase, print)
{
- CaptureBasePtr C(std::make_shared<CaptureBase>("DUMMY", 1.2)); // timestamp = 1.2
- auto f = FeatureBase::emplace<FeatureBase>(C, "DUMMY", Vector2d::Zero(), Matrix2d::Identity());
+ CaptureBasePtr C(std::make_shared<CaptureBase>("DUMMY", 1.2)); // timestamp = 1.2
+ auto f = FeatureBase::emplace<FeatureBase>(C, "DUMMY", Vector2d::Zero(), Matrix2d::Identity());
C->print(4, 1, 1, 1);
}
TEST(CaptureBase, process)
{
- SensorBasePtr S = FactorySensorFile::create("SensorOdom2d", wolf_dir + "/test/yaml/sensor_odom_2d.yaml", {wolf_dir});
+ SensorBasePtr S =
+ FactorySensorFile::create("SensorOdom2d", wolf_dir + "/test/yaml/sensor_odom_2d.yaml", {wolf_dir});
CaptureBasePtr C(std::make_shared<CaptureBase>("DUMMY", 1.5, nullptr));
- ASSERT_DEATH({C->process();},""); // No sensor in the capture should fail
+ ASSERT_DEATH({ C->process(); }, ""); // No sensor in the capture should fail
C->setSensor(S);
- ASSERT_TRUE(C->process()); // This should not fail (although it does nothing)
+ ASSERT_TRUE(C->process()); // This should not fail (although it does nothing)
}
TEST(CaptureBase, move_from_F_to_KF)
{
ProblemPtr problem = Problem::create("PO", 2);
- auto KF = problem->emplaceFrame(0.0); // dummy F object
+ auto KF = problem->emplaceFrame(0.0); // dummy F object
auto KC = CaptureBase::emplace<CaptureBase>(KF, "Dummy", 0.0);
- auto F = std::make_shared<FrameBase>(0.0, nullptr); // dummy F object
+ auto F = std::make_shared<FrameBase>(0.0, nullptr); // dummy F object
auto C = CaptureBase::emplace<CaptureBase>(F, "Dummy", 0.0);
@@ -130,7 +134,7 @@ TEST(CaptureBase, move_from_F_to_null)
FrameBasePtr F0 = nullptr;
- auto F = std::make_shared<FrameBase>(0.0, nullptr); // dummy F object
+ auto F = std::make_shared<FrameBase>(0.0, nullptr); // dummy F object
auto C = CaptureBase::emplace<CaptureBase>(F, "Dummy", 0.0);
@@ -146,7 +150,7 @@ TEST(CaptureBase, move_from_null_to_KF)
{
ProblemPtr problem = Problem::create("PO", 2);
- auto KF = problem->emplaceFrame(0.0); // dummy F object
+ auto KF = problem->emplaceFrame(0.0); // dummy F object
auto KC = CaptureBase::emplace<CaptureBase>(KF, "Dummy", 0.0);
@@ -164,7 +168,7 @@ TEST(CaptureBase, move_from_null_to_F)
{
ProblemPtr problem = Problem::create("PO", 2);
- auto F = std::make_shared<FrameBase>(0.0, nullptr); // dummy F object
+ auto F = std::make_shared<FrameBase>(0.0, nullptr); // dummy F object
auto C = std::make_shared<CaptureBase>("Dummy", 0.0);
@@ -179,19 +183,17 @@ TEST(CaptureBase, move_from_KF_to_F)
{
ProblemPtr problem = Problem::create("PO", 2);
- auto KF = problem->emplaceFrame(0.0); // dummy F object
+ auto KF = problem->emplaceFrame(0.0); // dummy F object
- auto F = std::make_shared<FrameBase>(0.0, nullptr); // dummy F object
+ auto F = std::make_shared<FrameBase>(0.0, nullptr); // dummy F object
auto C = CaptureBase::emplace<CaptureBase>(KF, "Dummy", 0.0);
ASSERT_DEATH(C->move(F), "");
}
-
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_converter.cpp b/test/gtest_converter.cpp
deleted file mode 100644
index 4992d5015e9445cc66ebf749989cada55974a7d4..0000000000000000000000000000000000000000
--- a/test/gtest_converter.cpp
+++ /dev/null
@@ -1,102 +0,0 @@
-// WOLF - Copyright (C) 2020,2021,2022,2023
-// Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
-// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
-// Joan Vallvé Navarro (jvallve@iri.upc.edu)
-// All rights reserved.
-//
-// This file is part of WOLF: http://www.iri.upc.edu/wolf
-// WOLF is free software: you can redistribute it and/or modify
-// it under the terms of the GNU Lesser General Public License as published by
-// 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/>.
-
-#include "core/utils/utils_gtest.h"
-#include "core/utils/converter.h"
-
-using namespace std;
-using namespace wolf;
-
-TEST(Converter, ParseToVector)
-{
- string v1 = "[3,4,5,6,7,8,9,10,11]";
- vector<int> v = converter<vector<int>>::convert(v1);
- ASSERT_EQ(v.size(),9);
- ASSERT_EQ(v[0],3);
- ASSERT_EQ(v[1],4);
- ASSERT_EQ(v[2],5);
- ASSERT_EQ(v[3],6);
- ASSERT_EQ(v[4],7);
- ASSERT_EQ(v[5],8);
- ASSERT_EQ(v[6],9);
- ASSERT_EQ(v[7],10);
- ASSERT_EQ(v[8],11);
- vector<string> vs {"a","b","c"};
- ASSERT_EQ(converter<string>::convert(vs), "[a,b,c]");
- string v2 = "[first,second,third,fourth]";
- vector<string> vv = converter<vector<string>>::convert(v2);
- ASSERT_EQ(vv.size(),4);
- ASSERT_EQ(vv[0],"first");
- ASSERT_EQ(vv[1],"second");
- ASSERT_EQ(vv[2],"third");
- ASSERT_EQ(vv[3],"fourth");
-
-}
-TEST(Converter, ParseToEigenMatrix)
-{
- string v1 = "[[3,3],3,4,5,6,7,8,9,10,11]";
- auto v = Eigen::Matrix<int, Eigen::Dynamic, Eigen::Dynamic, 3, 5, 5>();
- EXPECT_NO_THROW(([=, &v]{v = converter<Eigen::Matrix<int, Eigen::Dynamic, Eigen::Dynamic, 3, 5, 5>>::convert(v1);}()));
- EXPECT_EQ(v.size(),9);
- string v2 = "[[3,3],3,4,5,6,7,8,9,10,11,12]";
- EXPECT_THROW(([=, &v]{v = converter<Eigen::Matrix<int, Eigen::Dynamic, Eigen::Dynamic, 3, 5, 5>>::convert(v2);}()), std::runtime_error);
- string v3 = "[[3],3,4,5,6,7,8,9,10,11]";
- EXPECT_THROW(([=, &v]{v = converter<Eigen::Matrix<int, Eigen::Dynamic, Eigen::Dynamic, 3, 5, 5>>::convert(v3);}()), std::runtime_error);
- string v4 = "[[3,3],3,4,5,6,7,8,9,10,11]";
- auto v41 = Eigen::Matrix<int, Eigen::Dynamic, Eigen::Dynamic, 3, 5, 5>();
- v41 = converter<Eigen::Matrix<int, Eigen::Dynamic, Eigen::Dynamic, 3, 5, 5>>::convert(v4);
- ASSERT_EQ(v41(0,0), 3);
- ASSERT_EQ(v41(1,1), 7);
- ASSERT_EQ(v41(2,2), 11);
- string v5 = "[[3,3],3,4,5,6,7,[8,9],10,11]";
- EXPECT_THROW(([=, &v]{v = converter<Eigen::Matrix<int, Eigen::Dynamic, Eigen::Dynamic, 3, 5, 5>>::convert(v5);}()), std::runtime_error);
-}
-TEST(Converter, ParseToMap)
-{
- string str = "[{x:1},{y:[1,23,3]},{z:3}]";
- EXPECT_THROW(([=]{auto a = converter<std::map<std::string, double>>::convert(str);}()), std::invalid_argument);
- string str2 = "[{x:[1]},{y:[1,23,3]},{z:[3]}]";
- EXPECT_NO_THROW(([=]{auto a = converter<std::map<std::string, std::vector<int>>>::convert(str2);}()));
- map<string, vector<int>> m = {{"x",vector<int>{1,2}}, {"y",vector<int>{}}, {"z",vector<int>{3}}};
- ASSERT_EQ(converter<string>::convert(m), "[{x:[1,2]},{y:[]},{z:[3]}]");
-}
-TEST(Converter, extraTests)
-{
- string str = "[{x:1},{y:[[{x:[1,2]},{y:[]},{z:[3]}]]},{z:[1,2,3,4,5]}]";
- auto v = converter<vector<string>>::convert(str);
- ASSERT_EQ(v[0], "{x:1}");
- ASSERT_EQ(v[1], "{y:[[{x:[1,2]},{y:[]},{z:[3]}]]}");
- ASSERT_EQ(v[2], "{z:[1,2,3,4,5]}");
-
- string str2 = "[]";
- auto v2 = converter<vector<string>>::convert(str2);
- // EXPECT_EQ(v2.size(), 1);
- EXPECT_TRUE(v2.empty());
-}
-TEST(Converter, noGeneralConvert)
-{
- class DUMMY{};
- EXPECT_THROW(([=]{converter<DUMMY>::convert("Should fail");}()), std::runtime_error);
-}
-int main(int argc, char **argv)
-{
- testing::InitGoogleTest(&argc, argv);
- return RUN_ALL_TESTS();
-}
diff --git a/test/gtest_emplace.cpp b/test/gtest_emplace.cpp
index f507ac65dd445b8479dd55a0ffdc6ff4afe31af6..3a6cb174c2924631c799e35c6082ec778fbfd55c 100644
--- a/test/gtest_emplace.cpp
+++ b/test/gtest_emplace.cpp
@@ -38,6 +38,7 @@
using namespace wolf;
using namespace Eigen;
+std::string wolf_dir = _WOLF_CODE_DIR;
TEST(Emplace, Landmark)
{
@@ -54,11 +55,8 @@ TEST(Emplace, Sensor)
{
ProblemPtr P = Problem::create("POV", 3);
- auto S = SensorBase::emplace<SensorDummy<3>>(
- P->getHardware(), // SensorBase is abstract
- std::make_shared<ParamsSensorDummy>(),
- SpecStateSensorComposite({{'P', SpecStateSensor("StatePoint3d", Vector3d::Zero(), "fix")},
- {'O', SpecStateSensor("StateQuaternion", Vector4d::Random().normalized(), "fix")}}));
+ auto S = SensorBase::emplace<SensorDummy3d>(
+ P->getHardware(), YAML::LoadFile(wolf_dir + "/test/yaml/sensor_tests/sensor_PO_3D_fix.yaml"), {wolf_dir});
ASSERT_EQ(P, S->getProblem());
ASSERT_EQ(P->getHardware(), S->getHardware());
@@ -84,30 +82,27 @@ TEST(Emplace, Processor)
{
ProblemPtr P = Problem::create("PO", 2);
- auto S = SensorBase::emplace<SensorDummy<2>>(
- P->getHardware(), // SensorBase is abstract
- std::make_shared<ParamsSensorDummy>(),
- SpecStateSensorComposite({{'P', SpecStateSensor("StatePoint2d", Vector2d::Zero(), "fix")},
- {'O', SpecStateSensor("StateAngle", Vector1d::Zero(), "fix")}}));
+ auto S = SensorBase::emplace<SensorDummy2d>(
+ P->getHardware(), YAML::LoadFile(wolf_dir + "/test/yaml/sensor_tests/sensor_PO_2D_fix.yaml"), {wolf_dir});
ASSERT_EQ(P, S->getProblem());
ASSERT_EQ(P->getHardware(), S->getHardware());
ASSERT_EQ(P->getHardware()->getSensorList().front(), S);
- auto prc = ProcessorOdom2d::emplace<ProcessorOdom2d>(S, std::make_shared<ParamsProcessorOdom2d>());
+ auto prc = ProcessorBase::emplace<ProcessorOdom2d>(
+ S, YAML::LoadFile(wolf_dir + "/test/yaml/processor_odom_2d.yaml"), {wolf_dir});
ASSERT_EQ(P, prc->getProblem());
ASSERT_EQ(S, prc->getSensor());
ASSERT_EQ(prc, S->getProcessorList().front());
- SensorBasePtr sen2 = SensorBase::emplace<SensorDummy<2>>(
- P->getHardware(), // SensorBase is abstract
- std::make_shared<ParamsSensorDummy>(),
- SpecStateSensorComposite({{'P', SpecStateSensor("StatePoint2d", Vector2d::Zero(), "fix")},
- {'O', SpecStateSensor("StateAngle", Vector1d::Zero(), "fix")}}));
+ SensorBasePtr sen2 = SensorBase::emplace<SensorDummy2d>(
+ P->getHardware(),
+ YAML::LoadFile(wolf_dir + "/test/yaml/sensor_tests/sensor_PO_2D_initial_guess.yaml"),
+ {wolf_dir});
- ProcessorOdom2dPtr prc2 =
- ProcessorOdom2d::emplace<ProcessorOdom2d>(sen2, std::make_shared<ParamsProcessorOdom2d>());
+ ProcessorOdom2dPtr prc2 = ProcessorBase::emplace<ProcessorOdom2d>(
+ sen2, YAML::LoadFile(wolf_dir + "/test/yaml/processor_odom_2d.yaml"), {wolf_dir});
ASSERT_EQ(P, prc->getProblem());
ASSERT_EQ(sen2, prc2->getSensor());
@@ -210,10 +205,7 @@ TEST(Emplace, EmplaceDerived)
ProblemPtr P = Problem::create("PO", 2);
auto S = SensorBase::emplace<SensorOdom2d>(
- P->getHardware(),
- std::make_shared<ParamsSensorOdom>(),
- SpecStateSensorComposite({{'P', SpecStateSensor("StatePoint2d", Vector2d::Zero(), "fix")},
- {'O', SpecStateSensor("StateAngle", Vector1d::Zero(), "fix")}}));
+ P->getHardware(), YAML::LoadFile(wolf_dir + "/test/yaml/sensor_odom_2d.yaml"), {wolf_dir});
ASSERT_EQ(P, S->getProblem());
ASSERT_EQ(P->getHardware(), S->getHardware());
diff --git a/test/gtest_example.cpp b/test/gtest_example.cpp
index e81269f3924200b7f10b5475f1fcbd447180c6ad..3bcd8720c630d2f284616b35d52367f795f00bfd 100644
--- a/test/gtest_example.cpp
+++ b/test/gtest_example.cpp
@@ -22,21 +22,21 @@
TEST(TestTest, DummyTestExample)
{
- EXPECT_FALSE(false);
+ EXPECT_FALSE(false);
- ASSERT_TRUE(true);
+ ASSERT_TRUE(true);
- int my_int = 5;
+ int my_int = 5;
- ASSERT_EQ(my_int, 5);
+ ASSERT_EQ(my_int, 5);
-// PRINTF("All good at TestTest::DummyTestExample !\n");
+ // PRINTF("All good at TestTest::DummyTestExample !\n");
}
int main(int argc, char **argv)
{
- testing::InitGoogleTest(&argc, argv);
- //::testing::GTEST_FLAG(filter) = "TestTest.DummyTestExample"; // Test only this one
- //::testing::GTEST_FLAG(filter) = "TestTest.*"; // Test only the tests in this group
- return RUN_ALL_TESTS();
+ testing::InitGoogleTest(&argc, argv);
+ //::testing::GTEST_FLAG(filter) = "TestTest.DummyTestExample"; // Test only this one
+ //::testing::GTEST_FLAG(filter) = "TestTest.*"; // Test only the tests in this group
+ return RUN_ALL_TESTS();
}
diff --git a/test/gtest_factor_absolute.cpp b/test/gtest_factor_absolute.cpp
index 5f3b37c73120312cd2ce3e7df5b160a232bcfe09..bc986b63f3112ddf6f4e9eaa93d0163708c42d8f 100644
--- a/test/gtest_factor_absolute.cpp
+++ b/test/gtest_factor_absolute.cpp
@@ -29,14 +29,15 @@ using namespace wolf;
using std::cout;
using std::endl;
-int N = 10;
+int N = 10;
+std::string wolf_dir = _WOLF_CODE_DIR;
-ProblemPtr problem;
-SolverCeresPtr solver;
-FrameBasePtr frm;
-CaptureBasePtr cap;
-VectorComposite pose;
-Matrix9d pose_cov = 1e-3 * Matrix9d::Identity();
+ProblemPtr problem;
+SolverManagerPtr solver;
+FrameBasePtr frm;
+CaptureBasePtr cap;
+VectorComposite pose;
+Matrix9d pose_cov = 1e-3 * Matrix9d::Identity();
void randomSetup(int dim)
{
@@ -46,7 +47,7 @@ void randomSetup(int dim)
// Problem and solver
problem = Problem::create("POV", dim);
- solver = std::make_shared<SolverCeres>(problem);
+ solver = SolverCeres::create(problem, wolf_dir + "/test/yaml/solver_ceres.yaml", {wolf_dir});
// random pose
if (dim == 2)
diff --git a/test/gtest_factor_autodiff.cpp b/test/gtest_factor_autodiff.cpp
index b4b22d47effddcff20cbcb5615488923def461ea..4d29a8cbb3409d3b786fb2c89a0811c45a9e2261 100644
--- a/test/gtest_factor_autodiff.cpp
+++ b/test/gtest_factor_autodiff.cpp
@@ -18,7 +18,7 @@
// 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/>.
-#include "dummy/factor_odom_2d_autodiff.h"
+#include "dummy/factor_relative_pose_2d_autodiff.h"
#include "dummy/factor_dummy_zero_1.h"
#include "dummy/factor_dummy_zero_15.h"
@@ -188,8 +188,8 @@ TEST(FactorAutodiff, EmplaceOdom2d)
FrameBasePtr fr2_ptr = problem->emplaceFrame(TimeStamp(2));
// SENSOR
- auto sensor_ptr = FactorySensorFile::create(
- "SensorOdom2d", wolf_dir + "/test/yaml/sensor_odom_2d.yaml", {wolf_dir});
+ auto sensor_ptr =
+ FactorySensorFile::create("SensorOdom2d", wolf_dir + "/test/yaml/sensor_odom_2d.yaml", {wolf_dir});
// CAPTURE
auto capture_ptr = CaptureBase::emplace<CaptureVoid>(fr2_ptr, TimeStamp(0), sensor_ptr);
@@ -198,7 +198,8 @@ TEST(FactorAutodiff, EmplaceOdom2d)
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);
+ auto factor_ptr =
+ FactorBase::emplace<FactorRelativePose2dAutodiff>(feature_ptr, feature_ptr, fr1_ptr, nullptr, false);
ASSERT_TRUE(factor_ptr->getFeature());
ASSERT_TRUE(factor_ptr->getFeature()->getCapture());
@@ -220,8 +221,8 @@ TEST(FactorAutodiff, ResidualOdom2d)
FrameBasePtr fr2_ptr = problem->emplaceFrame(TimeStamp(2), "PO", f2_pose);
// SENSOR
- auto sensor_ptr = FactorySensorFile::create(
- "SensorOdom2d", wolf_dir + "/test/yaml/sensor_odom_2d.yaml", {wolf_dir});
+ auto sensor_ptr =
+ FactorySensorFile::create("SensorOdom2d", wolf_dir + "/test/yaml/sensor_odom_2d.yaml", {wolf_dir});
// CAPTURE
auto capture_ptr = CaptureBase::emplace<CaptureVoid>(fr2_ptr, TimeStamp(0), sensor_ptr);
@@ -232,7 +233,8 @@ TEST(FactorAutodiff, ResidualOdom2d)
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);
+ auto factor_ptr =
+ FactorBase::emplace<FactorRelativePose2dAutodiff>(feature_ptr, feature_ptr, fr1_ptr, nullptr, false);
// EVALUATE
@@ -263,8 +265,8 @@ TEST(FactorAutodiff, JacobianOdom2d)
FrameBasePtr fr2_ptr = problem->emplaceFrame(TimeStamp(2), "PO", f2_pose);
// SENSOR
- auto sensor_ptr = FactorySensorFile::create(
- "SensorOdom2d", wolf_dir + "/test/yaml/sensor_odom_2d.yaml", {wolf_dir});
+ auto sensor_ptr =
+ FactorySensorFile::create("SensorOdom2d", wolf_dir + "/test/yaml/sensor_odom_2d.yaml", {wolf_dir});
// CAPTURE
auto capture_ptr = CaptureBase::emplace<CaptureVoid>(fr2_ptr, TimeStamp(0), sensor_ptr);
@@ -275,7 +277,8 @@ TEST(FactorAutodiff, JacobianOdom2d)
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);
+ auto factor_ptr =
+ FactorBase::emplace<FactorRelativePose2dAutodiff>(feature_ptr, feature_ptr, fr1_ptr, nullptr, false);
// COMPUTE JACOBIANS
@@ -337,8 +340,8 @@ TEST(FactorAutodiff, AutodiffVsAnalytic)
FrameBasePtr fr2_ptr = problem->emplaceFrame(TimeStamp(2), "PO", f2_pose);
// SENSOR
- auto sensor_ptr = FactorySensorFile::create(
- "SensorOdom2d", wolf_dir + "/test/yaml/sensor_odom_2d.yaml", {wolf_dir});
+ auto sensor_ptr =
+ FactorySensorFile::create("SensorOdom2d", wolf_dir + "/test/yaml/sensor_odom_2d.yaml", {wolf_dir});
// CAPTURE
auto capture_ptr = CaptureBase::emplace<CaptureVoid>(fr2_ptr, TimeStamp(0), sensor_ptr);
@@ -350,7 +353,7 @@ TEST(FactorAutodiff, AutodiffVsAnalytic)
// FACTOR
auto fac_autodiff_ptr =
- FactorBase::emplace<FactorOdom2dAutodiff>(feature_ptr, feature_ptr, fr1_ptr, nullptr, false);
+ FactorBase::emplace<FactorRelativePose2dAutodiff>(feature_ptr, feature_ptr, fr1_ptr, nullptr, false);
auto fac_analytic_ptr =
FactorBase::emplace<FactorRelativePose2d>(feature_ptr,
feature_ptr->getMeasurement(),
diff --git a/test/gtest_factor_base.cpp b/test/gtest_factor_base.cpp
index 39d55aad73a0f1ccc0e4a69c12d8d4246db7ac27..f7ea48c4c93a5bf92e93a59df2549f787e6c333d 100644
--- a/test/gtest_factor_base.cpp
+++ b/test/gtest_factor_base.cpp
@@ -35,20 +35,30 @@ class FactorBaseTest : public testing::Test
void SetUp() override
{
- auto rand_specs =
- SpecStateSensorComposite({{'P', SpecStateSensor("StatePoint2d", Vector2d::Random(), "initial_guess")},
- {'O', SpecStateSensor("StateAngle", Vector1d::Random(), "initial_guess")}});
-
- S0 = SensorDummy2d::create(std::make_shared<ParamsSensorDummy>(), rand_specs);
- S1 = SensorDummy2d::create(std::make_shared<ParamsSensorDummy>(), rand_specs);
- F0 = std::make_shared<FrameBase>(0.0, rand_specs.cast<SpecStateComposite>());
- F1 = std::make_shared<FrameBase>(1.0, rand_specs.cast<SpecStateComposite>());
- C0 = std::make_shared<CaptureBase>("Capture", 0.0, nullptr);
- C0->emplaceStateBlocks(rand_specs.cast<SpecStateComposite>());
+ YAML::Node rand_params;
+ rand_params["name"] = "Sensor0";
+ rand_params["states"]["P"]["type"] = "StatePoint2d";
+ rand_params["states"]["P"]["state"] = Vector2d::Random();
+ rand_params["states"]["P"]["mode"] = "initial_guess";
+ rand_params["states"]["P"]["dynamic"] = false;
+ rand_params["states"]["O"]["type"] = "StateAngle";
+ rand_params["states"]["O"]["state"] = Vector1d::Random();
+ rand_params["states"]["O"]["mode"] = "initial_guess";
+ rand_params["states"]["O"]["dynamic"] = false;
+ rand_params["noise_p_std"] = 0.1;
+ rand_params["noise_o_std"] = 0.1;
+
+ S0 = SensorDummy2d::create(rand_params, {});
+ rand_params["name"] = "Sensor1";
+ S1 = SensorDummy2d::create(rand_params, {});
+ F0 = std::make_shared<FrameBase>(0.0, SpecStateComposite(rand_params["states"]));
+ F1 = std::make_shared<FrameBase>(1.0, SpecStateComposite(rand_params["states"]));
+ C0 = std::make_shared<CaptureBase>("Capture", 0.0, nullptr);
+ C0->emplaceStateBlocks(SpecStateComposite(rand_params["states"]));
C1 = std::make_shared<CaptureBase>("Capture", 1.0, nullptr);
- C1->emplaceStateBlocks(rand_specs.cast<SpecStateComposite>());
- L0 = std::make_shared<LandmarkBase>("Lmk", rand_specs.cast<SpecStateComposite>());
- L1 = std::make_shared<LandmarkBase>("Lmk", rand_specs.cast<SpecStateComposite>());
+ C1->emplaceStateBlocks(SpecStateComposite(rand_params["states"]));
+ L0 = std::make_shared<LandmarkBase>("Lmk", SpecStateComposite(rand_params["states"]));
+ L1 = std::make_shared<LandmarkBase>("Lmk", SpecStateComposite(rand_params["states"]));
}
};
diff --git a/test/gtest_factor_block_difference.cpp b/test/gtest_factor_block_difference.cpp
index b9c736e9d4669d7bbac1f3f853fc4df556aa11ff..79d819a9d438ac9b68b6490e203ca42af2a84952 100644
--- a/test/gtest_factor_block_difference.cpp
+++ b/test/gtest_factor_block_difference.cpp
@@ -31,15 +31,16 @@
using namespace Eigen;
using namespace wolf;
-const Vector6d zero6 = Vector6d::Zero();
-const Vector3d zero3 = zero6.head<3>();
-const Matrix3d idty02 = Matrix3d::Identity() * 0.02;
+std::string wolf_dir = _WOLF_CODE_DIR;
+const Vector6d zero6 = Vector6d::Zero();
+const Vector3d zero3 = zero6.head<3>();
+const Matrix3d idty02 = Matrix3d::Identity() * 0.02;
class FixtureFactorBlockDifference : public testing::Test
{
public:
ProblemPtr problem_;
- SolverCeresPtr solver_;
+ SolverManagerPtr solver_;
FrameBasePtr KF0_;
FrameBasePtr KF1_;
CaptureBasePtr Cap_;
@@ -51,7 +52,7 @@ class FixtureFactorBlockDifference : public testing::Test
{
// Problem and solver
problem_ = Problem::create("POV", 3);
- solver_ = std::make_shared<SolverCeres>(problem_);
+ solver_ = SolverCeres::create(problem_, wolf_dir + "/test/yaml/solver_ceres.yaml", {wolf_dir});
TimeStamp t0(0);
TimeStamp t1(1);
@@ -230,7 +231,6 @@ TEST_F(FixtureFactorBlockDifference, DiffPxy)
nullptr,
false);
-
// perturbate
KF0_->getP()->setState(Vector3d::Random());
KF1_->getP()->setState(Vector3d::Random());
@@ -261,7 +261,6 @@ TEST_F(FixtureFactorBlockDifference, DiffPyz)
nullptr,
false);
-
// perturbate
KF0_->getP()->setState(Vector3d::Random());
KF1_->getP()->setState(Vector3d::Random());
diff --git a/test/gtest_factor_diff_drive.cpp b/test/gtest_factor_diff_drive.cpp
index 7a3cd06c424ee66f4a5c6e4c3e881b08003ed5a9..5f2da6cf15c983aae5c10a8d530f32a82cdc5117 100644
--- a/test/gtest_factor_diff_drive.cpp
+++ b/test/gtest_factor_diff_drive.cpp
@@ -21,7 +21,7 @@
#include "core/utils/utils_gtest.h"
#include "core/ceres_wrapper/solver_ceres.h"
-#include "core/processor/processor_diff_drive.h"
+#include "dummy/processor_diff_drive_mock.h"
#include "core/sensor/sensor_diff_drive.h"
#include "core/factor/factor_diff_drive.h"
#include "core/frame/frame_base.h"
@@ -30,113 +30,21 @@
using namespace wolf;
using namespace Eigen;
-using namespace std;
-string wolf_dir = _WOLF_CODE_DIR;
-
-WOLF_PTR_TYPEDEFS(ProcessorDiffDrivePublic);
-
-class ProcessorDiffDrivePublic : public ProcessorDiffDrive
-{
- using Base = ProcessorDiffDrive;
-
- public:
- ProcessorDiffDrivePublic(ParamsProcessorDiffDrivePtr _params_motion) : ProcessorDiffDrive(_params_motion)
- {
- //
- }
- void configure(SensorBasePtr _sensor) override
- {
- Base::configure(_sensor);
- }
- void computeCurrentDelta(const VectorXd& _data,
- const MatrixXd& _data_cov,
- const VectorXd& _calib,
- const double _dt,
- VectorXd& _delta,
- MatrixXd& _delta_cov,
- MatrixXd& _jacobian_calib) const override
- {
- Base::computeCurrentDelta(_data, _data_cov, _calib, _dt, _delta, _delta_cov, _jacobian_calib);
- }
-
- void deltaPlusDelta(const VectorXd& _delta1,
- const VectorXd& _delta2,
- const double _dt2,
- VectorXd& _delta1_plus_delta2) const override
- {
- Base::deltaPlusDelta(_delta1, _delta2, _dt2, _delta1_plus_delta2);
- }
-
- void deltaPlusDelta(const VectorXd& _delta1,
- const VectorXd& _delta2,
- const double _dt2,
- VectorXd& _delta1_plus_delta2,
- MatrixXd& _jacobian1,
- MatrixXd& _jacobian2) const override
- {
- Base::deltaPlusDelta(_delta1, _delta2, _dt2, _delta1_plus_delta2, _jacobian1, _jacobian2);
- }
-
- VectorXd getCalibration(const CaptureBaseConstPtr _capture) const override
- {
- return Base::getCalibration(_capture);
- }
-
- VectorXd deltaZero() const override
- {
- return Base::deltaZero();
- }
-
- CaptureMotionPtr emplaceCapture(const FrameBasePtr& _frame_own,
- const SensorBasePtr& _sensor,
- const TimeStamp& _ts,
- const VectorXd& _data,
- const MatrixXd& _data_cov,
- const VectorXd& _calib,
- const VectorXd& _calib_preint,
- const CaptureBasePtr& _capture_origin) override
- {
- return Base::emplaceCapture(
- _frame_own, _sensor, _ts, _data, _data_cov, _calib, _calib_preint, _capture_origin);
- }
-
- // FeatureBasePtr emplaceFeature(CaptureMotionPtr _capture_own) override
- // {
- // return Base::emplaceFeature(_capture_own);
- // }
-
- // FactorBasePtr emplaceFactor(FeatureBasePtr _feature_motion,
- // CaptureBasePtr _capture_origin) override
- // {
- // return Base::emplaceFactor(_feature_motion, _capture_origin);
- // }
-
- ParamsProcessorDiffDrivePtr getParams()
- {
- return params_prc_diff_drv_selfcal_;
- }
-
- double getRadPerTick()
- {
- return radians_per_tick_;
- }
-};
+std::string wolf_dir = _WOLF_CODE_DIR;
class FactorDiffDriveTest : public testing::Test
{
public:
- ProblemPtr problem;
- SolverCeresPtr solver;
- TrajectoryBasePtr trajectory;
- ParamsSensorDiffDrivePtr params_sen;
- SensorDiffDrivePtr sensor;
- ParamsProcessorDiffDrivePtr params_proc;
- ProcessorDiffDrivePublicPtr processor;
- FrameBasePtr F0, F1;
- CaptureMotionPtr C0, C1;
- FeatureMotionPtr f1;
- FactorDiffDrivePtr c1;
+ ProblemPtr problem;
+ SolverManagerPtr solver;
+ TrajectoryBasePtr trajectory;
+ SensorDiffDrivePtr sensor;
+ ProcessorDiffDriveMockPtr processor;
+ FrameBasePtr F0, F1;
+ CaptureMotionPtr C0, C1;
+ FeatureMotionPtr f1;
+ FactorDiffDrivePtr c1;
Vector2d data0, data1;
Matrix2d data_cov;
@@ -149,30 +57,19 @@ class FactorDiffDriveTest : public testing::Test
{
problem = Problem::create("PO", 2);
trajectory = problem->getTrajectory();
+ solver = SolverCeres::create(problem, wolf_dir + "/test/yaml/solver_ceres.yaml", {wolf_dir});
- solver = make_shared<SolverCeres>(problem);
-
- // make a sensor first
- params_sen = make_shared<ParamsSensorDiffDrive>();
- params_sen->ticks_per_wheel_revolution = 100; // DO NOT MODIFY THESE VALUES !!!
- params_sen->name = "cool sensor";
-
- SpecStateSensorComposite priors;
- priors.emplace('P', SpecStateSensor("StatePoint2d", Vector2d::Zero(), "fix")); // default not dynamic
- priors.emplace('O', SpecStateSensor("StateAngle", Vector1d::Zero(), "fix")); // default not dynamic
- priors.emplace('I',
- SpecStateSensor("StateParams3", Vector3d::Ones(), "initial_guess")); // default not dynamic
-
- sensor = SensorBase::emplace<SensorDiffDrive>(problem->getHardware(), params_sen, priors);
+ // sensor
+ sensor = std::static_pointer_cast<SensorDiffDrive>(problem->installSensor(
+ "SensorDiffDrive", wolf_dir + "/test/yaml/sensor_diff_drive_gtest_factor_diff_drive.yaml", {wolf_dir}));
calib = sensor->getIntrinsic()->getState();
- // params and processor
- params_proc = make_shared<ParamsProcessorDiffDrive>();
- params_proc->angle_turned = 1;
- params_proc->dist_traveled = 2;
- params_proc->max_buff_length = 3;
- params_proc->name = "diff drive processor";
- processor = ProcessorBase::emplace<ProcessorDiffDrivePublic>(sensor, params_proc);
+ // processor
+ processor = std::static_pointer_cast<ProcessorDiffDriveMock>(
+ problem->installProcessor("ProcessorDiffDriveMock",
+ sensor,
+ wolf_dir + "/test/yaml/processor_diff_drive.yaml",
+ {wolf_dir + "/test/dummy", wolf_dir + "/schema/processor"}));
// frames
F0 = problem->emplaceFrame(0.0, "PO", Vector3d::Zero());
@@ -224,8 +121,8 @@ TEST_F(FactorDiffDriveTest, constructor)
auto c1_obj = FactorDiffDrive(f1, C0, processor, false);
ASSERT_EQ(c1_obj.getType(), "FactorDiffDrive");
- // std: make_shared
- c1 = make_shared<FactorDiffDrive>(f1, C0, processor, false);
+ // std: std::make_shared
+ c1 = std::make_shared<FactorDiffDrive>(f1, C0, processor, false);
ASSERT_NE(c1, nullptr);
ASSERT_EQ(c1->getType(), "FactorDiffDrive");
@@ -265,8 +162,8 @@ TEST_F(FactorDiffDriveTest, residual_right_turn_90_deg)
MatrixXd delta_cov(3, 3);
double dt = 1.0;
- data1(0) = 0.50 * params_sen->ticks_per_wheel_revolution;
- data1(1) = 0.25 * params_sen->ticks_per_wheel_revolution;
+ data1(0) = 0.50 * sensor->getTicksPerWheelRevolution();
+ data1(1) = 0.25 * sensor->getTicksPerWheelRevolution();
processor->computeCurrentDelta(data1, data_cov, calib, dt, delta, delta_cov, J_delta_calib);
delta1.setZero();
@@ -302,8 +199,8 @@ TEST_F(FactorDiffDriveTest, solve_F1)
MatrixXd delta_cov(3, 3);
double dt = 1.0;
- data1(0) = 0.50 * params_sen->ticks_per_wheel_revolution;
- data1(1) = 0.25 * params_sen->ticks_per_wheel_revolution;
+ data1(0) = 0.50 * sensor->getTicksPerWheelRevolution();
+ data1(1) = 0.25 * sensor->getTicksPerWheelRevolution();
processor->computeCurrentDelta(data1, data_cov, calib, dt, delta, delta_cov, J_delta_calib);
delta1.setZero();
@@ -330,7 +227,7 @@ TEST_F(FactorDiffDriveTest, solve_F1)
sensor->fixIntrinsics();
F1->perturb(0.1);
- string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
+ auto report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
// WOLF_TRACE("\n", report);
problem->print(1, 0, 1, 1);
@@ -347,8 +244,8 @@ TEST_F(FactorDiffDriveTest, solve_sensor_intrinsics)
MatrixXd delta_cov(3, 3);
double dt = 1.0;
- data1(0) = 0.50 * params_sen->ticks_per_wheel_revolution;
- data1(1) = 0.25 * params_sen->ticks_per_wheel_revolution;
+ data1(0) = 0.50 * sensor->getTicksPerWheelRevolution();
+ data1(1) = 0.25 * sensor->getTicksPerWheelRevolution();
processor->computeCurrentDelta(data1, data_cov, calib, dt, delta, delta_cov, J_delta_calib);
delta1.setZero();
@@ -376,7 +273,7 @@ TEST_F(FactorDiffDriveTest, solve_sensor_intrinsics)
sensor->unfixIntrinsics();
sensor->perturb(0.1);
- string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
+ auto report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
// WOLF_TRACE("\n", report);
problem->print(1, 0, 1, 1);
@@ -422,34 +319,21 @@ TEST(FactorDiffDrive, preintegrate_and_solve_sensor_intrinsics_gt)
* - The sensor intrinsics are also estimated.
*/
- ProblemPtr problem = Problem::create("PO", 2);
- SolverCeresPtr solver = make_shared<SolverCeres>(problem);
-
- // make a sensor first
- ParamsSensorDiffDrivePtr params_sen = make_shared<ParamsSensorDiffDrive>();
- params_sen->ticks_per_wheel_revolution = 100; // DO NOT MODIFY THESE VALUES !!!
- params_sen->name = "cool sensor";
-
- SpecStateSensorComposite prior_sen;
- prior_sen.emplace('P', SpecStateSensor("StatePoint2d", Vector2d::Zero(), "fix")); // default not dynamic
- prior_sen.emplace('O', SpecStateSensor("StateAngle", Vector1d::Zero(), "fix")); // default not dynamic
- prior_sen.emplace('I', SpecStateSensor("StateParams3", Vector3d::Ones(), "initial_guess")); // default not dynamic
-
- auto sensor = SensorBase::emplace<SensorDiffDrive>(problem->getHardware(), params_sen, prior_sen);
+ ProblemPtr problem = Problem::create("PO", 2);
+ SolverManagerPtr solver = SolverCeres::create(problem, wolf_dir + "/test/yaml/solver_ceres.yaml", {wolf_dir});
+ // sensor
+ auto sensor = std::static_pointer_cast<SensorDiffDrive>(problem->installSensor(
+ "SensorDiffDrive", wolf_dir + "/test/yaml/sensor_diff_drive_gtest_factor_diff_drive.yaml", {wolf_dir}));
auto calib_preint = sensor->getIntrinsic()->getState();
Vector3d calib_gt(1, 1, 1);
- // params and processor
- ParamsProcessorDiffDrivePtr params_proc = make_shared<ParamsProcessorDiffDrive>();
- params_proc->angle_turned = 99;
- params_proc->dist_traveled = 99;
- params_proc->max_buff_length = 99;
- params_proc->voting_active = true;
- params_proc->max_time_span = 1.5;
- params_proc->name = "diff drive processor";
-
- auto processor = ProcessorBase::emplace<ProcessorDiffDrivePublic>(sensor, params_proc);
+ // processor
+ auto processor = std::static_pointer_cast<ProcessorDiffDriveMock>(
+ problem->installProcessor("ProcessorDiffDriveMock",
+ sensor,
+ wolf_dir + "/test/yaml/processor_diff_drive.yaml",
+ {wolf_dir + "/test/dummy", wolf_dir + "/schema/processor"}));
TimeStamp t(0.0);
double dt = 1.0;
@@ -469,10 +353,10 @@ TEST(FactorDiffDrive, preintegrate_and_solve_sensor_intrinsics_gt)
Matrix2d data_cov;
data_cov.setIdentity();
- data(0) = 0.50 * params_sen->ticks_per_wheel_revolution / N;
- data(1) = 0.25 * params_sen->ticks_per_wheel_revolution / N;
+ data(0) = 0.50 * sensor->getTicksPerWheelRevolution() / N;
+ data(1) = 0.25 * sensor->getTicksPerWheelRevolution() / N;
- auto C = make_shared<CaptureDiffDrive>(t, sensor, data, data_cov, nullptr);
+ auto C = std::make_shared<CaptureDiffDrive>(t, sensor, data, data_cov, nullptr);
for (int n = 0; n < N; n++)
{
@@ -482,8 +366,8 @@ TEST(FactorDiffDrive, preintegrate_and_solve_sensor_intrinsics_gt)
}
// left turn 90 deg in N steps --> ends up in (3, -3, 0)
- data(0) = 0.25 * params_sen->ticks_per_wheel_revolution / N;
- data(1) = 0.50 * params_sen->ticks_per_wheel_revolution / N;
+ data(0) = 0.25 * sensor->getTicksPerWheelRevolution() / N;
+ data(1) = 0.50 * sensor->getTicksPerWheelRevolution() / N;
C->setData(data);
for (int n = 0; n < N; n++)
{
@@ -503,7 +387,7 @@ TEST(FactorDiffDrive, preintegrate_and_solve_sensor_intrinsics_gt)
Vector3d calib_pert = sensor->getIntrinsic()->getState();
WOLF_TRACE("\n ========== SOLVE =========");
- string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
+ auto report = solver->solve(SolverManager::ReportVerbosity::FULL);
WOLF_TRACE("\n", report);
WOLF_TRACE("x1 : ", problem->getState(N * dt).vector("PO").transpose());
@@ -517,7 +401,7 @@ TEST(FactorDiffDrive, preintegrate_and_solve_sensor_intrinsics_gt)
ASSERT_POSE2d_APPROX(problem->getState(N * dt).vector("PO"), x1, 1e-6);
ASSERT_POSE2d_APPROX(problem->getState(2 * N * dt).vector("PO"), x2, 1e-6);
- cout << "\n\n" << endl;
+ std::cout << "\n\n" << std::endl;
}
TEST(FactorDiffDrive, preintegrate_and_solve_sensor_intrinsics)
@@ -558,33 +442,22 @@ TEST(FactorDiffDrive, preintegrate_and_solve_sensor_intrinsics)
* - The sensor intrinsics are also estimated.
*/
- ProblemPtr problem = Problem::create("PO", 2);
- SolverCeresPtr solver = make_shared<SolverCeres>(problem);
-
- // make a sensor first
- ParamsSensorDiffDrivePtr params_sen = make_shared<ParamsSensorDiffDrive>();
- params_sen->ticks_per_wheel_revolution = 100; // DO NOT MODIFY THESE VALUES !!!
- params_sen->name = "cool sensor";
-
- SpecStateSensorComposite prior_sen;
- prior_sen.emplace('P', SpecStateSensor("StatePoint2d", Vector2d::Zero(), "fix")); // default not dynamic
- prior_sen.emplace('O', SpecStateSensor("StateAngle", Vector1d::Zero(), "fix")); // default not dynamic
- prior_sen.emplace('I', SpecStateSensor("StateParams3", Vector3d::Ones(), "initial_guess")); // default not dynamic
+ ProblemPtr problem = Problem::create("PO", 2);
+ SolverManagerPtr solver = SolverCeres::create(problem, wolf_dir + "/test/yaml/solver_ceres.yaml", {wolf_dir});
- auto sensor = SensorBase::emplace<SensorDiffDrive>(problem->getHardware(), params_sen, prior_sen);
+ // sensor
+ auto sensor = std::static_pointer_cast<SensorDiffDrive>(problem->installSensor(
+ "SensorDiffDrive", wolf_dir + "/test/yaml/sensor_diff_drive_gtest_factor_diff_drive.yaml", {wolf_dir}));
+ sensor->getIntrinsic()->perturb(0.2);
auto calib_preint = sensor->getIntrinsic()->getState();
Vector3d calib_gt(1, 1, 1);
- // params and processor
- ParamsProcessorDiffDrivePtr params = make_shared<ParamsProcessorDiffDrive>();
- params->angle_turned = 99;
- params->dist_traveled = 99;
- params->max_buff_length = 99;
- params->voting_active = true;
- params->max_time_span = 1.5;
- params->name = "diff drive processor";
-
- auto processor = ProcessorBase::emplace<ProcessorDiffDrivePublic>(sensor, params);
+ // processor
+ auto processor = std::static_pointer_cast<ProcessorDiffDriveMock>(
+ problem->installProcessor("ProcessorDiffDriveMock",
+ sensor,
+ wolf_dir + "/test/yaml/processor_diff_drive.yaml",
+ {wolf_dir + "/test/dummy", wolf_dir + "/schema/processor"}));
TimeStamp t(0.0);
double dt = 1.0;
@@ -604,10 +477,10 @@ TEST(FactorDiffDrive, preintegrate_and_solve_sensor_intrinsics)
Matrix2d data_cov;
data_cov.setIdentity();
- data(0) = 0.50 * params_sen->ticks_per_wheel_revolution / N;
- data(1) = 0.25 * params_sen->ticks_per_wheel_revolution / N;
+ data(0) = 0.50 * sensor->getTicksPerWheelRevolution() / N;
+ data(1) = 0.25 * sensor->getTicksPerWheelRevolution() / N;
- auto C = make_shared<CaptureDiffDrive>(t, sensor, data, data_cov, nullptr);
+ auto C = std::make_shared<CaptureDiffDrive>(t, sensor, data, data_cov, nullptr);
for (int n = 0; n < N; n++)
{
@@ -617,8 +490,8 @@ TEST(FactorDiffDrive, preintegrate_and_solve_sensor_intrinsics)
}
// left turn 90 deg in N steps --> ends up in (3, -3, 0)
- data(0) = 0.25 * params_sen->ticks_per_wheel_revolution / N;
- data(1) = 0.50 * params_sen->ticks_per_wheel_revolution / N;
+ data(0) = 0.25 * sensor->getTicksPerWheelRevolution() / N;
+ data(1) = 0.50 * sensor->getTicksPerWheelRevolution() / N;
C->setData(data);
@@ -631,7 +504,6 @@ TEST(FactorDiffDrive, preintegrate_and_solve_sensor_intrinsics)
auto F2 = problem->getLastFrame();
- // VectorComposite x2c(x2, "PO", {2,1});
F2->setState(x2, "PO"); // Impose known final state regardless of integrated value.
// Fix boundaries
@@ -639,7 +511,7 @@ TEST(FactorDiffDrive, preintegrate_and_solve_sensor_intrinsics)
F2->fix();
WOLF_TRACE("\n ========== SOLVE =========");
- string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
+ auto report = solver->solve(SolverManager::ReportVerbosity::FULL);
WOLF_TRACE("\n", report);
WOLF_TRACE("x1 : ", problem->getState(N * dt).vector("PO").transpose());
diff --git a/test/gtest_factor_distance_3d.cpp b/test/gtest_factor_distance_3d.cpp
index 109300794585ecc401d3c383e7fe80682ed433db..3fb3794742c80d96377e48255b1e9be6b80f4164 100644
--- a/test/gtest_factor_distance_3d.cpp
+++ b/test/gtest_factor_distance_3d.cpp
@@ -28,6 +28,7 @@
using namespace wolf;
using namespace Eigen;
+std::string wolf_dir = _WOLF_CODE_DIR;
class FactorAutodiffDistance3d_Test : public testing::Test
{
@@ -46,8 +47,8 @@ class FactorAutodiffDistance3d_Test : public testing::Test
Vector1d dist;
Matrix1d dist_cov;
- ProblemPtr problem;
- SolverCeresPtr solver;
+ ProblemPtr problem;
+ SolverManagerPtr solver;
void SetUp() override
{
@@ -66,7 +67,7 @@ class FactorAutodiffDistance3d_Test : public testing::Test
dist_cov = Matrix1d(0.01);
problem = Problem::create("PO", 3);
- solver = std::make_shared<SolverCeres>(problem);
+ solver = SolverCeres::create(problem, wolf_dir + "/test/yaml/solver_ceres.yaml", {wolf_dir});
F1 = problem->emplaceFrame(1.0, "PO", pose1);
F2 = problem->emplaceFrame(2.0, "PO", pose2);
diff --git a/test/gtest_factor_pose_2d.cpp b/test/gtest_factor_pose_2d.cpp
index 81f2fece09e3410761d3a32545335228c6cbaea9..e0f307ba537ab2d474c0591fe27a0783abc6e905 100644
--- a/test/gtest_factor_pose_2d.cpp
+++ b/test/gtest_factor_pose_2d.cpp
@@ -28,17 +28,16 @@
using namespace Eigen;
using namespace wolf;
-using std::cout;
-using std::endl;
-int N = 1e2;
+int N = 1e2;
+std::string wolf_dir = _WOLF_CODE_DIR;
// Input odometry data and covariance
Matrix3d data_cov = 0.1 * Matrix3d::Identity();
// Problem and solver
-ProblemPtr problem_ptr = Problem::create("PO", 2);
-SolverCeres solver(problem_ptr);
+auto problem_ptr = Problem::create("PO", 2);
+auto solver_ptr = SolverCeres::create(problem_ptr, wolf_dir + "/test/yaml/solver_ceres.yaml", {wolf_dir});
// Two frames
FrameBasePtr frm = problem_ptr->emplaceFrame(0, "PO", Vector3d::Zero());
@@ -72,7 +71,7 @@ TEST(FactorPose2d, solve_f)
frm->perturb(10);
// solve for frm1
- std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ std::string report = solver_ptr->solve(SolverManager::ReportVerbosity::FULL);
ASSERT_POSE2d_APPROX(frm->getStateVector("PO"), x0, 1e-6);
diff --git a/test/gtest_factor_pose_2d_with_extrinsics.cpp b/test/gtest_factor_pose_2d_with_extrinsics.cpp
index d88818d0e513147cf4419f113acef5002a76ef4a..ce1b772428d84413be23dc0e508c0662e1e711cc 100644
--- a/test/gtest_factor_pose_2d_with_extrinsics.cpp
+++ b/test/gtest_factor_pose_2d_with_extrinsics.cpp
@@ -40,14 +40,12 @@ std::string wolf_dir = _WOLF_CODE_DIR;
Matrix3d data_cov = 0.1 * Matrix3d::Identity();
// Problem and solver
-ProblemPtr problem_ptr = Problem::create("PO", 2);
-SolverCeres solver(problem_ptr);
+auto problem_ptr = Problem::create("PO", 2);
+auto solver_ptr = SolverCeres::create(problem_ptr, wolf_dir + "/test/yaml/solver_ceres.yaml", {wolf_dir});
// Sensor
auto sensor_pose2d =
- problem_ptr->installSensor("SensorPose2d", wolf_dir + "/test/yaml/sensor_pose_2d.yaml", {wolf_dir});
-auto processor_pose2d =
- ProcessorBase::emplace<ProcessorPose2d>(sensor_pose2d, std::make_shared<ParamsProcessorPose>());
+ problem_ptr -> installSensor("SensorPose2d", wolf_dir + "/test/yaml/sensor_pose_2d.yaml", {wolf_dir});
// Two frames
FrameBasePtr frm = problem_ptr->emplaceFrame(0, "PO", Vector3d::Zero());
@@ -88,7 +86,7 @@ TEST(FactorPose2dWithExtrinsics, solve_f_fix_s)
fea1->getMeasurementSquareRootInformationUpper(),
frm,
sensor_pose2d,
- processor_pose2d,
+ nullptr,
false);
// Perturb frm, fix the extrinsics
@@ -97,7 +95,7 @@ TEST(FactorPose2dWithExtrinsics, solve_f_fix_s)
frm->perturb(10);
// solve for frm1
- std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ std::string report = solver_ptr->solve(SolverManager::ReportVerbosity::FULL);
ASSERT_POSE2d_APPROX(frm->getStateVector("PO"), x0, 1e-6);
@@ -137,7 +135,7 @@ TEST(FactorPose2dWithExtrinsics, fix_f_solve_s)
fea1->getMeasurementSquareRootInformationUpper(),
frm,
sensor_pose2d,
- processor_pose2d,
+ nullptr,
false);
// Perturb extrinsics, fix the frame
@@ -146,7 +144,7 @@ TEST(FactorPose2dWithExtrinsics, fix_f_solve_s)
sensor_pose2d->perturb(1);
// solve for frm1
- std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ std::string report = solver_ptr->solve(SolverManager::ReportVerbosity::FULL);
ASSERT_POSE2d_APPROX(sensor_pose2d->getStateVector("PO"), xs, 1e-6);
diff --git a/test/gtest_factor_pose_3d.cpp b/test/gtest_factor_pose_3d.cpp
index 82b5b63a5a81560139b8718c2e2f2306491fc8e0..1283b4e9d997f79c36335c09b9816cdb96f9be82 100644
--- a/test/gtest_factor_pose_3d.cpp
+++ b/test/gtest_factor_pose_3d.cpp
@@ -31,7 +31,8 @@ using namespace wolf;
using std::cout;
using std::endl;
-int N = 1e2;
+int N = 1e2;
+std::string wolf_dir = _WOLF_CODE_DIR;
Vector7d pose6toPose7(Vector6d _pose6)
{
@@ -42,8 +43,8 @@ Vector7d pose6toPose7(Vector6d _pose6)
Matrix6d data_cov = 0.1 * Matrix6d::Identity();
// Problem and solver
-ProblemPtr problem_ptr = Problem::create("PO", 3);
-SolverCeres solver(problem_ptr);
+auto problem_ptr = Problem::create("PO", 3);
+auto solver_ptr = SolverCeres::create(problem_ptr, wolf_dir + "/test/yaml/solver_ceres.yaml", {wolf_dir});
// Two frames
FrameBasePtr frm = problem_ptr->emplaceFrame(0, "PO", (Vector7d() << 0, 0, 0, 0, 0, 0, 1).finished());
@@ -77,7 +78,7 @@ TEST(FactorPose3dWithExtrinsics, solve_f_fix_s)
frm->perturb(10);
// solve for frm1
- std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ std::string report = solver_ptr->solve(SolverManager::ReportVerbosity::FULL);
ASSERT_POSE3d_APPROX(frm->getStateVector("PO"), x0, 1e-6);
diff --git a/test/gtest_factor_pose_3d_with_extrinsics.cpp b/test/gtest_factor_pose_3d_with_extrinsics.cpp
index 741d1a6197c5fc820ffb92ac8480221deda85afb..7993a7b08251a15b34d66efba62f512316e3a48e 100644
--- a/test/gtest_factor_pose_3d_with_extrinsics.cpp
+++ b/test/gtest_factor_pose_3d_with_extrinsics.cpp
@@ -40,14 +40,12 @@ std::string wolf_dir = _WOLF_CODE_DIR;
Matrix6d data_cov = 0.1 * Matrix6d::Identity();
// Problem and solver
-ProblemPtr problem_ptr = Problem::create("PO", 3);
-SolverCeres solver(problem_ptr);
+auto problem_ptr = Problem::create("PO", 3);
+auto solver_ptr = SolverCeres::create(problem_ptr, wolf_dir + "/test/yaml/solver_ceres.yaml", {wolf_dir});
// Sensor
auto sensor_pose3d =
- problem_ptr->installSensor("SensorPose3d", wolf_dir + "/test/yaml/sensor_pose_3d.yaml", {wolf_dir});
-auto processor_pose2d =
- ProcessorBase::emplace<ProcessorPose3d>(sensor_pose3d, std::make_shared<ParamsProcessorPose>());
+ problem_ptr -> installSensor("SensorPose3d", wolf_dir + "/test/yaml/sensor_pose_3d.yaml", {wolf_dir});
// Two frames
FrameBasePtr frm = problem_ptr->emplaceFrame(0, "PO", (Vector7d() << 0, 0, 0, 0, 0, 0, 1).finished());
@@ -88,7 +86,7 @@ TEST(FactorPose3dWithExtrinsics, solve_f_fix_s)
fea1->getMeasurementSquareRootInformationUpper(),
frm,
sensor_pose3d,
- processor_pose2d,
+ nullptr,
false);
// Perturb frm, fix the extrinsics
@@ -97,7 +95,7 @@ TEST(FactorPose3dWithExtrinsics, solve_f_fix_s)
frm->perturb(10);
// solve for frm1
- std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ std::string report = solver_ptr->solve(SolverManager::ReportVerbosity::FULL);
ASSERT_POSE3d_APPROX(frm->getStateVector("PO"), x0, 1e-6);
@@ -137,7 +135,7 @@ TEST(FactorPose3dWithExtrinsics, fix_f_solve_s)
fea1->getMeasurementSquareRootInformationUpper(),
frm,
sensor_pose3d,
- processor_pose2d,
+ nullptr,
false);
// Perturb extrinsics, fix the frame
@@ -146,7 +144,7 @@ TEST(FactorPose3dWithExtrinsics, fix_f_solve_s)
sensor_pose3d->perturb(1);
// solve for frm1
- std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ std::string report = solver_ptr->solve(SolverManager::ReportVerbosity::FULL);
ASSERT_POSE3d_APPROX(sensor_pose3d->getStateVector("PO"), xs, 1e-6);
diff --git a/test/gtest_factor_prior.cpp b/test/gtest_factor_prior.cpp
index 0df79a69f15b837e6a0bbc328ab64cc3fb040318..f3449dbeced9d862572583581a0e0b45dbad2f97 100644
--- a/test/gtest_factor_prior.cpp
+++ b/test/gtest_factor_prior.cpp
@@ -19,7 +19,7 @@
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#include "core/utils/utils_gtest.h"
-
+#include "core/common/wolf.h"
#include "core/problem/problem.h"
#include "core/sensor/sensor_odom.h"
#include "core/ceres_wrapper/solver_ceres.h"
@@ -29,43 +29,47 @@
using namespace wolf;
using namespace Eigen;
-ProblemPtr problem_ptr = Problem::create("PO", 3);
-SolverCeresPtr solver_ceres = std::make_shared<SolverCeres>(problem_ptr);
-Vector3d initial_extrinsics_p((Vector3d() << 1, 2, 3).finished());
-Vector4d initial_extrinsics_o((Vector4d() << 1, 0, 0, 0).finished());
-Vector7d initial_extrinsics((Vector7d() << initial_extrinsics_p, initial_extrinsics_o).finished());
-Vector7d prior_extrinsics((Vector7d() << 10, 20, 30, 0, 0, 0, 1).finished());
-Vector7d prior2_extrinsics((Vector7d() << 100, 200, 300, 0, 0, 0, 1).finished());
+std::string wolf_dir = _WOLF_CODE_DIR;
+
+ProblemPtr problem_ptr = Problem::create("PO", 3);
+SolverManagerPtr solver = SolverCeres::create(problem_ptr, wolf_dir + "/test/yaml/solver_ceres.yaml", {wolf_dir});
+
+Vector3d initial_extrinsics_p((Vector3d() << 1, 2, 3).finished());
+Vector4d initial_extrinsics_o((Vector4d() << 1, 0, 0, 0).finished());
+Vector3d prior_extrinsics_p((Vector3d() << 10, 20, 30).finished());
+Vector4d prior_extrinsics_o((Vector4d() << 0, 1, 0, 0).finished());
+Vector3d prior2_extrinsics_p((Vector3d() << 100, 200, 300).finished());
-ParamsSensorOdomPtr params = std::make_shared<ParamsSensorOdom>();
-SpecStateSensorComposite priors{{'P', SpecStateSensor("StatePoint3d", initial_extrinsics_p, "initial_guess")},
- {'O', SpecStateSensor("StateQuaternion", initial_extrinsics_o, "initial_guess")}};
-SensorBasePtr sensor_ptr_ = SensorBase::emplace<SensorOdom3d>(problem_ptr->getHardware(), params, priors);
+SensorBasePtr sensor_ptr_ = problem_ptr->installSensor("SensorOdom3d",
+ wolf_dir + "/test/yaml/sensor_odom_3d_gtest_factor_prior.yaml",
+ {wolf_dir + "/schema"});
TEST(ParameterPrior, add_prior_p)
{
ASSERT_TRUE(problem_ptr->check(0));
ASSERT_TRUE(sensor_ptr_->getP());
+ ASSERT_FALSE(sensor_ptr_->getPriorFactor('P'));
ASSERT_MATRIX_APPROX(sensor_ptr_->getP()->getState(), initial_extrinsics_p, Constants::EPS);
- sensor_ptr_->emplaceFactorPrior('P',prior_extrinsics.head(3), Matrix3d::Identity());
+ sensor_ptr_->emplaceFactorStateBlock('P', prior_extrinsics_p, Matrix3d::Identity());
- std::string report = solver_ceres->solve(SolverManager::ReportVerbosity::BRIEF);
+ std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
- ASSERT_MATRIX_APPROX(sensor_ptr_->getP()->getState(), prior_extrinsics.head(3), 1e-6);
+ ASSERT_MATRIX_APPROX(sensor_ptr_->getP()->getState(), prior_extrinsics_p, 1e-6);
}
TEST(ParameterPrior, add_prior_o)
{
ASSERT_TRUE(problem_ptr->check(0));
ASSERT_TRUE(sensor_ptr_->getO());
+ ASSERT_FALSE(sensor_ptr_->getPriorFactor('O'));
ASSERT_QUATERNION_VECTOR_APPROX(sensor_ptr_->getO()->getState(), initial_extrinsics_o, Constants::EPS);
- sensor_ptr_->emplaceFactorPrior('O',prior_extrinsics.tail(4), Matrix3d::Identity());
+ sensor_ptr_->emplaceFactorStateBlock('O', prior_extrinsics_o, Matrix3d::Identity());
- std::string report = solver_ceres->solve(SolverManager::ReportVerbosity::BRIEF);
+ std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
- ASSERT_QUATERNION_VECTOR_APPROX(sensor_ptr_->getO()->getState(), prior_extrinsics.tail(4), 1e-6);
+ ASSERT_QUATERNION_VECTOR_APPROX(sensor_ptr_->getO()->getState(), prior_extrinsics_o, 1e-6);
}
TEST(ParameterPrior, prior_p_overwrite)
@@ -73,37 +77,38 @@ TEST(ParameterPrior, prior_p_overwrite)
ASSERT_TRUE(sensor_ptr_->getPriorFactor('P'));
ASSERT_FALSE(sensor_ptr_->getPriorFactorMap().empty());
- ASSERT_THROW(sensor_ptr_->emplaceFactorPrior('P',prior2_extrinsics.head(3), Matrix3d::Identity()), std::runtime_error);
+ ASSERT_THROW(sensor_ptr_->emplaceFactorStateBlock('P', prior2_extrinsics_p, Matrix3d::Identity()),
+ std::runtime_error);
sensor_ptr_->getPriorFactor('P')->remove();
- sensor_ptr_->emplaceFactorPrior('P',prior2_extrinsics.head(3), Matrix3d::Identity());
+ sensor_ptr_->emplaceFactorStateBlock('P', prior2_extrinsics_p, Matrix3d::Identity());
ASSERT_TRUE(sensor_ptr_->getPriorFactor('P'));
ASSERT_FALSE(sensor_ptr_->getPriorFactorMap().empty());
- std::string report = solver_ceres->solve(SolverManager::ReportVerbosity::BRIEF);
+ std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
- ASSERT_MATRIX_APPROX(sensor_ptr_->getP()->getState(), prior2_extrinsics.head(3), 1e-6);
+ ASSERT_MATRIX_APPROX(sensor_ptr_->getP()->getState(), prior2_extrinsics_p, 1e-6);
}
TEST(ParameterPrior, prior_p_segment)
{
sensor_ptr_->getPriorFactor('P')->remove();
- sensor_ptr_->emplaceFactorPrior('P',prior_extrinsics.segment(1, 2), Matrix2d::Identity(), 1);
+ sensor_ptr_->emplaceFactorStateBlock('P', prior_extrinsics_p.segment(1, 2), Matrix2d::Identity(), 1);
- std::string report = solver_ceres->solve(SolverManager::ReportVerbosity::BRIEF);
+ std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
- ASSERT_MATRIX_APPROX(sensor_ptr_->getP()->getState().segment(1, 2), prior_extrinsics.segment(1, 2), 1e-6);
+ ASSERT_MATRIX_APPROX(sensor_ptr_->getP()->getState().segment(1, 2), prior_extrinsics_p.segment(1, 2), 1e-6);
}
TEST(ParameterPrior, prior_o_segment)
{
sensor_ptr_->getPriorFactor('O')->remove();
-
+
// constraining segment of quaternion is not allowed
- ASSERT_THROW(sensor_ptr_->emplaceFactorPrior('O', prior_extrinsics.segment(1, 2), Matrix2d::Identity(), 1),
+ ASSERT_THROW(sensor_ptr_->emplaceFactorStateBlock('O', prior_extrinsics_p.segment(1, 2), Matrix2d::Identity(), 1),
std::runtime_error);
}
diff --git a/test/gtest_factor_relative_pose_2d.cpp b/test/gtest_factor_relative_pose_2d.cpp
index 88eb1fd390cb19e1f98e1c9e17baa0172ba4a105..05902b77c3b8568e4d1748d2aa104f545fb80ef3 100644
--- a/test/gtest_factor_relative_pose_2d.cpp
+++ b/test/gtest_factor_relative_pose_2d.cpp
@@ -32,6 +32,8 @@ using namespace wolf;
using std::cout;
using std::endl;
+std::string wolf_dir = _WOLF_CODE_DIR;
+
int N = 1e2;
// Vectors
@@ -41,8 +43,8 @@ Vector3d delta, x_0, x_1, x_f, x_l;
Matrix3d data_cov = 0.1 * Matrix3d::Identity();
// Problem and solver
-ProblemPtr problem_ptr = Problem::create("PO", 2);
-SolverCeres solver(problem_ptr);
+auto problem_ptr = Problem::create("PO", 2);
+auto solver_ptr = SolverCeres::create(problem_ptr, wolf_dir + "/test/yaml/solver_ceres.yaml", {wolf_dir});
// Two frames
FrameBasePtr frm0 = problem_ptr->emplaceFrame(0.0, "PO", Vector3d::Zero());
@@ -63,11 +65,6 @@ FactorRelativePose2dPtr fac = nullptr;
void generateRandomProblemFrame()
{
- // solver options
- solver.getSolverOptions().max_num_iterations = 100;
- solver.getSolverOptions().gradient_tolerance = 1e-9;
- solver.getSolverOptions().function_tolerance = 1e-9;
-
if (fea) fea->remove(false);
// Random delta
@@ -101,11 +98,6 @@ void generateRandomProblemFrame()
void generateRandomProblemLandmark()
{
- // solver options
- solver.getSolverOptions().max_num_iterations = 100;
- solver.getSolverOptions().gradient_tolerance = 1e-9;
- solver.getSolverOptions().function_tolerance = 1e-9;
-
if (fea) fea->remove(false);
// Random delta
@@ -187,7 +179,7 @@ TEST(FactorRelativePose2d, frame_solve_frame1)
frm1->perturb(1);
// solve for frm1
- std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ std::string report = solver_ptr->solve(SolverManager::ReportVerbosity::FULL);
// WOLF_INFO(report);
checksFrameFrame();
@@ -207,7 +199,7 @@ TEST(FactorRelativePose2d, frame_solve_frame0)
frm0->perturb(1);
// solve for frm0
- std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ std::string report = solver_ptr->solve(SolverManager::ReportVerbosity::FULL);
WOLF_INFO(report);
checksFrameFrame();
@@ -228,7 +220,7 @@ TEST(FactorRelativePose2d, landmark_solve_lmk)
lmk->perturb(1);
// solve for landmark
- std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ std::string report = solver_ptr->solve(SolverManager::ReportVerbosity::FULL);
// WOLF_INFO(report);
checksFrameLandmark();
@@ -248,7 +240,7 @@ TEST(FactorRelativePose2d, landmark_solve_frame)
frm1->perturb(1);
// solve for frm0
- std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ std::string report = solver_ptr->solve(SolverManager::ReportVerbosity::FULL);
// WOLF_INFO(report);
checksFrameLandmark();
diff --git a/test/gtest_factor_odom_2d_autodiff.cpp b/test/gtest_factor_relative_pose_2d_autodiff.cpp
similarity index 75%
rename from test/gtest_factor_odom_2d_autodiff.cpp
rename to test/gtest_factor_relative_pose_2d_autodiff.cpp
index f1aa3daa91b6ca878939ce95275da6858622f531..9e2900fe85a8960637de8a187b8a7e8c4f7bb164 100644
--- a/test/gtest_factor_odom_2d_autodiff.cpp
+++ b/test/gtest_factor_relative_pose_2d_autodiff.cpp
@@ -19,24 +19,25 @@
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#include "core/utils/utils_gtest.h"
-#include "dummy/factor_odom_2d_autodiff.h"
+#include "dummy/factor_relative_pose_2d_autodiff.h"
#include "core/ceres_wrapper/solver_ceres.h"
#include "core/capture/capture_odom_2d.h"
#include "core/math/rotations.h"
-
using namespace Eigen;
using namespace wolf;
using std::cout;
using std::endl;
+std::string wolf_dir = _WOLF_CODE_DIR;
+
// Input odometry data and covariance
-Matrix3d data_cov = 0.1*Matrix3d::Identity();
+Matrix3d data_cov = 0.1 * Matrix3d::Identity();
// Problem and solver
-ProblemPtr problem_ptr = Problem::create("PO", 2);
-SolverCeres solver(problem_ptr);
+auto problem_ptr = Problem::create("PO", 2);
+auto solver_ptr = SolverCeres::create(problem_ptr, wolf_dir + "/test/yaml/solver_ceres.yaml", {wolf_dir});
// Two frames
FrameBasePtr frm0 = problem_ptr->emplaceFrame(TimeStamp(0), "PO", Vector3d::Zero());
@@ -45,12 +46,12 @@ FrameBasePtr frm1 = problem_ptr->emplaceFrame(TimeStamp(1), "PO", Vector3d::Zero
// Capture from frm1 to frm0
auto cap1 = CaptureBase::emplace<CaptureOdom2d>(frm1, 1, nullptr, Vector3d::Zero(), data_cov);
-TEST(FactorOdom2dAutodiff, check_tree)
+TEST(FactorRelativePose2dAutodiff, check_tree)
{
ASSERT_TRUE(problem_ptr->check(0));
}
-TEST(FactorOdom2dAutodiff, fix_0_solve)
+TEST(FactorRelativePose2dAutodiff, fix_0_solve)
{
for (int i = 0; i < 1e3; i++)
{
@@ -65,16 +66,16 @@ TEST(FactorOdom2dAutodiff, fix_0_solve)
// x1 groundtruth
Vector3d x1;
x1.head<2>() = x0.head<2>() + Rotation2Dd(x0(2)) * delta.head<2>();
- x1(2) = pi2pi(x0(2) + delta(2));
+ x1(2) = pi2pi(x0(2) + delta(2));
// Set states
- frm0->setState(x0,"PO");
- frm1->setState(x1,"PO");
+ frm0->setState(x0, "PO");
+ frm1->setState(x1, "PO");
cap1->setData(delta);
// feature & factor with delta measurement
auto fea1 = FeatureBase::emplace<FeatureBase>(cap1, "FeatureOdom2d", delta, data_cov);
- FactorBase::emplace<FactorOdom2dAutodiff>(fea1, fea1, frm0, nullptr, false);
+ FactorBase::emplace<FactorRelativePose2dAutodiff>(fea1, fea1, frm0, nullptr, false);
// Fix frm0, perturb frm1
frm0->fix();
@@ -82,7 +83,7 @@ TEST(FactorOdom2dAutodiff, fix_0_solve)
frm1->perturb(5);
// solve for frm1
- std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
+ std::string report = solver_ptr->solve(SolverManager::ReportVerbosity::BRIEF);
ASSERT_POSE2d_APPROX(frm1->getStateVector("PO"), x1, 1e-6);
@@ -91,7 +92,7 @@ TEST(FactorOdom2dAutodiff, fix_0_solve)
}
}
-TEST(FactorOdom2dAutodiff, fix_1_solve)
+TEST(FactorRelativePose2dAutodiff, fix_1_solve)
{
for (int i = 0; i < 1e3; i++)
{
@@ -106,16 +107,16 @@ TEST(FactorOdom2dAutodiff, fix_1_solve)
// x1 groundtruth
Vector3d x1;
x1.head<2>() = x0.head<2>() + Rotation2Dd(x0(2)) * delta.head<2>();
- x1(2) = pi2pi(x0(2) + delta(2));
+ x1(2) = pi2pi(x0(2) + delta(2));
// Set states
- frm0->setState(x0,"PO");
- frm1->setState(x1,"PO");
+ frm0->setState(x0, "PO");
+ frm1->setState(x1, "PO");
cap1->setData(delta);
// feature & factor with delta measurement
auto fea1 = FeatureBase::emplace<FeatureBase>(cap1, "FeatureOdom2d", delta, data_cov);
- FactorBase::emplace<FactorOdom2dAutodiff>(fea1, fea1, frm0, nullptr, false);
+ FactorBase::emplace<FactorRelativePose2dAutodiff>(fea1, fea1, frm0, nullptr, false);
// Fix frm1, perturb frm0
frm1->fix();
@@ -123,7 +124,7 @@ TEST(FactorOdom2dAutodiff, fix_1_solve)
frm0->perturb(5);
// solve for frm0
- std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
+ std::string report = solver_ptr->solve(SolverManager::ReportVerbosity::BRIEF);
ASSERT_POSE2d_APPROX(frm0->getStateVector("PO"), x0, 1e-6);
@@ -134,6 +135,6 @@ TEST(FactorOdom2dAutodiff, fix_1_solve)
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_factor_relative_pose_2d_with_extrinsics.cpp b/test/gtest_factor_relative_pose_2d_with_extrinsics.cpp
index e43bff360f7f637d30d29f9cebee79de35eddebd..3c55d0b50c4dd276fd25c1a34fa971c50beae47e 100644
--- a/test/gtest_factor_relative_pose_2d_with_extrinsics.cpp
+++ b/test/gtest_factor_relative_pose_2d_with_extrinsics.cpp
@@ -45,12 +45,11 @@ Vector3d delta, x_0, x_1, x_f, x_l, x_s;
Matrix3d data_cov = 1e-3 * Matrix3d::Identity();
// Problem and solver
-ProblemPtr problem_ptr = Problem::create("PO", 2);
-SolverCeres solver(problem_ptr);
+auto problem_ptr = Problem::create("PO", 2);
+auto solver_ptr = SolverCeres::create(problem_ptr, wolf_dir + "/test/yaml/solver_ceres.yaml", {wolf_dir});
// Sensor
-auto sensor = problem_ptr->installSensor("SensorOdom2d", wolf_dir + "/test/yaml/sensor_odom_2d.yaml", {wolf_dir});
-auto processor_odom2d = ProcessorBase::emplace<ProcessorOdom2d>(sensor, std::make_shared<ParamsProcessorOdom2d>());
+auto sensor = problem_ptr -> installSensor("SensorOdom2d", wolf_dir + "/test/yaml/sensor_odom_2d.yaml", {wolf_dir});
// Two frames
FrameBasePtr frm0 = problem_ptr->emplaceFrame(0, "PO", Vector3d::Zero());
@@ -71,11 +70,6 @@ FactorRelativePose2dWithExtrinsicsPtr fac = nullptr;
void generateRandomProblemFrame()
{
- // solver options
- solver.getSolverOptions().max_num_iterations = 100;
- solver.getSolverOptions().gradient_tolerance = 1e-9;
- solver.getSolverOptions().function_tolerance = 1e-9;
-
if (fea) fea->remove(false);
// Random delta
@@ -116,11 +110,6 @@ void generateRandomProblemFrame()
void generateRandomProblemLandmark()
{
- // solver options
- solver.getSolverOptions().max_num_iterations = 100;
- solver.getSolverOptions().gradient_tolerance = 1e-9;
- solver.getSolverOptions().function_tolerance = 1e-9;
-
if (fea) fea->remove(false);
// Random delta
@@ -219,7 +208,7 @@ TEST(FactorRelativePose2dWithExtrinsics, frame_solve_frame1)
frm1->perturb();
// solve for frm1
- std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ std::string report = solver_ptr->solve(SolverManager::ReportVerbosity::FULL);
// WOLF_INFO(report);
checksFrameFrame();
@@ -243,7 +232,7 @@ TEST(FactorRelativePose2dWithExtrinsics, frame_solve_frame0)
frm0->perturb();
// solve for frm0
- std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ std::string report = solver_ptr->solve(SolverManager::ReportVerbosity::FULL);
// WOLF_INFO(report);
checksFrameFrame();
@@ -268,7 +257,7 @@ TEST(FactorRelativePose2dWithExtrinsics, frame_solve_extrinsics_p)
sensor->getP()->perturb();
// solve for frm0
- std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ std::string report = solver_ptr->solve(SolverManager::ReportVerbosity::FULL);
// checksFrameFrame();
}
@@ -291,7 +280,7 @@ TEST(FactorRelativePose2dWithExtrinsics, frame_solve_extrinsics_o)
sensor->getO()->perturb();
// solve for frm0
- std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ std::string report = solver_ptr->solve(SolverManager::ReportVerbosity::FULL);
// WOLF_INFO(report);
checksFrameFrame();
@@ -316,7 +305,7 @@ TEST(FactorRelativePose2dWithExtrinsics, landmark_solve_lmk)
lmk->perturb();
// solve for landmark
- std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ std::string report = solver_ptr->solve(SolverManager::ReportVerbosity::FULL);
// WOLF_INFO(report);
checksFrameLandmark();
@@ -340,7 +329,7 @@ TEST(FactorRelativePose2dWithExtrinsics, landmark_solve_frame)
frm1->perturb();
// solve for frm0
- std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ std::string report = solver_ptr->solve(SolverManager::ReportVerbosity::FULL);
// WOLF_INFO(report);
checksFrameLandmark();
@@ -364,7 +353,7 @@ TEST(FactorRelativePose2dWithExtrinsics, landmark_extrinsics_p_solve)
sensor->getP()->perturb();
// solve for frm0
- std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ std::string report = solver_ptr->solve(SolverManager::ReportVerbosity::FULL);
// WOLF_INFO(report);
checksFrameLandmark();
@@ -390,7 +379,7 @@ TEST(FactorRelativePose2dWithExtrinsics, landmark_extrinsics_o_solve)
// std::cout << "Sensor O (perturbed): " << sensor->getO()->getState().transpose() << std::endl;
// solve for frm0
- std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ std::string report = solver_ptr->solve(SolverManager::ReportVerbosity::FULL);
// WOLF_INFO(report);
checksFrameLandmark();
diff --git a/test/gtest_factor_relative_pose_3d.cpp b/test/gtest_factor_relative_pose_3d.cpp
index 859e93ed9790ce2491216c798a26947434ee4ae5..74dc315287c4dbc3d79b9f377658870b231382f8 100644
--- a/test/gtest_factor_relative_pose_3d.cpp
+++ b/test/gtest_factor_relative_pose_3d.cpp
@@ -40,12 +40,11 @@ Vector7d delta, x_0, x_1, x_f, x_l;
// Input odometry data and covariance
Matrix6d data_cov = 1e-3 * Matrix6d::Identity();
-//Matrix6d data_cov = (Vector6d() << 1e-3, 1e-3, 1e-3, 1e-6, 1e-6, 1e-6).finished().asDiagonal();
+// Matrix6d data_cov = (Vector6d() << 1e-3, 1e-3, 1e-3, 1e-6, 1e-6, 1e-6).finished().asDiagonal();
// Problem and solver
-ProblemPtr problem_ptr = Problem::create("PO", 3);
-
-SolverCeres solver(problem_ptr);
+auto problem_ptr = Problem::create("PO", 3);
+auto solver_ptr = SolverCeres::create(problem_ptr, wolf_dir + "/test/yaml/solver_ceres.yaml", {wolf_dir});
// Two frames
FrameBasePtr frm0 = problem_ptr->emplaceFrame(TimeStamp(0), problem_ptr->stateZero());
@@ -69,11 +68,6 @@ FactorRelativePose3dPtr fac = nullptr;
void generateRandomProblemFrame()
{
- // solver options
- solver.getSolverOptions().max_num_iterations = 100;
- solver.getSolverOptions().gradient_tolerance = 1e-9;
- solver.getSolverOptions().function_tolerance = 1e-9;
-
if (fea) fea->remove(false);
// Random delta
@@ -109,11 +103,6 @@ void generateRandomProblemFrame()
void generateRandomProblemLandmark()
{
- // solver options
- solver.getSolverOptions().max_num_iterations = 100;
- solver.getSolverOptions().gradient_tolerance = 1e-9;
- solver.getSolverOptions().function_tolerance = 1e-9;
-
if (fea) fea->remove(false);
// Random delta
@@ -196,7 +185,7 @@ TEST(FactorRelativePose3d, frame_solve_frame1)
frm1->perturb(1);
// solve for frm1
- std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ std::string report = solver_ptr->solve(SolverManager::ReportVerbosity::FULL);
// WOLF_INFO(report);
checksFrameFrame();
@@ -216,8 +205,8 @@ TEST(FactorRelativePose3d, frame_solve_frame0)
frm0->perturb(1);
// solve for frm0
- std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
- //WOLF_INFO(report);
+ std::string report = solver_ptr->solve(SolverManager::ReportVerbosity::FULL);
+ // WOLF_INFO(report);
checksFrameFrame();
}
@@ -237,7 +226,7 @@ TEST(FactorRelativePose3d, landmark_solve_lmk)
lmk->perturb(1);
// solve for landmark
- std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ std::string report = solver_ptr->solve(SolverManager::ReportVerbosity::FULL);
// WOLF_INFO(report);
checksFrameLandmark();
@@ -257,7 +246,7 @@ TEST(FactorRelativePose3d, landmark_solve_frame)
frm1->perturb(1);
// solve for frm0
- std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ std::string report = solver_ptr->solve(SolverManager::ReportVerbosity::FULL);
// WOLF_INFO(report);
checksFrameLandmark();
diff --git a/test/gtest_factor_relative_pose_3d_with_extrinsics.cpp b/test/gtest_factor_relative_pose_3d_with_extrinsics.cpp
index 86e8c1364e7c9367ff432a31786807e07ab3b108..db1e95c723307a7588424a2bc777fdb5538dc4c0 100644
--- a/test/gtest_factor_relative_pose_3d_with_extrinsics.cpp
+++ b/test/gtest_factor_relative_pose_3d_with_extrinsics.cpp
@@ -40,15 +40,13 @@ Vector7d delta, x_0, x_1, x_f, x_l, x_s;
// Input odometry data and covariance
Matrix6d data_cov = 1e-3 * Matrix6d::Identity();
-// Matrix6d data_cov = (Vector6d() << 1e-3, 1e-3, 1e-3, 1e-6, 1e-6, 1e-6).finished().asDiagonal();
// Problem and solver
-ProblemPtr problem_ptr = Problem::create("PO", 3);
-
-SolverCeres solver(problem_ptr);
+auto problem_ptr = Problem::create("PO", 3);
+auto solver_ptr = SolverCeres::create(problem_ptr, wolf_dir + "/test/yaml/solver_ceres.yaml", {wolf_dir});
// Sensor
-auto sensor = problem_ptr->installSensor("SensorOdom3d", wolf_dir + "/test/yaml/sensor_odom_3d.yaml", {wolf_dir});
+auto sensor = problem_ptr -> installSensor("SensorOdom3d", wolf_dir + "/test/yaml/sensor_odom_3d.yaml", {wolf_dir});
// Two frames
FrameBasePtr frm0 = problem_ptr->emplaceFrame(0, "PO", (Vector7d() << 0, 0, 0, 0, 0, 0, 1).finished());
@@ -70,11 +68,6 @@ FactorRelativePose3dWithExtrinsicsPtr fac = nullptr;
void generateRandomProblemFrame()
{
- // solver options
- solver.getSolverOptions().max_num_iterations = 100;
- solver.getSolverOptions().gradient_tolerance = 1e-9;
- solver.getSolverOptions().function_tolerance = 1e-9;
-
if (fea) fea->remove(false);
// Random delta
@@ -118,11 +111,6 @@ void generateRandomProblemFrame()
void generateRandomProblemLandmark()
{
- // solver options
- solver.getSolverOptions().max_num_iterations = 100;
- solver.getSolverOptions().gradient_tolerance = 1e-9;
- solver.getSolverOptions().function_tolerance = 1e-9;
-
if (fea) fea->remove(false);
// Random delta
@@ -222,7 +210,7 @@ TEST(FactorRelativePose3dWithExtrinsics, frame_solve_frame1)
frm1->perturb(1);
// solve for frm1
- std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ std::string report = solver_ptr->solve(SolverManager::ReportVerbosity::FULL);
// WOLF_INFO(report);
checksFrameFrame();
@@ -246,7 +234,7 @@ TEST(FactorRelativePose3dWithExtrinsics, frame_solve_frame0)
frm0->perturb(1);
// solve for frm0
- std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ std::string report = solver_ptr->solve(SolverManager::ReportVerbosity::FULL);
// WOLF_INFO(report);
checksFrameFrame();
@@ -270,7 +258,7 @@ TEST(FactorRelativePose3dWithExtrinsics, frame_solve_extrinsics_p)
sensor->getP()->perturb(1);
// solve for frm0
- std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ std::string report = solver_ptr->solve(SolverManager::ReportVerbosity::FULL);
// WOLF_INFO(report);
// checksFrameFrame(); // JV: The position extrinsics in case of pair of frames is not observable
@@ -294,7 +282,7 @@ TEST(FactorRelativePose3dWithExtrinsics, frame_solve_extrinsics_o)
sensor->getO()->perturb(.2);
// solve for frm0
- std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ std::string report = solver_ptr->solve(SolverManager::ReportVerbosity::FULL);
// WOLF_INFO(report);
checksFrameFrame();
@@ -319,7 +307,7 @@ TEST(FactorRelativePose3dWithExtrinsics, landmark_solve_lmk)
lmk->perturb(1);
// solve for landmark
- std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ std::string report = solver_ptr->solve(SolverManager::ReportVerbosity::FULL);
// WOLF_INFO(report);
checksFrameLandmark();
@@ -343,7 +331,7 @@ TEST(FactorRelativePose3dWithExtrinsics, landmark_solve_frame)
frm1->perturb(1);
// solve for frm0
- std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ std::string report = solver_ptr->solve(SolverManager::ReportVerbosity::FULL);
// WOLF_INFO(report);
checksFrameLandmark();
@@ -367,7 +355,7 @@ TEST(FactorRelativePose3dWithExtrinsics, landmark_extrinsics_p_solve)
sensor->getP()->perturb(1);
// solve for frm0
- std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ std::string report = solver_ptr->solve(SolverManager::ReportVerbosity::FULL);
// WOLF_INFO(report);
checksFrameLandmark();
@@ -391,7 +379,7 @@ TEST(FactorRelativePose3dWithExtrinsics, landmark_extrinsics_o_solve)
sensor->getO()->perturb(.2);
// solve for frm0
- std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ std::string report = solver_ptr->solve(SolverManager::ReportVerbosity::FULL);
// WOLF_INFO(report);
checksFrameLandmark();
diff --git a/test/gtest_factor_relative_position_2d.cpp b/test/gtest_factor_relative_position_2d.cpp
index 19c65d36849131960f616b04f3bafa1a90c36dcb..2d62a11b9f9feab75d4da47fabbb3fe694e6477e 100644
--- a/test/gtest_factor_relative_position_2d.cpp
+++ b/test/gtest_factor_relative_position_2d.cpp
@@ -45,8 +45,8 @@ Vector2d delta, x_l;
Matrix2d data_cov = 0.1 * Matrix2d::Identity();
// Problem and solver
-ProblemPtr problem_ptr = Problem::create("PO", 2);
-SolverCeres solver(problem_ptr);
+auto problem_ptr = Problem::create("PO", 2);
+auto solver_ptr = SolverCeres::create(problem_ptr, wolf_dir + "/test/yaml/solver_ceres.yaml", {wolf_dir});
// Frames
FrameBasePtr frm0 = problem_ptr->emplaceFrame(0, "PO", Vector3d::Zero());
@@ -67,11 +67,6 @@ FactorRelativePosition2dPtr fac = nullptr;
void generateRandomProblemFrame()
{
- // solver options
- solver.getSolverOptions().max_num_iterations = 100;
- solver.getSolverOptions().gradient_tolerance = 1e-9;
- solver.getSolverOptions().function_tolerance = 1e-9;
-
if (fea) fea->remove(false);
// Random delta
@@ -102,11 +97,6 @@ void generateRandomProblemFrame()
void generateRandomProblemLandmark()
{
- // solver options
- solver.getSolverOptions().max_num_iterations = 100;
- solver.getSolverOptions().gradient_tolerance = 1e-9;
- solver.getSolverOptions().function_tolerance = 1e-9;
-
if (fea) fea->remove(false);
// Random delta
@@ -187,7 +177,7 @@ TEST(FactorRelativePosition2d, frame_solve_frame1_p)
frm1->getP()->perturb();
// solve for frm1
- std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ std::string report = solver_ptr->solve(SolverManager::ReportVerbosity::FULL);
// WOLF_INFO(report);
checksFrameFrame();
@@ -208,7 +198,7 @@ TEST(FactorRelativePosition2d, frame_solve_frame0_p)
frm0->getP()->perturb();
// solve for frm0
- std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ std::string report = solver_ptr->solve(SolverManager::ReportVerbosity::FULL);
// WOLF_INFO(report);
checksFrameFrame();
@@ -229,7 +219,7 @@ TEST(FactorRelativePosition2d, frame_solve_frame0_o)
frm0->getO()->perturb();
// solve for frm0
- std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ std::string report = solver_ptr->solve(SolverManager::ReportVerbosity::FULL);
// WOLF_INFO(report);
checksFrameFrame();
@@ -250,7 +240,7 @@ TEST(FactorRelativePosition2d, landmark_solve_lmk)
lmk->perturb();
// solve for landmark
- std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ std::string report = solver_ptr->solve(SolverManager::ReportVerbosity::FULL);
// WOLF_INFO(report);
checksFrameLandmark();
@@ -271,7 +261,7 @@ TEST(FactorRelativePosition2d, landmark_solve_frame_p)
frm1->getP()->perturb();
// solve for frm0
- std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ std::string report = solver_ptr->solve(SolverManager::ReportVerbosity::FULL);
// WOLF_INFO(report);
checksFrameLandmark();
@@ -292,7 +282,7 @@ TEST(FactorRelativePosition2d, landmark_solve_frame_o)
frm1->getO()->perturb();
// solve for frm0
- std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ std::string report = solver_ptr->solve(SolverManager::ReportVerbosity::FULL);
// WOLF_INFO(report);
checksFrameLandmark();
diff --git a/test/gtest_factor_relative_position_2d_with_extrinsics.cpp b/test/gtest_factor_relative_position_2d_with_extrinsics.cpp
index 5e63eeea24890a3727201bc0bfda1420a6af4f64..08f34a8e80a0d8a9dd9d909f2613d7ae7bcc1866 100644
--- a/test/gtest_factor_relative_position_2d_with_extrinsics.cpp
+++ b/test/gtest_factor_relative_position_2d_with_extrinsics.cpp
@@ -45,11 +45,11 @@ Vector2d delta, x_l;
Matrix2d data_cov = 0.1 * Matrix2d::Identity();
// Problem and solver
-ProblemPtr problem_ptr = Problem::create("PO", 2);
-SolverCeres solver(problem_ptr);
+auto problem_ptr = Problem::create("PO", 2);
+auto solver_ptr = SolverCeres::create(problem_ptr, wolf_dir + "/test/yaml/solver_ceres.yaml", {wolf_dir});
// Sensor
-auto sensor = problem_ptr->installSensor("SensorOdom2d", wolf_dir + "/test/yaml/sensor_odom_2d.yaml", {wolf_dir});
+auto sensor = problem_ptr -> installSensor("SensorOdom2d", wolf_dir + "/test/yaml/sensor_odom_2d.yaml", {wolf_dir});
// Frame
FrameBasePtr frm = problem_ptr->emplaceFrame(0, "PO", Vector3d::Zero());
@@ -69,11 +69,6 @@ FactorRelativePosition2dWithExtrinsicsPtr fac = nullptr;
void generateRandomProblemLandmark()
{
- // solver options
- solver.getSolverOptions().max_num_iterations = 100;
- solver.getSolverOptions().gradient_tolerance = 1e-9;
- solver.getSolverOptions().function_tolerance = 1e-9;
-
if (fea) fea->remove(false);
// Random delta
@@ -97,14 +92,8 @@ void generateRandomProblemLandmark()
// feature & factor with delta measurement
fea = FeatureBase::emplace<FeatureBase>(cap, "FeatureLandmarkPosition2d", delta, data_cov);
- fac = FactorBase::emplace<FactorRelativePosition2dWithExtrinsics>(fea,
- fea->getMeasurement(),
- fea->getMeasurementSquareRootInformationUpper(),
- frm,
- lmk,
- sensor,
- nullptr,
- false);
+ fac = FactorBase::emplace<FactorRelativePosition2dWithExtrinsics>(
+ fea, fea->getMeasurement(), fea->getMeasurementSquareRootInformationUpper(), frm, lmk, sensor, nullptr, false);
}
void checksFrameLandmark()
@@ -147,7 +136,7 @@ TEST(FactorRelativePosition2dWithExtrinsics, landmark_solve_lmk)
lmk->perturb(1);
// solve for landmark
- std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ std::string report = solver_ptr->solve(SolverManager::ReportVerbosity::FULL);
// WOLF_INFO(report);
checksFrameLandmark();
@@ -170,7 +159,7 @@ TEST(FactorRelativePosition2dWithExtrinsics, landmark_solve_frame_p)
frm->getP()->perturb(1);
// solve for frm0
- std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ std::string report = solver_ptr->solve(SolverManager::ReportVerbosity::FULL);
// WOLF_INFO(report);
checksFrameLandmark();
@@ -193,7 +182,7 @@ TEST(FactorRelativePosition2dWithExtrinsics, landmark_solve_frame_o)
frm->getO()->perturb(1);
// solve for frm0
- std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ std::string report = solver_ptr->solve(SolverManager::ReportVerbosity::FULL);
// WOLF_INFO(report);
checksFrameLandmark();
@@ -215,7 +204,7 @@ TEST(FactorRelativePosition2dWithExtrinsics, landmark_extrinsics_p_solve)
sensor->getP()->perturb(1);
// solve for frm0
- std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ std::string report = solver_ptr->solve(SolverManager::ReportVerbosity::FULL);
// WOLF_INFO(report);
checksFrameLandmark();
@@ -239,7 +228,7 @@ TEST(FactorRelativePosition2dWithExtrinsics, landmark_extrinsics_o_solve)
// std::cout << "Sensor O (perturbed): " << sensor->getO()->getState().transpose() << std::endl;
// solve for frm0
- std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ std::string report = solver_ptr->solve(SolverManager::ReportVerbosity::FULL);
// WOLF_INFO(report);
checksFrameLandmark();
diff --git a/test/gtest_factor_relative_position_3d.cpp b/test/gtest_factor_relative_position_3d.cpp
index 368f033745acaa40b04237cbcf3939bd7c570226..3b6776e341bf529766d1da80d2810eed2db16f8f 100644
--- a/test/gtest_factor_relative_position_3d.cpp
+++ b/test/gtest_factor_relative_position_3d.cpp
@@ -43,9 +43,8 @@ Vector3d delta1, delta2, delta3, x_1, x_2, x_3;
Matrix3d data_cov = (Vector3d() << 1e-3, 1e-3, 1e-3).finished().asDiagonal();
// Problem and solver
-ProblemPtr problem_ptr = Problem::create("PO", 3);
-
-SolverCeres solver(problem_ptr);
+auto problem_ptr = Problem::create("PO", 3);
+auto solver_ptr = SolverCeres::create(problem_ptr, wolf_dir + "/test/yaml/solver_ceres.yaml", {wolf_dir});
// Two frames
FrameBasePtr frm = problem_ptr->emplaceFrame(0, "PO", (Vector7d() << 0, 0, 0, 0, 0, 0, 1).finished());
@@ -79,11 +78,6 @@ FactorRelativePosition3dPtr fac1, fac2, fac3;
////////////////////////////////////////////////////////
void generateRandomProblemFrame()
{
- // solver options
- solver.getSolverOptions().max_num_iterations = 100;
- solver.getSolverOptions().gradient_tolerance = 1e-9;
- solver.getSolverOptions().function_tolerance = 1e-9;
-
if (cap1)
{
cap1->remove(false);
@@ -147,11 +141,6 @@ void generateRandomProblemFrame()
void generateRandomProblemLandmark()
{
- // solver options
- solver.getSolverOptions().max_num_iterations = 100;
- solver.getSolverOptions().gradient_tolerance = 1e-9;
- solver.getSolverOptions().function_tolerance = 1e-9;
-
if (cap1)
{
cap1->remove(false);
@@ -293,7 +282,7 @@ TEST(FactorRelativePosition3d, frame_solve_frame)
frm->perturb(1);
// solve for frm1
- std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ std::string report = solver_ptr->solve(SolverManager::ReportVerbosity::FULL);
// WOLF_INFO(report);
checksFrameFrame();
@@ -317,7 +306,7 @@ TEST(FactorRelativePosition3d, frame_solve_frames)
frm3->getP()->perturb(1);
// solve for frm0
- std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ std::string report = solver_ptr->solve(SolverManager::ReportVerbosity::FULL);
// WOLF_INFO(report);
checksFrameFrame();
@@ -340,7 +329,7 @@ TEST(FactorRelativePosition3d, landmark_solve_frame)
frm->perturb(1);
// solve for frm0
- std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ std::string report = solver_ptr->solve(SolverManager::ReportVerbosity::FULL);
// WOLF_INFO(report);
checksFrameLandmark();
@@ -364,7 +353,7 @@ TEST(FactorRelativePosition3d, landmark_solve_lmks)
lmk3->perturb(1);
// solve for landmark
- std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ std::string report = solver_ptr->solve(SolverManager::ReportVerbosity::FULL);
// WOLF_INFO(report);
checksFrameLandmark();
diff --git a/test/gtest_factor_relative_position_3d_with_extrinsics.cpp b/test/gtest_factor_relative_position_3d_with_extrinsics.cpp
index 61c3a89b7c742836d9fc0cd19a244f4a127c0305..3b44c9a915582c82b61f16fa087859b21edbafb5 100644
--- a/test/gtest_factor_relative_position_3d_with_extrinsics.cpp
+++ b/test/gtest_factor_relative_position_3d_with_extrinsics.cpp
@@ -45,12 +45,11 @@ Vector3d delta1, delta2, delta3, x_l1, x_l2, x_l3;
Matrix3d data_cov = (Vector3d() << 1e-3, 1e-3, 1e-3).finished().asDiagonal();
// Problem and solver
-ProblemPtr problem_ptr = Problem::create("PO", 3);
-
-SolverCeres solver(problem_ptr);
+auto problem_ptr = Problem::create("PO", 3);
+auto solver_ptr = SolverCeres::create(problem_ptr, wolf_dir + "/test/yaml/solver_ceres.yaml", {wolf_dir});
// Sensor
-auto sensor = problem_ptr->installSensor("SensorOdom3d", wolf_dir + "/test/yaml/sensor_odom_3d.yaml", {wolf_dir});
+auto sensor = problem_ptr -> installSensor("SensorOdom3d", wolf_dir + "/test/yaml/sensor_odom_3d.yaml", {wolf_dir});
// Frame
FrameBasePtr frm = problem_ptr->emplaceFrame(0, "PO", (Vector7d() << 0, 0, 0, 0, 0, 0, 1).finished());
@@ -80,11 +79,6 @@ FactorRelativePosition3dWithExtrinsicsPtr fac1, fac2, fac3;
void generateRandomProblemLandmark()
{
- // solver options
- solver.getSolverOptions().max_num_iterations = 100;
- solver.getSolverOptions().gradient_tolerance = 1e-9;
- solver.getSolverOptions().function_tolerance = 1e-9;
-
if (fea1)
{
fea1->remove(false);
@@ -123,30 +117,33 @@ void generateRandomProblemLandmark()
fea1 = FeatureBase::emplace<FeatureBase>(cap, "FeatureLandmarkPose", delta1, data_cov);
fea2 = FeatureBase::emplace<FeatureBase>(cap, "FeatureLandmarkPose", delta2, data_cov);
fea3 = FeatureBase::emplace<FeatureBase>(cap, "FeatureLandmarkPose", delta3, data_cov);
- fac1 = FactorBase::emplace<FactorRelativePosition3dWithExtrinsics>(fea1,
- fea1->getMeasurement(),
- fea1->getMeasurementSquareRootInformationUpper(),
- frm,
- lmk1,
- sensor,
- nullptr,
- false);
- fac2 = FactorBase::emplace<FactorRelativePosition3dWithExtrinsics>(fea2,
- fea2->getMeasurement(),
- fea2->getMeasurementSquareRootInformationUpper(),
- frm,
- lmk2,
- sensor,
- nullptr,
- false);
- fac3 = FactorBase::emplace<FactorRelativePosition3dWithExtrinsics>(fea3,
- fea3->getMeasurement(),
- fea3->getMeasurementSquareRootInformationUpper(),
- frm,
- lmk3,
- sensor,
- nullptr,
- false);
+ fac1 =
+ FactorBase::emplace<FactorRelativePosition3dWithExtrinsics>(fea1,
+ fea1->getMeasurement(),
+ fea1->getMeasurementSquareRootInformationUpper(),
+ frm,
+ lmk1,
+ sensor,
+ nullptr,
+ false);
+ fac2 =
+ FactorBase::emplace<FactorRelativePosition3dWithExtrinsics>(fea2,
+ fea2->getMeasurement(),
+ fea2->getMeasurementSquareRootInformationUpper(),
+ frm,
+ lmk2,
+ sensor,
+ nullptr,
+ false);
+ fac3 =
+ FactorBase::emplace<FactorRelativePosition3dWithExtrinsics>(fea3,
+ fea3->getMeasurement(),
+ fea3->getMeasurementSquareRootInformationUpper(),
+ frm,
+ lmk3,
+ sensor,
+ nullptr,
+ false);
}
void checksFrameLandmark()
@@ -213,7 +210,7 @@ TEST(FactorRelativePosition3dWithExtrinsics, landmark_solve_lmks)
lmk3->perturb(1);
// solve for landmark
- std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ std::string report = solver_ptr->solve(SolverManager::ReportVerbosity::FULL);
// WOLF_INFO(report);
checksFrameLandmark();
@@ -241,7 +238,7 @@ TEST(FactorRelativePosition3dWithExtrinsics, landmark_solve_frame)
frm->perturb(1);
// solve for frm0
- std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ std::string report = solver_ptr->solve(SolverManager::ReportVerbosity::FULL);
// WOLF_INFO(report);
checksFrameLandmark();
@@ -269,7 +266,7 @@ TEST(FactorRelativePosition3dWithExtrinsics, landmark_extrinsics_p_solve)
sensor->getP()->perturb(1);
// solve for frm0
- std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ std::string report = solver_ptr->solve(SolverManager::ReportVerbosity::FULL);
// WOLF_INFO(report);
checksFrameLandmark();
@@ -297,7 +294,7 @@ TEST(FactorRelativePosition3dWithExtrinsics, landmark_extrinsics_o_solve)
sensor->getO()->perturb(.2);
// solve for frm0
- std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ std::string report = solver_ptr->solve(SolverManager::ReportVerbosity::FULL);
// WOLF_INFO(report);
checksFrameLandmark();
diff --git a/test/gtest_factor_velocity_local_direction_3d.cpp b/test/gtest_factor_velocity_local_direction_3d.cpp
index f07b2ccbe117f73995a29c2afa8dc58352f0c349..81e4f5d150980f0de33b4583c1d55d8d75f6fc04 100644
--- a/test/gtest_factor_velocity_local_direction_3d.cpp
+++ b/test/gtest_factor_velocity_local_direction_3d.cpp
@@ -25,169 +25,153 @@
#include "core/capture/capture_odom_2d.h"
#include "core/math/rotations.h"
-
using namespace Eigen;
using namespace wolf;
-int N_TESTS = 100;
+int N_TESTS = 100;
+std::string wolf_dir = _WOLF_CODE_DIR;
class FactorVelocityLocalDirection3dTest : public testing::Test
{
- protected:
- ProblemPtr problem;
- SolverManagerPtr solver;
- FrameBasePtr frm;
- StateBlockPtr sb_p;
- StateBlockPtr sb_o;
- StateBlockPtr sb_v;
- CaptureBasePtr cap;
-
- int n_solved;
- std::vector<double> convergence, iterations, times, error;
-
- virtual void SetUp()
+ protected:
+ ProblemPtr problem;
+ SolverManagerPtr solver;
+ FrameBasePtr frm;
+ StateBlockPtr sb_p;
+ StateBlockPtr sb_o;
+ StateBlockPtr sb_v;
+ CaptureBasePtr cap;
+
+ int n_solved;
+ std::vector<double> convergence, iterations, times, error;
+
+ virtual void SetUp()
+ {
+ // create problem
+ problem = Problem::create("POV", 3);
+
+ // create solver
+ solver = SolverCeres::create(problem, wolf_dir + "/test/yaml/solver_ceres.yaml", {wolf_dir});
+
+ // Frame
+ frm = problem->emplaceFrame(0.0, "POV", (Vector10d() << 0, 0, 0, 0, 0, 0, 1, 1, 0, 0).finished());
+ sb_p = frm->getP();
+ sb_p->fix();
+ sb_o = frm->getO();
+ sb_v = frm->getV();
+
+ // Capture
+ cap = CaptureBase::emplace<CaptureBase>(frm, "CaptureBase", frm->getTimeStamp(), nullptr);
+ }
+
+ FactorBasePtr emplaceFeatureAndFactor(const Vector3d& v_local, const double& angle_stdev)
+ {
+ // emplace feature
+ FeatureBasePtr fea =
+ FeatureBase::emplace<FeatureBase>(cap, "FeatureBase", v_local, Matrix1d(angle_stdev * angle_stdev));
+
+ // emplace factor
+ return FactorBase::emplace<FactorVelocityLocalDirection3d>(
+ fea, fea->getMeasurement(), fea->getMeasurementSquareRootInformationUpper(), frm, 0, nullptr, false);
+ }
+
+ bool testLocalVelocity(const Quaterniond& o, const Vector3d& v_local, bool estimate_o, bool estimate_v)
+ {
+ assert(cap->getFeatureList().empty());
+
+ // set state
+ sb_o->setState(o.coeffs());
+ sb_v->setState(o * v_local);
+
+ // fix or unfix & perturb
+ if (not estimate_o)
+ sb_o->fix();
+ else
{
- // create problem
- problem = Problem::create("POV", 3);
-
- // create solver
- auto params_solver = std::make_shared<ParamsCeres>();
- params_solver->solver_options.max_num_iterations = 1e3;
- solver = std::make_shared<SolverCeres>(problem, params_solver);
-
- // Frame
- frm = problem->emplaceFrame(0.0, "POV", (Vector10d() << 0,0,0,0,0,0,1,1,0,0).finished());
- sb_p = frm->getP();
- sb_p->fix();
- sb_o = frm->getO();
- sb_v = frm->getV();
-
- // Capture
- cap = CaptureBase::emplace<CaptureBase>(frm, "CaptureBase",
- frm->getTimeStamp(), nullptr);
+ sb_o->unfix();
+ sb_o->perturb();
}
-
- FactorBasePtr emplaceFeatureAndFactor(const Vector3d& v_local,
- const double& angle_stdev)
+ if (not estimate_v)
+ sb_v->fix();
+ else
{
- // emplace feature
- FeatureBasePtr fea = FeatureBase::emplace<FeatureBase>(cap, "FeatureBase",
- v_local,
- Matrix1d(angle_stdev * angle_stdev));
-
- // emplace factor
- return FactorBase::emplace<FactorVelocityLocalDirection3d>(fea,
- fea->getMeasurement(),
- fea->getMeasurementSquareRootInformationUpper(),
- frm,
- 0,
- nullptr,
- false);
+ sb_v->unfix();
+ sb_v->setState(Vector3d::Random());
}
- bool testLocalVelocity(const Quaterniond& o,
- const Vector3d& v_local,
- bool estimate_o,
- bool estimate_v)
+ // emplace feature & factor
+ auto fac = emplaceFeatureAndFactor(v_local, 0.01);
+
+ // solve
+ std::string report = solver->solve(SolverManager::ReportVerbosity::FULL);
+
+ // local coordinates
+ auto v_est_local_normalized =
+ (Quaterniond(Vector4d(sb_o->getState())).conjugate() * sb_v->getState()).normalized();
+ auto cos_angle_local = v_est_local_normalized.dot(v_local);
+ if (cos_angle_local > 1) cos_angle_local = 1;
+ if (cos_angle_local < -1) cos_angle_local = -1;
+
+ // global coordinates
+ auto v_global = Quaterniond(Vector4d(sb_o->getState())) * v_local;
+ auto v_est_normalized = sb_v->getState().normalized();
+ auto cos_angle_global = v_est_normalized.dot(v_global);
+ if (cos_angle_global > 1) cos_angle_global = 1;
+ if (cos_angle_global < -1) cos_angle_global = -1;
+
+ // Check angle error
+ if (std::abs(acos(cos_angle_local)) > M_PI / 180 or std::abs(acos(cos_angle_global)) > M_PI / 180)
{
- assert(cap->getFeatureList().empty());
-
- // set state
- sb_o->setState(o.coeffs());
- sb_v->setState(o * v_local);
-
- // fix or unfix & perturb
- if (not estimate_o)
- sb_o->fix();
- else
- {
- sb_o->unfix();
- sb_o->perturb();
- }
- if (not estimate_v)
- sb_v->fix();
- else
- {
- sb_v->unfix();
- sb_v->setState(Vector3d::Random());
- }
-
- // emplace feature & factor
- auto fac = emplaceFeatureAndFactor(v_local, 0.01);
-
- // solve
- std::string report = solver->solve(SolverManager::ReportVerbosity::FULL);
-
- // local coordinates
- auto v_est_local_normalized = (Quaterniond(Vector4d(sb_o->getState())).conjugate() * sb_v->getState()).normalized();
- auto cos_angle_local = v_est_local_normalized.dot(v_local);
- if (cos_angle_local > 1)
- cos_angle_local = 1;
- if (cos_angle_local < -1)
- cos_angle_local = -1;
-
- // global coordinates
- auto v_global = Quaterniond(Vector4d(sb_o->getState())) * v_local;
- auto v_est_normalized = sb_v->getState().normalized();
- auto cos_angle_global = v_est_normalized.dot(v_global);
- if (cos_angle_global > 1)
- cos_angle_global = 1;
- if (cos_angle_global < -1)
- cos_angle_global = -1;
-
- // Check angle error
- if (std::abs(acos(cos_angle_local)) > M_PI/180 or
- std::abs(acos(cos_angle_global)) > M_PI/180)
- {
- WOLF_ERROR("\n\n!!!!!!!!!!!! ERROR TOO BIG iteration: ", iterations.size());
- WOLF_INFO("cos(angle local) = ", cos_angle_local);
- WOLF_INFO("angle local = ", acos(cos_angle_local));
- WOLF_INFO("cos(angle global) = ", cos_angle_global);
- WOLF_INFO("angle global = ", acos(cos_angle_global));
-
- problem->print(4,1,1,1);
- WOLF_INFO(report);
-
- // Reset
- fac->getFeature()->remove();
-
- return false;
- }
+ WOLF_ERROR("\n\n!!!!!!!!!!!! ERROR TOO BIG iteration: ", iterations.size());
+ WOLF_INFO("cos(angle local) = ", cos_angle_local);
+ WOLF_INFO("angle local = ", acos(cos_angle_local));
+ WOLF_INFO("cos(angle global) = ", cos_angle_global);
+ WOLF_INFO("angle global = ", acos(cos_angle_global));
+
+ problem->print(4, 1, 1, 1);
+ WOLF_INFO(report);
// Reset
fac->getFeature()->remove();
- // Update performaces
- convergence.push_back(solver->hasConverged() ? 1 : 0);
- iterations.push_back(solver->iterations());
- times.push_back(solver->totalTime());
- error.push_back(acos(cos_angle_local));
-
- return true;
+ return false;
}
- void printAverageResults()
- {
- WOLF_INFO("/////////////////// Average results: n_solved = ", iterations.size());
- double conv_avg = Eigen::Map<Eigen::VectorXd>(convergence.data(), convergence.size()).mean();
- double iter_avg = Eigen::Map<Eigen::VectorXd>(iterations.data(), iterations.size()).mean();
- double time_avg = Eigen::Map<Eigen::VectorXd>(times.data(), times.size()).mean();
- double err_avg = Eigen::Map<Eigen::VectorXd>(error.data(), error.size()).mean();
-
- double iter_stdev = (Eigen::Map<Eigen::ArrayXd>(iterations.data(), iterations.size()) - iter_avg).matrix().norm();
- double time_stdev = (Eigen::Map<Eigen::ArrayXd>(times.data(), times.size()) - time_avg).matrix().norm();
- double err_stdev = (Eigen::Map<Eigen::ArrayXd>(error.data(), error.size()) - err_avg).matrix().norm();
-
- WOLF_INFO("\tconvergence: ", 100 * conv_avg, "%");
- WOLF_INFO("\titerations: ", iter_avg, " - stdev: ", iter_stdev);
- WOLF_INFO("\ttime: ", 1000 * time_avg, " ms", " - stdev: ", time_stdev);
- WOLF_INFO("\tangle error: ", err_avg, " - stdev: ", err_stdev);
- }
+ // Reset
+ fac->getFeature()->remove();
+
+ // Update performaces
+ convergence.push_back(solver->hasConverged() ? 1 : 0);
+ iterations.push_back(solver->iterations());
+ times.push_back(solver->totalTime());
+ error.push_back(acos(cos_angle_local));
+
+ return true;
+ }
+
+ void printAverageResults()
+ {
+ WOLF_INFO("/////////////////// Average results: n_solved = ", iterations.size());
+ double conv_avg = Eigen::Map<Eigen::VectorXd>(convergence.data(), convergence.size()).mean();
+ double iter_avg = Eigen::Map<Eigen::VectorXd>(iterations.data(), iterations.size()).mean();
+ double time_avg = Eigen::Map<Eigen::VectorXd>(times.data(), times.size()).mean();
+ double err_avg = Eigen::Map<Eigen::VectorXd>(error.data(), error.size()).mean();
+
+ double iter_stdev =
+ (Eigen::Map<Eigen::ArrayXd>(iterations.data(), iterations.size()) - iter_avg).matrix().norm();
+ double time_stdev = (Eigen::Map<Eigen::ArrayXd>(times.data(), times.size()) - time_avg).matrix().norm();
+ double err_stdev = (Eigen::Map<Eigen::ArrayXd>(error.data(), error.size()) - err_avg).matrix().norm();
+
+ WOLF_INFO("\tconvergence: ", 100 * conv_avg, "%");
+ WOLF_INFO("\titerations: ", iter_avg, " - stdev: ", iter_stdev);
+ WOLF_INFO("\ttime: ", 1000 * time_avg, " ms", " - stdev: ", time_stdev);
+ WOLF_INFO("\tangle error: ", err_avg, " - stdev: ", err_stdev);
+ }
};
// Input odometry data and covariance
Vector3d v_local(1.0, 0.0, 0.0);
-double angle_stdev = 0.1;
+double angle_stdev = 0.1;
TEST_F(FactorVelocityLocalDirection3dTest, check_tree)
{
@@ -201,35 +185,29 @@ TEST_F(FactorVelocityLocalDirection3dTest, check_tree)
TEST_F(FactorVelocityLocalDirection3dTest, origin_solveV)
{
for (auto i = 0; i < N_TESTS; i++)
- ASSERT_TRUE(testLocalVelocity(Quaterniond::Identity(),
- Vector3d::Random().normalized(),
- false, true));
+ ASSERT_TRUE(testLocalVelocity(Quaterniond::Identity(), Vector3d::Random().normalized(), false, true));
printAverageResults();
}
TEST_F(FactorVelocityLocalDirection3dTest, random_solveV)
{
for (auto i = 0; i < N_TESTS; i++)
- ASSERT_TRUE(testLocalVelocity(Quaterniond(Vector4d::Random().normalized()),
- Vector3d::Random().normalized(),
- false, true));
+ ASSERT_TRUE(testLocalVelocity(
+ Quaterniond(Vector4d::Random().normalized()), Vector3d::Random().normalized(), false, true));
printAverageResults();
}
TEST_F(FactorVelocityLocalDirection3dTest, origin_solveV_degenerated)
{
for (auto i = 0; i < N_TESTS; i++)
- ASSERT_TRUE(testLocalVelocity(Quaterniond::Identity(),
- (Vector3d() << 1, 0, 0).finished() + Vector3d::Random() * 1e-4,
- false, true));
+ ASSERT_TRUE(testLocalVelocity(
+ Quaterniond::Identity(), (Vector3d() << 1, 0, 0).finished() + Vector3d::Random() * 1e-4, false, true));
for (auto i = 0; i < N_TESTS; i++)
- ASSERT_TRUE(testLocalVelocity(Quaterniond::Identity(),
- (Vector3d() << 0, 1, 0).finished() + Vector3d::Random() * 1e-4,
- false, true));
+ ASSERT_TRUE(testLocalVelocity(
+ Quaterniond::Identity(), (Vector3d() << 0, 1, 0).finished() + Vector3d::Random() * 1e-4, false, true));
for (auto i = 0; i < N_TESTS; i++)
- ASSERT_TRUE(testLocalVelocity(Quaterniond::Identity(),
- (Vector3d() << 0, 0, 1).finished() + Vector3d::Random() * 1e-4,
- false, true));
+ ASSERT_TRUE(testLocalVelocity(
+ Quaterniond::Identity(), (Vector3d() << 0, 0, 1).finished() + Vector3d::Random() * 1e-4, false, true));
printAverageResults();
}
@@ -238,50 +216,47 @@ TEST_F(FactorVelocityLocalDirection3dTest, random_solveV_degenerated)
for (auto i = 0; i < N_TESTS; i++)
ASSERT_TRUE(testLocalVelocity(Quaterniond(Vector4d::Random().normalized()),
(Vector3d() << 0, 0, 1).finished() + Vector3d::Random() * 1e-4,
- false, true));
+ false,
+ true));
for (auto i = 0; i < N_TESTS; i++)
ASSERT_TRUE(testLocalVelocity(Quaterniond(Vector4d::Random().normalized()),
(Vector3d() << 0, 1, 0).finished() + Vector3d::Random() * 1e-4,
- false, true));
+ false,
+ true));
for (auto i = 0; i < N_TESTS; i++)
ASSERT_TRUE(testLocalVelocity(Quaterniond(Vector4d::Random().normalized()),
(Vector3d() << 0, 0, 1).finished() + Vector3d::Random() * 1e-4,
- false, true));
+ false,
+ true));
printAverageResults();
}
TEST_F(FactorVelocityLocalDirection3dTest, origin_solveO)
{
for (auto i = 0; i < N_TESTS; i++)
- ASSERT_TRUE(testLocalVelocity(Quaterniond::Identity(),
- Vector3d::Random().normalized(),
- true, false));
+ ASSERT_TRUE(testLocalVelocity(Quaterniond::Identity(), Vector3d::Random().normalized(), true, false));
printAverageResults();
}
TEST_F(FactorVelocityLocalDirection3dTest, random_solveO)
{
for (auto i = 0; i < N_TESTS; i++)
- ASSERT_TRUE(testLocalVelocity(Quaterniond(Vector4d::Random().normalized()),
- Vector3d::Random().normalized(),
- true, false));
+ ASSERT_TRUE(testLocalVelocity(
+ Quaterniond(Vector4d::Random().normalized()), Vector3d::Random().normalized(), true, false));
printAverageResults();
}
TEST_F(FactorVelocityLocalDirection3dTest, origin_solveO_degenerated)
{
for (auto i = 0; i < N_TESTS; i++)
- ASSERT_TRUE(testLocalVelocity(Quaterniond::Identity(),
- (Vector3d() << 1, 0, 0).finished() + Vector3d::Random() * 1e-4,
- true, false));
+ ASSERT_TRUE(testLocalVelocity(
+ Quaterniond::Identity(), (Vector3d() << 1, 0, 0).finished() + Vector3d::Random() * 1e-4, true, false));
for (auto i = 0; i < N_TESTS; i++)
- ASSERT_TRUE(testLocalVelocity(Quaterniond::Identity(),
- (Vector3d() << 0, 1, 0).finished() + Vector3d::Random() * 1e-4,
- true, false));
+ ASSERT_TRUE(testLocalVelocity(
+ Quaterniond::Identity(), (Vector3d() << 0, 1, 0).finished() + Vector3d::Random() * 1e-4, true, false));
for (auto i = 0; i < N_TESTS; i++)
- ASSERT_TRUE(testLocalVelocity(Quaterniond::Identity(),
- (Vector3d() << 0, 0, 1).finished() + Vector3d::Random() * 1e-4,
- true, false));
+ ASSERT_TRUE(testLocalVelocity(
+ Quaterniond::Identity(), (Vector3d() << 0, 0, 1).finished() + Vector3d::Random() * 1e-4, true, false));
printAverageResults();
}
@@ -290,20 +265,23 @@ TEST_F(FactorVelocityLocalDirection3dTest, random_solveO_degenerated)
for (auto i = 0; i < N_TESTS; i++)
ASSERT_TRUE(testLocalVelocity(Quaterniond(Vector4d::Random().normalized()),
(Vector3d() << 0, 0, 1).finished() + Vector3d::Random() * 1e-4,
- true, false));
+ true,
+ false));
for (auto i = 0; i < N_TESTS; i++)
ASSERT_TRUE(testLocalVelocity(Quaterniond(Vector4d::Random().normalized()),
(Vector3d() << 0, 1, 0).finished() + Vector3d::Random() * 1e-4,
- true, false));
+ true,
+ false));
for (auto i = 0; i < N_TESTS; i++)
ASSERT_TRUE(testLocalVelocity(Quaterniond(Vector4d::Random().normalized()),
(Vector3d() << 0, 0, 1).finished() + Vector3d::Random() * 1e-4,
- true, false));
+ true,
+ false));
printAverageResults();
}
-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_factory_state_block.cpp b/test/gtest_factory_state_block.cpp
index bf28bfcfaea925a14115236e7eeab088c5058656..ff2c68f9452f5260530e79ad3b3745e7a1f9b5ae 100644
--- a/test/gtest_factory_state_block.cpp
+++ b/test/gtest_factory_state_block.cpp
@@ -17,12 +17,6 @@
//
// 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/>.
-/*
- * gtest_factory_state_block.cpp
- *
- * Created on: Apr 28, 2020
- * Author: jsola
- */
#include "core/utils/utils_gtest.h"
@@ -31,7 +25,6 @@
#include "core/sensor/factory_sensor.h"
#include "core/state_block/state_block_derived.h"
-
using namespace wolf;
using namespace Eigen;
@@ -46,75 +39,76 @@ TEST(FactoryStateBlock, creator_non_registered_key)
TEST(FactoryStateBlock, creator_StateBlock)
{
- ASSERT_THROW(auto sbp = FactoryStateBlock::create("StateBlock", Vector3d(1,2,3), false), std::runtime_error);
+ ASSERT_THROW(auto sbp = FactoryStateBlock::create("StateBlock", Vector3d(1, 2, 3), false), std::runtime_error);
}
TEST(FactoryStateBlock, creator_Quaternion)
{
- auto sbq = FactoryStateBlock::create("StateQuaternion", Vector4d(1,2,3,4).normalized(), false);
+ auto sbq = FactoryStateBlock::create("StateQuaternion", Vector4d(1, 2, 3, 4).normalized(), false);
- ASSERT_EQ(sbq->getSize() , 4);
+ ASSERT_EQ(sbq->getSize(), 4);
ASSERT_EQ(sbq->getLocalSize(), 3);
ASSERT_TRUE(sbq->hasLocalParametrization());
}
TEST(FactoryStateBlock, creator_Quaternion_not_normalized)
{
- ASSERT_THROW(auto sbq = FactoryStateBlock::create("StateQuaternion", Vector4d(1,2,3,4), false), std::runtime_error);
+ ASSERT_THROW(auto sbq = FactoryStateBlock::create("StateQuaternion", Vector4d(1, 2, 3, 4), false),
+ std::runtime_error);
}
TEST(FactoryStateBlock, creator_Angle)
{
auto sba = FactoryStateBlock::create("StateAngle", Vector1d(1), false);
- ASSERT_EQ(sba->getSize() , 1);
+ ASSERT_EQ(sba->getSize(), 1);
ASSERT_EQ(sba->getLocalSize(), 1);
ASSERT_TRUE(sba->hasLocalParametrization());
}
TEST(FactoryStateBlock, creator_Homogeneous3d)
{
- auto sbh = FactoryStateBlock::create("StateHomogeneous3d", Vector4d(1,2,3,4), false);
+ auto sbh = FactoryStateBlock::create("StateHomogeneous3d", Vector4d(1, 2, 3, 4), false);
- ASSERT_EQ(sbh->getSize() , 4);
+ ASSERT_EQ(sbh->getSize(), 4);
ASSERT_EQ(sbh->getLocalSize(), 3);
ASSERT_TRUE(sbh->hasLocalParametrization());
}
TEST(FactoryStateBlock, creator_Point)
{
- auto sba = FactoryStateBlock::create("StatePoint2d", Vector2d(1,2), false);
+ auto sba = FactoryStateBlock::create("StatePoint2d", Vector2d(1, 2), false);
- ASSERT_EQ(sba->getSize() , 2);
+ ASSERT_EQ(sba->getSize(), 2);
ASSERT_EQ(sba->getLocalSize(), 2);
ASSERT_FALSE(sba->hasLocalParametrization());
- sba = FactoryStateBlock::create("StatePoint3d", Vector3d(1,2,3), false);
+ sba = FactoryStateBlock::create("StatePoint3d", Vector3d(1, 2, 3), false);
- ASSERT_EQ(sba->getSize() , 3);
+ ASSERT_EQ(sba->getSize(), 3);
ASSERT_EQ(sba->getLocalSize(), 3);
ASSERT_FALSE(sba->hasLocalParametrization());
// fails
- ASSERT_THROW(sba = FactoryStateBlock::create("StatePoint2d", Vector1d(1), false) , std::runtime_error);
+ ASSERT_THROW(sba = FactoryStateBlock::create("StatePoint2d", Vector1d(1), false), std::runtime_error);
}
TEST(FactoryStateBlock, creator_Vector)
{
- auto sba = FactoryStateBlock::create("StateVector2d", Vector2d(1,2), false);
+ auto sba = FactoryStateBlock::create("StateVector2d", Vector2d(1, 2), false);
- ASSERT_EQ(sba->getSize() , 2);
+ ASSERT_EQ(sba->getSize(), 2);
ASSERT_EQ(sba->getLocalSize(), 2);
ASSERT_FALSE(sba->hasLocalParametrization());
- sba = FactoryStateBlock::create("StateVector3d", Vector3d(1,2,3), false);
+ sba = FactoryStateBlock::create("StateVector3d", Vector3d(1, 2, 3), false);
- ASSERT_EQ(sba->getSize() , 3);
+ ASSERT_EQ(sba->getSize(), 3);
ASSERT_EQ(sba->getLocalSize(), 3);
ASSERT_FALSE(sba->hasLocalParametrization());
// fails
- ASSERT_THROW(sba = FactoryStateBlock::create("StatePoint2d", Vector1d(1), false) , std::runtime_error);
+ ASSERT_THROW(sba = FactoryStateBlock::create("StatePoint2d", Vector1d(1), false), std::runtime_error);
}
TEST(FactoryStateBlock, creator_Params)
@@ -145,7 +139,7 @@ TEST(FactoryStateBlock, creator_Params)
ASSERT_FALSE(sb1->hasLocalParametrization());
// fails
- ASSERT_THROW(auto sba = FactoryStateBlock::create("StateParams2", Vector1d(1), false) , std::runtime_error);
+ ASSERT_THROW(auto sba = FactoryStateBlock::create("StateParams2", Vector1d(1), false), std::runtime_error);
}
////////////////////////////////////////////////////////
@@ -166,7 +160,7 @@ TEST(FactoryStateBlockIdentity, creator_Quaternion)
{
auto sbq = FactoryStateBlockIdentity::create("StateQuaternion");
- ASSERT_EQ(sbq->getSize() , 4);
+ ASSERT_EQ(sbq->getSize(), 4);
ASSERT_EQ(sbq->getLocalSize(), 3);
ASSERT_TRUE(sbq->hasLocalParametrization());
ASSERT_TRUE(sbq->isValid());
@@ -177,7 +171,7 @@ TEST(FactoryStateBlockIdentity, creator_Angle)
{
auto sba = FactoryStateBlockIdentity::create("StateAngle");
- ASSERT_EQ(sba->getSize() , 1);
+ ASSERT_EQ(sba->getSize(), 1);
ASSERT_EQ(sba->getLocalSize(), 1);
ASSERT_TRUE(sba->hasLocalParametrization());
ASSERT_TRUE(sba->isValid());
@@ -188,7 +182,7 @@ TEST(FactoryStateBlockIdentity, creator_Homogeneous3d)
{
auto sbh = FactoryStateBlockIdentity::create("StateHomogeneous3d");
- ASSERT_EQ(sbh->getSize() , 4);
+ ASSERT_EQ(sbh->getSize(), 4);
ASSERT_EQ(sbh->getLocalSize(), 3);
ASSERT_TRUE(sbh->hasLocalParametrization());
ASSERT_TRUE(sbh->isValid());
@@ -199,15 +193,15 @@ TEST(FactoryStateBlockIdentity, creator_Point)
{
auto sbp2d = FactoryStateBlockIdentity::create("StatePoint2d");
- ASSERT_EQ(sbp2d->getSize() , 2);
+ ASSERT_EQ(sbp2d->getSize(), 2);
ASSERT_EQ(sbp2d->getLocalSize(), 2);
ASSERT_FALSE(sbp2d->hasLocalParametrization());
ASSERT_TRUE(sbp2d->isValid());
ASSERT_MATRIX_APPROX(Vector2d::Zero(), sbp2d->getState(), Constants::EPS_SMALL);
- auto sbp3d = FactoryStateBlockIdentity::create("StatePoint3d");
+ auto sbp3d = FactoryStateBlockIdentity::create("StatePoint3d");
- ASSERT_EQ(sbp3d->getSize() , 3);
+ ASSERT_EQ(sbp3d->getSize(), 3);
ASSERT_EQ(sbp3d->getLocalSize(), 3);
ASSERT_FALSE(sbp3d->hasLocalParametrization());
ASSERT_TRUE(sbp3d->isValid());
@@ -218,7 +212,7 @@ TEST(FactoryStateBlockIdentity, creator_Vector)
{
auto sbv2d = FactoryStateBlockIdentity::create("StateVector2d");
- ASSERT_EQ(sbv2d->getSize() , 2);
+ ASSERT_EQ(sbv2d->getSize(), 2);
ASSERT_EQ(sbv2d->getLocalSize(), 2);
ASSERT_FALSE(sbv2d->hasLocalParametrization());
ASSERT_TRUE(sbv2d->isValid());
@@ -226,7 +220,7 @@ TEST(FactoryStateBlockIdentity, creator_Vector)
auto sbv3d = FactoryStateBlockIdentity::create("StateVector3d");
- ASSERT_EQ(sbv3d->getSize() , 3);
+ ASSERT_EQ(sbv3d->getSize(), 3);
ASSERT_EQ(sbv3d->getLocalSize(), 3);
ASSERT_FALSE(sbv3d->hasLocalParametrization());
ASSERT_TRUE(sbv3d->isValid());
@@ -278,7 +272,7 @@ TEST(FactoryStateBlockIdentity, creator_Params)
ASSERT_FALSE(sb8->hasLocalParametrization());
ASSERT_FALSE(sb9->hasLocalParametrization());
ASSERT_FALSE(sb10->hasLocalParametrization());
-
+
ASSERT_TRUE(sb1->isValid());
ASSERT_TRUE(sb2->isValid());
ASSERT_TRUE(sb3->isValid());
@@ -289,7 +283,7 @@ TEST(FactoryStateBlockIdentity, creator_Params)
ASSERT_TRUE(sb8->isValid());
ASSERT_TRUE(sb9->isValid());
ASSERT_TRUE(sb10->isValid());
-
+
ASSERT_MATRIX_APPROX(Vector1d::Zero(), sb1->getState(), Constants::EPS_SMALL);
ASSERT_MATRIX_APPROX(Vector2d::Zero(), sb2->getState(), Constants::EPS_SMALL);
ASSERT_MATRIX_APPROX(Vector3d::Zero(), sb3->getState(), Constants::EPS_SMALL);
@@ -351,7 +345,7 @@ TEST(FactoryStateBlockIdentityVector, creator_Point)
TEST(FactoryStateBlockIdentityVector, creator_Vector)
{
auto vec = FactoryStateBlockIdentityVector::create("StateVector2d");
-
+
ASSERT_MATRIX_APPROX(Vector2d::Zero(), vec, Constants::EPS_SMALL);
vec = FactoryStateBlockIdentityVector::create("StateVector3d");
@@ -382,7 +376,7 @@ TEST(FactoryStateBlockIdentityVectorVector, creator_Params)
ASSERT_EQ(vec_8.size(), 8);
ASSERT_EQ(vec_9.size(), 9);
ASSERT_EQ(vec_10.size(), 10);
-
+
ASSERT_MATRIX_APPROX(Vector1d::Zero(), vec_1, Constants::EPS_SMALL);
ASSERT_MATRIX_APPROX(Vector2d::Zero(), vec_2, Constants::EPS_SMALL);
ASSERT_MATRIX_APPROX(Vector3d::Zero(), vec_3, Constants::EPS_SMALL);
diff --git a/test/gtest_feature_base.cpp b/test/gtest_feature_base.cpp
index 716653011ac3be883a28aa384995e24ee4999f21..7db36fade14f11d0edb605edb7eaabc1a6931a86 100644
--- a/test/gtest_feature_base.cpp
+++ b/test/gtest_feature_base.cpp
@@ -17,12 +17,6 @@
//
// 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/>.
-/*
- * gtest_feature_base.cpp
- *
- * Created on: Apr 10, 2017
- * Author: jsola
- */
#include "core/feature/feature_base.h"
@@ -33,41 +27,52 @@ using namespace Eigen;
TEST(FeatureBase, ConstructorCov)
{
- Vector3d m; m << 1,2,3;
- Matrix3d M; M.setRandom(); M = M*M.transpose();
- Matrix3d I = M.inverse();
- Eigen::LLT<Eigen::MatrixXd> llt_of_info(I); // compute the Cholesky decomposition of A
- Eigen::MatrixXd U = llt_of_info.matrixU();
- Eigen::MatrixXd L = llt_of_info.matrixL();
+ Vector3d m;
+ m << 1, 2, 3;
+ Matrix3d M;
+ M.setRandom();
+ M = M * M.transpose();
+ Matrix3d I = M.inverse();
+ Eigen::LLT<Eigen::MatrixXd> llt_of_info(I); // compute the Cholesky decomposition of A
+ Eigen::MatrixXd U = llt_of_info.matrixU();
+ Eigen::MatrixXd L = llt_of_info.matrixL();
FeatureBasePtr f(std::make_shared<FeatureBase>("DUMMY", m, M));
ASSERT_MATRIX_APPROX(f->getMeasurement(), m, 1e-6);
ASSERT_MATRIX_APPROX(f->getMeasurementCovariance(), M, 1e-6);
- ASSERT_MATRIX_APPROX(f->getMeasurementSquareRootInformationUpper().transpose() * f->getMeasurementSquareRootInformationUpper(), U.transpose()*U, 1e-6);
+ ASSERT_MATRIX_APPROX(
+ f->getMeasurementSquareRootInformationUpper().transpose() * f->getMeasurementSquareRootInformationUpper(),
+ U.transpose() * U,
+ 1e-6);
ASSERT_MATRIX_APPROX(f->getMeasurementSquareRootInformationUpper(), U, 1e-6);
}
TEST(FeatureBase, ConstructorInfo)
{
- Vector3d m; m << 1,2,3;
- Matrix3d M; M.setRandom(); M = M*M.transpose();
- Matrix3d I = M.inverse();
- Eigen::LLT<Eigen::MatrixXd> llt_of_info(I); // compute the Cholesky decomposition of A
- Eigen::MatrixXd U = llt_of_info.matrixU();
- Eigen::MatrixXd L = llt_of_info.matrixL();
+ Vector3d m;
+ m << 1, 2, 3;
+ Matrix3d M;
+ M.setRandom();
+ M = M * M.transpose();
+ Matrix3d I = M.inverse();
+ Eigen::LLT<Eigen::MatrixXd> llt_of_info(I); // compute the Cholesky decomposition of A
+ Eigen::MatrixXd U = llt_of_info.matrixU();
+ Eigen::MatrixXd L = llt_of_info.matrixL();
FeatureBasePtr f(std::make_shared<FeatureBase>("DUMMY", m, I, FeatureBase::UncertaintyType::UNCERTAINTY_IS_INFO));
ASSERT_MATRIX_APPROX(f->getMeasurement(), m, 1e-6);
ASSERT_MATRIX_APPROX(f->getMeasurementCovariance(), M, 1e-6);
- ASSERT_MATRIX_APPROX(f->getMeasurementSquareRootInformationUpper().transpose() * f->getMeasurementSquareRootInformationUpper(), U.transpose()*U, 1e-6);
+ ASSERT_MATRIX_APPROX(
+ f->getMeasurementSquareRootInformationUpper().transpose() * f->getMeasurementSquareRootInformationUpper(),
+ U.transpose() * U,
+ 1e-6);
ASSERT_MATRIX_APPROX(f->getMeasurementSquareRootInformationUpper(), U, 1e-6);
}
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_frame_base.cpp b/test/gtest_frame_base.cpp
index a74d7fd726aec1fe91b11f699f0738c805512cfa..0510064021e4d52d7c8ae65d624701dff690899f 100644
--- a/test/gtest_frame_base.cpp
+++ b/test/gtest_frame_base.cpp
@@ -69,8 +69,7 @@ TEST(FrameBase, LinksBasic)
ASSERT_FALSE(F->getTrajectory());
ASSERT_FALSE(F->getProblem());
- auto S = FactorySensorFile::create(
- "SensorOdom2d", wolf_dir + "/test/yaml/sensor_odom_2d.yaml", {wolf_dir});
+ auto S = FactorySensorFile::create("SensorOdom2d", wolf_dir + "/test/yaml/sensor_odom_2d.yaml", {wolf_dir});
ASSERT_FALSE(F->getCaptureOf(S));
ASSERT_TRUE(F->getCaptureList().empty());
ASSERT_TRUE(F->getFactoredBySet().empty());
@@ -86,12 +85,8 @@ TEST(FrameBase, LinksToTree)
FrameBase::emplace<FrameBase>(T, 1, make_shared<StatePoint2d>(Vector2d::Zero()), make_shared<StateAngle>(0));
auto F2 =
FrameBase::emplace<FrameBase>(T, 2, make_shared<StatePoint2d>(Vector2d::Zero()), make_shared<StateAngle>(0));
- auto C = CaptureBase::emplace<CaptureMotion>(F1, "CaptureMotion", 1, S, Vector3d::Zero(), nullptr);
- WOLF_INFO("F2->getCaptureList().size() ", F2->getCaptureList().size());
- auto p = std::make_shared<ProcessorOdom2d>(std::make_shared<ParamsProcessorOdom2d>());
- WOLF_INFO("F2->getCaptureList().size() ", F2->getCaptureList().size());
- p->link(S);
- WOLF_INFO("F2->getCaptureList().size() ", F2->getCaptureList().size());
+ auto C = CaptureBase::emplace<CaptureMotion>(F1, "CaptureMotion", 1, S, Vector3d::Zero(), nullptr);
+ auto p = P->installProcessor("ProcessorOdom2d", S, wolf_dir + "/test/yaml/processor_odom_2d.yaml", {wolf_dir});
auto f = FeatureBase::emplace<FeatureBase>(C, "f", Vector1d(1), Matrix<double, 1, 1>::Identity() * .01);
auto fac = FactorBase::emplace<FactorRelativePose2d>(
f, Vector3d::Random(), Matrix3d::Identity(), F1, F2, p, false, TOP_MOTION);
diff --git a/test/gtest_local_param.cpp b/test/gtest_local_param.cpp
index 73080589f56fb0940f50ca3c2ae05d6ecd9691bb..f6c0df1ad7cf7a4e6251bf027ec840138740ef82 100644
--- a/test/gtest_local_param.cpp
+++ b/test/gtest_local_param.cpp
@@ -20,7 +20,6 @@
#include "core/utils/utils_gtest.h"
-
#include "core/state_block/local_parametrization_quaternion.h"
#include "core/state_block/local_parametrization_homogeneous.h"
#include "core/math/rotations.h"
@@ -29,20 +28,20 @@
#include <iostream>
-#define JAC_NUMERIC(T, _x0, _J, dx) \
-{ \
- VectorXd dv(3); \
- Map<const VectorXd> _dv (dv.data(), 3); \
- VectorXd xo(4); \
- Map<VectorXd> _xo (xo.data(), 4); \
- for (int i = 0; i< 3; i++) \
- {\
- dv.setZero();\
- dv(i) = dx;\
- T.plus(_x0, _dv, _xo);\
- _J.col(i) = (_xo - _x0)/dx; \
- } \
-}
+#define JAC_NUMERIC(T, _x0, _J, dx) \
+ { \
+ VectorXd dv(3); \
+ Map<const VectorXd> _dv(dv.data(), 3); \
+ VectorXd xo(4); \
+ Map<VectorXd> _xo(xo.data(), 4); \
+ for (int i = 0; i < 3; i++) \
+ { \
+ dv.setZero(); \
+ dv(i) = dx; \
+ T.plus(_x0, _dv, _xo); \
+ _J.col(i) = (_xo - _x0) / dx; \
+ } \
+ }
using namespace Eigen;
using namespace std;
@@ -50,40 +49,39 @@ using namespace wolf;
TEST(TestLocalParametrization, QuaternionLocal)
{
-
// Storage
VectorXd x_storage(22);
- MatrixXd M_storage(1,12); // matrix dimensions do not matter, just storage size.
+ MatrixXd M_storage(1, 12); // matrix dimensions do not matter, just storage size.
x_storage.setRandom();
M_storage.setZero();
// QUATERNION ------------------------------------------
- Map<VectorXd> q(&x_storage(0),4);
+ Map<VectorXd> q(&x_storage(0), 4);
q.normalize();
- Map<VectorXd> da(&x_storage(4),3);
+ Map<VectorXd> da(&x_storage(4), 3);
da /= 10.0;
- Map<VectorXd> qo_m(&x_storage(7),4);
- Map<MatrixRowXd> J(&M_storage(0,0),4,3);
- MatrixXd J_num(4,3);
+ Map<VectorXd> qo_m(&x_storage(7), 4);
+ Map<MatrixRowXd> J(&M_storage(0, 0), 4, 3);
+ MatrixXd J_num(4, 3);
LocalParametrizationQuaternion<DQ_LOCAL> Qpar_loc;
- Map<const VectorXd> q_m(q.data(),4);
- Map<const VectorXd> da_m(da.data(),3);
+ Map<const VectorXd> q_m(q.data(), 4);
+ Map<const VectorXd> da_m(da.data(), 3);
- Qpar_loc.plus(q_m,da_m,qo_m);
+ Qpar_loc.plus(q_m, da_m, qo_m);
ASSERT_DOUBLE_EQ(qo_m.norm(), 1);
Quaterniond qref = Map<Quaterniond>(q.data()) * wolf::v2q(da);
- ASSERT_QUATERNION_VECTOR_APPROX(qo_m,qref.coeffs(),Constants::EPS);
+ ASSERT_QUATERNION_VECTOR_APPROX(qo_m, qref.coeffs(), Constants::EPS);
- Qpar_loc.computeJacobian(q_m,J);
+ Qpar_loc.computeJacobian(q_m, J);
JAC_NUMERIC(Qpar_loc, q_m, J_num, 1e-9)
- ASSERT_NEAR((J-J_num).norm(), 0, 1e-6);
+ ASSERT_NEAR((J - J_num).norm(), 0, 1e-6);
Map<const VectorXd> qoc_m(qo_m.data(), 4);
Map<VectorXd> da2_m(x_storage.data() + 10, 3);
@@ -91,44 +89,43 @@ TEST(TestLocalParametrization, QuaternionLocal)
Qpar_loc.minus(q_m, qoc_m, da2_m);
ASSERT_MATRIX_APPROX(da_m, da2_m, 1e-10);
-
}
TEST(TestLocalParametrization, QuaternionGlobal)
{
// Storage
VectorXd x_storage(22);
- MatrixXd M_storage(1,12); // matrix dimensions do not matter, just storage size.
+ MatrixXd M_storage(1, 12); // matrix dimensions do not matter, just storage size.
x_storage.setRandom();
M_storage.setZero();
// QUATERNION ------------------------------------------
- Map<VectorXd> q(&x_storage(0),4);
+ Map<VectorXd> q(&x_storage(0), 4);
q.normalize();
- Map<VectorXd> da(&x_storage(4),3);
+ Map<VectorXd> da(&x_storage(4), 3);
da /= 10.0;
- Map<VectorXd> qo_m(&x_storage(7),4);
- Map<MatrixRowXd> J(&M_storage(0,0),4,3);
- MatrixXd J_num(4,3);
+ Map<VectorXd> qo_m(&x_storage(7), 4);
+ Map<MatrixRowXd> J(&M_storage(0, 0), 4, 3);
+ MatrixXd J_num(4, 3);
LocalParametrizationQuaternion<DQ_GLOBAL> Qpar_glob;
- Map<const VectorXd> q_m(q.data(),4);
- Map<const VectorXd> da_m(da.data(),3);
+ Map<const VectorXd> q_m(q.data(), 4);
+ Map<const VectorXd> da_m(da.data(), 3);
- Qpar_glob.plus(q_m,da_m,qo_m);
+ Qpar_glob.plus(q_m, da_m, qo_m);
ASSERT_DOUBLE_EQ(qo_m.norm(), 1);
- Quaterniond qref = wolf::v2q(da) * Map<Quaterniond>(q.data());
- ASSERT_QUATERNION_VECTOR_APPROX(qo_m,qref.coeffs(),Constants::EPS);
+ Quaterniond qref = wolf::v2q(da) * Map<Quaterniond>(q.data());
+ ASSERT_QUATERNION_VECTOR_APPROX(qo_m, qref.coeffs(), Constants::EPS);
- Qpar_glob.computeJacobian(q_m,J);
+ Qpar_glob.computeJacobian(q_m, J);
JAC_NUMERIC(Qpar_glob, q_m, J_num, 1e-9)
- ASSERT_NEAR((J-J_num).norm(), 0, 1e-6);
+ ASSERT_NEAR((J - J_num).norm(), 0, 1e-6);
Map<const VectorXd> qoc_m(qo_m.data(), 4);
Map<VectorXd> da2_m(x_storage.data() + 10, 3);
@@ -136,37 +133,36 @@ TEST(TestLocalParametrization, QuaternionGlobal)
Qpar_glob.minus(q_m, qoc_m, da2_m);
ASSERT_MATRIX_APPROX(da_m, da2_m, 1e-10);
-
}
TEST(TestLocalParametrization, Homogeneous)
{
// Storage
VectorXd x_storage(22);
- MatrixXd M_storage(1,12); // matrix dimensions do not matter, just storage size.
+ MatrixXd M_storage(1, 12); // matrix dimensions do not matter, just storage size.
// HOMOGENEOUS ----------------------------------------
- Map<VectorXd> h(&x_storage(11),4);
+ Map<VectorXd> h(&x_storage(11), 4);
h.setRandom();
- Map<VectorXd> d(&x_storage(15),3);
- d << .1,.2,.3;
- Map<VectorXd> ho_m(&x_storage(18),4);
- Map<MatrixRowXd> J(&M_storage(0,0),4,3);
- MatrixXd J_num(4,3);
+ Map<VectorXd> d(&x_storage(15), 3);
+ d << .1, .2, .3;
+ Map<VectorXd> ho_m(&x_storage(18), 4);
+ Map<MatrixRowXd> J(&M_storage(0, 0), 4, 3);
+ MatrixXd J_num(4, 3);
LocalParametrizationHomogeneous Hpar;
- Map<const VectorXd> h_m(h.data(),4);
- Map<const VectorXd> d_m(d.data(),3);
+ Map<const VectorXd> h_m(h.data(), 4);
+ Map<const VectorXd> d_m(d.data(), 3);
- Hpar.plus(h_m,d_m,ho_m);
+ Hpar.plus(h_m, d_m, ho_m);
ASSERT_DOUBLE_EQ(ho_m.norm(), h.norm());
- Hpar.computeJacobian(h_m,J);
+ Hpar.computeJacobian(h_m, J);
JAC_NUMERIC(Hpar, h_m, J_num, 1e-9)
- ASSERT_NEAR((J-J_num).norm(), 0, 1e-6);
+ ASSERT_NEAR((J - J_num).norm(), 0, 1e-6);
Map<const VectorXd> hoc_m(ho_m.data(), 4);
Map<VectorXd> d2_m(x_storage.data() + 10, 3);
@@ -174,12 +170,10 @@ TEST(TestLocalParametrization, Homogeneous)
Hpar.minus(h_m, hoc_m, d2_m);
ASSERT_MATRIX_APPROX(d_m, d2_m, 1e-10);
-
}
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_logging.cpp b/test/gtest_logging.cpp
index 5b6add59afcf6e269b5eb2e9e4badd7483071c47..0f843330a5db975fefe2fc6ce7d436aefa715a3f 100644
--- a/test/gtest_logging.cpp
+++ b/test/gtest_logging.cpp
@@ -21,7 +21,6 @@
#include "core/common/wolf.h"
#include "core/utils/utils_gtest.h"
-
TEST(logging, info)
{
WOLF_INFO("test info ", 5, " ", 0.123);
@@ -49,7 +48,6 @@ TEST(logging, debug)
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_make_posdef.cpp b/test/gtest_make_posdef.cpp
index 40bf058829cb85e641b0024be5bb37e6df959ca3..1fc54de9fd095d5057557092ef0de56fc6dd13d8 100644
--- a/test/gtest_make_posdef.cpp
+++ b/test/gtest_make_posdef.cpp
@@ -27,7 +27,7 @@ using namespace wolf;
TEST(MakePosDefTest, OkTest)
{
- MatrixXd M = MatrixXd::Identity(3,3);
+ MatrixXd M = MatrixXd::Identity(3, 3);
EXPECT_TRUE(isCovariance(M));
EXPECT_FALSE(makePosDef(M));
@@ -35,7 +35,7 @@ TEST(MakePosDefTest, OkTest)
TEST(MakePosDefTest, FixTest)
{
- MatrixXd M = MatrixXd::Zero(3,3);
+ MatrixXd M = MatrixXd::Zero(3, 3);
EXPECT_FALSE(isCovariance(M));
EXPECT_TRUE(makePosDef(M));
@@ -44,6 +44,6 @@ TEST(MakePosDefTest, FixTest)
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_map_yaml.cpp b/test/gtest_map_yaml.cpp
index d811cacc9977e1491e637a19caad5b9dd8893d3f..51a8aa9eb3dbd48ea504138b63660c80f206f4ef 100644
--- a/test/gtest_map_yaml.cpp
+++ b/test/gtest_map_yaml.cpp
@@ -45,11 +45,11 @@ Vector3d p3_3d = Vector3d::Random();
Vector4d o2_3d = Vector4d::Random().normalized();
Vector4d o3_3d = Vector4d::Random().normalized();
-int int_param = (int) (Vector1d::Random()(0) * 1e4);
+int int_param = (int)(Vector1d::Random()(0) * 1e4);
double double_param = Vector1d::Random()(0) * 1e4;
std::string wolf_dir = _WOLF_CODE_DIR;
-std::string map_path = wolf_dir + "/test/yaml/maps";
+std::string map_path = wolf_dir + "/test/yaml/maps";
TEST(MapYaml, save_2d)
{
@@ -66,7 +66,7 @@ TEST(MapYaml, save_2d)
LandmarkBase::emplace<LandmarkDummy>(problem->getMap(), p3_sb, o3_sb, int_param, double_param);
// Check Problem functions
- std::string filename = map_path + "/map_2d_problem.yaml";
+ std::string filename = map_path + "/map_2d_problem.yaml";
std::cout << "Saving map to file: " << filename << std::endl;
problem->saveMap(filename, "2d map saved from Problem::saveMap()");
@@ -84,7 +84,7 @@ TEST(MapYaml, load_2d)
for (auto map : maps)
{
- std::string filename = map_path + "/" + map;
+ std::string filename = map_path + "/" + map;
std::cout << "Loading map to file: " << filename << std::endl;
problem->loadMap(filename);
@@ -130,8 +130,7 @@ TEST(MapYaml, load_2d)
// empty the map
{
auto lmk_list = problem->getMap()->getLandmarkList();
- for (auto lmk : lmk_list)
- lmk->remove();
+ for (auto lmk : lmk_list) lmk->remove();
}
ASSERT_TRUE(problem->getMap()->getLandmarkList().empty());
}
@@ -152,7 +151,7 @@ TEST(MapYaml, save_3d)
LandmarkBase::emplace<LandmarkDummy>(problem->getMap(), p3_sb, o3_sb, int_param, double_param);
// Check Problem functions
- std::string filename = map_path + "/map_3d_problem.yaml";
+ std::string filename = map_path + "/map_3d_problem.yaml";
std::cout << "Saving map to file: " << filename << std::endl;
problem->saveMap(filename, "3d map saved from Problem::saveMap()");
@@ -165,12 +164,12 @@ TEST(MapYaml, save_3d)
TEST(MapYaml, load_3d)
{
ProblemPtr problem = Problem::create("PO", 3);
-
+
std::list<std::string> maps{"map_3d_problem.yaml", "map_3d_map.yaml"};
for (auto map : maps)
{
- std::string filename = map_path + "/" + map;
+ std::string filename = map_path + "/" + map;
std::cout << "Loading map to file: " << filename << std::endl;
problem->loadMap(filename);
@@ -218,8 +217,7 @@ TEST(MapYaml, load_3d)
// empty the map
{
auto lmk_list = problem->getMap()->getLandmarkList();
- for (auto lmk : lmk_list)
- lmk->remove();
+ for (auto lmk : lmk_list) lmk->remove();
}
ASSERT_TRUE(problem->getMap()->getLandmarkList().empty());
}
@@ -227,18 +225,18 @@ TEST(MapYaml, load_3d)
TEST(MapYaml, remove_map_files)
{
- std::string filename = map_path + "/map_2d_problem.yaml";
+ std::string filename = map_path + "/map_2d_problem.yaml";
ASSERT_TRUE(std::remove(filename.c_str()) == 0);
- filename = map_path + "/map_2d_map.yaml";
+ filename = map_path + "/map_2d_map.yaml";
ASSERT_TRUE(std::remove(filename.c_str()) == 0);
- filename = map_path + "/map_3d_problem.yaml";
+ filename = map_path + "/map_3d_problem.yaml";
ASSERT_TRUE(std::remove(filename.c_str()) == 0);
- filename = map_path + "/map_3d_map.yaml";
+ filename = map_path + "/map_3d_map.yaml";
ASSERT_TRUE(std::remove(filename.c_str()) == 0);
}
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_motion_buffer.cpp b/test/gtest_motion_buffer.cpp
index 3f2e06e16ac957b53431fb070bba83555d1198f4..c9ee303dea57d4486836de3effac3aca4b3d9472 100644
--- a/test/gtest_motion_buffer.cpp
+++ b/test/gtest_motion_buffer.cpp
@@ -17,16 +17,9 @@
//
// 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/>.
-/*
- * gtest_motion_buffer.cpp
- *
- * Created on: Nov 12, 2016
- * Author: jsola
- */
#include "core/utils/utils_gtest.h"
-
#include "core/processor/motion_buffer.h"
#include "core/common/wolf.h"
@@ -40,22 +33,23 @@ using namespace wolf;
Motion newMotion(TimeStamp t, double d, double D, double C, double J_d, double J_D)
{
Motion m(t, 1, 1, 1, 0);
- m.delta_(0) = d;
- m.delta_integr_(0) = D;
- m.delta_cov_(0) = C;
- m.jacobian_delta_(0,0) = J_d;
- m.jacobian_delta_integr_(0,0) = J_D;
+ m.delta_(0) = d;
+ m.delta_integr_(0) = D;
+ m.delta_cov_(0) = C;
+ m.jacobian_delta_(0, 0) = J_d;
+ m.jacobian_delta_integr_(0, 0) = J_D;
return m;
}
-namespace{
+namespace
+{
TimeStamp t0(0), t1(1), t2(2), t3(3), t4(4);
-Motion m0 = newMotion(t0, 0, 0 , 0, .1, 1); // ts, delta, Delta, delta_cov, J_delta, J_Delta
-Motion m1 = newMotion(t1, 1, 1 , 1, .1, 1);
-Motion m2 = newMotion(t2, 2, 3 , 1, .1, 1);
-Motion m3 = newMotion(t3, 3, 6 , 1, .1, 1);
-Motion m4 = newMotion(t4, 4, 10, 1, .1, 1);
-}
+Motion m0 = newMotion(t0, 0, 0, 0, .1, 1); // ts, delta, Delta, delta_cov, J_delta, J_Delta
+Motion m1 = newMotion(t1, 1, 1, 1, .1, 1);
+Motion m2 = newMotion(t2, 2, 3, 1, .1, 1);
+Motion m3 = newMotion(t3, 3, 6, 1, .1, 1);
+Motion m4 = newMotion(t4, 4, 10, 1, .1, 1);
+} // namespace
TEST(MotionBuffer, QueryTimeStamps)
{
@@ -65,20 +59,20 @@ TEST(MotionBuffer, QueryTimeStamps)
MB.push_back(m1);
TimeStamp t;
- t.set(-1); // t is older than m0.ts_ -> return m0
- ASSERT_EQ(MB.getMotion(t).ts_ , m0.ts_);
+ t.set(-1); // t is older than m0.ts_ -> return m0
+ ASSERT_EQ(MB.getMotion(t).ts_, m0.ts_);
- t.set( 0); // t is exactly m0.ts_ -> return m0
- ASSERT_EQ(MB.getMotion(t).ts_ , m0.ts_);
+ t.set(0); // t is exactly m0.ts_ -> return m0
+ ASSERT_EQ(MB.getMotion(t).ts_, m0.ts_);
- t.set(0.5); // t is between m0.ts_ and m1.ts_ -> return m0
- ASSERT_EQ(MB.getMotion(t).ts_ , m0.ts_);
+ t.set(0.5); // t is between m0.ts_ and m1.ts_ -> return m0
+ ASSERT_EQ(MB.getMotion(t).ts_, m0.ts_);
- t.set(+1); // t is exactly m1.ts_ -> return m1
- ASSERT_EQ(MB.getMotion(t).ts_ , m1.ts_);
+ t.set(+1); // t is exactly m1.ts_ -> return m1
+ ASSERT_EQ(MB.getMotion(t).ts_, m1.ts_);
- t.set(+2); // t is newer than m1.ts_ -> return m1
- ASSERT_EQ(MB.getMotion(t).ts_ , m1.ts_);
+ t.set(+2); // t is newer than m1.ts_ -> return m1
+ ASSERT_EQ(MB.getMotion(t).ts_, m1.ts_);
}
TEST(MotionBuffer, getMotion)
@@ -117,64 +111,64 @@ TEST(MotionBuffer, Split)
MB.push_back(m1);
MB.push_back(m2);
MB.push_back(m3);
- MB.push_back(m4); // put 5 motions
+ MB.push_back(m4); // put 5 motions
MotionBuffer MB_old;
- TimeStamp t = 1.5; // between m1 and m2
+ TimeStamp t = 1.5; // between m1 and m2
MB.split(t, MB_old);
- ASSERT_EQ(MB_old.size(), (unsigned int) 2);
- ASSERT_EQ(MB .size(), (unsigned int) 3);
+ ASSERT_EQ(MB_old.size(), (unsigned int)2);
+ ASSERT_EQ(MB.size(), (unsigned int)3);
ASSERT_EQ(MB_old.getMotion(t1).ts_, t1);
- ASSERT_EQ(MB_old.getMotion(t2).ts_, t1); // last ts is t1
- ASSERT_EQ(MB .getMotion(t1).ts_, t2); // first ts is t2
- ASSERT_EQ(MB .getMotion(t2).ts_, t2);
+ ASSERT_EQ(MB_old.getMotion(t2).ts_, t1); // last ts is t1
+ ASSERT_EQ(MB.getMotion(t1).ts_, t2); // first ts is t2
+ ASSERT_EQ(MB.getMotion(t2).ts_, t2);
}
// TEST(MotionBuffer, integrateCovariance)
// {
// MotionBuffer MB;
-//
+//
// MB.push_back(m0);
// MB.push_back(m1);
// MB.push_back(m2);
// MB.push_back(m3);
// MB.push_back(m4); // put 5 motions
-//
+//
// Eigen::MatrixXd cov = MB.integrateCovariance();
// ASSERT_NEAR(cov(0), 0.04, 1e-8);
-//
+//
// }
-//
+//
// TEST(MotionBuffer, integrateCovariance_ts)
// {
// MotionBuffer MB;
-//
+//
// MB.push_back(m0);
// MB.push_back(m1);
// MB.push_back(m2);
// MB.push_back(m3);
// MB.push_back(m4); // put 5 motions
-//
+//
// Eigen::MatrixXd cov = MB.integrateCovariance(t2);
// ASSERT_NEAR(cov(0), 0.02, 1e-8);
// }
-//
+//
// TEST(MotionBuffer, integrateCovariance_ti_tf)
// {
// MotionBuffer MB;
-//
+//
// MB.push_back(m0);
// MB.push_back(m1);
// MB.push_back(m2);
// MB.push_back(m3);
// MB.push_back(m4); // put 5 motions
-//
+//
// Eigen::MatrixXd cov = MB.integrateCovariance(t1, t3);
// ASSERT_NEAR(cov(0), 0.03, 1e-8);
-//
+//
// cov = MB.integrateCovariance(t0, t3); // first delta_cov is zero so it does not integrate
// ASSERT_NEAR(cov(0), 0.03, 1e-8);
// }
@@ -188,17 +182,16 @@ TEST(MotionBuffer, print)
MB.push_back(m2);
MB.print();
- MB.print(0,0,0,0);
- MB.print(1,0,0,0);
- MB.print(0,1,0,0);
- MB.print(0,0,1,0);
- MB.print(0,0,0,1);
- MB.print(1,1,1,1);
+ MB.print(0, 0, 0, 0);
+ MB.print(1, 0, 0, 0);
+ MB.print(0, 1, 0, 0);
+ MB.print(0, 0, 1, 0);
+ MB.print(0, 0, 0, 1);
+ MB.print(1, 1, 1, 1);
}
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_motion_provider.cpp b/test/gtest_motion_provider.cpp
index 5ae1c763553ae7f04564e919c807e9dc7dc112d1..51152122d45d6e607c4fd04cbf953434df831ec2 100644
--- a/test/gtest_motion_provider.cpp
+++ b/test/gtest_motion_provider.cpp
@@ -18,8 +18,9 @@
// 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/>.
-//Wolf
+// Wolf
#include "dummy/processor_motion_provider_dummy.h"
+#include "dummy/processor_motion_provider_dummy_pov.h"
#include "core/processor/motion_provider.h"
#include "core/utils/utils_gtest.h"
@@ -35,58 +36,43 @@
using namespace wolf;
using namespace Eigen;
-
class MotionProviderTest : public testing::Test
{
- public:
- ProblemPtr problem;
- SensorBasePtr sen;
- ProcessorBasePtr prc1, prc2, prc3;
- MotionProviderPtr im1, im2, im3;
-
- std::string wolf_dir = _WOLF_CODE_DIR;
- double dt = 0.01;
-
- void SetUp() override
- {
- // Wolf problem
- problem = Problem::create("POV", 2);
-
- // Install odom sensor
- sen = problem->installSensor("SensorOdom2d",
- wolf_dir + "/test/yaml/sensor_odom_2d.yaml",
+ public:
+ ProblemPtr problem;
+ SensorBasePtr sen;
+ ProcessorBasePtr prc1, prc2, prc3;
+ MotionProviderPtr mp1, mp2, mp3;
+
+ std::string wolf_dir = _WOLF_CODE_DIR;
+
+ void SetUp() override
+ {
+ // Wolf problem
+ problem = Problem::create("POV", 2);
+
+ // Install odom sensor
+ sen = problem->installSensor("SensorOdom2d", wolf_dir + "/test/yaml/sensor_odom_2d.yaml", {wolf_dir});
+
+ // Install 3 odom processors
+ prc1 = problem->installProcessor("ProcessorMotionProviderDummy",
+ sen,
+ wolf_dir + "/test/yaml/processor_motion_provider_dummy1.yaml",
+ {wolf_dir});
+ mp1 = std::dynamic_pointer_cast<MotionProvider>(prc1);
+
+ prc2 = problem->installProcessor("ProcessorMotionProviderDummy",
+ sen,
+ wolf_dir + "/test/yaml/processor_motion_provider_dummy2.yaml",
{wolf_dir});
+ mp2 = std::dynamic_pointer_cast<MotionProvider>(prc2);
- // Install 3 odom processors
- ParamsMotionProviderDummyPtr prc1_params = std::make_shared<ParamsMotionProviderDummy>();
- prc1_params->time_tolerance = dt/2;
- prc1_params->state_types = {{'P',"StatePoint2d"},{'O',"StateAngle"}};
- prc1_params->state_provider = false;
- prc1_params->name = "not getter processor";
-
- prc1 = ProcessorBase::emplace<MotionProviderDummy>(sen, prc1_params);
- im1 = std::dynamic_pointer_cast<MotionProvider>(prc1);
-
- ParamsMotionProviderDummyPtr prc2_params = std::make_shared<ParamsMotionProviderDummy>();
- prc2_params->time_tolerance = dt/2;
- prc2_params->state_types = {{'P',"StatePoint2d"},{'O',"StateAngle"}};
- prc2_params->state_provider = true;
- prc2_params->state_provider_order = 1;
- prc1_params->name = "getter processor";
-
- prc2 = ProcessorBase::emplace<MotionProviderDummy>(sen, prc2_params);
- im2 = std::dynamic_pointer_cast<MotionProvider>(prc2);
-
- ParamsMotionProviderDummyPtr prc3_params = std::make_shared<ParamsMotionProviderDummy>();
- prc3_params->time_tolerance = dt/2;
- prc3_params->state_types = {{'P',"StatePoint2d"},{'O',"StateAngle"},{'V',"StateVector2d"}};
- prc3_params->state_provider = true;
- prc3_params->state_provider_order = 1;
- prc1_params->name = "getter processor lower priority";
-
- prc3 = ProcessorBase::emplace<MotionProviderDummy>(sen, prc3_params);
- im3 = std::dynamic_pointer_cast<MotionProvider>(prc3);
- }
+ prc3 = problem->installProcessor("ProcessorMotionProviderDummyPOV",
+ sen,
+ wolf_dir + "/test/yaml/processor_motion_provider_dummy3.yaml",
+ {wolf_dir});
+ mp3 = std::dynamic_pointer_cast<MotionProvider>(prc3);
+ }
};
/*
@@ -102,68 +88,68 @@ class MotionProviderTest : public testing::Test
TEST_F(MotionProviderTest, install)
{
// All MotionProvider() = true
- ASSERT_TRUE (prc1->isMotionProvider());
- ASSERT_TRUE (prc2->isMotionProvider());
- ASSERT_TRUE (prc3->isMotionProvider());
- ASSERT_TRUE (im1 != nullptr);
- ASSERT_TRUE (im2 != nullptr);
- ASSERT_TRUE (im3 != nullptr);
+ ASSERT_TRUE(prc1->isMotionProvider());
+ ASSERT_TRUE(prc2->isMotionProvider());
+ ASSERT_TRUE(prc3->isMotionProvider());
+ ASSERT_TRUE(mp1 != nullptr);
+ ASSERT_TRUE(mp2 != nullptr);
+ ASSERT_TRUE(mp3 != nullptr);
// well configured
- ASSERT_FALSE(im1->isStateGetter());
- ASSERT_TRUE(im2->isStateGetter());
- ASSERT_TRUE(im3->isStateGetter());
- ASSERT_EQ(im2->getOrder(), 1);
- ASSERT_EQ(im3->getOrder(), 2); // If duplicated priority, 2nd is changed to +1 priority. A WARN should be raised.
- ASSERT_TRUE(im1->getStateTypes().has('P'));
- ASSERT_TRUE(im1->getStateTypes().has('O'));
- ASSERT_FALSE(im1->getStateTypes().has('V'));
- ASSERT_TRUE(im2->getStateTypes().has('P'));
- ASSERT_TRUE(im2->getStateTypes().has('O'));
- ASSERT_FALSE(im2->getStateTypes().has('V'));
- ASSERT_TRUE(im3->getStateTypes().has('P'));
- ASSERT_TRUE(im3->getStateTypes().has('O'));
- ASSERT_TRUE(im3->getStateTypes().has('V'));
+ ASSERT_FALSE(mp1->isStateGetter());
+ ASSERT_TRUE(mp2->isStateGetter());
+ ASSERT_TRUE(mp3->isStateGetter());
+ ASSERT_EQ(mp2->getOrder(), 1);
+ ASSERT_EQ(mp3->getOrder(), 2); // If duplicated priority, 2nd is changed to +1 priority. A WARN should be raised.
+ ASSERT_TRUE(mp1->getStateTypes().has('P'));
+ ASSERT_TRUE(mp1->getStateTypes().has('O'));
+ ASSERT_FALSE(mp1->getStateTypes().has('V'));
+ ASSERT_TRUE(mp2->getStateTypes().has('P'));
+ ASSERT_TRUE(mp2->getStateTypes().has('O'));
+ ASSERT_FALSE(mp2->getStateTypes().has('V'));
+ ASSERT_TRUE(mp3->getStateTypes().has('P'));
+ ASSERT_TRUE(mp3->getStateTypes().has('O'));
+ ASSERT_TRUE(mp3->getStateTypes().has('V'));
// Only 2 and 3 in problem::motion_provider_map_
ASSERT_EQ(problem->getMotionProviderMap().size(), 2);
- ASSERT_EQ(problem->getMotionProviderMap().begin()->second, im2);
- ASSERT_EQ(std::next(problem->getMotionProviderMap().begin())->second, im3);
+ ASSERT_EQ(problem->getMotionProviderMap().begin()->second, mp2);
+ ASSERT_EQ(std::next(problem->getMotionProviderMap().begin())->second, mp3);
}
TEST_F(MotionProviderTest, odometry)
{
- VectorComposite odom_p("P",{Vector2d::Zero()});
- VectorComposite odom_pov("POV",{Vector2d::Zero(),Vector1d::Zero(),Vector2d::Zero()});
+ VectorComposite odom_p("P", {Vector2d::Zero()});
+ VectorComposite odom_pov("POV", {Vector2d::Zero(), Vector1d::Zero(), Vector2d::Zero()});
// Error: required PO keys to be added
- ASSERT_DEATH({im1->setOdometry(odom_p);},"");
- im1->setOdometry(odom_pov);
- im2->setOdometry(odom_pov);
- im3->setOdometry(odom_pov);
-
- // im1 ->set odom = 0, 0, 0
- VectorComposite odom1("PO",{Vector2d::Zero(),Vector1d::Zero()});
- im1->setOdometry(odom1);
- auto odom1_get = im1->getOdometry();
+ ASSERT_DEATH({ mp1->setOdometry(odom_p); }, "");
+ mp1->setOdometry(odom_pov);
+ mp2->setOdometry(odom_pov);
+ mp3->setOdometry(odom_pov);
+
+ // mp1 ->set odom = 0, 0, 0
+ VectorComposite odom1("PO", {Vector2d::Zero(), Vector1d::Zero()});
+ mp1->setOdometry(odom1);
+ auto odom1_get = mp1->getOdometry();
EXPECT_TRUE(odom1_get.count('P') == 1);
EXPECT_TRUE(odom1_get.count('O') == 1);
EXPECT_MATRIX_APPROX(odom1_get.at('P'), odom1.at('P'), 1e-9);
EXPECT_MATRIX_APPROX(odom1_get.at('O'), odom1.at('O'), 1e-9);
- // im1 ->set odom = 1, 1, 1
- VectorComposite odom2("PO",{Vector2d::Ones(),Vector1d::Ones()});
- im2->setOdometry(odom2);
- auto odom2_get = im2->getOdometry();
+ // mp1 ->set odom = 1, 1, 1
+ VectorComposite odom2("PO", {Vector2d::Ones(), Vector1d::Ones()});
+ mp2->setOdometry(odom2);
+ auto odom2_get = mp2->getOdometry();
EXPECT_TRUE(odom2_get.count('P') == 1);
EXPECT_TRUE(odom2_get.count('O') == 1);
EXPECT_MATRIX_APPROX(odom2_get.at('P'), odom2.at('P'), 1e-9);
EXPECT_MATRIX_APPROX(odom2_get.at('O'), odom2.at('O'), 1e-9);
- // im1 ->set odom = 2, 2, 2, 2, 2
- VectorComposite odom3("POV",{2 * Vector2d::Ones(), 2 * Vector1d::Ones(), 2 * Vector2d::Ones()});
- im3->setOdometry(odom3);
- auto odom3_get = im3->getOdometry();
+ // mp1 ->set odom = 2, 2, 2, 2, 2
+ VectorComposite odom3("POV", {2 * Vector2d::Ones(), 2 * Vector1d::Ones(), 2 * Vector2d::Ones()});
+ mp3->setOdometry(odom3);
+ auto odom3_get = mp3->getOdometry();
EXPECT_TRUE(odom3_get.count('P') == 1);
EXPECT_TRUE(odom3_get.count('O') == 1);
EXPECT_TRUE(odom3_get.count('V') == 1);
@@ -171,7 +157,7 @@ TEST_F(MotionProviderTest, odometry)
EXPECT_MATRIX_APPROX(odom3_get.at('O'), odom3.at('O'), 1e-9);
EXPECT_MATRIX_APPROX(odom3_get.at('V'), odom3.at('V'), 1e-9);
- // problem::getOdometry(): by priority P and O should come from im2 and V from im3
+ // problem::getOdometry(): by priority P and O should come from mp2 and V from mp3
auto odom_get = problem->getOdometry();
EXPECT_TRUE(odom_get.count('P') == 1);
EXPECT_TRUE(odom_get.count('O') == 1);
@@ -180,7 +166,7 @@ TEST_F(MotionProviderTest, odometry)
EXPECT_MATRIX_APPROX(odom_get.at('O'), odom2.at('O'), 1e-9);
EXPECT_MATRIX_APPROX(odom_get.at('V'), odom3.at('V'), 1e-9);
- // problem::getState(): by priority P and O should come from im2 and V from im3
+ // problem::getState(): by priority P and O should come from mp2 and V from mp3
auto state_get = problem->getState();
EXPECT_TRUE(state_get.count('P') == 1);
EXPECT_TRUE(state_get.count('O') == 1);
@@ -192,6 +178,6 @@ TEST_F(MotionProviderTest, odometry)
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_node_state_blocks.cpp b/test/gtest_node_state_blocks.cpp
index db73f71bffa8ed7285a7e3da8601a81127aa2988..24329ff6e48323b8a09fcc92206a482e2c709bc2 100644
--- a/test/gtest_node_state_blocks.cpp
+++ b/test/gtest_node_state_blocks.cpp
@@ -122,9 +122,9 @@ TEST(NodeStateBlocksTest, constructor_specs)
ASSERT_FALSE(N->isFrame());
ASSERT_FALSE(N->isLandmark());
ASSERT_FALSE(N->isSensor());
-
+
// apply priors
- N->applyPriors(specs);
+ N->emplacePriors(specs);
checkNode(N, 'P', p_state, true, MatrixXd(0, 0));
checkNode(N, 'O', o_state, false, o_cov);
@@ -165,8 +165,11 @@ TEST(NodeStateBlocksTest, constructor_types_vectors)
ASSERT_FALSE(N->isSensor());
// apply priors
- vectors = VectorComposite({{'P', Vector3d::Zero()}, {'O', Quaterniond::Identity().coeffs()}, {'I', Vector5d::Zero()}, {'A', Vector1d::Zero()}});
- N->applyPriors(SpecStateComposite(types, vectors, true)); // changes states and fix
+ vectors = VectorComposite({{'P', Vector3d::Zero()},
+ {'O', Quaterniond::Identity().coeffs()},
+ {'I', Vector5d::Zero()},
+ {'A', Vector1d::Zero()}});
+ N->emplacePriors(SpecStateComposite(types, vectors, true)); // changes states and fix
checkNode(N, 'P', Vector3d::Zero(), true, MatrixXd(0, 0));
checkNode(N, 'O', Quaterniond::Identity().coeffs(), true, MatrixXd(0, 0));
@@ -217,12 +220,6 @@ TEST_F(NodeStateBlocksDerivedTest, Notifications_add_makeKF)
ASSERT_FALSE(problem->getStateBlockNotification(sbp0, n));
- // F0->link(problem->getTrajectory());
-
- // ASSERT_FALSE(problem->getStateBlockNotification(sbp0, n));
-
- // ASSERT_FALSE(problem->getStateBlockNotification(sbv0, n));
-
F0->addStateBlock('V', sbv0);
ASSERT_FALSE(problem->getStateBlockNotification(sbv0, n));
@@ -242,7 +239,6 @@ TEST_F(NodeStateBlocksDerivedTest, Notifications_makeKF_add)
// first make KF, then add SB
- // F1->link(problem->getTrajectory());
F1->link(problem);
F1->addStateBlock('P', sbp1);
@@ -262,7 +258,7 @@ TEST_F(NodeStateBlocksDerivedTest, Notifications_makeKF_add)
TEST_F(NodeStateBlocksDerivedTest, Add_solve_notify_solve_add)
{
- SolverCeresPtr solver = std::make_shared<SolverCeres>(problem);
+ auto solver = SolverCeres::create(problem, wolf_dir + "/test/yaml/solver_ceres.yaml", {wolf_dir});
Notification n;
diff --git a/test/gtest_problem.cpp b/test/gtest_problem.cpp
index 96e662876c2bb87ad1ad490e87d71b6941b55421..c5bab34b3f523a624726e93de72757a4f8797dd7 100644
--- a/test/gtest_problem.cpp
+++ b/test/gtest_problem.cpp
@@ -27,7 +27,7 @@
#include "core/processor/processor_odom_3d.h"
#include "dummy/processor_tracker_feature_dummy.h"
#include "core/solver/solver_manager.h"
-#include "dummy/solver_manager_dummy.h"
+#include "dummy/solver_dummy.h"
#include "core/sensor/sensor_diff_drive.h"
#include "core/processor/processor_diff_drive.h"
#include "core/capture/capture_diff_drive.h"
@@ -68,13 +68,9 @@ TEST(Problem, Sensors)
ProblemPtr P = Problem::create("POV", 3);
// add a dummy sensor
- auto params = std::make_shared<ParamsSensorDummy>();
- params->name = "dummy_name";
- auto S = SensorBase::emplace<SensorDummy3d>(
- P->getHardware(),
- params,
- SpecStateSensorComposite({{'P', SpecStateSensor("StatePoint3d", Vector3d::Zero(), "fix")},
- {'O', SpecStateSensor("StateQuaternion", Vector4d::Random().normalized(), "fix")}}));
+ auto S = SensorBase::emplace<SensorDummy3d>(
+ P->getHardware(), YAML::LoadFile(wolf_dir + "/test/yaml/sensor_tests/sensor_PO_3D_fix.yaml"), {wolf_dir});
+
// check pointers
ASSERT_EQ(P, S->getProblem());
ASSERT_EQ(P->getHardware(), S->getHardware());
@@ -88,16 +84,12 @@ TEST(Problem, Processor)
ASSERT_TRUE(P->getMotionProviderMap().empty());
// add a motion sensor and processor
- auto params = std::make_shared<ParamsSensorOdom>();
- params->name = "dummy_name";
- auto Sm = SensorBase::emplace<SensorOdom3d>(
- P->getHardware(),
- params,
- SpecStateSensorComposite({{'P', SpecStateSensor("StatePoint3d", Vector3d::Zero(), "fix")},
- {'O', SpecStateSensor("StateQuaternion", Vector4d::Random().normalized(), "fix")}}));
+ auto Sm = SensorBase::emplace<SensorOdom3d>(
+ P->getHardware(), YAML::LoadFile(wolf_dir + "/test/yaml/sensor_odom_3d.yaml"), {wolf_dir});
// add motion processor
- auto Pm = ProcessorBase::emplace<ProcessorOdom3d>(Sm, std::make_shared<ParamsProcessorOdom3d>());
+ auto Pm = ProcessorBase::emplace<ProcessorOdom3d>(
+ Sm, YAML::LoadFile(wolf_dir + "/test/yaml/processor_odom_3d.yaml"), {wolf_dir});
// check motion processor IS NOT by emplace
ASSERT_EQ(P->getMotionProviderMap().begin()->second, Pm);
@@ -106,7 +98,7 @@ TEST(Problem, Processor)
TEST(Problem, Installers)
{
std::string wolf_dir = _WOLF_CODE_DIR;
- ProblemPtr P = Problem::create("PO", 3);
+ ProblemPtr P = Problem::create("PO", 3);
Eigen::Vector7d xs;
xs.setRandom();
xs.tail(4).normalize();
@@ -305,10 +297,8 @@ TEST(Problem, StateBlocks)
SensorBasePtr Sm = P->installSensor("SensorOdom3d", wolf_dir + "/test/yaml/sensor_odom_3d.yaml", {wolf_dir});
ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd)2);
- auto pt = P->installProcessor("ProcessorTrackerFeatureDummy",
- Sm,
- wolf_dir + "/test/yaml/processor_tracker_feature_dummy.yaml",
- {wolf_dir});
+ auto pt = P->installProcessor(
+ "ProcessorTrackerFeatureDummy", Sm, wolf_dir + "/test/yaml/processor_tracker_feature_dummy.yaml", {wolf_dir});
auto pm = P->installProcessor("ProcessorOdom3d", Sm, wolf_dir + "/test/yaml/processor_odom_3d.yaml", {wolf_dir});
// 2 state blocks, estimated
@@ -332,7 +322,7 @@ TEST(Problem, StateBlocks)
P->print(4, 1, 1, 1);
// consume notifications
- SolverManagerDummyPtr SM = std::make_shared<SolverManagerDummy>(P);
+ SolverManagerPtr SM = SolverDummy::create(P, wolf_dir + "/test/yaml/solver_dummy.yaml", {wolf_dir});
ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd)(2 + 2));
SM->update(); // calls P->consumeNotifications();
ASSERT_EQ(P->getStateBlockNotificationMapSize(),
@@ -364,18 +354,10 @@ TEST(Problem, Covariances)
Eigen::Vector3d xs2d;
SensorBasePtr Sm = P->installSensor("SensorOdom3d", wolf_dir + "/test/yaml/sensor_odom_3d.yaml", {wolf_dir});
- SensorBasePtr St =
- P->installSensor("SensorOdom3d", wolf_dir + "/test/yaml/sensor_odom_3d_other.yaml", {wolf_dir});
-
- ParamsProcessorTrackerFeaturePtr params = std::make_shared<ParamsProcessorTrackerFeature>();
- params->time_tolerance = 0.1;
- params->max_new_features = 5;
- params->min_features_for_keyframe = 10;
-
- auto pt = P->installProcessor("ProcessorTrackerFeatureDummy",
- Sm,
- wolf_dir + "/test/yaml/processor_tracker_feature_dummy.yaml",
- {wolf_dir});
+ SensorBasePtr St = P->installSensor("SensorOdom3d", wolf_dir + "/test/yaml/sensor_odom_3d_other.yaml", {wolf_dir});
+
+ auto pt = P->installProcessor(
+ "ProcessorTrackerFeatureDummy", Sm, wolf_dir + "/test/yaml/processor_tracker_feature_dummy.yaml", {wolf_dir});
auto pm = P->installProcessor("ProcessorOdom3d", St, wolf_dir + "/test/yaml/processor_odom_3d.yaml", {wolf_dir});
// 4 state blocks, estimated
@@ -401,16 +383,8 @@ TEST(Problem, perturb)
auto problem = Problem::create("PO", 2);
// make a sensor first
- auto param = std::make_shared<ParamsSensorDiffDrive>();
- param->name = "sensor diff drive 1";
- param->ticks_per_wheel_revolution = 100;
- auto sensor = SensorBase::emplace<SensorDiffDrive>(
- problem->getHardware(),
- param,
- SpecStateSensorComposite(
- {{'P', SpecStateSensor("StatePoint2d", Vector2d::Zero(), "fix")},
- {'O', SpecStateSensor("StateAngle", Vector1d::Zero(), "fix")},
- {'I', SpecStateSensor("StateParams3", Vector3d::Ones(), "initial_guess", VectorXd(0), true)}}));
+ auto sensor = problem->installSensor(
+ "SensorDiffDrive", wolf_dir + "/test/yaml/sensor_diff_drive_gtest_problem.yaml", {wolf_dir});
Vector3d pose;
pose << 0, 0, 0;
@@ -488,24 +462,12 @@ TEST(Problem, check)
{
auto problem = Problem::create("PO", 2);
- // make a sensor first
- auto param = std::make_shared<ParamsSensorDiffDrive>();
- param->name = "sensor diff drive 1";
- param->ticks_per_wheel_revolution = 100;
-
- auto sensor = SensorBase::emplace<SensorDiffDrive>(
- problem->getHardware(),
- param,
- SpecStateSensorComposite(
- {{'P', SpecStateSensor("StatePoint2d", Vector2d::Zero(), "fix")},
- {'O', SpecStateSensor("StateAngle", Vector1d::Zero(), "fix")},
- {'I', SpecStateSensor("StateParams3", Vector3d::Ones(), "initial_guess", VectorXd(0), true)}}));
-
- auto sensor_lmk = SensorBase::emplace<SensorDummy2d>(
- problem->getHardware(),
- std::make_shared<ParamsSensorDummy>(),
- SpecStateSensorComposite({{'P', SpecStateSensor("StatePoint2d", Vector2d::Zero(), "fix")},
- {'O', SpecStateSensor("StateAngle", Vector1d::Zero(), "fix")}}));
+ // make sensors first
+ auto sensor = problem->installSensor(
+ "SensorDiffDrive", wolf_dir + "/test/yaml/sensor_diff_drive_gtest_problem.yaml", {wolf_dir});
+
+ auto sensor_lmk = problem->installSensor(
+ "SensorDummy2d", wolf_dir + "/test/yaml/sensor_tests/sensor_PO_2D_fix.yaml", {wolf_dir});
// landmarks
for (int l = 0; l < 5; l++)
@@ -640,7 +602,8 @@ TEST(Problem, check)
ASSERT_TRUE(problem->check(true, std::cout));
ASSERT_TRUE(L->isRemoving());
- // do not remove landmark automatically by removing all its factors (and respective features to allow the next test)
+ // do not remove landmark automatically by removing all its factors (and respective features to allow the next
+ // test)
L = problem->getMap()->getLandmarkList().front();
facs_lmk = L->getFactoredBySet();
for (auto fac : facs_lmk) fac->getFeature()->remove(false);
@@ -648,7 +611,7 @@ TEST(Problem, check)
ASSERT_TRUE(problem->check(true, std::cout));
ASSERT_FALSE(L->isRemoving());
- // remove all landmarks should remove all captures of sensor_lmk
+ // remove all landmarks should remove all captures of sensor_lmk
// (since they only have a feature that should be removed as well)
for (auto lmk : problem->getMap()->getLandmarkList()) lmk->remove(true);
diff --git a/test/gtest_processor_base.cpp b/test/gtest_processor_base.cpp
index 9c9e9571fe6f8e2ae78844f00f0551ebde81491a..a32fc468932f9a8a0c908c3ff1569fd68421893b 100644
--- a/test/gtest_processor_base.cpp
+++ b/test/gtest_processor_base.cpp
@@ -17,14 +17,8 @@
//
// 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/>.
-/*
- * gtest_capture_base.cpp
- *
- * Created on: Feb 15, 2018
- * Author: asantamaria
- */
-
-//Wolf
+
+// Wolf
#include "core/utils/utils_gtest.h"
#include "core/processor/processor_odom_2d.h"
@@ -45,97 +39,75 @@
using namespace wolf;
using namespace Eigen;
+std::string wolf_dir = _WOLF_CODE_DIR;
+
TEST(ProcessorBase, MotionProvider)
{
- using namespace wolf;
- using std::shared_ptr;
- using std::make_shared;
- using std::static_pointer_cast;
- using Eigen::Vector2d;
-
- std::string wolf_dir = _WOLF_CODE_DIR;
-
- double dt = 0.01;
-
// Wolf problem
ProblemPtr problem = Problem::create("PO", 2);
// Install tracker (sensor and processor)
- auto sens_trk = problem->installSensor("SensorDummy2d",
- wolf_dir + "/test/yaml/sensor_tests/sensor_PO_2D_fix.yaml",
- {wolf_dir});
- auto proc_trk_params = make_shared<ParamsProcessorTrackerFeatureDummy>();
- proc_trk_params->name = "proc tracker";
- auto proc_trk = ProcessorBase::emplace<ProcessorTrackerFeatureDummy>(sens_trk, proc_trk_params);
+ auto sens_trk = problem->installSensor(
+ "SensorDummy2d", wolf_dir + "/test/yaml/sensor_tests/sensor_PO_2D_fix.yaml", {wolf_dir});
+ auto proc_trk = problem->installProcessor("ProcessorTrackerFeatureDummy",
+ sens_trk,
+ wolf_dir + "/test/yaml/processor_tracker_feature_dummy.yaml",
+ {wolf_dir});
// Install odometer (sensor and processor)
- auto sens_odo = problem->installSensor("SensorOdom2d",
- wolf_dir + "/test/yaml/sensor_odom_2d.yaml",
- {wolf_dir});
- auto proc_odo_params = make_shared<ParamsProcessorOdom2d>();
- proc_odo_params->time_tolerance = dt/2;
- proc_odo_params->name = "odom processor";
- auto proc_odo = ProcessorBase::emplace<ProcessorOdom2d>(sens_odo, proc_odo_params);
+ auto sens_odo = problem->installSensor("SensorOdom2d", wolf_dir + "/test/yaml/sensor_odom_2d.yaml", {wolf_dir});
+ auto proc_odo = problem->installProcessor(
+ "ProcessorOdom2d", sens_trk, wolf_dir + "/test/yaml/processor_odom_2d.yaml", {wolf_dir});
ASSERT_FALSE(proc_trk->isMotionProvider());
- ASSERT_TRUE (proc_odo->isMotionProvider());
+ ASSERT_TRUE(proc_odo->isMotionProvider());
}
-
TEST(ProcessorBase, KeyFrameCallback)
{
-
- using namespace wolf;
- using std::shared_ptr;
- using std::make_shared;
- using std::static_pointer_cast;
- using Eigen::Vector2d;
-
- std::string wolf_dir = _WOLF_CODE_DIR;
-
- double dt = 0.01;
+ double dt = 1;
// Wolf problem
ProblemPtr problem = Problem::create("PO", 2);
// Install tracker (sensor and processor)
- auto sens_trk = problem->installSensor("SensorDummy2d",
- wolf_dir + "/test/yaml/sensor_tests/sensor_PO_2D_fix.yaml",
- {wolf_dir});
- auto proc_trk_params = make_shared<ParamsProcessorTrackerFeatureDummy>();
- proc_trk_params->name = "proc tracker";
- auto proc_trk = ProcessorBase::emplace<ProcessorTrackerFeatureDummy>(sens_trk, proc_trk_params);
+ auto sens_trk = problem->installSensor(
+ "SensorDummy2d", wolf_dir + "/test/yaml/sensor_tests/sensor_PO_2D_fix.yaml", {wolf_dir});
+ auto proc_trk = problem->installProcessor("ProcessorTrackerFeatureDummy",
+ sens_trk,
+ wolf_dir + "/test/yaml/processor_tracker_feature_dummy.yaml",
+ {wolf_dir});
+ proc_trk->setVotingActive(false);
// Install odometer (sensor and processor)
- auto sens_odo = problem->installSensor("SensorOdom2d",
- wolf_dir + "/test/yaml/sensor_odom_2d.yaml",
- {wolf_dir});
- auto proc_odo_params = make_shared<ParamsProcessorOdom2d>();
- proc_odo_params->time_tolerance = dt/2;
- proc_odo_params->name = "odom processor";
- auto proc_odo = ProcessorBase::emplace<ProcessorOdom2d>(sens_odo, proc_odo_params);
+ auto sens_odo = problem->installSensor("SensorOdom2d", wolf_dir + "/test/yaml/sensor_odom_2d.yaml", {wolf_dir});
+ auto proc_odo = problem->installProcessor(
+ "ProcessorOdom2d", sens_trk, wolf_dir + "/test/yaml/processor_odom_2d.yaml", {wolf_dir});
+ proc_odo->setTimeTolerance(dt / 2);
+ proc_odo->setVotingActive(false);
std::cout << "sensor & processor created and added to wolf problem" << std::endl;
// Sequence to test Key Frame creations (callback calls)
// initialize
- TimeStamp t(0.0);
- SpecStateComposite prior{{'P',SpecState("StatePoint2d",Vector2d(0,0),"factor",Vector2d(sqrt(0.1),sqrt(0.1)))},
- {'O',SpecState("StateAngle",Vector1d(0),"factor",Vector1d(sqrt(0.1)))}};
+ TimeStamp t(0.0);
+ SpecStateComposite prior{
+ {'P', SpecState("StatePoint2d", Vector2d(0, 0), "factor", Vector2d(sqrt(0.1), sqrt(0.1)))},
+ {'O', SpecState("StateAngle", Vector1d(0), "factor", Vector1d(sqrt(0.1)))}};
problem->setPrior(prior, t);
- CaptureOdom2dPtr capt_odo = make_shared<CaptureOdom2d>(t, sens_odo, Vector2d(0.5,0));
+ CaptureOdom2dPtr capt_odo = std::make_shared<CaptureOdom2d>(t, sens_odo, Vector2d(0.5, 0));
// Track
- CaptureVoidPtr capt_trk(make_shared<CaptureVoid>(t, sens_trk));
+ CaptureVoidPtr capt_trk(std::make_shared<CaptureVoid>(t, sens_trk));
proc_trk->captureCallback(capt_trk);
- for (size_t ii=0; ii<10; ii++ )
+ for (size_t ii = 0; ii < 10; ii++)
{
// Move
- t = t+dt;
- WOLF_INFO("----------------------- ts: ", t , " --------------------------");
+ t = t + dt;
+ WOLF_INFO("----------------------- ts: ", t, " --------------------------");
std::cout << "1\n";
capt_odo->setTimeStamp(t);
@@ -144,24 +116,30 @@ TEST(ProcessorBase, KeyFrameCallback)
std::cout << "3\n";
// Track
- capt_trk = make_shared<CaptureVoid>(t, sens_trk);
+ capt_trk = std::make_shared<CaptureVoid>(t, sens_trk);
std::cout << "4\n";
- problem->print(4,1,1,1, std::cout);
+ problem->print(4, 1, 1, 1, std::cout);
proc_trk->captureCallback(capt_trk);
std::cout << "5\n";
- problem->print(4,1,1,0);
+ // keyframe creation
+ if (ii == 5)
+ problem->emplaceFrame(t,
+ SpecStateComposite{{'P', SpecState("StatePoint2d", Vector2d(0, 0), "initial_guess")},
+ {'O', SpecState("StateAngle", Vector1d(0), "initial_guess")}});
+
+ problem->print(4, 1, 1, 0);
std::cout << "6\n";
// Only odom creating KFs
- ASSERT_EQ( problem->getLastFrame()->getKeys().size(),2);
- ASSERT_TRUE( problem->getLastFrame()->getKeys().find('P') != std::string::npos);
- ASSERT_TRUE( problem->getLastFrame()->getKeys().find('O') != std::string::npos);
+ ASSERT_EQ(problem->getLastFrame()->getKeys().size(), 2);
+ ASSERT_TRUE(problem->getLastFrame()->getKeys().find('P') != std::string::npos);
+ ASSERT_TRUE(problem->getLastFrame()->getKeys().find('O') != std::string::npos);
}
}
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_diff_drive.cpp b/test/gtest_processor_diff_drive.cpp
index c96c47e476d2f9f824987c063665d62e4f158e1f..563679fcdc3b0f7c400ed648a7ca9858226e587c 100644
--- a/test/gtest_processor_diff_drive.cpp
+++ b/test/gtest_processor_diff_drive.cpp
@@ -18,7 +18,7 @@
// 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/>.
-#include "core/processor/processor_diff_drive.h"
+#include "dummy/processor_diff_drive_mock.h"
#include "core/sensor/sensor_diff_drive.h"
#include "core/utils/utils_gtest.h"
@@ -26,124 +26,29 @@
using namespace wolf;
using namespace Eigen;
-
-WOLF_PTR_TYPEDEFS(ProcessorDiffDrivePublic);
-
-class ProcessorDiffDrivePublic : public ProcessorDiffDrive
-{
- using Base = ProcessorDiffDrive;
-
- public:
- ProcessorDiffDrivePublic(ParamsProcessorDiffDrivePtr _params_motion) : ProcessorDiffDrive(_params_motion)
- {
- //
- }
- void configure(SensorBasePtr _sensor) override
- {
- Base::configure(_sensor);
- }
- void computeCurrentDelta(const Eigen::VectorXd& _data,
- const Eigen::MatrixXd& _data_cov,
- const Eigen::VectorXd& _calib,
- const double _dt,
- Eigen::VectorXd& _delta,
- Eigen::MatrixXd& _delta_cov,
- Eigen::MatrixXd& _jacobian_calib) const override
- {
- Base::computeCurrentDelta(_data, _data_cov, _calib, _dt, _delta, _delta_cov, _jacobian_calib);
- }
-
- void deltaPlusDelta(const Eigen::VectorXd& _delta1,
- const Eigen::VectorXd& _delta2,
- const double _dt2,
- Eigen::VectorXd& _delta1_plus_delta2) const override
- {
- Base::deltaPlusDelta(_delta1, _delta2, _dt2, _delta1_plus_delta2);
- }
-
- void deltaPlusDelta(const Eigen::VectorXd& _delta1,
- const Eigen::VectorXd& _delta2,
- const double _dt2,
- Eigen::VectorXd& _delta1_plus_delta2,
- Eigen::MatrixXd& _jacobian1,
- Eigen::MatrixXd& _jacobian2) const override
- {
- Base::deltaPlusDelta(_delta1, _delta2, _dt2, _delta1_plus_delta2, _jacobian1, _jacobian2);
- }
-
- void statePlusDelta(const VectorComposite& _x,
- const Eigen::VectorXd& _delta,
- const double _dt,
- VectorComposite& _x_plus_delta) const override
- {
- Base::statePlusDelta(_x, _delta, _dt, _x_plus_delta);
- }
-
- Eigen::VectorXd deltaZero() const override
- {
- return Base::deltaZero();
- }
-
- CaptureMotionPtr emplaceCapture(const FrameBasePtr& _frame_own,
- const SensorBasePtr& _sensor,
- const TimeStamp& _ts,
- const VectorXd& _data,
- const MatrixXd& _data_cov,
- const VectorXd& _calib,
- const VectorXd& _calib_preint,
- const CaptureBasePtr& _capture_origin) override
- {
- return Base::emplaceCapture(
- _frame_own, _sensor, _ts, _data, _data_cov, _calib, _calib_preint, _capture_origin);
- }
-
- ParamsProcessorDiffDrivePtr getParams()
- {
- return params_prc_diff_drv_selfcal_;
- }
-
- double getRadPerTick()
- {
- return radians_per_tick_;
- }
-};
+std::string wolf_dir = _WOLF_CODE_DIR;
class ProcessorDiffDriveTest : public testing::Test
{
public:
- ParamsSensorDiffDrivePtr params_sen;
- SensorDiffDrivePtr sensor;
- ParamsProcessorDiffDrivePtr params;
- ProcessorDiffDrivePublicPtr processor;
- ProblemPtr problem;
+ SensorDiffDrivePtr sensor;
+ ProcessorDiffDriveMockPtr processor;
+ ProblemPtr problem;
void SetUp() override
{
problem = Problem::create("PO", 2);
- // make a sensor first
- params_sen = std::make_shared<ParamsSensorDiffDrive>();
- params_sen->name = "cool sensor";
- params_sen->ticks_per_wheel_revolution = 100; // DO NOT MODIFY THESE VALUES !!!
-
- SpecStateSensorComposite priors{
- {'P', SpecStateSensor("StatePoint2d", Vector2d::Zero(), "fix")}, // default not dynamic
- {'O', SpecStateSensor("StateAngle", Vector1d::Zero(), "fix")}, // default not dynamic
- {'I',
- SpecStateSensor(
- "StateParams3", Vector3d::Ones(), "initial_guess")}}; // not dynamic DO NOT MODIFY THESE VALUES !!!
-
- sensor = SensorBase::emplace<SensorDiffDrive>(problem->getHardware(), params_sen, priors);
-
- // params and processor
- params = std::make_shared<ParamsProcessorDiffDrive>();
- params->voting_active = true;
- params->angle_turned = 1;
- params->dist_traveled = 2;
- params->max_buff_length = 3;
- params->max_time_span = 2.5;
- params->unmeasured_perturbation_std = 1e-4;
- processor = ProcessorBase::emplace<ProcessorDiffDrivePublic>(sensor, params);
+ // sensor
+ sensor = std::static_pointer_cast<SensorDiffDrive>(problem->installSensor(
+ "SensorDiffDrive", wolf_dir + "/test/yaml/sensor_diff_drive_gtest_factor_diff_drive.yaml", {wolf_dir}));
+
+ // processor
+ processor = std::static_pointer_cast<ProcessorDiffDriveMock>(
+ problem->installProcessor("ProcessorDiffDriveMock",
+ sensor,
+ wolf_dir + "/test/yaml/processor_diff_drive.yaml",
+ {wolf_dir + "/test/dummy", wolf_dir + "/schema/processor"}));
}
void TearDown() override {}
@@ -151,9 +56,7 @@ class ProcessorDiffDriveTest : public testing::Test
TEST(ProcessorDiffDrive, constructor)
{
- auto params = std::make_shared<ParamsProcessorDiffDrive>();
-
- ASSERT_NE(params, nullptr);
+ auto params = YAML::LoadFile(wolf_dir + "/test/yaml/processor_diff_drive.yaml");
auto prc = std::make_shared<ProcessorDiffDrive>(params);
@@ -165,12 +68,12 @@ TEST(ProcessorDiffDrive, constructor)
TEST_F(ProcessorDiffDriveTest, params)
{
// read
- ASSERT_EQ(processor->getParams()->angle_turned, 1.0);
- ASSERT_EQ(processor->getParams()->dist_traveled, 2.0);
+ ASSERT_EQ(processor->getMaxAngleTurned(), 99);
+ ASSERT_EQ(processor->getMaxDistTraveled(), 99);
// write
- processor->getParams()->angle_turned = 5;
- ASSERT_EQ(processor->getParams()->angle_turned, 5.0);
+ processor->setMaxAngleTurned(5);
+ ASSERT_EQ(processor->getMaxAngleTurned(), 5.0);
}
TEST_F(ProcessorDiffDriveTest, configure)
@@ -195,15 +98,15 @@ TEST_F(ProcessorDiffDriveTest, computeCurrentDelta)
ASSERT_POSE2d_APPROX(delta, Vector3d::Zero(), 1e-8);
// 2. left turn 90 deg --> ends up in (1.5, 1.5, pi/2)
- data(0) = 0.25 * params_sen->ticks_per_wheel_revolution;
- data(1) = 0.50 * params_sen->ticks_per_wheel_revolution;
+ data(0) = 0.25 * sensor->getTicksPerWheelRevolution();
+ data(1) = 0.50 * sensor->getTicksPerWheelRevolution();
processor->computeCurrentDelta(data, data_cov, calib, dt, delta, delta_cov, J_delta_calib);
ASSERT_POSE2d_APPROX(delta, Vector3d(1.5, 1.5, M_PI_2), 1e-6);
// 2. right turn 90 deg --> ends up in (1.5, -1.5, -pi/2)
- data(0) = 0.50 * params_sen->ticks_per_wheel_revolution;
- data(1) = 0.25 * params_sen->ticks_per_wheel_revolution;
+ data(0) = 0.50 * sensor->getTicksPerWheelRevolution();
+ data(1) = 0.25 * sensor->getTicksPerWheelRevolution();
processor->computeCurrentDelta(data, data_cov, calib, dt, delta, delta_cov, J_delta_calib);
ASSERT_POSE2d_APPROX(delta, Vector3d(1.5, -1.5, -M_PI_2), 1e-6);
@@ -220,8 +123,8 @@ TEST_F(ProcessorDiffDriveTest, deltaPlusDelta)
MatrixXd delta_cov(3, 3), J_delta_calib(3, 3);
// 1. left turn 90 deg in 3 steps of 30 deg --> ends up in (1.5, 1.5, pi/2)
- data(0) = 0.25 * params_sen->ticks_per_wheel_revolution / 3;
- data(1) = 0.50 * params_sen->ticks_per_wheel_revolution / 3;
+ data(0) = 0.25 * sensor->getTicksPerWheelRevolution() / 3;
+ data(1) = 0.50 * sensor->getTicksPerWheelRevolution() / 3;
processor->computeCurrentDelta(data, data_cov, calib, dt, delta, delta_cov, J_delta_calib);
delta1.setZero();
@@ -234,8 +137,8 @@ TEST_F(ProcessorDiffDriveTest, deltaPlusDelta)
ASSERT_POSE2d_APPROX(delta2, Vector3d(1.5, 1.5, M_PI_2), 1e-6);
// 2. right turn 90 deg in 4 steps of 22.5 deg --> ends up in (1.5, -1.5, -pi/2)
- data(0) = 0.50 * params_sen->ticks_per_wheel_revolution / 4;
- data(1) = 0.25 * params_sen->ticks_per_wheel_revolution / 4;
+ data(0) = 0.50 * sensor->getTicksPerWheelRevolution() / 4;
+ data(1) = 0.25 * sensor->getTicksPerWheelRevolution() / 4;
processor->computeCurrentDelta(data, data_cov, calib, dt, delta, delta_cov, J_delta_calib);
delta1.setZero();
@@ -262,8 +165,8 @@ TEST_F(ProcessorDiffDriveTest, statePlusDelta)
MatrixXd delta_cov(3, 3), J_delta_calib(3, 3);
// 1. left turn 90 deg in 3 steps of 30 deg --> ends up in (1.5, 1.5, pi/2)
- data(0) = 0.25 * params_sen->ticks_per_wheel_revolution / 3;
- data(1) = 0.50 * params_sen->ticks_per_wheel_revolution / 3;
+ data(0) = 0.25 * sensor->getTicksPerWheelRevolution() / 3;
+ data(1) = 0.50 * sensor->getTicksPerWheelRevolution() / 3;
processor->computeCurrentDelta(data, data_cov, calib, dt, delta, delta_cov, J_delta_calib);
x1.setZero();
@@ -276,8 +179,8 @@ TEST_F(ProcessorDiffDriveTest, statePlusDelta)
ASSERT_POSE2d_APPROX(x2.vector("PO"), Vector3d(1.5, 1.5, M_PI_2), 1e-6);
// 2. right turn 90 deg in 4 steps of 22.5 deg --> ends up in (1.5, -1.5, -pi/2)
- data(0) = 0.50 * params_sen->ticks_per_wheel_revolution / 4;
- data(1) = 0.25 * params_sen->ticks_per_wheel_revolution / 4;
+ data(0) = 0.50 * sensor->getTicksPerWheelRevolution() / 4;
+ data(1) = 0.25 * sensor->getTicksPerWheelRevolution() / 4;
processor->computeCurrentDelta(data, data_cov, calib, dt, delta, delta_cov, J_delta_calib);
x1.setZero();
@@ -308,8 +211,8 @@ TEST_F(ProcessorDiffDriveTest, process)
// 1. left turn 90 deg in N steps of 90/N deg --> ends up in (1.5, 1.5, pi/2)
int N = 9;
- data(0) = 0.25 * params_sen->ticks_per_wheel_revolution / N;
- data(1) = 0.50 * params_sen->ticks_per_wheel_revolution / N;
+ data(0) = 0.25 * sensor->getTicksPerWheelRevolution() / N;
+ data(1) = 0.50 * sensor->getTicksPerWheelRevolution() / N;
auto C = std::make_shared<CaptureDiffDrive>(t, sensor, data, data_cov, F0->getCaptureList().front());
diff --git a/test/gtest_processor_fixed_wing_model.cpp b/test/gtest_processor_fixed_wing_model.cpp
index 93c4eac2c5a1e90a13cb4e9c4c42788fa01f9106..ac9903bdd8f54d9dbd19b329e1387b86d7b46b5f 100644
--- a/test/gtest_processor_fixed_wing_model.cpp
+++ b/test/gtest_processor_fixed_wing_model.cpp
@@ -37,31 +37,27 @@ std::string wolf_dir = _WOLF_CODE_DIR;
class ProcessorFixedWingModelTest : public testing::Test
{
- protected:
- ProblemPtr problem;
- SolverManagerPtr solver;
- SensorBasePtr sensor;
- ProcessorBasePtr processor;
-
- virtual void SetUp()
- {
- // create problem
- problem = Problem::create("POV", 3);
-
- // create solver
- solver = std::make_shared<SolverCeres>(problem);
-
- // Emplace sensor
- sensor = problem->installSensor("SensorMotionModel", wolf_dir + "/test/yaml/sensor_pose_3d.yaml", {wolf_dir});
-
- // Emplace processor
- ParamsProcessorFixedWingModelPtr params = std::make_shared<ParamsProcessorFixedWingModel>();
- params->velocity_local = (Vector3d() << 1, 0, 0).finished();
- params->angle_stdev = 0.1;
- params->min_vel_norm = 0;
- params->name = "processor_1";
- processor = ProcessorBase::emplace<ProcessorFixedWingModel>(sensor, params);
- }
+ protected:
+ ProblemPtr problem;
+ SolverManagerPtr solver;
+ SensorBasePtr sensor;
+ ProcessorBasePtr processor;
+
+ virtual void SetUp()
+ {
+ // create problem
+ problem = Problem::create("POV", 3);
+
+ // create solver
+ solver = SolverCeres::create(problem, wolf_dir + "/test/yaml/solver_ceres.yaml", {wolf_dir});
+
+ // Emplace sensor
+ sensor = problem->installSensor("SensorMotionModel", wolf_dir + "/test/yaml/sensor_pose_3d.yaml", {wolf_dir});
+
+ // Emplace processor
+ processor = problem->installProcessor(
+ "ProcessorFixedWingModel", sensor, wolf_dir + "/test/yaml/processor_fixed_wing_model.yaml", {wolf_dir});
+ }
};
TEST_F(ProcessorFixedWingModelTest, setup)
@@ -72,7 +68,7 @@ TEST_F(ProcessorFixedWingModelTest, setup)
TEST_F(ProcessorFixedWingModelTest, keyFrameCallback)
{
// new frame
- auto frm1 = problem->emplaceFrame(1, "POV", (Vector10d() << 0,0,0,0,0,0,1,1,0,0).finished());
+ auto frm1 = problem->emplaceFrame(1, "POV", (Vector10d() << 0, 0, 0, 0, 0, 0, 1, 1, 0, 0).finished());
// keyframecallback
problem->keyFrameCallback(frm1, nullptr);
@@ -96,7 +92,7 @@ TEST_F(ProcessorFixedWingModelTest, keyFrameCallback)
TEST_F(ProcessorFixedWingModelTest, keyFrameCallbackRepeated)
{
// new frame
- auto frm1 = problem->emplaceFrame(1, "POV", (Vector10d() << 0,0,0,0,0,0,1,1,0,0).finished());
+ auto frm1 = problem->emplaceFrame(1, "POV", (Vector10d() << 0, 0, 0, 0, 0, 0, 1, 1, 0, 0).finished());
// keyframecallback
problem->keyFrameCallback(frm1, nullptr);
@@ -123,7 +119,7 @@ TEST_F(ProcessorFixedWingModelTest, keyFrameCallbackRepeated)
TEST_F(ProcessorFixedWingModelTest, solve_origin)
{
// new frame
- auto frm1 = problem->emplaceFrame(1, "POV", (Vector10d() << 0,0,0,0,0,0,1,1,0,0).finished());
+ auto frm1 = problem->emplaceFrame(1, "POV", (Vector10d() << 0, 0, 0, 0, 0, 0, 1, 1, 0, 0).finished());
// keyframecallback
problem->keyFrameCallback(frm1, nullptr);
@@ -146,6 +142,6 @@ TEST_F(ProcessorFixedWingModelTest, solve_origin)
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_landmark_external.cpp b/test/gtest_processor_landmark_external.cpp
index 92f30bb2796fd5ba2f241de5fe26a5164467b033..7292ac09ee5d05a929c911852e44572bca8db13c 100644
--- a/test/gtest_processor_landmark_external.cpp
+++ b/test/gtest_processor_landmark_external.cpp
@@ -96,7 +96,7 @@ void ProcessorLandmarkExternalTest::initProblem(int _dim,
t = TimeStamp(0);
problem = Problem::create("PO", dim);
- solver = std::make_shared<SolverCeres>(problem);
+ solver = SolverCeres::create(problem, wolf_dir + "/test/yaml/solver_ceres.yaml", {wolf_dir});
// Sensors
sensor = problem->installSensor("SensorOdom" + toString(dim) + "d",
@@ -120,36 +120,22 @@ void ProcessorLandmarkExternalTest::initProblem(int _dim,
sensor->setState(sensor_state, "PO");
// 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);
-
if (dim == 2)
- {
- auto params_odom = std::make_shared<ParamsProcessorOdom2d>();
- params_odom->state_provider = 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>(problem->installProcessor(
+ "ProcessorOdom2d", sensor_odom, wolf_dir + "/test/yaml/processor_odom_2d_inactive.yaml", {wolf_dir}));
else
- {
- auto params_odom = std::make_shared<ParamsProcessorOdom3d>();
- params_odom->state_provider = 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>(problem->installProcessor(
+ "ProcessorOdom3d", sensor_odom, wolf_dir + "/test/yaml/processor_odom_3d_inactive.yaml", {wolf_dir}));
+
+ // Take parameters from YAML and modify some of them
+ YAML::Node params = YAML::LoadFile(wolf_dir + "/test/yaml/processor_landmark_external.yaml");
+ params["time_tolerance"] = dt / 2;
+ params["use_orientation"] = orientation;
+ params["filter"]["quality_th"] = _quality_th;
+ params["match_dist_th"] = _dist_th;
+ params["filter"]["track_length_th"] = _track_length_th;
+ params["keyframe_vote"]["time_span"] = _time_span;
+ processor = ProcessorBase::emplace<ProcessorLandmarkExternal>(sensor, params, {wolf_dir});
// Emplace frame
auto F0 = problem->emplaceFrame(
@@ -359,8 +345,6 @@ void ProcessorLandmarkExternalTest::testConfiguration(int dim,
WOLF_INFO(report);
// check values
- // assertVectorComposite(sensor->getState("PO"), state_sensor);
- // assertVectorComposite(problem->getState("PO"), state_robot);
for (auto lmk_map : problem->getMap()->getLandmarkList())
{
assertVectorComposite(lmk_map->getState(), state_landmarks.at(lmk_map->id()));
diff --git a/test/gtest_processor_loop_closure.cpp b/test/gtest_processor_loop_closure.cpp
index 4a4cc56f19a6c372253834b13b7261d81f82a036..f0a9dc3ccc123dd214c1989c1f6d4c48c8731327 100644
--- a/test/gtest_processor_loop_closure.cpp
+++ b/test/gtest_processor_loop_closure.cpp
@@ -50,9 +50,11 @@ class ProcessorLoopClosureTest : public testing::Test
sensor = problem->installSensor("SensorOdom2d", wolf_dir + "/test/yaml/sensor_odom_2d.yaml", {wolf_dir});
// install processor
- ParamsProcessorLoopClosurePtr params = std::make_shared<ParamsProcessorLoopClosure>();
- params->time_tolerance = 0.5;
- processor = ProcessorBase::emplace<ProcessorLoopClosureDummy>(sensor, params);
+ processor = std::static_pointer_cast<ProcessorLoopClosureDummy>(
+ problem->installProcessor("ProcessorLoopClosureDummy",
+ sensor,
+ wolf_dir + "/test/yaml/processor_loop_closure_dummy.yaml",
+ {wolf_dir}));
}
FrameBasePtr emplaceFrame(TimeStamp ts, const Vector3d& x)
diff --git a/test/gtest_processor_motion.cpp b/test/gtest_processor_motion.cpp
index 1058584a1f89481b9c6a3d3fe149a0ec1837c019..831ce76eb64aac89953bec868f783b4e6bd7602b 100644
--- a/test/gtest_processor_motion.cpp
+++ b/test/gtest_processor_motion.cpp
@@ -22,7 +22,6 @@
#include "core/common/wolf.h"
-
#include "core/sensor/sensor_odom.h"
#include "core/processor/processor_odom_2d.h"
@@ -32,70 +31,82 @@ using std::static_pointer_cast;
class ProcessorOdom2dPublic : public ProcessorOdom2d
{
- public:
- ProcessorOdom2dPublic(ParamsProcessorOdom2dPtr params) : ProcessorOdom2d(params)
- {
- //
- }
- ~ProcessorOdom2dPublic() override{}
-
- void splitBuffer(const wolf::CaptureMotionPtr& capture_source,
- TimeStamp ts_split,
- const wolf::CaptureMotionPtr& capture_target)
- {
- ProcessorOdom2d::splitBuffer(capture_source,
- ts_split,
- capture_target);
- }
+ public:
+ ProcessorOdom2dPublic(const YAML::Node& params) : ProcessorOdom2d(params)
+ {
+ //
+ }
+ WOLF_PROCESSOR_CREATE(ProcessorOdom2dPublic);
+
+ ~ProcessorOdom2dPublic() override {}
+
+ void splitBuffer(const wolf::CaptureMotionPtr& capture_source,
+ TimeStamp ts_split,
+ const wolf::CaptureMotionPtr& capture_target)
+ {
+ ProcessorOdom2d::splitBuffer(capture_source, ts_split, capture_target);
+ }
};
WOLF_PTR_TYPEDEFS(ProcessorOdom2dPublic);
+WOLF_REGISTER_PROCESSOR(ProcessorOdom2dPublic);
-class ProcessorMotion_test : public testing::Test{
- public:
- double dt;
- ProblemPtr problem;
- SensorOdom2dPtr sensor;
- ProcessorOdom2dPublicPtr processor;
- CaptureMotionPtr capture;
- Vector2d data;
- Matrix2d data_cov;
-
- void SetUp() override
- {
- std::string wolf_dir = _WOLF_CODE_DIR;
-
- dt = 1.0;
- problem = Problem::create("PO", 2);
- sensor = static_pointer_cast<SensorOdom2d>(problem->installSensor("SensorOdom2d",
- wolf_dir + "/test/yaml/sensor_odom_2d.yaml",
- {wolf_dir}));
- ParamsProcessorOdom2dPtr params(std::make_shared<ParamsProcessorOdom2d>());
- params->time_tolerance = 0.5;
- params->dist_traveled = 100;
- params->angle_turned = 6.28;
- params->max_time_span = 2.5*dt; // force KF at every third process()
- params->cov_det = 100;
- params->unmeasured_perturbation_std = 0.001;
- processor = ProcessorBase::emplace<ProcessorOdom2dPublic>(sensor, params);
- capture = std::make_shared<CaptureMotion>("CaptureOdom2d", 0.0, sensor, data, data_cov, nullptr);
- }
-
- void TearDown() override{}
+class ProcessorMotion_test : public testing::Test
+{
+ public:
+ double dt;
+ ProblemPtr problem;
+ SensorOdom2dPtr sensor;
+ ProcessorOdom2dPublicPtr processor;
+ CaptureMotionPtr capture;
+ Vector2d data;
+ Matrix2d data_cov;
+
+ void SetUp() override
+ {
+ std::string wolf_dir = _WOLF_CODE_DIR;
+
+ dt = 1.0;
+ problem = Problem::create("PO", 2);
+
+ // install processor
+ sensor = static_pointer_cast<SensorOdom2d>(
+ problem->installSensor("SensorOdom2d", wolf_dir + "/test/yaml/sensor_odom_2d.yaml", {wolf_dir}));
+
+ // load yaml and modify values
+ YAML::Node params = YAML::LoadFile(wolf_dir + "/test/yaml/processor_odom_2d.yaml");
+ params["keyframe_vote"]["voting_active"] = false;
+ params["sensor_name"] = sensor->getName();
+ params["type"] = "ProcessorOdom2dPublic";
+ params["time_tolerance"] = 0.5;
+ params["keyframe_vote"]["max_dist_traveled"] = 100;
+ params["keyframe_vote"]["max_angle_turned"] = 6.28;
+ params["keyframe_vote"]["max_time_span"] = 2.5 * dt; // force KF at every third process()
+ params["keyframe_vote"]["cov_det"] = 100;
+ params["unmeasured_perturbation_std"] = 0.001;
+
+ // install processor
+ processor = static_pointer_cast<ProcessorOdom2dPublic>(problem->installProcessor(params));
+
+ capture = std::make_shared<CaptureMotion>("CaptureOdom2d", 0.0, sensor, data, data_cov, nullptr);
+ }
+ void TearDown() override {}
};
TEST_F(ProcessorMotion_test, IntegrateStraightAutoPrior)
{
// Prior
- Vector3d x0; x0 << 0, 0, 0;
- Matrix3d P0; P0.setIdentity();
+ Vector3d x0;
+ x0 << 0, 0, 0;
+ Matrix3d P0;
+ P0.setIdentity();
- data << 1, 0; // advance straight
+ data << 1, 0; // advance straight
data_cov.setIdentity();
TimeStamp t(0.0);
- for (int i = 0; i<9; i++)
+ for (int i = 0; i < 9; i++)
{
t += dt;
capture->setTimeStamp(t);
@@ -105,20 +116,22 @@ TEST_F(ProcessorMotion_test, IntegrateStraightAutoPrior)
WOLF_DEBUG("t: ", t, " x: ", problem->getState().vector("PO").transpose());
}
- ASSERT_POSE2d_APPROX(problem->getState().vector("PO"), (Vector3d()<<9,0,0).finished(), 1e-8);
+ ASSERT_POSE2d_APPROX(problem->getState().vector("PO"), (Vector3d() << 9, 0, 0).finished(), 1e-8);
}
TEST_F(ProcessorMotion_test, getState_structure)
{
// Prior
- Vector3d x0; x0 << 0, 0, 0;
- Matrix3d P0; P0.setIdentity();
+ Vector3d x0;
+ x0 << 0, 0, 0;
+ Matrix3d P0;
+ P0.setIdentity();
- data << 1, 0; // advance straight
+ data << 1, 0; // advance straight
data_cov.setIdentity();
TimeStamp t(0.0);
- for (int i = 0; i<9; i++)
+ for (int i = 0; i < 9; i++)
{
t += dt;
capture->setTimeStamp(t);
@@ -128,27 +141,28 @@ TEST_F(ProcessorMotion_test, getState_structure)
WOLF_DEBUG("t: ", t, " x: ", problem->getState().vector("PO").transpose());
}
- ASSERT_TRUE (processor->getState("P").count('P'));
+ ASSERT_TRUE(processor->getState("P").count('P'));
ASSERT_FALSE(processor->getState("P").count('O'));
ASSERT_FALSE(processor->getState("O").count('P'));
- ASSERT_TRUE (processor->getState("O").count('O'));
+ ASSERT_TRUE(processor->getState("O").count('O'));
WOLF_DEBUG("processor->getState(\"V\") = ", processor->getState('V'));
- ASSERT_EQ (processor->getState("V").size(), 0);
+ ASSERT_EQ(processor->getState("V").size(), 0);
}
-
TEST_F(ProcessorMotion_test, getState_time_structure)
{
// Prior
- Vector3d x0; x0 << 0, 0, 0;
- Matrix3d P0; P0.setIdentity();
+ Vector3d x0;
+ x0 << 0, 0, 0;
+ Matrix3d P0;
+ P0.setIdentity();
- data << 1, 0; // advance straight
+ data << 1, 0; // advance straight
data_cov.setIdentity();
TimeStamp t(0.0);
- for (int i = 0; i<9; i++)
+ for (int i = 0; i < 9; i++)
{
t += dt;
capture->setTimeStamp(t);
@@ -158,33 +172,31 @@ TEST_F(ProcessorMotion_test, getState_time_structure)
WOLF_DEBUG("t: ", t, " x: ", problem->getState().vector("PO").transpose());
}
- problem->print(2,1,1,1);
+ problem->print(2, 1, 1, 1);
- ASSERT_TRUE (processor->getState(7, "P").count('P'));
+ ASSERT_TRUE(processor->getState(7, "P").count('P'));
ASSERT_FALSE(processor->getState(7, "P").count('O'));
ASSERT_FALSE(processor->getState(7, "O").count('P'));
- ASSERT_TRUE (processor->getState(7, "O").count('O'));
+ ASSERT_TRUE(processor->getState(7, "O").count('O'));
WOLF_DEBUG("processor->getState(7, \"V\") = ", processor->getState(7, "V"));
- ASSERT_EQ (processor->getState(7, "V").size(), 0);
+ ASSERT_EQ(processor->getState(7, "V").size(), 0);
}
-
-
TEST_F(ProcessorMotion_test, IntegrateStraightFactorPrior)
{
// Prior
- TimeStamp t(0.0);
+ TimeStamp t(0.0);
SpecStateComposite prior;
- prior.emplace('P',SpecState("StatePoint2d",Vector2d(0,0),"factor",Vector2d(1,1)));
- prior.emplace('O',SpecState("StateAngle",Vector1d(0),"factor",Vector1d(1)));
+ prior.emplace('P', SpecState("StatePoint2d", Vector2d(0, 0), "factor", Vector2d(1, 1)));
+ prior.emplace('O', SpecState("StateAngle", Vector1d(0), "factor", Vector1d(1)));
auto KF_0 = problem->setPrior(prior, t);
processor->setOrigin(KF_0);
- data << 1, 0; // advance straight
+ data << 1, 0; // advance straight
data_cov.setIdentity();
- for (int i = 0; i<9; i++)
+ for (int i = 0; i < 9; i++)
{
t += dt;
capture->setTimeStamp(t);
@@ -194,22 +206,22 @@ TEST_F(ProcessorMotion_test, IntegrateStraightFactorPrior)
WOLF_DEBUG("t: ", t, " x: ", problem->getState().vector("PO").transpose());
}
- ASSERT_POSE2d_APPROX(problem->getState().vector("PO"), (Vector3d()<<9,0,0).finished(), 1e-8);
+ ASSERT_POSE2d_APPROX(problem->getState().vector("PO"), (Vector3d() << 9, 0, 0).finished(), 1e-8);
}
TEST_F(ProcessorMotion_test, IntegrateStraightFixPrior)
{
// Prior
- TimeStamp t(0.0);
- SpecStateComposite prior{{'P',SpecState("StatePoint2d",Vector2d(0,0),"fix")},
- {'O',SpecState("StateAngle",Vector1d(0),"fix")}};
- auto KF_0 = problem->setPrior(prior, t);
+ TimeStamp t(0.0);
+ SpecStateComposite prior{{'P', SpecState("StatePoint2d", Vector2d(0, 0), "fix")},
+ {'O', SpecState("StateAngle", Vector1d(0), "fix")}};
+ auto KF_0 = problem->setPrior(prior, t);
processor->setOrigin(KF_0);
- data << 1, 0; // advance straight
+ data << 1, 0; // advance straight
data_cov.setIdentity();
- for (int i = 0; i<9; i++)
+ for (int i = 0; i < 9; i++)
{
t += dt;
capture->setTimeStamp(t);
@@ -219,20 +231,22 @@ TEST_F(ProcessorMotion_test, IntegrateStraightFixPrior)
WOLF_DEBUG("t: ", t, " x: ", problem->getState().vector("PO").transpose());
}
- ASSERT_POSE2d_APPROX(problem->getState().vector("PO"), (Vector3d()<<9,0,0).finished(), 1e-8);
+ ASSERT_POSE2d_APPROX(problem->getState().vector("PO"), (Vector3d() << 9, 0, 0).finished(), 1e-8);
}
TEST_F(ProcessorMotion_test, IntegrateCircleAutoPrior)
{
// Prior
- Vector3d x0; x0 << 0, 0, 0;
- Matrix3d P0; P0.setIdentity();
+ Vector3d x0;
+ x0 << 0, 0, 0;
+ Matrix3d P0;
+ P0.setIdentity();
- data << 1, 2*M_PI/10; // advance in circle
+ data << 1, 2 * M_PI / 10; // advance in circle
data_cov.setIdentity();
TimeStamp t(0.0);
- for (int i = 0; i<10; i++) // one full turn exactly
+ for (int i = 0; i < 10; i++) // one full turn exactly
{
t += dt;
capture->setTimeStamp(t);
@@ -242,22 +256,22 @@ TEST_F(ProcessorMotion_test, IntegrateCircleAutoPrior)
WOLF_DEBUG("t: ", t, " x: ", problem->getState().vector("PO").transpose());
}
- ASSERT_POSE2d_APPROX(problem->getState().vector("PO"), (Vector3d()<<0,0,0).finished(), 1e-8);
+ ASSERT_POSE2d_APPROX(problem->getState().vector("PO"), (Vector3d() << 0, 0, 0).finished(), 1e-8);
}
TEST_F(ProcessorMotion_test, IntegrateCircleFactorPrior)
{
// Prior
- TimeStamp t(0.0);
- SpecStateComposite prior{{'P',SpecState("StatePoint2d",Vector2d(0,0),"factor",Vector2d(1,1))},
- {'O',SpecState("StateAngle",Vector1d(0),"factor",Vector1d(1))}};
- auto KF_0 = problem->setPrior(prior, t);
+ TimeStamp t(0.0);
+ SpecStateComposite prior{{'P', SpecState("StatePoint2d", Vector2d(0, 0), "factor", Vector2d(1, 1))},
+ {'O', SpecState("StateAngle", Vector1d(0), "factor", Vector1d(1))}};
+ auto KF_0 = problem->setPrior(prior, t);
processor->setOrigin(KF_0);
- data << 1, 2*M_PI/10; // advance in circle
+ data << 1, 2 * M_PI / 10; // advance in circle
data_cov.setIdentity();
- for (int i = 0; i<10; i++) // one full turn exactly
+ for (int i = 0; i < 10; i++) // one full turn exactly
{
t += dt;
capture->setTimeStamp(t);
@@ -267,23 +281,23 @@ TEST_F(ProcessorMotion_test, IntegrateCircleFactorPrior)
WOLF_DEBUG("t: ", t, " x: ", problem->getState().vector("PO").transpose());
}
- ASSERT_POSE2d_APPROX(problem->getState().vector("PO"), (Vector3d()<<0,0,0).finished(), 1e-8);
+ ASSERT_POSE2d_APPROX(problem->getState().vector("PO"), (Vector3d() << 0, 0, 0).finished(), 1e-8);
}
TEST_F(ProcessorMotion_test, IntegrateCircleFixPrior)
{
// Prior
- TimeStamp t(0.0);
+ TimeStamp t(0.0);
SpecStateComposite prior;
- prior.emplace('P',SpecState("StatePoint2d",Vector2d(0,0),"fix"));
- prior.emplace('O',SpecState("StateAngle",Vector1d(0),"fix"));
+ prior.emplace('P', SpecState("StatePoint2d", Vector2d(0, 0), "fix"));
+ prior.emplace('O', SpecState("StateAngle", Vector1d(0), "fix"));
auto KF_0 = problem->setPrior(prior, t);
processor->setOrigin(KF_0);
- data << 1, 2*M_PI/10; // advance in circle
+ data << 1, 2 * M_PI / 10; // advance in circle
data_cov.setIdentity();
- for (int i = 0; i<10; i++) // one full turn exactly
+ for (int i = 0; i < 10; i++) // one full turn exactly
{
t += dt;
capture->setTimeStamp(t);
@@ -293,20 +307,22 @@ TEST_F(ProcessorMotion_test, IntegrateCircleFixPrior)
WOLF_DEBUG("t: ", t, " x: ", problem->getState().vector("PO").transpose());
}
- ASSERT_POSE2d_APPROX(problem->getState().vector("PO"), (Vector3d()<<0,0,0).finished(), 1e-8);
+ ASSERT_POSE2d_APPROX(problem->getState().vector("PO"), (Vector3d() << 0, 0, 0).finished(), 1e-8);
}
TEST_F(ProcessorMotion_test, mergeCaptures)
{
// Prior
- Vector3d x0; x0 << 0, 0, 0;
- Matrix3d P0; P0.setIdentity();
+ Vector3d x0;
+ x0 << 0, 0, 0;
+ Matrix3d P0;
+ P0.setIdentity();
- data << 1, 2*M_PI/10; // advance in circle
+ data << 1, 2 * M_PI / 10; // advance in circle
data_cov.setIdentity();
TimeStamp t(0.0);
- for (int i = 0; i<10; i++) // one full turn exactly
+ for (int i = 0; i < 10; i++) // one full turn exactly
{
t += dt;
capture->setTimeStamp(t);
@@ -316,39 +332,33 @@ TEST_F(ProcessorMotion_test, mergeCaptures)
WOLF_DEBUG("t: ", t, " x: ", problem->getState().vector("PO").transpose());
}
- SensorBasePtr S = processor->getSensor();
+ SensorBasePtr S = processor->getSensor();
TimeStamp t_target = 8.5;
FrameBasePtr F_target = problem->emplaceFrame(t_target);
CaptureMotionPtr C_source = std::dynamic_pointer_cast<CaptureMotion>(processor->getLast());
- CaptureMotionPtr C_target = CaptureBase::emplace<CaptureMotion>(F_target,
- "CaptureOdom2d",
- t_target,
- sensor,
- data,
- nullptr);
+ CaptureMotionPtr C_target =
+ CaptureBase::emplace<CaptureMotion>(F_target, "CaptureOdom2d", t_target, sensor, data, nullptr);
// copy initial buffer
MotionBuffer initial_buffer = C_source->getBuffer();
- processor->splitBuffer(C_source,
- t_target,
- C_target);
+ processor->splitBuffer(C_source, t_target, C_target);
- C_target->getBuffer().print(1,1,1,0);
- C_source->getBuffer().print(1,1,1,0);
+ C_target->getBuffer().print(1, 1, 1, 0);
+ C_source->getBuffer().print(1, 1, 1, 0);
// merge captures
processor->mergeCaptures(C_target, C_source);
- C_target->getBuffer().print(1,1,1,0);
- C_source->getBuffer().print(1,1,1,0);
+ C_target->getBuffer().print(1, 1, 1, 0);
+ C_source->getBuffer().print(1, 1, 1, 0);
// Check that merged buffer is the initial one (before splitting)
EXPECT_EQ(C_source->getBuffer().size(), initial_buffer.size());
auto init_it = initial_buffer.begin();
- auto res_it = C_source->getBuffer().begin();
+ auto res_it = C_source->getBuffer().begin();
while (init_it != initial_buffer.end())
{
EXPECT_EQ(init_it->data_size_, init_it->data_size_);
@@ -357,10 +367,10 @@ TEST_F(ProcessorMotion_test, mergeCaptures)
EXPECT_EQ(init_it->calib_size_, init_it->calib_size_);
EXPECT_EQ(init_it->ts_, init_it->ts_);
- EXPECT_MATRIX_APPROX(init_it->data_ ,init_it->data_, 1e-12);
- EXPECT_MATRIX_APPROX(init_it->data_cov_,init_it->data_cov_, 1e-12);
+ EXPECT_MATRIX_APPROX(init_it->data_, init_it->data_, 1e-12);
+ EXPECT_MATRIX_APPROX(init_it->data_cov_, init_it->data_cov_, 1e-12);
EXPECT_MATRIX_APPROX(init_it->delta_, init_it->delta_, 1e-12);
- EXPECT_MATRIX_APPROX(init_it->delta_cov_, init_it->delta_cov_, 1e-12);
+ EXPECT_MATRIX_APPROX(init_it->delta_cov_, init_it->delta_cov_, 1e-12);
EXPECT_MATRIX_APPROX(init_it->delta_integr_, init_it->delta_integr_, 1e-12);
EXPECT_MATRIX_APPROX(init_it->delta_integr_cov_, init_it->delta_integr_cov_, 1e-12);
EXPECT_MATRIX_APPROX(init_it->jacobian_delta_, init_it->jacobian_delta_, 1e-12);
@@ -375,14 +385,16 @@ TEST_F(ProcessorMotion_test, mergeCaptures)
TEST_F(ProcessorMotion_test, splitBufferAutoPrior)
{
// Prior
- Vector3d x0; x0 << 0, 0, 0;
- Matrix3d P0; P0.setIdentity();
+ Vector3d x0;
+ x0 << 0, 0, 0;
+ Matrix3d P0;
+ P0.setIdentity();
- data << 1, 2*M_PI/10; // advance in circle
+ data << 1, 2 * M_PI / 10; // advance in circle
data_cov.setIdentity();
TimeStamp t(0.0);
- for (int i = 0; i<10; i++) // one full turn exactly
+ for (int i = 0; i < 10; i++) // one full turn exactly
{
t += dt;
capture->setTimeStamp(t);
@@ -392,39 +404,33 @@ TEST_F(ProcessorMotion_test, splitBufferAutoPrior)
WOLF_DEBUG("t: ", t, " x: ", problem->getState().vector("PO").transpose());
}
- SensorBasePtr S = processor->getSensor();
+ SensorBasePtr S = processor->getSensor();
TimeStamp t_target = 8.5;
FrameBasePtr F_target = problem->emplaceFrame(t_target);
CaptureMotionPtr C_source = std::dynamic_pointer_cast<CaptureMotion>(processor->getLast());
- CaptureMotionPtr C_target = CaptureBase::emplace<CaptureMotion>(F_target,
- "ODOM 2d",
- t_target,
- sensor,
- data,
- nullptr);
-
- processor->splitBuffer(C_source,
- t_target,
- C_target);
-
- C_target->getBuffer().print(1,1,1,0);
- C_source->getBuffer().print(1,1,1,0);
+ CaptureMotionPtr C_target =
+ CaptureBase::emplace<CaptureMotion>(F_target, "ODOM 2d", t_target, sensor, data, nullptr);
+
+ processor->splitBuffer(C_source, t_target, C_target);
+
+ C_target->getBuffer().print(1, 1, 1, 0);
+ C_source->getBuffer().print(1, 1, 1, 0);
}
TEST_F(ProcessorMotion_test, splitBufferFactorPrior)
{
// Prior
- TimeStamp t(0.0);
- SpecStateComposite prior{{'P',SpecState("StatePoint2d",Vector2d(0,0),"factor",Vector2d(1,1))},
- {'O',SpecState("StateAngle",Vector1d(0),"factor",Vector1d(1))}};
- auto KF_0 = problem->setPrior(prior, t);
+ TimeStamp t(0.0);
+ SpecStateComposite prior{{'P', SpecState("StatePoint2d", Vector2d(0, 0), "factor", Vector2d(1, 1))},
+ {'O', SpecState("StateAngle", Vector1d(0), "factor", Vector1d(1))}};
+ auto KF_0 = problem->setPrior(prior, t);
processor->setOrigin(KF_0);
- data << 1, 2*M_PI/10; // advance in circle
+ data << 1, 2 * M_PI / 10; // advance in circle
data_cov.setIdentity();
- for (int i = 0; i<10; i++) // one full turn exactly
+ for (int i = 0; i < 10; i++) // one full turn exactly
{
t += dt;
capture->setTimeStamp(t);
@@ -434,39 +440,33 @@ TEST_F(ProcessorMotion_test, splitBufferFactorPrior)
WOLF_DEBUG("t: ", t, " x: ", problem->getState().vector("PO").transpose());
}
- SensorBasePtr S = processor->getSensor();
+ SensorBasePtr S = processor->getSensor();
TimeStamp t_target = 8.5;
FrameBasePtr F_target = problem->emplaceFrame(t_target);
CaptureMotionPtr C_source = std::dynamic_pointer_cast<CaptureMotion>(processor->getLast());
- CaptureMotionPtr C_target = CaptureBase::emplace<CaptureMotion>(F_target,
- "ODOM 2d",
- t_target,
- sensor,
- data,
- nullptr);
-
- processor->splitBuffer(C_source,
- t_target,
- C_target);
-
- C_target->getBuffer().print(1,1,1,0);
- C_source->getBuffer().print(1,1,1,0);
+ CaptureMotionPtr C_target =
+ CaptureBase::emplace<CaptureMotion>(F_target, "ODOM 2d", t_target, sensor, data, nullptr);
+
+ processor->splitBuffer(C_source, t_target, C_target);
+
+ C_target->getBuffer().print(1, 1, 1, 0);
+ C_source->getBuffer().print(1, 1, 1, 0);
}
TEST_F(ProcessorMotion_test, splitBufferFixPrior)
{
// Prior
- TimeStamp t(0.0);
- SpecStateComposite prior{{'P',SpecState("StatePoint2d",Vector2d(0,0),"fix")},
- {'O',SpecState("StateAngle",Vector1d(0),"fix")}};
- auto KF_0 = problem->setPrior(prior, t);
+ TimeStamp t(0.0);
+ SpecStateComposite prior{{'P', SpecState("StatePoint2d", Vector2d(0, 0), "fix")},
+ {'O', SpecState("StateAngle", Vector1d(0), "fix")}};
+ auto KF_0 = problem->setPrior(prior, t);
processor->setOrigin(KF_0);
- data << 1, 2*M_PI/10; // advance in circle
+ data << 1, 2 * M_PI / 10; // advance in circle
data_cov.setIdentity();
- for (int i = 0; i<10; i++) // one full turn exactly
+ for (int i = 0; i < 10; i++) // one full turn exactly
{
t += dt;
capture->setTimeStamp(t);
@@ -476,61 +476,53 @@ TEST_F(ProcessorMotion_test, splitBufferFixPrior)
WOLF_DEBUG("t: ", t, " x: ", problem->getState().vector("PO").transpose());
}
- SensorBasePtr S = processor->getSensor();
+ SensorBasePtr S = processor->getSensor();
TimeStamp t_target = 8.5;
FrameBasePtr F_target = problem->emplaceFrame(t_target);
CaptureMotionPtr C_source = std::dynamic_pointer_cast<CaptureMotion>(processor->getLast());
- CaptureMotionPtr C_target = CaptureBase::emplace<CaptureMotion>(F_target,
- "ODOM 2d",
- t_target,
- sensor,
- data,
- nullptr);
-
- processor->splitBuffer(C_source,
- t_target,
- C_target);
-
- C_target->getBuffer().print(1,1,1,0);
- C_source->getBuffer().print(1,1,1,0);
+ CaptureMotionPtr C_target =
+ CaptureBase::emplace<CaptureMotion>(F_target, "ODOM 2d", t_target, sensor, data, nullptr);
+
+ processor->splitBuffer(C_source, t_target, C_target);
+
+ C_target->getBuffer().print(1, 1, 1, 0);
+ C_source->getBuffer().print(1, 1, 1, 0);
}
TEST_F(ProcessorMotion_test, initOdometry)
{
auto odometry = processor->getOdometry();
- ASSERT_MATRIX_APPROX(odometry.at('P'), Vector2d(0,0), 1e-20);
- ASSERT_MATRIX_APPROX(odometry.at('O'), Vector1d(0 ), 1e-20);
+ ASSERT_MATRIX_APPROX(odometry.at('P'), Vector2d(0, 0), 1e-20);
+ ASSERT_MATRIX_APPROX(odometry.at('O'), Vector1d(0), 1e-20);
}
TEST_F(ProcessorMotion_test, integrateOdometry)
{
auto odometry = processor->getOdometry();
- ASSERT_MATRIX_APPROX(odometry.at('P'), Vector2d(0,0), 1e-20);
- ASSERT_MATRIX_APPROX(odometry.at('O'), Vector1d(0 ), 1e-20);
+ ASSERT_MATRIX_APPROX(odometry.at('P'), Vector2d(0, 0), 1e-20);
+ ASSERT_MATRIX_APPROX(odometry.at('O'), Vector1d(0), 1e-20);
- data << 1,0;
+ data << 1, 0;
capture->setData(data);
capture->setTimeStamp(capture->getTimeStamp() + 1.0);
capture->process();
odometry = processor->getOdometry();
- ASSERT_MATRIX_APPROX(odometry.at('P'), Vector2d(1,0), 1e-20);
- ASSERT_MATRIX_APPROX(odometry.at('O'), Vector1d(0 ), 1e-20);
+ ASSERT_MATRIX_APPROX(odometry.at('P'), Vector2d(1, 0), 1e-20);
+ ASSERT_MATRIX_APPROX(odometry.at('O'), Vector1d(0), 1e-20);
capture->setTimeStamp(capture->getTimeStamp() + 1.0);
capture->process();
odometry = processor->getOdometry();
- ASSERT_MATRIX_APPROX(odometry.at('P'), Vector2d(2,0), 1e-20);
- ASSERT_MATRIX_APPROX(odometry.at('O'), Vector1d(0 ), 1e-20);
+ ASSERT_MATRIX_APPROX(odometry.at('P'), Vector2d(2, 0), 1e-20);
+ ASSERT_MATRIX_APPROX(odometry.at('O'), Vector1d(0), 1e-20);
}
-
-int main(int argc, char **argv)
+int main(int argc, char** argv)
{
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}
-
diff --git a/test/gtest_odom_2d.cpp b/test/gtest_processor_odom_2d.cpp
similarity index 62%
rename from test/gtest_odom_2d.cpp
rename to test/gtest_processor_odom_2d.cpp
index e67a26a48b9c473f1c2b51d6ac72a1c97742cd80..ebd0973f461e47405a53fe6b33fa23c5e98b00cb 100644
--- a/test/gtest_odom_2d.cpp
+++ b/test/gtest_processor_odom_2d.cpp
@@ -45,6 +45,8 @@
using namespace wolf;
using namespace Eigen;
+std::string wolf_dir = _WOLF_CODE_DIR;
+
VectorXd plus(const VectorXd& _pose, const Vector2d& _data)
{
VectorXd _pose_plus_data(3);
@@ -81,129 +83,12 @@ MatrixXd plus_jac_x(const VectorXd& _pose, const Vector2d& _data)
return Jx;
}
-void show(const ProblemPtr& problem)
-{
- using std::cout;
- using std::endl;
- cout << std::setprecision(4);
-
- for (auto F_pair : problem->getTrajectory()->getFrameMap())
- {
- auto F = F_pair.second;
- cout << "----- Key Frame " << F->id() << " -----" << endl;
- if (!F->getCaptureList().empty())
- {
- auto C = F->getCaptureList().front();
- if (!C->getFeatureList().empty())
- {
- auto f = C->getFeatureList().front();
- cout << " feature measure: \n"
- << F->getCaptureList().front()->getFeatureList().front()->getMeasurement().transpose() << endl;
- cout << " feature covariance: \n"
- << F->getCaptureList().front()->getFeatureList().front()->getMeasurementCovariance() << endl;
- }
- }
- cout << " state: \n" << F->getStateVector("PO").transpose() << endl;
- Eigen::MatrixXd cov;
- F->getCovariance("PO", cov);
- cout << " covariance: \n" << cov << endl;
- }
-}
-
-TEST(Odom2d, FactorFix_and_FactorOdom2d)
-{
- using std::cout;
- using std::endl;
-
- // RATIONALE:
- // We build this tree (a is `absolute`, m is `motion`):
- // KF0 -- m -- KF1 -- m -- KF2
- // |
- // a
- // |
- // GND
- // `absolute` is made with FactorFix
- // `motion` is made with FactorOdom2d
-
- std::cout << std::setprecision(4);
-
- TimeStamp t0(0.0), t = t0;
- double dt = .01;
- VectorComposite x0("PO", Vector3d(0, 0, 0), {2, 1});
- VectorComposite s0("PO", Vector3d(sqrt(0.1), sqrt(0.1), sqrt(0.1)), {2, 1});
- Vector3d delta(2, 0, 0);
- Matrix3d delta_cov;
- delta_cov << .02, 0, 0, 0, .025, .02, 0, .02, .02;
-
- ProblemPtr Pr = Problem::create("PO", 2);
- SolverCeres solver(Pr);
-
- // KF0 and absolute prior
- SpecStateComposite prior;
- prior.emplace('P', SpecState("StatePoint2d", x0.at('P'), "factor", s0.at('P')));
- prior.emplace('O', SpecState("StateAngle", x0.at('O'), "factor", s0.at('O')));
- FrameBasePtr F0 = Pr->setPrior(prior, t0);
-
- // KF1 and motion from KF0
- t += dt;
- FrameBasePtr F1 = Pr->emplaceFrame(t, "PO", Vector3d::Zero());
- auto C1 = CaptureBase::emplace<CaptureBase>(F1, "CaptureOdom2d", t);
- auto f1 = FeatureBase::emplace<FeatureBase>(C1, "FeatureOdom2d", delta, delta_cov);
- auto c1 = FactorBase::emplace<FactorRelativePose2d>(
- f1, f1->getMeasurement(), f1->getMeasurementSquareRootInformationUpper(), F0, F1, nullptr, false, TOP_MOTION);
-
- // KF2 and motion from KF1
- t += dt;
- FrameBasePtr F2 = Pr->emplaceFrame(t, "PO", Vector3d::Zero());
- auto C2 = CaptureBase::emplace<CaptureBase>(F2, "CaptureOdom2d", t);
- auto f2 = FeatureBase::emplace<FeatureBase>(C2, "FeatureOdom2d", delta, delta_cov);
- auto c2 = FactorBase::emplace<FactorRelativePose2d>(
- f2, f2->getMeasurement(), f2->getMeasurementSquareRootInformationUpper(), F1, F2, nullptr, false, TOP_MOTION);
-
- ASSERT_TRUE(Pr->check(0));
-
- // cout << "===== full ====" << endl;
- F0->setState(Vector3d(1, 2, 3), "PO");
- F1->setState(Vector3d(2, 3, 1), "PO");
- F2->setState(Vector3d(3, 1, 2), "PO");
- std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
- // std::cout << report << std::endl;
-
- solver.computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS);
- // show(Pr);
-
- Matrix3d P1, P2;
- P1 << 0.12, 0, 0, 0, 0.525, 0.22, 0, 0.22, 0.12;
- P2 << 0.14, 0, 0, 0, 1.91, 0.48, 0, 0.48, 0.14;
-
- // get covariances
- MatrixXd P0_solver, P1_solver, P2_solver;
- ASSERT_TRUE(F0->getCovariance("PO", P0_solver));
- ASSERT_TRUE(F1->getCovariance("PO", P1_solver));
- ASSERT_TRUE(F2->getCovariance("PO", P2_solver));
-
- ASSERT_POSE2d_APPROX(F0->getStateVector("PO"), Vector3d(0, 0, 0), 1e-6);
- auto s0_vector = s0.vector("PO");
- ASSERT_MATRIX_APPROX(P0_solver, MatrixXd((s0_vector.array() * s0_vector.array()).matrix().asDiagonal()), 1e-6);
- ASSERT_POSE2d_APPROX(F1->getStateVector("PO"), Vector3d(2, 0, 0), 1e-6);
- ASSERT_MATRIX_APPROX(P1_solver, P1, 1e-6);
- ASSERT_POSE2d_APPROX(F2->getStateVector("PO"), Vector3d(4, 0, 0), 1e-6);
- ASSERT_MATRIX_APPROX(P2_solver, P2, 1e-6);
-}
-
-TEST(Odom2d, VoteForKfAndSolve)
+TEST(ProcessorOdom2d, VoteForKfAndSolve)
{
- std::string wolf_dir = _WOLF_CODE_DIR;
-
std::cout << std::setprecision(4);
// time
TimeStamp t0(0.0), t = t0;
double dt = .01;
- // Origin frame:
- // Vector3d x0(0,0,0);
- VectorComposite x0("PO", Vector3d(0, 0, 0), {2, 1});
- // Eigen::Matrix3d P0 = Eigen::Matrix3d::Identity() * 0.1;
- VectorComposite s0("PO", Vector3d(sqrt(0.1), sqrt(0.1), sqrt(0.1)), {2, 1});
// motion data
VectorXd data(Vector2d(1, M_PI_4)); // advance 1m turn pi/4 rad (45 deg). Need 8 steps for a complete turn
Eigen::MatrixXd data_cov = Eigen::MatrixXd::Identity(2, 2) * 0.01;
@@ -213,34 +98,25 @@ TEST(Odom2d, VoteForKfAndSolve)
ProblemPtr problem = Problem::create("PO", 2);
SensorBasePtr sensor_odom2d =
problem->installSensor("SensorOdom2d", wolf_dir + "/test/yaml/sensor_odom_2d.yaml", {wolf_dir});
- ParamsProcessorOdom2dPtr params(std::make_shared<ParamsProcessorOdom2d>());
- params->voting_active = true;
- params->dist_traveled = 100;
- params->angle_turned = data(1) * 2.5; // force KF at every third process()
- params->max_time_span = 100;
- params->cov_det = 100;
- params->time_tolerance = dt / 2;
- params->unmeasured_perturbation_std = 0.00;
- Matrix3d unmeasured_cov =
- params->unmeasured_perturbation_std * params->unmeasured_perturbation_std * Matrix3d::Identity();
- ProcessorOdom2dPtr processor_odom2d = ProcessorBase::emplace<ProcessorOdom2d>(sensor_odom2d, params);
+ ProcessorOdom2dPtr processor_odom2d = std::static_pointer_cast<ProcessorOdom2d>(problem->installProcessor(
+ "ProcessorOdom2d", sensor_odom2d, wolf_dir + "/test/yaml/processor_odom_2d.yaml", {wolf_dir}));
// NOTE: We integrate and create KFs as follows:
// i= 0 1 2 3 4 5 6
// KF - * -- * -- KF - * -- * -- KF - *
// Ceres wrapper
- SolverCeres solver(problem);
+ auto solver = SolverCeres::create(problem, wolf_dir + "/test/yaml/solver_ceres.yaml", {wolf_dir});
// Origin Key Frame
SpecStateComposite prior;
- prior.emplace('P', SpecState("StatePoint2d", x0.at('P'), "factor", s0.at('P')));
- prior.emplace('O', SpecState("StateAngle", x0.at('O'), "factor", s0.at('O')));
+ prior.emplace('P', SpecState("StatePoint2d", Vector2d::Zero(), "factor", Vector2d(sqrt(0.1), sqrt(0.1))));
+ prior.emplace('O', SpecState("StateAngle", Vector1d::Zero(), "factor", Vector1d(sqrt(0.1))));
FrameBasePtr KF = problem->setPrior(prior, t0);
processor_odom2d->setOrigin(KF);
- solver.solve(SolverManager::ReportVerbosity::BRIEF);
- solver.computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS);
+ solver->solve(SolverManager::ReportVerbosity::BRIEF);
+ solver->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS);
// std::cout << "Initial pose : " << problem->getCurrentState().transpose() << std::endl;
// std::cout << "Initial covariance : " << std::endl << problem->getLastFrameCovariance() << std::endl;
@@ -284,11 +160,11 @@ TEST(Odom2d, VoteForKfAndSolve)
}
else
{
- Ju = plus_jac_u(integrated_delta, data);
- Jx = plus_jac_x(integrated_delta, data);
- integrated_delta = plus(integrated_delta, data);
- integrated_delta_cov =
- Jx * integrated_delta_cov * Jx.transpose() + Ju * data_cov * Ju.transpose() + unmeasured_cov;
+ Ju = plus_jac_u(integrated_delta, data);
+ Jx = plus_jac_x(integrated_delta, data);
+ integrated_delta = plus(integrated_delta, data);
+ integrated_delta_cov = Jx * integrated_delta_cov * Jx.transpose() + Ju * data_cov * Ju.transpose() +
+ processor_odom2d->getUnmeasuredPerturbationCov();
}
WOLF_INFO("n: ", n, " t:", t);
@@ -302,7 +178,8 @@ TEST(Odom2d, VoteForKfAndSolve)
Ju = plus_jac_u(integrated_pose, data);
Jx = plus_jac_x(integrated_pose, data);
integrated_pose = plus(integrated_pose, data);
- integrated_cov = Jx * integrated_cov * Jx.transpose() + Ju * data_cov * Ju.transpose() + unmeasured_cov;
+ integrated_cov = Jx * integrated_cov * Jx.transpose() + Ju * data_cov * Ju.transpose() +
+ processor_odom2d->getUnmeasuredPerturbationCov();
ASSERT_POSE2d_APPROX(processor_odom2d->getState().vector("PO"), integrated_pose, 1e-6);
@@ -313,8 +190,8 @@ TEST(Odom2d, VoteForKfAndSolve)
}
// Solve
- std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
- solver.computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS);
+ std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
+ solver->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS);
problem->print(4, 1, 1, 1);
@@ -331,7 +208,7 @@ TEST(Odom2d, VoteForKfAndSolve)
}
}
-TEST(Odom2d, KF_callback)
+TEST(ProcessorOdom2d, KF_callback)
{
using std::cout;
using std::endl;
@@ -342,9 +219,7 @@ TEST(Odom2d, KF_callback)
// time
TimeStamp t0(0.0), t = t0;
double dt = .01;
- VectorComposite x0("PO", Vector3d(0, 0, 0), {2, 1});
- VectorComposite x0_cov("PO", Vector3d(sqrt(0.1), sqrt(0.1), sqrt(0.1)), {2, 1});
- VectorXd data(Vector2d(1, M_PI / 4)); // advance 1m
+ VectorXd data(Vector2d(1, M_PI_4)); // advance 1m
Eigen::MatrixXd data_cov = Eigen::MatrixXd::Identity(2, 2) * 0.01;
int N = 8; // number of process() steps
@@ -360,25 +235,16 @@ TEST(Odom2d, KF_callback)
ProblemPtr problem = Problem::create("PO", 2);
SensorBasePtr sensor_odom2d =
problem->installSensor("SensorOdom2d", wolf_dir + "/test/yaml/sensor_odom_2d.yaml", {wolf_dir});
- ParamsProcessorOdom2dPtr params(std::make_shared<ParamsProcessorOdom2d>());
- params->dist_traveled = 100;
- params->angle_turned = data(1) * 2.5; // force KF at every third process()
- params->max_time_span = 100;
- params->cov_det = 100;
- params->time_tolerance = dt / 2;
- params->unmeasured_perturbation_std = 0.000001;
- Matrix3d unmeasured_cov =
- params->unmeasured_perturbation_std * params->unmeasured_perturbation_std * Matrix3d::Identity();
- ProcessorOdom2dPtr processor_odom2d = ProcessorBase::emplace<ProcessorOdom2d>(sensor_odom2d, params);
- // processor_odom2d->setTimeTolerance(dt/2);
+ ProcessorOdom2dPtr processor_odom2d = std::static_pointer_cast<ProcessorOdom2d>(problem->installProcessor(
+ "ProcessorOdom2d", sensor_odom2d, wolf_dir + "/test/yaml/processor_odom_2d_inactive.yaml", {wolf_dir}));
// Ceres wrapper
- SolverCeres solver(problem);
+ auto solver = SolverCeres::create(problem, wolf_dir + "/test/yaml/solver_ceres.yaml", {wolf_dir});
// Origin Key Frame
SpecStateComposite prior;
- prior.emplace('P', SpecState("StatePoint2d", x0.at('P'), "factor", x0_cov.at('P')));
- prior.emplace('O', SpecState("StateAngle", x0.at('O'), "factor", x0_cov.at('O')));
+ prior.emplace('P', SpecState("StatePoint2d", Vector2d::Zero(), "factor", Vector2d(sqrt(0.1), sqrt(0.1))));
+ prior.emplace('O', SpecState("StateAngle", Vector1d::Zero(), "factor", Vector1d(sqrt(0.1))));
FrameBasePtr keyframe_0 = problem->setPrior(prior, t0);
processor_odom2d->setOrigin(keyframe_0);
@@ -414,17 +280,18 @@ TEST(Odom2d, KF_callback)
ASSERT_TRUE(problem->check(0));
// Integrate Delta
- Ju = plus_jac_u(integrated_delta, data);
- Jx = plus_jac_x(integrated_delta, data);
- integrated_delta = plus(integrated_delta, data);
- integrated_delta_cov =
- Jx * integrated_delta_cov * Jx.transpose() + Ju * data_cov * Ju.transpose() + unmeasured_cov;
+ Ju = plus_jac_u(integrated_delta, data);
+ Jx = plus_jac_x(integrated_delta, data);
+ integrated_delta = plus(integrated_delta, data);
+ integrated_delta_cov = Jx * integrated_delta_cov * Jx.transpose() + Ju * data_cov * Ju.transpose() +
+ processor_odom2d->getUnmeasuredPerturbationCov();
// Integrate pose
Ju = plus_jac_u(integrated_pose, data);
Jx = plus_jac_x(integrated_pose, data);
integrated_pose = plus(integrated_pose, data);
- integrated_cov = Jx * integrated_cov * Jx.transpose() + Ju * data_cov * Ju.transpose() + unmeasured_cov;
+ integrated_cov = Jx * integrated_cov * Jx.transpose() + Ju * data_cov * Ju.transpose() +
+ processor_odom2d->getUnmeasuredPerturbationCov();
// Store integrals
integrated_pose_vector.push_back(integrated_pose);
@@ -460,9 +327,9 @@ TEST(Odom2d, KF_callback)
MotionBuffer key_buffer_n = key_capture_n->getBuffer();
- std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
+ std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
// std::cout << report << std::endl;
- solver.computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS);
+ solver->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS);
ASSERT_POSE2d_APPROX(problem->getLastFrame()->getStateVector("PO"), integrated_pose_vector[n_split], 1e-6);
MatrixXd computed_cov;
@@ -497,8 +364,8 @@ TEST(Odom2d, KF_callback)
keyframe_1->setState(Vector3d(2, 3, 1), "PO");
keyframe_2->setState(Vector3d(3, 1, 2), "PO");
- report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
- solver.computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS);
+ report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
+ solver->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS);
problem->print(4, 1, 1, 1);
diff --git a/test/gtest_processor_odom_3d.cpp b/test/gtest_processor_odom_3d.cpp
index 8f1d5270afdf481ce5ac557219428fbebf9948f5..b0d7b61dd35cb4b6d10fbae131997d345ba9c0b1 100644
--- a/test/gtest_processor_odom_3d.cpp
+++ b/test/gtest_processor_odom_3d.cpp
@@ -19,139 +19,138 @@
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#include "core/utils/utils_gtest.h"
-
#include "core/common/wolf.h"
-
-
#include "core/processor/processor_odom_3d.h"
-
#include <iostream>
-#define JAC_NUMERIC(prc_ptr, D, d, dt, J_D, J_d, dx) \
-{ VectorXd Do(7); \
- prc_ptr->deltaPlusDelta(D, d, dt, Do); \
- VectorXd dd(7); \
- VectorXd DD(7); \
- VectorXd DDo(7); \
- for (int i = 0; i< 7; i++) {\
- dd = d;\
- DD = D; \
- dd(i) += dx;\
- prc_ptr->deltaPlusDelta(D, dd, dt, DDo);\
- J_d.col(i) = (DDo - Do)/dx; \
- dd = d;\
- DD = D; \
- DD(i) += dx; \
- prc_ptr->deltaPlusDelta(DD, d, dt, DDo); \
- J_D.col(i) = (DDo - Do)/dx; \
- }\
-}
+#define JAC_NUMERIC(prc_ptr, D, d, dt, J_D, J_d, dx) \
+ { \
+ VectorXd Do(7); \
+ prc_ptr->deltaPlusDelta(D, d, dt, Do); \
+ VectorXd dd(7); \
+ VectorXd DD(7); \
+ VectorXd DDo(7); \
+ for (int i = 0; i < 7; i++) \
+ { \
+ dd = d; \
+ DD = D; \
+ dd(i) += dx; \
+ prc_ptr->deltaPlusDelta(D, dd, dt, DDo); \
+ J_d.col(i) = (DDo - Do) / dx; \
+ dd = d; \
+ DD = D; \
+ DD(i) += dx; \
+ prc_ptr->deltaPlusDelta(DD, d, dt, DDo); \
+ J_D.col(i) = (DDo - Do) / dx; \
+ } \
+ }
using namespace Eigen;
using namespace std;
using namespace wolf;
-/** Gain access to members of ProcessorOdom3d
- */
-class ProcessorOdom3dTest : public ProcessorOdom3d
+class ProcessorOdom3d_test : public testing::Test
{
- public:
- ProcessorOdom3dTest();
+ public:
+ ProcessorOdom3dPtr prc;
+
+ void SetUp() override
+ {
+ // create processor
+ prc = std::static_pointer_cast<ProcessorOdom3d>(ProcessorOdom3d::create(
+ std::string(_WOLF_CODE_DIR) + "/test/yaml/processor_odom_3d.yaml", {std::string(_WOLF_CODE_DIR)}));
+ }
};
-ProcessorOdom3dTest::ProcessorOdom3dTest() : ProcessorOdom3d(std::make_shared<ParamsProcessorOdom3d>())
-{
- //
-}
-TEST(ProcessorOdom3d, computeCurrentDelta)
+TEST_F(ProcessorOdom3d_test, computeCurrentDelta)
{
- // One instance of the processor to test
- ProcessorOdom3dTest prc;
-
// input data
- Vector6d data; data.setRandom();
- double dt = 1; // irrelevant, just for the API.
+ Vector6d data;
+ data.setRandom();
+ double dt = 1; // irrelevant, just for the API.
// Build delta from Eigen tools
- Vector3d data_dp = data.head<3>();
- Vector3d data_do = data.tail<3>();
- Vector3d delta_dp = data_dp;
+ Vector3d data_dp = data.head<3>();
+ Vector3d data_do = data.tail<3>();
+ Vector3d delta_dp = data_dp;
Quaterniond delta_dq = v2q(data_do);
- Vector7d delta;
+ Vector7d delta;
delta.head<3>() = delta_dp;
delta.tail<4>() = delta_dq.coeffs();
- double dvar = 0.1;
- double rvar = 0.2;
- Vector6d diag; diag << dvar, dvar, dvar, rvar, rvar, rvar;
- Matrix6d data_cov = diag.asDiagonal();
+ double dvar = 0.1;
+ double rvar = 0.2;
+ Vector6d diag;
+ diag << dvar, dvar, dvar, rvar, rvar, rvar;
+ Matrix6d data_cov = diag.asDiagonal();
Matrix6d delta_cov = data_cov;
// return values for data2delta()
VectorXd delta_ret(7);
- MatrixXd delta_cov_ret(6,6);
- MatrixXd jac_delta_calib(6,0);
+ MatrixXd delta_cov_ret(6, 6);
+ MatrixXd jac_delta_calib(6, 0);
// call the function under test
- prc.computeCurrentDelta(data, data_cov, VectorXd::Zero(0), dt, delta_ret, delta_cov_ret, jac_delta_calib);
-
- ASSERT_POSE3d_APPROX(delta_ret , delta, Constants::EPS_SMALL);
- ASSERT_MATRIX_APPROX(delta_cov_ret , delta_cov, Constants::EPS_SMALL);
+ prc->computeCurrentDelta(data, data_cov, VectorXd::Zero(0), dt, delta_ret, delta_cov_ret, jac_delta_calib);
+ ASSERT_POSE3d_APPROX(delta_ret, delta, Constants::EPS_SMALL);
+ ASSERT_MATRIX_APPROX(delta_cov_ret, delta_cov, Constants::EPS_SMALL);
}
-TEST(ProcessorOdom3d, deltaPlusDelta)
+TEST_F(ProcessorOdom3d_test, deltaPlusDelta)
{
- ProcessorOdom3dTest prc;
-
- VectorXd D(7); D.setRandom(); D.tail<4>().normalize();
- VectorXd d(7); d.setRandom(); d *= 1; d.tail<4>().normalize();
+ VectorXd D(7);
+ D.setRandom();
+ D.tail<4>().normalize();
+ VectorXd d(7);
+ d.setRandom();
+ d *= 1;
+ d.tail<4>().normalize();
// Integrated delta value to check aginst
// Dp_int <-- Dp + Dq * dp
// Dq_int <-- Dq * dq
VectorXd D_int_check(7);
- D_int_check.head<3>() = D.head<3>() + Quaterniond(D.data()+3) * d.head<3>();
- D_int_check.tail<4>() = (Quaterniond(D.data()+3) * Quaterniond(d.data()+3)).coeffs();
+ D_int_check.head<3>() = D.head<3>() + Quaterniond(D.data() + 3) * d.head<3>();
+ D_int_check.tail<4>() = (Quaterniond(D.data() + 3) * Quaterniond(d.data() + 3)).coeffs();
- double dt = 1; // dummy, not used in Odom3d
+ double dt = 1; // dummy, not used in Odom3d
VectorXd D_int(7);
- prc.deltaPlusDelta(D, d, dt, D_int);
+ prc->deltaPlusDelta(D, d, dt, D_int);
- ASSERT_POSE3d_APPROX(D_int , D_int_check, 1e-10);
+ ASSERT_POSE3d_APPROX(D_int, D_int_check, 1e-10);
}
-TEST(ProcessorOdom3d, deltaPlusDelta_Jac)
+TEST_F(ProcessorOdom3d_test, deltaPlusDelta_Jac)
{
- std::shared_ptr<ProcessorOdom3dTest> prc_ptr = std::make_shared<ProcessorOdom3dTest>();
-
- VectorXd D(7); D.setRandom(); D.tail<4>().normalize();
- VectorXd d(7); d.setRandom(); d *= 1; d.tail<4>().normalize();
- double dt = 1;
+ VectorXd D(7);
+ D.setRandom();
+ D.tail<4>().normalize();
+ VectorXd d(7);
+ d.setRandom();
+ d *= 1;
+ d.tail<4>().normalize();
+ double dt = 1;
VectorXd Do(7);
- MatrixXd D_D(6,6);
- MatrixXd D_d(6,6);
+ MatrixXd D_D(6, 6);
+ MatrixXd D_d(6, 6);
- prc_ptr->deltaPlusDelta(D, d, dt, Do, D_D, D_d);
+ prc->deltaPlusDelta(D, d, dt, Do, D_D, D_d);
WOLF_DEBUG("DD:\n ", D_D);
WOLF_DEBUG("Dd:\n ", D_d);
- MatrixXd J_D(7,7), J_d(7,7);
+ MatrixXd J_D(7, 7), J_d(7, 7);
- JAC_NUMERIC(prc_ptr, D, d, dt, J_D, J_d, 1000*Constants::EPS);
+ JAC_NUMERIC(prc, D, d, dt, J_D, J_d, 1000 * Constants::EPS);
WOLF_DEBUG("J_D:\n ", J_D);
WOLF_DEBUG("J_d:\n ", J_d);
-
}
-
-
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_and_factor_pose_3d_with_extrinsics.cpp b/test/gtest_processor_pose_3d.cpp
similarity index 88%
rename from test/gtest_processor_and_factor_pose_3d_with_extrinsics.cpp
rename to test/gtest_processor_pose_3d.cpp
index a4eae0ba9b88689c5b25699dfdf075b96509ca4f..773ae79e1f2c72031eaacf064fb43ae2aae97306 100644
--- a/test/gtest_processor_and_factor_pose_3d_with_extrinsics.cpp
+++ b/test/gtest_processor_pose_3d.cpp
@@ -31,7 +31,9 @@
using namespace Eigen;
using namespace wolf;
-class FactorPose3dWithExtrinsicsBase_Test : public testing::Test
+std::string wolf_dir = _WOLF_CODE_DIR;
+
+class ProcessorPose3d_Base_Test : public testing::Test
{
/**
Factor graph implemented common for all the tests
@@ -48,12 +50,12 @@ class FactorPose3dWithExtrinsicsBase_Test : public testing::Test
// The problem and Pose Sensor/Processor are implemented
public:
- ProblemPtr problem_;
- SolverCeresPtr solver_;
- SensorBasePtr sensor_pose_;
- FrameBasePtr KF1_;
- FrameBasePtr KF2_;
- FrameBasePtr KF3_;
+ ProblemPtr problem_;
+ SolverManagerPtr solver_;
+ SensorBasePtr sensor_pose_;
+ FrameBasePtr KF1_;
+ FrameBasePtr KF2_;
+ FrameBasePtr KF3_;
Vector7d pose1_;
Vector7d pose2_;
@@ -110,32 +112,26 @@ class FactorPose3dWithExtrinsicsBase_Test : public testing::Test
// Problem and solver_
problem_ = Problem::create("PO", 3);
- solver_ = std::make_shared<SolverCeres>(problem_);
-
- // pose sensor and proc (to get extrinsics in the prb)
- auto params_sp = std::make_shared<ParamsSensorPose>();
- params_sp->name = "pose sensor";
- params_sp->std_noise = Vector6d::Ones();
- sensor_pose_ = SensorBase::emplace<SensorPose3d>(
- problem_->getHardware(),
- params_sp,
- SpecStateSensorComposite{{'P', SpecStateSensor("StatePoint3d", b_p_bm_, "initial_guess")},
- {'O', SpecStateSensor("StateQuaternion", b_q_m_.coeffs(), "initial_guess")}});
- auto params_proc = std::make_shared<ParamsProcessorPose>();
- params_proc->name = "pose processor";
- params_proc->time_tolerance = 0.5;
- auto proc_pose = ProcessorBase::emplace<ProcessorPose3d>(sensor_pose_, params_proc);
+ solver_ = SolverCeres::create(problem_, wolf_dir + "/test/yaml/solver_ceres.yaml", {wolf_dir});
+
+ // pose sensor (load yaml and modify to put random extrinsics)
+ auto params = YAML::LoadFile(wolf_dir + "/test/yaml/sensor_pose_3d_initial_guess.yaml");
+ params["states"]["P"]["state"] = b_p_bm_;
+ params["states"]["O"]["state"] = b_q_m_.coeffs();
+ sensor_pose_ = problem_->installSensor(params, {wolf_dir});
+ auto proc_pose = problem_->installProcessor(
+ "ProcessorPose3d", sensor_pose_, wolf_dir + "/test/yaml/processor_pose_3d.yaml", {wolf_dir});
}
void TearDown() override{};
};
-class FactorPose3dWithExtrinsicsMANUAL_Test : public FactorPose3dWithExtrinsicsBase_Test
+class ProcessorPose3d_Manual_Test : public ProcessorPose3d_Base_Test
{
public:
void SetUp() override
{
- FactorPose3dWithExtrinsicsBase_Test::SetUp();
+ ProcessorPose3d_Base_Test::SetUp();
// Two frames
KF1_ = problem_->emplaceFrame(1, "PO", pose1_);
KF2_ = problem_->emplaceFrame(2, "PO", pose2_);
@@ -207,12 +203,12 @@ class FactorPose3dWithExtrinsicsMANUAL_Test : public FactorPose3dWithExtrinsicsB
void TearDown() override{};
};
-class FactorPose3dWithExtrinsics_ProcessorWithKeyFrameCallbackFirst_Test : public FactorPose3dWithExtrinsicsBase_Test
+class ProcessorPose3d_WithKeyFrameCallbackFirst_Test : public ProcessorPose3d_Base_Test
{
public:
void SetUp() override
{
- FactorPose3dWithExtrinsicsBase_Test::SetUp();
+ ProcessorPose3d_Base_Test::SetUp();
// Two frames
KF1_ = problem_->emplaceFrame(1, "PO", pose1_);
KF2_ = problem_->emplaceFrame(2, "PO", pose2_);
@@ -261,12 +257,12 @@ class FactorPose3dWithExtrinsics_ProcessorWithKeyFrameCallbackFirst_Test : publi
void TearDown() override{};
};
-class FactorPose3dWithExtrinsics_ProcessorWithProcessFirst_Test : public FactorPose3dWithExtrinsicsBase_Test
+class ProcessorPose3d_WithProcessFirst_Test : public ProcessorPose3d_Base_Test
{
public:
void SetUp() override
{
- FactorPose3dWithExtrinsicsBase_Test::SetUp();
+ ProcessorPose3d_Base_Test::SetUp();
// Two frames
KF1_ = problem_->emplaceFrame(1, "PO", pose1_);
KF2_ = problem_->emplaceFrame(2, "PO", pose2_);
@@ -317,12 +313,12 @@ class FactorPose3dWithExtrinsics_ProcessorWithProcessFirst_Test : public FactorP
void TearDown() override{};
};
-TEST_F(FactorPose3dWithExtrinsicsMANUAL_Test, check_tree)
+TEST_F(ProcessorPose3d_Manual_Test, check_tree)
{
ASSERT_TRUE(problem_->check(0));
}
-TEST_F(FactorPose3dWithExtrinsicsMANUAL_Test, error)
+TEST_F(ProcessorPose3d_Manual_Test, error)
{
// Since initialized at the right state, error function should return 0
std::vector<FrameBasePtr> KF_vec = {KF1_, KF2_, KF3_};
@@ -339,7 +335,7 @@ TEST_F(FactorPose3dWithExtrinsicsMANUAL_Test, error)
}
}
-TEST_F(FactorPose3dWithExtrinsicsMANUAL_Test, solve)
+TEST_F(ProcessorPose3d_Manual_Test, solve)
{
problem_->perturb();
std::string report = solver_->solve(SolverManager::ReportVerbosity::FULL);
@@ -350,12 +346,12 @@ TEST_F(FactorPose3dWithExtrinsicsMANUAL_Test, solve)
ASSERT_QUATERNION_VECTOR_APPROX(sensor_pose_->getO()->getState(), b_q_m_.coeffs(), 1e-6);
}
-TEST_F(FactorPose3dWithExtrinsics_ProcessorWithKeyFrameCallbackFirst_Test, check_tree)
+TEST_F(ProcessorPose3d_WithKeyFrameCallbackFirst_Test, check_tree)
{
ASSERT_TRUE(problem_->check(0));
}
-TEST_F(FactorPose3dWithExtrinsics_ProcessorWithKeyFrameCallbackFirst_Test, solve)
+TEST_F(ProcessorPose3d_WithKeyFrameCallbackFirst_Test, solve)
{
problem_->perturb();
std::string report = solver_->solve(SolverManager::ReportVerbosity::FULL);
@@ -366,12 +362,12 @@ TEST_F(FactorPose3dWithExtrinsics_ProcessorWithKeyFrameCallbackFirst_Test, solve
ASSERT_QUATERNION_VECTOR_APPROX(sensor_pose_->getO()->getState(), b_q_m_.coeffs(), 1e-6);
}
-TEST_F(FactorPose3dWithExtrinsics_ProcessorWithProcessFirst_Test, check_tree)
+TEST_F(ProcessorPose3d_WithProcessFirst_Test, check_tree)
{
ASSERT_TRUE(problem_->check(0));
}
-TEST_F(FactorPose3dWithExtrinsics_ProcessorWithProcessFirst_Test, solve)
+TEST_F(ProcessorPose3d_WithProcessFirst_Test, solve)
{
problem_->perturb();
std::string report = solver_->solve(SolverManager::ReportVerbosity::FULL);
diff --git a/test/gtest_processor_tracker_feature_dummy.cpp b/test/gtest_processor_tracker_feature_dummy.cpp
index 55e25b981d36bbb31ead58e780dec7b188e95056..41da876257905410f3803e511482e92bc4c57006 100644
--- a/test/gtest_processor_tracker_feature_dummy.cpp
+++ b/test/gtest_processor_tracker_feature_dummy.cpp
@@ -30,99 +30,12 @@ using namespace wolf;
std::string wolf_dir = _WOLF_CODE_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;
- };
-};
-
-bool isFeatureLinked(FeatureBasePtr ftr, CaptureBasePtr cap)
-{
- auto feature_list = cap->getFeatureList();
- return ftr->getCapture() == cap && std::find(feature_list.begin(), feature_list.end(), ftr) != feature_list.end();
-}
-
-bool isFactorLinked(FactorBasePtr fac, FeatureBasePtr ftr)
-{
- auto factor_list = ftr->getFactorList();
- return fac->getFeature() == ftr && std::find(factor_list.begin(), factor_list.end(), fac) != factor_list.end();
-}
-
-// Use the following in case you want to initialize tests with predefines variables or methods.
class ProcessorTrackerFeatureDummyTest : public testing::Test
{
public:
- ProblemPtr problem;
- SensorBasePtr sensor;
- ParamsProcessorTrackerFeatureDummyPtr params;
- ProcessorTrackerFeatureDummyDummyPtr processor;
+ ProblemPtr problem;
+ SensorBasePtr sensor;
+ ProcessorTrackerFeatureDummyPtr processor;
~ProcessorTrackerFeatureDummyTest() override {}
@@ -135,14 +48,11 @@ class ProcessorTrackerFeatureDummyTest : public testing::Test
sensor = problem->installSensor("SensorOdom2d", wolf_dir + "/test/yaml/sensor_odom_2d.yaml", {wolf_dir});
// Install processor
- params = std::make_shared<ParamsProcessorTrackerFeatureDummy>();
- params->max_new_features = 10;
- params->min_features_for_keyframe = 7;
- params->time_tolerance = 0.25;
- params->voting_active = true;
- params->n_tracks_lost = 1; // 1 (the first) track is lost each time trackFeatures is called
- processor = std::make_shared<ProcessorTrackerFeatureDummyDummy>(params);
- processor->link(sensor);
+ processor = std::static_pointer_cast<ProcessorTrackerFeatureDummy>(
+ problem->installProcessor("ProcessorTrackerFeatureDummy",
+ sensor,
+ wolf_dir + "/test/yaml/processor_tracker_feature_dummy.yaml",
+ {wolf_dir}));
}
};
@@ -161,13 +71,16 @@ TEST_F(ProcessorTrackerFeatureDummyTest, callDetectNewFeatures)
FeatureBasePtrList feat_list;
// demo callDetectNewFeatures
- unsigned int n_feat = processor->callDetectNewFeatures(params->max_new_features, last_cap, feat_list);
+ unsigned int n_feat =
+ processor->callDetectNewFeatures(processor->getParams()["max_new_features"].as<int>(), last_cap, feat_list);
ASSERT_EQ(n_feat, feat_list.size());
- ASSERT_EQ(n_feat, params->max_new_features);
+ ASSERT_EQ(n_feat, processor->getParams()["max_new_features"].as<int>());
// check the features are emplaced
ASSERT_EQ(n_feat, last_cap->getFeatureList().size());
- for (auto feat : feat_list) ASSERT_TRUE(isFeatureLinked(feat, last_cap));
+
+ // check everything well linked
+ ASSERT_TRUE(problem->check(0));
}
TEST_F(ProcessorTrackerFeatureDummyTest, trackFeatures)
@@ -179,7 +92,7 @@ TEST_F(ProcessorTrackerFeatureDummyTest, trackFeatures)
FeatureBasePtrList feat_list;
// fill feat_last list
- processor->callDetectNewFeatures(params->max_new_features, last_cap, feat_list);
+ processor->callDetectNewFeatures(processor->getParams()["max_new_features"].as<int>(), last_cap, feat_list);
// Put a capture on incoming_ptr_
CaptureBasePtr inc_cap = std::make_shared<CaptureVoid>(1, sensor);
@@ -194,7 +107,9 @@ TEST_F(ProcessorTrackerFeatureDummyTest, trackFeatures)
// check the features are emplaced
ASSERT_EQ(inc_cap->getFeatureList().size(), feat_list_out.size());
- for (auto feat : feat_list_out) ASSERT_TRUE(isFeatureLinked(feat, inc_cap));
+
+ // check everything well linked
+ ASSERT_TRUE(problem->check(0));
}
TEST_F(ProcessorTrackerFeatureDummyTest, processNew)
@@ -214,9 +129,8 @@ TEST_F(ProcessorTrackerFeatureDummyTest, processNew)
ASSERT_EQ(processor->getIncoming()->getFeatureList().size(), 9); // 1 track lost
ASSERT_EQ(processor->getMatchesLastFromIncoming().size(), 9); // 1 track lost
- // check the features are emplaced
- for (auto feat : last_cap->getFeatureList()) ASSERT_TRUE(isFeatureLinked(feat, last_cap));
- for (auto feat : inc_cap->getFeatureList()) ASSERT_TRUE(isFeatureLinked(feat, inc_cap));
+ // check everything well linked
+ ASSERT_TRUE(problem->check(0));
}
TEST_F(ProcessorTrackerFeatureDummyTest, processKnown)
@@ -248,12 +162,8 @@ TEST_F(ProcessorTrackerFeatureDummyTest, processKnown)
ASSERT_EQ(processor->getIncoming()->getFeatureList().size(), 13); // 1 track lost
ASSERT_EQ(processor->getMatchesLastFromIncoming().size(), 13); // 1 track lost
- // check the features are emplaced
- for (auto feat : processor->getOrigin()->getFeatureList())
- ASSERT_TRUE(isFeatureLinked(feat, processor->getOrigin()));
- for (auto feat : processor->getLast()->getFeatureList()) ASSERT_TRUE(isFeatureLinked(feat, processor->getLast()));
- for (auto feat : processor->getIncoming()->getFeatureList())
- ASSERT_TRUE(isFeatureLinked(feat, processor->getIncoming()));
+ // check everything well linked
+ ASSERT_TRUE(problem->check(0));
}
TEST_F(ProcessorTrackerFeatureDummyTest, emplaceFactor)
@@ -272,13 +182,15 @@ TEST_F(ProcessorTrackerFeatureDummyTest, emplaceFactor)
FactorBasePtr fac = processor->callEmplaceFactor(ftr, ftr_other);
// check factor correctly emplaced
- ASSERT_TRUE(isFactorLinked(fac, ftr));
ASSERT_EQ(fac->getFramesFactored().size(), 2);
ASSERT_TRUE(fac->getCapturesFactored().empty());
ASSERT_TRUE(fac->getLandmarksFactored().empty());
ASSERT_TRUE(fac->getSensorsFactored().empty());
ASSERT_TRUE(fac->hasFrame(frm));
ASSERT_TRUE(fac->hasFrame(frm_other));
+
+ // check everything well linked
+ ASSERT_TRUE(problem->check(0));
}
TEST_F(ProcessorTrackerFeatureDummyTest, establishFactors)
@@ -332,6 +244,9 @@ TEST_F(ProcessorTrackerFeatureDummyTest, establishFactors)
}
ASSERT_EQ(n_factors_ori, 14);
ASSERT_EQ(n_factors_last, 14);
+
+ // check everything well linked
+ ASSERT_TRUE(problem->check(0));
}
TEST_F(ProcessorTrackerFeatureDummyTest, process)
@@ -350,8 +265,8 @@ TEST_F(ProcessorTrackerFeatureDummyTest, process)
CaptureBasePtr cap2 = std::make_shared<CaptureVoid>(1, sensor);
cap2->process();
- ASSERT_EQ(processor->getOrigin()->getFeatureList().size(), params->max_new_features);
- ASSERT_EQ(processor->getLast()->getFeatureList().size(), params->max_new_features - 1);
+ ASSERT_EQ(processor->getOrigin()->getFeatureList().size(), processor->getParams()["max_new_features"].as<int>());
+ ASSERT_EQ(processor->getLast()->getFeatureList().size(), processor->getParams()["max_new_features"].as<int>() - 1);
ASSERT_TRUE(problem->check(0));
// 3RD TIME
@@ -359,7 +274,7 @@ TEST_F(ProcessorTrackerFeatureDummyTest, process)
CaptureBasePtr cap3 = std::make_shared<CaptureVoid>(2, sensor);
cap3->process();
- ASSERT_EQ(processor->getLast()->getFeatureList().size(), params->max_new_features - 2);
+ ASSERT_EQ(processor->getLast()->getFeatureList().size(), processor->getParams()["max_new_features"].as<int>() - 2);
ASSERT_TRUE(problem->check(0));
// 4TH TIME
@@ -367,7 +282,7 @@ TEST_F(ProcessorTrackerFeatureDummyTest, process)
CaptureBasePtr cap4 = std::make_shared<CaptureVoid>(3, sensor);
cap4->process();
- ASSERT_EQ(processor->getLast()->getFeatureList().size(), params->max_new_features - 3);
+ ASSERT_EQ(processor->getLast()->getFeatureList().size(), processor->getParams()["max_new_features"].as<int>() - 3);
ASSERT_TRUE(problem->check(0));
// 5TH TIME -> KF in cap4 (tracked features < 7 (params->min_features_for_keyframe))
@@ -382,7 +297,9 @@ TEST_F(ProcessorTrackerFeatureDummyTest, process)
WOLF_INFO("checking factors...");
problem->print(4, 1, 0, 0);
TrackMatrix track_matrix = processor->getTrackMatrix();
- ASSERT_EQ(cap4->getFeatureList().size(), params->min_features_for_keyframe + params->max_new_features);
+ ASSERT_EQ(cap4->getFeatureList().size(),
+ processor->getParams()["keyframe_vote"]["min_features_for_keyframe"].as<int>() +
+ processor->getParams()["max_new_features"].as<int>());
ASSERT_TRUE(problem->check(0));
unsigned int n_factors0 = 0;
unsigned int n_factors4 = 0;
diff --git a/test/gtest_processor_tracker_landmark_dummy.cpp b/test/gtest_processor_tracker_landmark_dummy.cpp
index 8cce8ff7fedfacae7c01167f9368af44e8831532..3f8dae8a4cd65384ff31e4f564794f6dbc530930 100644
--- a/test/gtest_processor_tracker_landmark_dummy.cpp
+++ b/test/gtest_processor_tracker_landmark_dummy.cpp
@@ -30,121 +30,12 @@ using namespace wolf;
std::string wolf_dir = _WOLF_CODE_DIR;
-WOLF_PTR_TYPEDEFS(ProcessorTrackerLandmarkDummyDummy);
-
-class ProcessorTrackerLandmarkDummyDummy : public ProcessorTrackerLandmarkDummy
-{
- public:
- ProcessorTrackerLandmarkDummyDummy(ParamsProcessorTrackerLandmarkDummyPtr& _params)
- : ProcessorTrackerLandmarkDummy(_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 callFindLandmarks(const LandmarkBasePtrList& _landmarks_in,
- const CaptureBasePtr& _capture,
- FeatureBasePtrList& _features_out,
- LandmarkMatchMap& _feature_landmark_correspondences)
- {
- return this->findLandmarks(_landmarks_in, _capture, _features_out, _feature_landmark_correspondences);
- }
-
- LandmarkBasePtr callEmplaceLandmark(FeatureBasePtr _feature_ptr)
- {
- return this->emplaceLandmark(_feature_ptr);
- }
- void callEmplaceNewLandmarks()
- {
- this->emplaceNewLandmarks();
- }
- FactorBasePtr callEmplaceFactor(FeatureBasePtr _feature_ptr, LandmarkBasePtr _landmark_ptr)
- {
- return this->emplaceFactor(_feature_ptr, _landmark_ptr);
- }
-
- void callEstablishFactors()
- {
- this->establishFactors();
- }
-
- void setNewFeaturesLast(FeatureBasePtrList& _new_features_list)
- {
- new_features_last_ = _new_features_list;
- }
-
- LandmarkMatchMap getMatchesLandmarkFromIncoming()
- {
- return matches_landmark_from_incoming_;
- }
- LandmarkMatchMap getMatchesLandmarkFromLast()
- {
- return matches_landmark_from_last_;
- }
- LandmarkBasePtrList getNewLandmarks()
- {
- return new_landmarks_;
- }
-
- void callReset()
- {
- this->resetDerived();
- origin_ptr_ = last_ptr_;
- last_ptr_ = incoming_ptr_;
- incoming_ptr_ = nullptr;
- };
-};
-
-bool isFeatureLinked(FeatureBasePtr ftr, CaptureBasePtr cap)
-{
- auto feature_list = cap->getFeatureList();
- return ftr->getCapture() == cap && std::find(feature_list.begin(), feature_list.end(), ftr) != feature_list.end();
-}
-
-bool isFactorLinked(FactorBasePtr fac, FeatureBasePtr ftr)
-{
- auto factor_list = ftr->getFactorList();
- return fac->getFeature() == ftr && std::find(factor_list.begin(), factor_list.end(), fac) != factor_list.end();
-}
-
-bool isLandmarkLinked(LandmarkBasePtr lmk, MapBasePtr map)
-{
- auto landmark_list = map->getLandmarkList();
- return lmk->getMap() == map && std::find(landmark_list.begin(), landmark_list.end(), lmk) != landmark_list.end();
-}
-
-// Use the following in case you want to initialize tests with predefines variables or methods.
class ProcessorTrackerLandmarkDummyTest : public testing::Test
{
public:
- ProblemPtr problem;
- SensorBasePtr sensor;
- ParamsProcessorTrackerLandmarkDummyPtr params;
- ProcessorTrackerLandmarkDummyDummyPtr processor;
+ ProblemPtr problem;
+ SensorBasePtr sensor;
+ ProcessorTrackerLandmarkDummyPtr processor;
~ProcessorTrackerLandmarkDummyTest() override {}
@@ -157,14 +48,11 @@ class ProcessorTrackerLandmarkDummyTest : public testing::Test
sensor = problem->installSensor("SensorOdom2d", wolf_dir + "/test/yaml/sensor_odom_2d.yaml", {wolf_dir});
// Install processor
- params = std::make_shared<ParamsProcessorTrackerLandmarkDummy>();
- params->max_new_features = 10;
- params->min_features_for_keyframe = 7;
- params->time_tolerance = 0.25;
- params->voting_active = true;
- params->n_landmarks_lost = 1; // 1 (the last) landmark is not found each time findLandmark is called
- processor = std::make_shared<ProcessorTrackerLandmarkDummyDummy>(params);
- processor->link(sensor);
+ processor = std::static_pointer_cast<ProcessorTrackerLandmarkDummy>(
+ problem->installProcessor("ProcessorTrackerLandmarkDummy",
+ sensor,
+ wolf_dir + "/test/yaml/processor_tracker_landmark_dummy.yaml",
+ {wolf_dir}));
}
};
@@ -183,13 +71,16 @@ TEST_F(ProcessorTrackerLandmarkDummyTest, detectNewFeatures)
FeatureBasePtrList feat_list;
// demo callDetectNewFeatures
- unsigned int n_feat = processor->callDetectNewFeatures(params->max_new_features, last_cap, feat_list);
- ASSERT_EQ(n_feat, feat_list.size()); // detected 10 features
- ASSERT_EQ(n_feat, params->max_new_features); // detected 10 features
+ unsigned int n_feat =
+ processor->callDetectNewFeatures(processor->getParams()["max_new_features"].as<int>(), last_cap, feat_list);
+ ASSERT_EQ(n_feat, feat_list.size()); // detected 10 features
+ ASSERT_EQ(n_feat, processor->getParams()["max_new_features"].as<int>()); // detected 10 features
// check the features are emplaced
ASSERT_EQ(n_feat, last_cap->getFeatureList().size());
- for (auto feat : feat_list) ASSERT_TRUE(isFeatureLinked(feat, last_cap));
+
+ // check everything well linked
+ ASSERT_TRUE(problem->check(0));
}
TEST_F(ProcessorTrackerLandmarkDummyTest, emplaceLandmark)
@@ -201,15 +92,17 @@ TEST_F(ProcessorTrackerLandmarkDummyTest, emplaceLandmark)
FeatureBasePtrList feat_list;
// demo callDetectNewFeatures
- unsigned int n_feat = processor->callDetectNewFeatures(params->max_new_features, last_cap, feat_list);
+ unsigned int n_feat =
+ processor->callDetectNewFeatures(processor->getParams()["max_new_features"].as<int>(), last_cap, feat_list);
for (auto ftr : feat_list)
{
auto lmk = processor->callEmplaceLandmark(ftr);
- // check that it is correctly emplaced
- ASSERT_TRUE(isLandmarkLinked(lmk, problem->getMap()));
}
ASSERT_EQ(problem->getMap()->getLandmarkList().size(), n_feat); // emplaced 10 landmarks
+
+ // check everything well linked
+ ASSERT_TRUE(problem->check(0));
}
TEST_F(ProcessorTrackerLandmarkDummyTest, emplaceNewLandmarks)
@@ -221,15 +114,16 @@ TEST_F(ProcessorTrackerLandmarkDummyTest, emplaceNewLandmarks)
FeatureBasePtrList feat_list;
// test detectNewFeatures
- unsigned int n_feat = processor->callDetectNewFeatures(params->max_new_features, last_cap, feat_list);
+ unsigned int n_feat =
+ processor->callDetectNewFeatures(processor->getParams()["max_new_features"].as<int>(), last_cap, feat_list);
// test createNewLandmarks
processor->setNewFeaturesLast(feat_list);
processor->callEmplaceNewLandmarks();
ASSERT_EQ(processor->getNewLandmarks().size(), n_feat); // emplaced 10 landmarks
- // check that it is correctly emplaced
- for (auto lmk : processor->getNewLandmarks()) ASSERT_TRUE(isLandmarkLinked(lmk, problem->getMap()));
+ // check everything well linked
+ ASSERT_TRUE(problem->check(0));
}
TEST_F(ProcessorTrackerLandmarkDummyTest, findLandmarks)
@@ -241,7 +135,7 @@ TEST_F(ProcessorTrackerLandmarkDummyTest, findLandmarks)
FeatureBasePtrList feat_list;
// test detectNewFeatures
- processor->callDetectNewFeatures(params->max_new_features, last_cap, feat_list);
+ processor->callDetectNewFeatures(processor->getParams()["max_new_features"].as<int>(), last_cap, feat_list);
// test createNewLandmarks
processor->setNewFeaturesLast(feat_list);
@@ -259,7 +153,8 @@ TEST_F(ProcessorTrackerLandmarkDummyTest, findLandmarks)
ASSERT_EQ(feature_landmark_correspondences.size(), feat_found.size());
ASSERT_EQ(feat_list.size(), feat_found.size() + 1); // one of each 10 tracks is lost
- for (auto feat : feat_found) ASSERT_TRUE(isFeatureLinked(feat, inc_cap));
+ // check everything well linked
+ ASSERT_TRUE(problem->check(0));
}
TEST_F(ProcessorTrackerLandmarkDummyTest, processNew)
@@ -282,6 +177,9 @@ TEST_F(ProcessorTrackerLandmarkDummyTest, processNew)
ASSERT_EQ(processor->getMatchesLandmarkFromLast().size(),
10); // all last features have the landmark correspondence
ASSERT_EQ(processor->getMatchesLandmarkFromIncoming().size(), 9); // 1 of each 10 landmarks is not found
+
+ // check everything well linked
+ ASSERT_TRUE(problem->check(0));
}
TEST_F(ProcessorTrackerLandmarkDummyTest, processKnown)
@@ -292,7 +190,7 @@ TEST_F(ProcessorTrackerLandmarkDummyTest, processKnown)
// create 10 landmarks and link them to map
FeatureBasePtrList feat_list;
- processor->callDetectNewFeatures(params->max_new_features, last_cap, feat_list);
+ processor->callDetectNewFeatures(processor->getParams()["max_new_features"].as<int>(), last_cap, feat_list);
processor->setNewFeaturesLast(feat_list);
processor->callEmplaceNewLandmarks();
ASSERT_EQ(problem->getMap()->getLandmarkList().size(), 10); // created 10 landmarks
@@ -302,10 +200,14 @@ TEST_F(ProcessorTrackerLandmarkDummyTest, processKnown)
processor->setInc(inc_cap);
// Test processKnown
- processor->callProcessKnown();
+ auto n_tracked_lmk = processor->callProcessKnown();
+ ASSERT_EQ(n_tracked_lmk, 9);
ASSERT_EQ(processor->getIncoming()->getFeatureList().size(), 9); // 1 of each 10 landmarks is not found
ASSERT_EQ(processor->getMatchesLandmarkFromIncoming().size(), 9); // 1 of each 10 landmarks is not found
+
+ // check everything well linked
+ ASSERT_TRUE(problem->check(0));
}
TEST_F(ProcessorTrackerLandmarkDummyTest, emplaceFactor)
@@ -325,6 +227,9 @@ TEST_F(ProcessorTrackerLandmarkDummyTest, emplaceFactor)
ASSERT_EQ(fac->getLandmarksFactored().size(), 1);
ASSERT_TRUE(fac->hasFrame(frm));
ASSERT_TRUE(fac->hasLandmark(lmk));
+
+ // check everything well linked
+ ASSERT_TRUE(problem->check(0));
}
TEST_F(ProcessorTrackerLandmarkDummyTest, establishFactors)
@@ -357,7 +262,8 @@ TEST_F(ProcessorTrackerLandmarkDummyTest, establishFactors)
n_factors_last++;
ASSERT_EQ(feat->getFactorList().size(), 1);
ASSERT_EQ(feat->getFactorList().front()->getFeature(), feat);
- ASSERT_EQ(feat->getFactorList().front()->getLandmarksFactored().front().lock(), landmark_from_last[feat]->landmark_ptr_);
+ ASSERT_EQ(feat->getFactorList().front()->getLandmarksFactored().front().lock(),
+ landmark_from_last[feat]->landmark_ptr_);
}
for (auto lmk : problem->getMap()->getLandmarkList())
@@ -369,6 +275,9 @@ TEST_F(ProcessorTrackerLandmarkDummyTest, establishFactors)
}
ASSERT_EQ(n_factors_last, 15);
ASSERT_EQ(n_factors_landmark, 15);
+
+ // check everything well linked
+ ASSERT_TRUE(problem->check(0));
}
TEST_F(ProcessorTrackerLandmarkDummyTest, process)
@@ -386,24 +295,24 @@ TEST_F(ProcessorTrackerLandmarkDummyTest, process)
CaptureBasePtr cap2 = std::make_shared<CaptureVoid>(1, sensor);
cap2->process();
- ASSERT_EQ(processor->getOrigin()->getFeatureList().size(), params->max_new_features);
- ASSERT_EQ(processor->getLast()->getFeatureList().size(), params->max_new_features - 1);
+ ASSERT_EQ(processor->getOrigin()->getFeatureList().size(), processor->getParams()["max_new_features"].as<int>());
+ ASSERT_EQ(processor->getLast()->getFeatureList().size(), processor->getParams()["max_new_features"].as<int>() - 1);
// 3RD TIME
WOLF_DEBUG("Third time...");
CaptureBasePtr cap3 = std::make_shared<CaptureVoid>(2, sensor);
cap3->process();
- ASSERT_EQ(processor->getLast()->getFeatureList().size(), params->max_new_features - 2);
+ ASSERT_EQ(processor->getLast()->getFeatureList().size(), processor->getParams()["max_new_features"].as<int>() - 2);
// 4TH TIME
WOLF_DEBUG("Forth time...");
CaptureBasePtr cap4 = std::make_shared<CaptureVoid>(3, sensor);
cap4->process();
- ASSERT_EQ(processor->getLast()->getFeatureList().size(), params->max_new_features - 3);
+ ASSERT_EQ(processor->getLast()->getFeatureList().size(), processor->getParams()["max_new_features"].as<int>() - 3);
- // 5TH TIME -> KF in cap4 (found landmarks < 7 (params->min_features_for_keyframe))
+ // 5TH TIME -> KF in cap4 (found landmarks < 7 (min_features_for_keyframe))
WOLF_DEBUG("Fifth time...");
CaptureBasePtr cap5 = std::make_shared<CaptureVoid>(4, sensor);
cap5->process();
diff --git a/test/gtest_rotation.cpp b/test/gtest_rotation.cpp
index 0cba6a4f5cea42c2259308e337329d4a81688251..1262d9873988c8dc11157cde66386316b6f3ba75 100644
--- a/test/gtest_rotation.cpp
+++ b/test/gtest_rotation.cpp
@@ -18,14 +18,14 @@
// 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/>.
-//Eigen
+// Eigen
#include <Eigen/Geometry>
-//Wolf
+// Wolf
#include "core/common/wolf.h"
#include "core/math/rotations.h"
-//std
+// std
#include <iostream>
#include <fstream>
#include <iomanip>
@@ -42,13 +42,13 @@ using namespace Eigen;
TEST(exp_q, unit_norm)
{
- Vector3d v0 = Vector3d::Random();
- double scale = 1.0;
+ Vector3d v0 = Vector3d::Random();
+ double scale = 1.0;
for (int i = 0; i < 20; i++)
{
- Vector3d v = v0 * scale;
+ Vector3d v = v0 * scale;
Quaterniond q = exp_q(v);
- ASSERT_NEAR(q.norm(), 1.0, 1e-10) << "Failed at scale 1e-" << i << " with angle = " << 2.0*q.vec().norm();
+ ASSERT_NEAR(q.norm(), 1.0, 1e-10) << "Failed at scale 1e-" << i << " with angle = " << 2.0 * q.vec().norm();
scale /= 10;
}
}
@@ -56,12 +56,12 @@ TEST(exp_q, unit_norm)
TEST(rotations, pi2pi)
{
ASSERT_NEAR(M_PI_2, pi2pi((double)M_PI_2), 1e-10);
- ASSERT_NEAR(-M_PI_2, pi2pi(3.0*M_PI_2), 1e-10);
+ ASSERT_NEAR(-M_PI_2, pi2pi(3.0 * M_PI_2), 1e-10);
ASSERT_NEAR(-M_PI_2, pi2pi(-M_PI_2), 1e-10);
- ASSERT_NEAR(M_PI_2, pi2pi(-3.0*M_PI_2), 1e-10);
+ ASSERT_NEAR(M_PI_2, pi2pi(-3.0 * M_PI_2), 1e-10);
// ASSERT_NEAR(M_PI, pi2pi(M_PI), 1e-10); // Exact PI is not safely testable because of numeric issues.
- ASSERT_NEAR(M_PI-.01, pi2pi(M_PI-.01), 1e-10);
- ASSERT_NEAR(-M_PI+.01, pi2pi(M_PI+.01), 1e-10);
+ ASSERT_NEAR(M_PI - .01, pi2pi(M_PI - .01), 1e-10);
+ ASSERT_NEAR(-M_PI + .01, pi2pi(M_PI + .01), 1e-10);
}
TEST(skew, Skew_vee)
@@ -81,27 +81,27 @@ TEST(skew, Skew_vee)
TEST(exp_q, v2q_q2v)
{
using namespace wolf;
- //defines scalars
- double deg_to_rad = M_PI/180.0;
+ // defines scalars
+ double deg_to_rad = M_PI / 180.0;
Vector4d vec0, vec1;
- //v2q
+ // v2q
Vector3d rot_vector0, rot_vector1;
rot_vector0 = Vector3d::Random();
- rot_vector1 = rot_vector0 * 100 *deg_to_rad; //far from origin
- rot_vector0 = rot_vector0*deg_to_rad;
+ rot_vector1 = rot_vector0 * 100 * deg_to_rad; // far from origin
+ rot_vector0 = rot_vector0 * deg_to_rad;
Quaterniond quat0, quat1;
quat0 = v2q(rot_vector0);
quat1 = v2q(rot_vector1);
- //q2v
+ // q2v
Vector3d quat_to_v0, quat_to_v1;
VectorXd quat_to_v0x, quat_to_v1x;
- quat_to_v0 = q2v(quat0);
- quat_to_v1 = q2v(quat1);
+ quat_to_v0 = q2v(quat0);
+ quat_to_v1 = q2v(quat1);
quat_to_v0x = q2v(quat0);
quat_to_v1x = q2v(quat1);
@@ -114,27 +114,27 @@ TEST(exp_q, v2q_q2v)
TEST(exp_R, v2R_R2v)
{
using namespace wolf;
- //First test is to verify we get the good result with v -> v2R -> R2v -> v
- //test 2 : how small can angles in rotation vector be ?
+ // First test is to verify we get the good result with v -> v2R -> R2v -> v
+ // test 2 : how small can angles in rotation vector be ?
- //definition
- double deg_to_rad = M_PI/180.0;
+ // definition
+ double deg_to_rad = M_PI / 180.0;
Vector3d rot_vector0, rot_vector1;
rot_vector0 = Vector3d::Random();
- rot_vector1 = rot_vector0 * 100 *deg_to_rad; //far from origin
- rot_vector0 = rot_vector0*deg_to_rad;
+ rot_vector1 = rot_vector0 * 100 * deg_to_rad; // far from origin
+ rot_vector0 = rot_vector0 * deg_to_rad;
Matrix3d rot0, rot1;
rot0 = v2R(rot_vector0);
rot1 = v2R(rot_vector1);
- //R2v
+ // R2v
Vector3d rot0_vec, rot1_vec;
rot0_vec = R2v(rot0);
rot1_vec = R2v(rot1);
- //check now
+ // check now
ASSERT_MATRIX_APPROX(rot0_vec, rot_vector0, wolf::Constants::EPS);
ASSERT_MATRIX_APPROX(rot1_vec, rot_vector1, wolf::Constants::EPS);
}
@@ -142,41 +142,43 @@ TEST(exp_R, v2R_R2v)
TEST(log_R, R2v_v2R_limits)
{
using namespace wolf;
- //test 2 : how small can angles in rotation vector be ?
- double scale = 1;
+ // test 2 : how small can angles in rotation vector be ?
+ double scale = 1;
Matrix3d v_to_R, initial_matrix;
- Vector3d R_to_v;
+ Vector3d R_to_v;
- //Vector3d rv;
- for(int i = 0; i<8; i++){
+ // Vector3d rv;
+ for (int i = 0; i < 8; i++)
+ {
initial_matrix = v2R(Vector3d::Random().eval() * scale);
- R_to_v = R2v(initial_matrix);
+ R_to_v = R2v(initial_matrix);
v_to_R = v2R(R_to_v);
ASSERT_MATRIX_APPROX(v_to_R, initial_matrix, wolf::Constants::EPS);
- scale = scale*0.1;
+ scale = scale * 0.1;
}
}
TEST(log_R, R2v_v2R_AAlimits)
{
using namespace wolf;
- //let's see how small the angles can be here : limit reached at scale/10 = 1e-16
- double scale = 1;
+ // let's see how small the angles can be here : limit reached at scale/10 = 1e-16
+ double scale = 1;
Matrix3d rotation_mat;
Vector3d rv;
- for(int i = 0; i<8; i++){
+ for (int i = 0; i < 8; i++)
+ {
rotation_mat = v2R(Vector3d::Random().eval() * scale);
- //rv = R2v(rotation_mat); //decomposing R2v below
+ // rv = R2v(rotation_mat); //decomposing R2v below
AngleAxis<double> aa0 = AngleAxis<double>(rotation_mat);
- rv = aa0.axis() * aa0.angle();
- //std::cout << "aa0.axis : " << aa0.axis().transpose() << ",\t aa0.angles :" << aa0.angle() <<std::endl;
+ rv = aa0.axis() * aa0.angle();
+ // std::cout << "aa0.axis : " << aa0.axis().transpose() << ",\t aa0.angles :" << aa0.angle() <<std::endl;
ASSERT_FALSE(rv == Vector3d::Zero());
- scale = scale*0.1;
+ scale = scale * 0.1;
}
}
@@ -186,47 +188,50 @@ TEST(exp_q, v2q2R2v)
double scale = 1;
// testing new path : vector -> quaternion -> matrix -> vector
- for(int i = 0; i< 8; i++){
- Vector3d vector_ = Vector3d::Random()*scale;
- Quaterniond quat_ = v2q(vector_);
- Matrix3d mat_ = quat_.matrix();
- Vector3d vector_bis = R2v(mat_);
+ for (int i = 0; i < 8; i++)
+ {
+ Vector3d vector_ = Vector3d::Random() * scale;
+ Quaterniond quat_ = v2q(vector_);
+ Matrix3d mat_ = quat_.matrix();
+ Vector3d vector_bis = R2v(mat_);
ASSERT_MATRIX_APPROX(vector_, vector_bis, wolf::Constants::EPS);
- scale = scale*0.1;
+ scale = scale * 0.1;
}
}
TEST(rotations, AngleAxis_limits)
{
using namespace wolf;
- //Hypothesis : problem with construction of AngleAxis objects.
- // Example : if R = I + [delta t]_x (happenning in the IMU case with delta t = 0.001). Then angle mays be evaluated as 0 (due to cosinus definition ?)
- // Here we try to get the AngleAxis object from a random rotation matrix, then get back to the rotation matrix using AngleAxis::toRotationMatrix()
+ // Hypothesis : problem with construction of AngleAxis objects.
+ // Example : if R = I + [delta t]_x (happenning in the IMU case with delta t = 0.001). Then angle mays be evaluated
+ // as 0 (due to cosinus definition ?) Here we try to get the AngleAxis object from a random rotation
+ // matrix, then get back to the rotation matrix using AngleAxis::toRotationMatrix()
- double scale = 1;
+ double scale = 1;
Matrix3d res, res_i, rotation_mati, rotation_mat;
Vector3d rv;
- for(int i = 0; i<8; i++){ //FIX ME : Random() will not create a rotation matrix. Then, R2v(Random_matrix()) makes no sense at all.
- rotation_mat = v2R(Vector3d::Random().eval() * scale);
+ for (int i = 0; i < 8; i++)
+ { // FIX ME : Random() will not create a rotation matrix. Then, R2v(Random_matrix()) makes no sense at all.
+ rotation_mat = v2R(Vector3d::Random().eval() * scale);
rotation_mati = rotation_mat;
- //rv = R2v(rotation_mat); //decomposing R2v below
+ // rv = R2v(rotation_mat); //decomposing R2v below
AngleAxis<double> aa0 = AngleAxis<double>(rotation_mat);
- rv = aa0.axis() * aa0.angle();
- //std::cout << "aa0.axis : " << aa0.axis().transpose() << ",\t aa0.angles :" << aa0.angle() <<std::endl;
+ rv = aa0.axis() * aa0.angle();
+ // std::cout << "aa0.axis : " << aa0.axis().transpose() << ",\t aa0.angles :" << aa0.angle() <<std::endl;
res = aa0.toRotationMatrix();
// now we set the diagonal to identity
AngleAxis<double> aa1 = AngleAxis<double>(rotation_mat);
- rv = aa1.axis() * aa1.angle();
- //std::cout << "aa1.axis : " << aa0.axis().transpose() << ",\t aa1.angles :" << aa0.angle() <<std::endl;
+ rv = aa1.axis() * aa1.angle();
+ // std::cout << "aa1.axis : " << aa0.axis().transpose() << ",\t aa1.angles :" << aa0.angle() <<std::endl;
res_i = aa1.toRotationMatrix();
ASSERT_MATRIX_APPROX(res, rotation_mat, wolf::Constants::EPS);
ASSERT_MATRIX_APPROX(res_i, rotation_mati, wolf::Constants::EPS);
- scale = scale*0.1;
+ scale = scale * 0.1;
}
}
@@ -234,27 +239,30 @@ TEST(compose, Quat_compos_const_rateOfTurn)
{
using namespace wolf;
- // ********* constant rate of turn *********
+ // ********* constant rate of turn *********
- /* First idea was to integrate data on SO3 (succesive composition of quaternions : q = q * dq(w*dt) <=> q = q * dq(w*dt) * q' (mathematically)) and in R3
- (q2v(v2q(v0*n*dt))). with v0 << 30.0*deg_to_rad, 5.0*deg_to_rad, 10.0*deg_to_rad : constant rate-of-turn in rad/s and dt the time step.
+ /* First idea was to integrate data on SO3 (succesive composition of quaternions : q = q * dq(w*dt) <=> q = q *
+ dq(w*dt) * q' (mathematically)) and in R3 (q2v(v2q(v0*n*dt))).
+ with v0 << 30.0*deg_to_rad, 5.0*deg_to_rad, 10.0*deg_to_rad : constant rate-of-turn in rad/s and dt the time step.
But this is not OK, we cannot expect those 2 rotation integration to be equal.
- The whole point of the SO3 thing is that we cannot integrate rotation in the R3 space and expect it to work. This is why we must integrate it in the manifold of SO3
-
- more specifically :
+ The whole point of the SO3 thing is that we cannot integrate rotation in the R3 space and expect it to work.
+ This is why we must integrate it in the manifold of SO3
+
+ more specifically :
- At a constant velocity, because we keep a constant rotation axis, the integral is the same.
- - for non-contant velocities, especially if we change the axis of rotation, then it’s not the same, and the only good method is the SO3.
+ - for non-contant velocities, especially if we change the axis of rotation, then it’s not the same, and the only
+ good method is the SO3.
We change the idea :
define orientation and derive ox, oy, oz so that we get the rate of turn wx, wy, wz.
Then compare the final orientation from rotation matrix composition and quaternion composition
*/
- double deg_to_rad = M_PI/180.0;
- Eigen::Matrix3d rot0(Eigen::Matrix3d::Identity());
+ double deg_to_rad = M_PI / 180.0;
+ Eigen::Matrix3d rot0(Eigen::Matrix3d::Identity());
Eigen::Quaterniond q0, qRot;
q0.setIdentity();
- Eigen::Vector3d tmp_vec; //will be used to store rate of turn data
+ Eigen::Vector3d tmp_vec; // will be used to store rate of turn data
const unsigned int x_rot_vel = 5; // deg/s
const unsigned int y_rot_vel = 2; // deg/s
@@ -275,61 +283,65 @@ TEST(compose, Quat_compos_const_rateOfTurn)
wz = z_rot_vel;
*/
- //there is no need to compute the rate of turn at each time because it is constant here :
+ // there is no need to compute the rate of turn at each time because it is constant here :
tmpx = deg_to_rad * x_rot_vel; // rad/s
tmpy = deg_to_rad * y_rot_vel;
tmpz = deg_to_rad * z_rot_vel;
tmp_vec << tmpx, tmpy, tmpz;
const double dt = 0.1;
- for(unsigned int data_iter = 0; data_iter < 100; data_iter ++)
- {
- rot0 = rot0 * v2R(tmp_vec*dt);
- q0 = q0 * v2q(tmp_vec*dt); //succesive composition of quaternions : q = q * dq(w*dt) <=> q = q * dq(w*dt) * q' (mathematically)
-
+ for (unsigned int data_iter = 0; data_iter < 100; data_iter++)
+ {
+ rot0 = rot0 * v2R(tmp_vec * dt);
+ q0 = q0 * v2q(tmp_vec * dt); // succesive composition of quaternions : q = q * dq(w*dt) <=> q = q * dq(w*dt) *
+ // q' (mathematically)
}
// Compare results from rotation matrix composition and quaternion composition
- qRot = (v2q(R2v(rot0)));
-
- Eigen::Vector3d final_orientation(q2v(qRot));
- ASSERT_TRUE((final_orientation - wolf::q2v(q0)).isMuchSmallerThan(1,wolf::Constants::EPS)) << "final orientation expected : " << final_orientation.transpose() <<
- "\n computed final orientation : " << wolf::q2v(q0).transpose() << std::endl;
+ qRot = (v2q(R2v(rot0)));
+
+ Eigen::Vector3d final_orientation(q2v(qRot));
+ ASSERT_TRUE((final_orientation - wolf::q2v(q0)).isMuchSmallerThan(1, wolf::Constants::EPS))
+ << "final orientation expected : " << final_orientation.transpose()
+ << "\n computed final orientation : " << wolf::q2v(q0).transpose() << std::endl;
}
TEST(compose, Quat_compos_var_rateOfTurn)
{
using namespace wolf;
- //********* changing rate of turn - same freq for all axis *********
+ //********* changing rate of turn - same freq for all axis *********
- /* First idea was to integrate data on SO3 (succesive composition of quaternions : q = q * dq(w*dt) <=> q = q * dq(w*dt) * q' (mathematically)) and in R3
- (q2v(v2q(v0*n*dt))). with v0 << 30.0*deg_to_rad, 5.0*deg_to_rad, 10.0*deg_to_rad : constant rate-of-turn in rad/s and dt the time step.
+ /* First idea was to integrate data on SO3 (succesive composition of quaternions : q = q * dq(w*dt) <=> q = q *
+ dq(w*dt) * q' (mathematically)) and in R3 (q2v(v2q(v0*n*dt))).
+ with v0 << 30.0*deg_to_rad, 5.0*deg_to_rad, 10.0*deg_to_rad : constant rate-of-turn in rad/s and dt the time step.
But this is not OK, we cannot expect those 2 rotation integration to be equal.
- The whole point of the SO3 thing is that we cannot integrate rotation in the R3 space and expect it to work. This is why we must integrate it in the manifold of SO3
-
- more specifically :
+ The whole point of the SO3 thing is that we cannot integrate rotation in the R3 space and expect it to work.
+ This is why we must integrate it in the manifold of SO3
+
+ more specifically :
- At a constant velocity, because we keep a constant rotation axis, the integral is the same.
- - for non-contant velocities, especially if we change the axis of rotation, then it’s not the same, and the only good method is the SO3.
+ - for non-contant velocities, especially if we change the axis of rotation, then it’s not the same, and the only
+ good method is the SO3.
We change the idea :
define orientation and derive ox, oy, oz so that we get the rate of turn wx, wy, wz.
Then compare the final orientation from ox, oy, oz and quaternion we get by data integration
- ******* RESULT : *******
+ ******* RESULT : *******
The error in this test is due to discretization. The smaller is dt and the better is the integration !
with dt = 0.001, the error is in 1e-5
*/
- double deg_to_rad = M_PI/180.0;
- Eigen::Matrix3d rot0(Eigen::Matrix3d::Identity());
+ double deg_to_rad = M_PI / 180.0;
+ Eigen::Matrix3d rot0(Eigen::Matrix3d::Identity());
Eigen::Quaterniond q0, qRot;
q0.setIdentity();
- Eigen::Vector3d tmp_vec; //will be used to store rate of turn data
- double time = 0;
- const unsigned int x_rot_vel = 15; // deg/s
- const unsigned int y_rot_vel = 15; // deg/s
+ Eigen::Vector3d tmp_vec; // will be used to store rate of turn data
+ double time = 0;
+ const unsigned int x_rot_vel = 15; // deg/s
+ const unsigned int y_rot_vel = 15; // deg/s
const unsigned int z_rot_vel = 15; // deg/s
double tmpx, tmpy, tmpz;
@@ -349,29 +361,31 @@ TEST(compose, Quat_compos_var_rateOfTurn)
const double dt = 0.001;
- for(unsigned int data_iter = 0; data_iter <= 10000; data_iter ++)
- {
- tmpx = M_PI*x_rot_vel*cos(wolf::toRad(x_rot_vel * time))*deg_to_rad;
- tmpy = M_PI*y_rot_vel*cos(wolf::toRad(y_rot_vel * time))*deg_to_rad;
- tmpz = M_PI*z_rot_vel*cos(wolf::toRad(z_rot_vel * time))*deg_to_rad;
+ for (unsigned int data_iter = 0; data_iter <= 10000; data_iter++)
+ {
+ tmpx = M_PI * x_rot_vel * cos(wolf::toRad(x_rot_vel * time)) * deg_to_rad;
+ tmpy = M_PI * y_rot_vel * cos(wolf::toRad(y_rot_vel * time)) * deg_to_rad;
+ tmpz = M_PI * z_rot_vel * cos(wolf::toRad(z_rot_vel * time)) * deg_to_rad;
tmp_vec << tmpx, tmpy, tmpz;
- rot0 = rot0 * v2R(tmp_vec*dt);
- q0 = q0 * v2q(tmp_vec*dt); //succesive composition of quaternions : q = q * dq(w*dt) <=> q = q * dq(w*dt) * q' (mathematically)
+ rot0 = rot0 * v2R(tmp_vec * dt);
+ q0 = q0 * v2q(tmp_vec * dt); // succesive composition of quaternions : q = q * dq(w*dt) <=> q = q * dq(w*dt) *
+ // q' (mathematically)
time += dt;
}
// Compare results from rotation matrix composition and quaternion composition
qRot = (v2q(R2v(rot0)));
-
+
Eigen::Vector3d final_orientation(q2v(qRot));
-
- EXPECT_TRUE((final_orientation - wolf::q2v(q0)).isMuchSmallerThan(1,wolf::Constants::EPS)) << "final orientation expected : " << final_orientation.transpose() <<
- "\n computed final orientation : " << wolf::q2v(q0).transpose() << std::endl;
- ASSERT_TRUE((final_orientation - wolf::q2v(q0)).isMuchSmallerThan(1,0.0001)) << "final orientation expected : " << final_orientation.transpose() <<
- "\n computed final orientation : " << wolf::q2v(q0).transpose() << std::endl;
+ EXPECT_TRUE((final_orientation - wolf::q2v(q0)).isMuchSmallerThan(1, wolf::Constants::EPS))
+ << "final orientation expected : " << final_orientation.transpose()
+ << "\n computed final orientation : " << wolf::q2v(q0).transpose() << std::endl;
+ ASSERT_TRUE((final_orientation - wolf::q2v(q0)).isMuchSmallerThan(1, 0.0001))
+ << "final orientation expected : " << final_orientation.transpose()
+ << "\n computed final orientation : " << wolf::q2v(q0).transpose() << std::endl;
}
TEST(compose, Quat_compos_var_rateOfTurn_diff)
@@ -380,33 +394,37 @@ TEST(compose, Quat_compos_var_rateOfTurn_diff)
// ********* changing rate of turn - different freq for 1 axis *********
- /* First idea was to integrate data on SO3 (succesive composition of quaternions : q = q * dq(w*dt) <=> q = q * dq(w*dt) * q' (mathematically)) and in R3
- (q2v(v2q(v0*n*dt))). with v0 << 30.0*deg_to_rad, 5.0*deg_to_rad, 10.0*deg_to_rad : constant rate-of-turn in rad/s and dt the time step.
+ /* First idea was to integrate data on SO3 (succesive composition of quaternions : q = q * dq(w*dt) <=> q = q *
+ dq(w*dt) * q' (mathematically)) and in R3 (q2v(v2q(v0*n*dt))).
+ with v0 << 30.0*deg_to_rad, 5.0*deg_to_rad, 10.0*deg_to_rad : constant rate-of-turn in rad/s and dt the time step.
But this is not OK, we cannot expect those 2 rotation integration to be equal.
- The whole point of the SO3 thing is that we cannot integrate rotation in the R3 space and expect it to work. This is why we must integrate it in the manifold of SO3
-
- more specifically :
+ The whole point of the SO3 thing is that we cannot integrate rotation in the R3 space and expect it to work.
+ This is why we must integrate it in the manifold of SO3.
+
+ more specifically :
- At a constant velocity, because we keep a constant rotation axis, the integral is the same.
- - for non-contant velocities, especially if we change the axis of rotation, then it’s not the same, and the only good method is the SO3.
+ - for non-contant velocities, especially if we change the axis of rotation, then it’s not the same, and the only
+ good method is the SO3.
We change the idea :
define orientation and derive ox, oy, oz so that we get the rate of turn wx, wy, wz.
Then compare the final orientation from ox, oy, oz and quaternion we get by data integration
- ******* RESULT : *******
+ ******* RESULT : *******
Things are more tricky here. The errors go growing with time.
- with dt = 0.001, the error is in 1e-4 for 1 s integration ! But this may also depend on the frequency given to the rotation on each of the axis.
+ with dt = 0.001, the error is in 1e-4 for 1 s integration ! But this may also depend on the frequency given to the
+ rotation on each of the axis.
*/
- double deg_to_rad = M_PI/180.0;
- Eigen::Matrix3d rot0(Eigen::Matrix3d::Identity());
+ double deg_to_rad = M_PI / 180.0;
+ Eigen::Matrix3d rot0(Eigen::Matrix3d::Identity());
Eigen::Quaterniond q0, qRot;
q0.setIdentity();
- Eigen::Vector3d tmp_vec; //will be used to store rate of turn data
- double time = 0;
- const unsigned int x_rot_vel = 1; // deg/s
- const unsigned int y_rot_vel = 3; // deg/s
+ Eigen::Vector3d tmp_vec; // will be used to store rate of turn data
+ double time = 0;
+ const unsigned int x_rot_vel = 1; // deg/s
+ const unsigned int y_rot_vel = 3; // deg/s
const unsigned int z_rot_vel = 6; // deg/s
double tmpx, tmpy, tmpz;
@@ -426,178 +444,191 @@ TEST(compose, Quat_compos_var_rateOfTurn_diff)
const double dt = 0.001;
- for(unsigned int data_iter = 0; data_iter <= 1000; data_iter ++)
- {
- tmpx = M_PI*x_rot_vel*cos(wolf::toRad(x_rot_vel * time))*deg_to_rad;
- tmpy = M_PI*y_rot_vel*cos(wolf::toRad(y_rot_vel * time))*deg_to_rad;
- tmpz = M_PI*z_rot_vel*cos(wolf::toRad(z_rot_vel * time))*deg_to_rad;
+ for (unsigned int data_iter = 0; data_iter <= 1000; data_iter++)
+ {
+ tmpx = M_PI * x_rot_vel * cos(wolf::toRad(x_rot_vel * time)) * deg_to_rad;
+ tmpy = M_PI * y_rot_vel * cos(wolf::toRad(y_rot_vel * time)) * deg_to_rad;
+ tmpz = M_PI * z_rot_vel * cos(wolf::toRad(z_rot_vel * time)) * deg_to_rad;
tmp_vec << tmpx, tmpy, tmpz;
- rot0 = rot0 * v2R(tmp_vec*dt);
- q0 = q0 * v2q(tmp_vec*dt); //succesive composition of quaternions : q = q * dq(w*dt) <=> q = q * dq(w*dt) * q' (mathematically)
+ rot0 = rot0 * v2R(tmp_vec * dt);
+ q0 = q0 * v2q(tmp_vec * dt); // succesive composition of quaternions : q = q * dq(w*dt) <=> q = q * dq(w*dt) *
+ // q' (mathematically)
time += dt;
}
// Compare results from rotation matrix composition and quaternion composition
qRot = (v2q(R2v(rot0)));
-
+
Eigen::Vector3d final_orientation(q2v(qRot));
- EXPECT_TRUE((final_orientation - wolf::q2v(q0)).isMuchSmallerThan(1,wolf::Constants::EPS)) << "final orientation expected : " << final_orientation.transpose() <<
- "\n computed final orientation : " << wolf::q2v(q0).transpose() << std::endl;
-
- ASSERT_TRUE((final_orientation - wolf::q2v(q0)).isMuchSmallerThan(1,0.001)) << "final orientation expected : " << final_orientation.transpose() <<
- "\n computed final orientation : " << wolf::q2v(q0).transpose() << std::endl;
+ EXPECT_TRUE((final_orientation - wolf::q2v(q0)).isMuchSmallerThan(1, wolf::Constants::EPS))
+ << "final orientation expected : " << final_orientation.transpose()
+ << "\n computed final orientation : " << wolf::q2v(q0).transpose() << std::endl;
+
+ ASSERT_TRUE((final_orientation - wolf::q2v(q0)).isMuchSmallerThan(1, 0.001))
+ << "final orientation expected : " << final_orientation.transpose()
+ << "\n computed final orientation : " << wolf::q2v(q0).transpose() << std::endl;
}
TEST(Plus, Random)
{
Quaterniond q;
- q .coeffs().setRandom().normalize();
+ q.coeffs().setRandom().normalize();
Vector3d v;
- v .setRandom();
-
- Quaterniond q2 = q * exp_q(v);
- Quaterniond q3 = exp_q(v) * q;
+ v.setRandom();
- ASSERT_QUATERNION_APPROX(plus(q,v) , q2, 1e-12);
- ASSERT_QUATERNION_APPROX(plus_right(q,v), q2, 1e-12);
- ASSERT_QUATERNION_APPROX(plus_left(v,q) , q3, 1e-12);
+ Quaterniond q2 = q * exp_q(v);
+ Quaterniond q3 = exp_q(v) * q;
+ ASSERT_QUATERNION_APPROX(plus(q, v), q2, 1e-12);
+ ASSERT_QUATERNION_APPROX(plus_right(q, v), q2, 1e-12);
+ ASSERT_QUATERNION_APPROX(plus_left(v, q), q3, 1e-12);
}
TEST(Plus, Identity_plus_small)
{
Quaterniond q;
- q .setIdentity();
+ q.setIdentity();
Vector3d v;
- v .setRandom();
- v *= 1e-6;
+ v.setRandom();
+ v *= 1e-6;
Quaterniond q2;
- q2.w() = 1;
- q2.vec() = 0.5*v;
+ q2.w() = 1;
+ q2.vec() = 0.5 * v;
- ASSERT_QUATERNION_APPROX(plus(q,v), q2, 1e-12);
+ ASSERT_QUATERNION_APPROX(plus(q, v), q2, 1e-12);
}
TEST(Minus_and_diff, Random)
{
Quaterniond q1, q2, qo;
- q1 .coeffs().setRandom().normalize();
- q2 .coeffs().setRandom().normalize();
+ q1.coeffs().setRandom().normalize();
+ q2.coeffs().setRandom().normalize();
- Vector3d vr = log_q(q1.conjugate() * q2);
- Vector3d vl = log_q(q2 * q1.conjugate());
+ Vector3d vr = log_q(q1.conjugate() * q2);
+ Vector3d vl = log_q(q2 * q1.conjugate());
ASSERT_MATRIX_APPROX(minus(q1, q2), vr, 1e-12);
ASSERT_MATRIX_APPROX(diff(q1, q2), vr, 1e-12);
ASSERT_MATRIX_APPROX(minus_left(q1, q2), vl, 1e-12);
qo = plus(q1, minus(q1, q2));
- if (q2.w() * qo.w() < 0) q2.coeffs() = -(q2.coeffs()); // allow q = -q
+ if (q2.w() * qo.w() < 0) q2.coeffs() = -(q2.coeffs()); // allow q = -q
ASSERT_QUATERNION_APPROX(qo, q2, 1e-12);
qo = plus(q1, diff(q1, q2));
- if (q2.w() * qo.w() < 0) q2.coeffs() = -(q2.coeffs()); // allow q = -q
+ if (q2.w() * qo.w() < 0) q2.coeffs() = -(q2.coeffs()); // allow q = -q
ASSERT_QUATERNION_APPROX(qo, q2, 1e-12);
qo = plus_left(minus_left(q1, q2), q1);
- if (q2.w() * qo.w() < 0) q2.coeffs() = -(q2.coeffs()); // allow q = -q
+ if (q2.w() * qo.w() < 0) q2.coeffs() = -(q2.coeffs()); // allow q = -q
ASSERT_QUATERNION_APPROX(qo, q2, 1e-12);
}
TEST(Jacobians, Jr)
{
- Vector3d theta; theta.setRandom();
- Vector3d dtheta; dtheta.setRandom(); dtheta *= 1e-4;
+ Vector3d theta;
+ theta.setRandom();
+ Vector3d dtheta;
+ dtheta.setRandom();
+ dtheta *= 1e-4;
// Check the main Jr property for q and R
// exp( theta + d_theta ) \approx exp(theta) * exp(Jr * d_theta)
Matrix3d Jr = jac_SO3_right(theta);
- ASSERT_QUATERNION_APPROX(exp_q(theta+dtheta), exp_q(theta) * exp_q(Jr*dtheta), 1e-8);
- ASSERT_MATRIX_APPROX(exp_R(theta+dtheta), (exp_R(theta) * exp_R(Jr*dtheta)), 1e-8);
+ ASSERT_QUATERNION_APPROX(exp_q(theta + dtheta), exp_q(theta) * exp_q(Jr * dtheta), 1e-8);
+ ASSERT_MATRIX_APPROX(exp_R(theta + dtheta), (exp_R(theta) * exp_R(Jr * dtheta)), 1e-8);
}
TEST(Jacobians, Jl)
{
- Vector3d theta; theta.setRandom();
- Vector3d dtheta; dtheta.setRandom(); dtheta *= 1e-4;
+ Vector3d theta;
+ theta.setRandom();
+ Vector3d dtheta;
+ dtheta.setRandom();
+ dtheta *= 1e-4;
// Check the main Jl property for q and R
// exp( theta + d_theta ) \approx exp(Jl * d_theta) * exp(theta)
Matrix3d Jl = jac_SO3_left(theta);
- ASSERT_QUATERNION_APPROX(exp_q(theta+dtheta), exp_q(Jl*dtheta) * exp_q(theta), 1e-8);
- ASSERT_MATRIX_APPROX(exp_R(theta+dtheta), (exp_R(Jl*dtheta) * exp_R(theta)), 1e-8);
+ ASSERT_QUATERNION_APPROX(exp_q(theta + dtheta), exp_q(Jl * dtheta) * exp_q(theta), 1e-8);
+ ASSERT_MATRIX_APPROX(exp_R(theta + dtheta), (exp_R(Jl * dtheta) * exp_R(theta)), 1e-8);
// Jl = Jr.tr
ASSERT_MATRIX_APPROX(Jl, jac_SO3_right(theta).transpose(), 1e-8);
// Jl = R*Jr
- ASSERT_MATRIX_APPROX(Jl, exp_R(theta)*jac_SO3_right(theta), 1e-8);
+ ASSERT_MATRIX_APPROX(Jl, exp_R(theta) * jac_SO3_right(theta), 1e-8);
}
TEST(Jacobians, Jr_inv)
{
- Vector3d theta; theta.setRandom();
- Vector3d dtheta; dtheta.setRandom(); dtheta *= 1e-4;
+ Vector3d theta;
+ theta.setRandom();
+ Vector3d dtheta;
+ dtheta.setRandom();
+ dtheta *= 1e-4;
Quaterniond q = v2q(theta);
Matrix3d R = v2R(theta);
// Check the main Jr_inv property for q and R
// log( R * exp(d_theta) ) \approx log( R ) + Jrinv * d_theta
Matrix3d Jr_inv = jac_SO3_right_inv(theta);
- ASSERT_MATRIX_APPROX(log_q(q * exp_q(dtheta)), log_q(q) + Jr_inv*dtheta, 1e-8);
- ASSERT_MATRIX_APPROX(log_R(R * exp_R(dtheta)), log_R(R) + Jr_inv*dtheta, 1e-8);
+ ASSERT_MATRIX_APPROX(log_q(q * exp_q(dtheta)), log_q(q) + Jr_inv * dtheta, 1e-8);
+ ASSERT_MATRIX_APPROX(log_R(R * exp_R(dtheta)), log_R(R) + Jr_inv * dtheta, 1e-8);
}
TEST(Jacobians, Jl_inv)
{
- Vector3d theta; theta.setRandom();
- Vector3d dtheta; dtheta.setRandom(); dtheta *= 1e-4;
+ Vector3d theta;
+ theta.setRandom();
+ Vector3d dtheta;
+ dtheta.setRandom();
+ dtheta *= 1e-4;
Quaterniond q = v2q(theta);
Matrix3d R = v2R(theta);
// Check the main Jl_inv property for q and R
// log( exp(d_theta) * R ) \approx log( R ) + Jlinv * d_theta
Matrix3d Jl_inv = jac_SO3_left_inv(theta);
- ASSERT_MATRIX_APPROX(log_q(exp_q(dtheta) * q), log_q(q) + Jl_inv*dtheta, 1e-8);
- ASSERT_MATRIX_APPROX(log_R(exp_R(dtheta) * R), log_R(R) + Jl_inv*dtheta, 1e-8);
+ ASSERT_MATRIX_APPROX(log_q(exp_q(dtheta) * q), log_q(q) + Jl_inv * dtheta, 1e-8);
+ ASSERT_MATRIX_APPROX(log_R(exp_R(dtheta) * R), log_R(R) + Jl_inv * dtheta, 1e-8);
}
TEST(Jacobians, compose)
{
-
- Vector3d th1(.1,.2,.3), th2(.3,.1,.2);
+ Vector3d th1(.1, .2, .3), th2(.3, .1, .2);
Quaterniond q1(exp_q(th1));
Quaterniond q2(exp_q(th2));
Quaterniond qc;
- Matrix3d J1a, J2a, J1n, J2n;
+ Matrix3d J1a, J2a, J1n, J2n;
// composition and analytic Jacobians
wolf::compose(q1, q2, qc, J1a, J2a);
// Numeric Jacobians
- double dx = 1e-6;
- Vector3d pert;
+ double dx = 1e-6;
+ Vector3d pert;
Quaterniond q1_pert, q2_pert, qc_pert;
- for (int i = 0; i<3; i++)
+ for (int i = 0; i < 3; i++)
{
pert.setZero();
pert(i) = dx;
// Jac wrt q1
- q1_pert = q1*exp_q(pert);
- qc_pert = q1_pert * q2;
- J1n.col(i) = log_q(qc.conjugate()*qc_pert) / dx;
+ q1_pert = q1 * exp_q(pert);
+ qc_pert = q1_pert * q2;
+ J1n.col(i) = log_q(qc.conjugate() * qc_pert) / dx;
// Jac wrt q2
- q2_pert = q2*exp_q(pert);
- qc_pert = q1 * q2_pert;
- J2n.col(i) = log_q(qc.conjugate()*qc_pert) / dx;
+ q2_pert = q2 * exp_q(pert);
+ qc_pert = q1 * q2_pert;
+ J2n.col(i) = log_q(qc.conjugate() * qc_pert) / dx;
}
ASSERT_MATRIX_APPROX(J1a, J1n, 1e-5);
@@ -606,34 +637,33 @@ TEST(Jacobians, compose)
TEST(Jacobians, between)
{
-
- Vector3d th1(.1,.2,.3), th2(.3,.1,.2);
+ Vector3d th1(.1, .2, .3), th2(.3, .1, .2);
Quaterniond q1(exp_q(th1));
Quaterniond q2(exp_q(th2));
Quaterniond qc;
- Matrix3d J1a, J2a, J1n, J2n;
+ Matrix3d J1a, J2a, J1n, J2n;
// composition and analytic Jacobians
wolf::between(q1, q2, qc, J1a, J2a);
// Numeric Jacobians
- double dx = 1e-6;
- Vector3d pert;
+ double dx = 1e-6;
+ Vector3d pert;
Quaterniond q1_pert, q2_pert, qc_pert;
- for (int i = 0; i<3; i++)
+ for (int i = 0; i < 3; i++)
{
pert.setZero();
pert(i) = dx;
// Jac wrt q1
- q1_pert = q1*exp_q(pert);
- qc_pert = q1_pert.conjugate() * q2;
- J1n.col(i) = log_q(qc.conjugate()*qc_pert) / dx;
+ q1_pert = q1 * exp_q(pert);
+ qc_pert = q1_pert.conjugate() * q2;
+ J1n.col(i) = log_q(qc.conjugate() * qc_pert) / dx;
// Jac wrt q2
- q2_pert = q2*exp_q(pert);
- qc_pert = q1.conjugate() * q2_pert;
- J2n.col(i) = log_q(qc.conjugate()*qc_pert) / dx;
+ q2_pert = q2 * exp_q(pert);
+ qc_pert = q1.conjugate() * q2_pert;
+ J2n.col(i) = log_q(qc.conjugate() * qc_pert) / dx;
}
ASSERT_MATRIX_APPROX(J1a, J1n, 1e-5);
@@ -642,74 +672,79 @@ TEST(Jacobians, between)
TEST(exp_q, small)
{
- Vector3d u; u.setRandom().normalize();
- Vector3d v;
+ Vector3d u;
+ u.setRandom().normalize();
+ Vector3d v;
Quaterniond q;
- double scale = 1.0;
- for (int i = 0; i<20; i++)
+ double scale = 1.0;
+ for (int i = 0; i < 20; i++)
{
- v = u*scale;
- q = exp_q(v);
+ v = u * scale;
+ q = exp_q(v);
WOLF_TRACE("scale = ", scale, "; ratio = ", (q.vec().array() / v.array()).transpose());
- scale /= 10;
+ scale /= 10;
}
- ASSERT_MATRIX_APPROX(q.vec()/(10*scale), u/2, 1e-12);
+ ASSERT_MATRIX_APPROX(q.vec() / (10 * scale), u / 2, 1e-12);
}
TEST(log_q, double_cover)
{
- Quaterniond qp; qp.coeffs().setRandom().normalize();
- Quaterniond qn; qn.coeffs() = - qp.coeffs();
+ Quaterniond qp;
+ qp.coeffs().setRandom().normalize();
+ Quaterniond qn;
+ qn.coeffs() = -qp.coeffs();
ASSERT_MATRIX_APPROX(log_q(qp), log_q(qn), 1e-16);
}
TEST(log_q, small)
{
- Vector3d u; u.setRandom().normalize();
+ Vector3d u;
+ u.setRandom().normalize();
double scale = 1.0;
- for (int i = 0; i<20; i++)
+ for (int i = 0; i < 20; i++)
{
- Vector3d v = u*scale;
- Quaterniond q = exp_q(v);
- Vector3d l = log_q(q);
+ Vector3d v = u * scale;
+ Quaterniond q = exp_q(v);
+ Vector3d l = log_q(q);
ASSERT_MATRIX_APPROX(v, l, 1e-10);
- scale /= 10;
+ scale /= 10;
}
}
TEST(Conversions, q2R_R2q)
{
- Vector3d v; v.setRandom();
+ Vector3d v;
+ v.setRandom();
Quaterniond q = v2q(v);
- Matrix3d R = v2R(v);
+ Matrix3d R = v2R(v);
Quaterniond q_R = R2q(R);
Quaterniond qq_R(R);
- ASSERT_NEAR(q.norm(), 1, wolf::Constants::EPS);
- ASSERT_NEAR(q_R.norm(), 1, wolf::Constants::EPS);
+ ASSERT_NEAR(q.norm(), 1, wolf::Constants::EPS);
+ ASSERT_NEAR(q_R.norm(), 1, wolf::Constants::EPS);
ASSERT_NEAR(qq_R.norm(), 1, wolf::Constants::EPS);
ASSERT_MATRIX_APPROX(q.coeffs(), R2q(R).coeffs(), wolf::Constants::EPS);
- ASSERT_MATRIX_APPROX(q.coeffs(), qq_R.coeffs(), wolf::Constants::EPS);
- ASSERT_MATRIX_APPROX(R, q2R(q), wolf::Constants::EPS);
- ASSERT_MATRIX_APPROX(R, qq_R.matrix(), wolf::Constants::EPS);
+ ASSERT_MATRIX_APPROX(q.coeffs(), qq_R.coeffs(), wolf::Constants::EPS);
+ ASSERT_MATRIX_APPROX(R, q2R(q), wolf::Constants::EPS);
+ ASSERT_MATRIX_APPROX(R, qq_R.matrix(), wolf::Constants::EPS);
}
TEST(Conversions, R2q_norm_of_q)
{
- Isometry3d T = Translation3d(1,2,3) * AngleAxisd(M_PI_4, Vector3d(1,2,3).normalized());
- Eigen::Vector4d cqt = R2q(T.linear()).coeffs();
- Eigen::Matrix3d R = T.linear();
+ Isometry3d T = Translation3d(1, 2, 3) * AngleAxisd(M_PI_4, Vector3d(1, 2, 3).normalized());
+ Eigen::Vector4d cqt = R2q(T.linear()).coeffs();
+ Eigen::Matrix3d R = T.linear();
Eigen::Vector4d cqt2 = R2q(R).coeffs();
- std::cout << "cqt: " << cqt.transpose() << std::endl;
- std::cout << "cqt2: " << cqt2.transpose() << std::endl;
- std::cout << "cqt.norm(): " << cqt.norm() << std::endl;
- std::cout << "cqt2.norm(): " << cqt2.norm() << std::endl;
+ std::cout << "cqt: " << cqt.transpose() << std::endl;
+ std::cout << "cqt2: " << cqt2.transpose() << std::endl;
+ std::cout << "cqt.norm(): " << cqt.norm() << std::endl;
+ std::cout << "cqt2.norm(): " << cqt2.norm() << std::endl;
ASSERT_NEAR(cqt.norm(), 1, 1e-8);
ASSERT_NEAR(cqt2.norm(), 1, 1e-8);
@@ -726,7 +761,7 @@ TEST(Conversions, R2q_norm_of_q)
TEST(Conversions, e2q_q2e)
{
- Vector3d e, eo;
+ Vector3d e, eo;
Quaterniond q;
e << 0.1, .2, .3;
@@ -738,14 +773,13 @@ TEST(Conversions, e2q_q2e)
eo = q2e(q.coeffs());
ASSERT_MATRIX_APPROX(eo, e, 1e-10);
-
}
TEST(Conversions, e2q_q2R_R2e)
{
- Vector3d e, eo;
+ Vector3d e, eo;
Quaterniond q;
- Matrix3d R;
+ Matrix3d R;
e << 0.1, .2, .3;
q = e2q(e);
@@ -770,9 +804,9 @@ TEST(Conversions, e2R_R2e)
TEST(Conversions, e2R_R2q_q2e)
{
- Vector3d e, eo;
+ Vector3d e, eo;
Quaterniond q;
- Matrix3d R;
+ Matrix3d R;
e << 0.1, 0.2, 0.3;
R = e2R(e);
@@ -788,8 +822,8 @@ int main(int argc, char **argv)
using namespace wolf;
/*
- LIST OF FUNCTIONS :
- - pi2pi
+ LIST OF FUNCTIONS :
+ - pi2pi
- skew -> Skew_vee OK
- vee -> Skew_vee OK
- v2q v -> v2q -> q2v -> v OK (precision wolf::Constants::EPS)
diff --git a/test/gtest_schema.cpp b/test/gtest_schema.cpp
index a969a201b9b119294f6edba56a25d1cf7b78fcd6..79dd09f53092c5dca94c52f00500fe5ef230172f 100644
--- a/test/gtest_schema.cpp
+++ b/test/gtest_schema.cpp
@@ -68,15 +68,15 @@ TEST(Schema, check_schema_existence)
// Check that there is an schema file for each of the registered creators of all Factories from yaml nodes/files
// (all except FactoryStateBlock)
- // FactorySolver
- auto registered_solvers = FactorySolver::getRegisteredKeys();
+ // FactorySolverNode
+ auto registered_solvers = FactorySolverNode::getRegisteredKeys();
for (auto key : registered_solvers)
{
EXPECT_TRUE(existsSchema(key));
}
- // FactoryMap
- auto registered_maps = FactoryMap::getRegisteredKeys();
+ // FactoryMapNode
+ auto registered_maps = FactoryMapNode::getRegisteredKeys();
for (auto key : registered_maps)
{
EXPECT_TRUE(existsSchema(key));
@@ -89,15 +89,15 @@ TEST(Schema, check_schema_existence)
EXPECT_TRUE(existsSchema(key));
}
- // FactoryProcessor
- auto registered_processors = FactoryProcessor::getRegisteredKeys();
+ // FactoryProcessorNode
+ auto registered_processors = FactoryProcessorNode::getRegisteredKeys();
for (auto key : registered_processors)
{
EXPECT_TRUE(existsSchema(key));
}
- // FactoryTreeManager
- auto registered_tree_managers = FactoryTreeManager::getRegisteredKeys();
+ // FactoryTreeManagerNode
+ auto registered_tree_managers = FactoryTreeManagerNode::getRegisteredKeys();
for (auto key : registered_tree_managers)
{
EXPECT_TRUE(existsSchema(key));
diff --git a/test/gtest_sensor_base.cpp b/test/gtest_sensor_base.cpp
index 1ab624a6a5072030f5d3444dab3cc903bd96b761..4d315e0d850fe7f930c357fdec9ed399f2f169b2 100644
--- a/test/gtest_sensor_base.cpp
+++ b/test/gtest_sensor_base.cpp
@@ -18,9 +18,11 @@
// 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/>.
+#include "core/internal/config.h"
#include "core/sensor/sensor_base.h"
#include "core/utils/utils_gtest.h"
#include "dummy/sensor_dummy.h"
+#include "dummy/sensor_dummy_poia.h"
#include "yaml-schema-cpp/yaml_server.hpp"
using namespace wolf;
@@ -29,22 +31,32 @@ using namespace yaml_schema_cpp;
std::string wolf_dir = _WOLF_CODE_DIR;
-VectorXd vector0 = VectorXd(0);
-VectorXd p_state_2D = VectorXd::Random(2);
-VectorXd p_state_3D = VectorXd::Random(3);
-VectorXd o_state_2D = VectorXd::Random(1);
-VectorXd o_state_3D = VectorXd::Random(4).normalized();
-VectorXd p_std_2D = VectorXd::Random(2).cwiseAbs();
-VectorXd p_std_3D = VectorXd::Random(3).cwiseAbs();
-VectorXd o_std_2D = VectorXd::Random(1).cwiseAbs();
-VectorXd o_std_3D = VectorXd::Random(3).cwiseAbs();
-MatrixXd p_cov_2D = p_std_2D.cwiseAbs2().asDiagonal();
-MatrixXd p_cov_3D = p_std_3D.cwiseAbs2().asDiagonal();
-MatrixXd o_cov_2D = o_std_2D.cwiseAbs2().asDiagonal();
-MatrixXd o_cov_3D = o_std_3D.cwiseAbs2().asDiagonal();
-double noise_p_std = 0.1;
-double noise_o_std = 0.01;
-MatrixXd noise_cov_dummy = Vector2d(noise_p_std, noise_o_std).cwiseAbs2().asDiagonal();
+ProblemPtr problem_2D = Problem::create("PO", 2);
+ProblemPtr problem_3D = Problem::create("PO", 3);
+VectorXd vector0 = VectorXd(0);
+VectorXd p_state_2D = VectorXd::Random(2);
+VectorXd p_state_3D = VectorXd::Random(3);
+VectorXd o_state_2D = VectorXd::Random(1);
+VectorXd o_state_3D = VectorXd::Random(4).normalized();
+VectorXd p_std_2D = VectorXd::Random(2).cwiseAbs();
+VectorXd p_std_3D = VectorXd::Random(3).cwiseAbs();
+VectorXd o_std_2D = VectorXd::Random(1).cwiseAbs();
+VectorXd o_std_3D = VectorXd::Random(3).cwiseAbs();
+VectorXd p_state_2D_yaml = (VectorXd(2) << 1, 2).finished();
+VectorXd p_state_3D_yaml = (VectorXd(3) << 1, 2, 3).finished();
+VectorXd o_state_2D_yaml = (VectorXd(1) << 1).finished();
+VectorXd o_state_3D_yaml = (VectorXd(4) << 1, 0, 0, 0).finished();
+VectorXd i_state_yaml = (VectorXd(5) << 1, 2, 3, 4, 5).finished();
+VectorXd a_state_yaml = (VectorXd(4) << 1, 0, 0, 0).finished();
+VectorXd p_std_2D_yaml = (VectorXd(2) << 0.1, 0.2).finished();
+VectorXd p_std_3D_yaml = (VectorXd(3) << 0.1, 0.2, 0.3).finished();
+VectorXd o_std_2D_yaml = (VectorXd(1) << 0.1).finished();
+VectorXd o_std_3D_yaml = (VectorXd(3) << 0.1, 0.2, 0.3).finished();
+VectorXd i_std_yaml = (VectorXd(5) << 0.1, 0.2, 0.3, 0.4, 0.5).finished();
+VectorXd a_std_yaml = (VectorXd(3) << 0.1, 0.2, 0.3).finished();
+double noise_p_std = 0.1;
+double noise_o_std = 0.01;
+MatrixXd noise_cov_dummy = Vector2d(noise_p_std, noise_o_std).cwiseAbs2().asDiagonal();
void checkSensor(SensorBasePtr S,
char _key,
@@ -87,533 +99,275 @@ void checkSensor(SensorBasePtr S,
}
}
-//////////////////////////////////////////////////////////////////////////////////////////////////////
-////////////////////////////////// CONSTRUCTOR WITH PRIORS AND PARAMS ////////////////////////////////
-//////////////////////////////////////////////////////////////////////////////////////////////////////
-// 2D Fix
-TEST(SensorBase, makeshared_priors_POfix2D)
+TEST(SensorBase, sensor_dummy)
{
- auto params = std::make_shared<ParamsSensorDummy>();
- params->name = "sensor_1";
- params->noise_p_std = noise_p_std;
- params->noise_o_std = noise_o_std;
-
- auto specs =
- SpecStateSensorComposite({{'P', SpecStateSensor("StatePoint2d", p_state_2D, "fix")}, // default not dynamic
- {'O', SpecStateSensor("StateAngle", o_state_2D, "fix")}});
-
- // create
- auto S = SensorDummy2d::create(params, specs);
-
- // noise
- ASSERT_MATRIX_APPROX(S->computeNoiseCov(vector0), noise_cov_dummy, Constants::EPS);
-
- // checks
- checkSensor(S, 'P', p_state_2D, true, vector0, false, vector0);
- checkSensor(S, 'O', o_state_2D, true, vector0, false, vector0);
-}
-
-// 3D Fix
-TEST(SensorBase, makeshared_priors_POfix3D)
-{
- auto params = std::make_shared<ParamsSensorDummy>();
- params->name = "sensor_1";
- params->noise_p_std = noise_p_std;
- params->noise_o_std = noise_o_std;
-
- auto specs =
- SpecStateSensorComposite({{'P', SpecStateSensor("StatePoint3d", p_state_3D, "fix")}, // default not dynamic
- {'O', SpecStateSensor("StateQuaternion", o_state_3D, "fix")}});
-
- // create
- auto S = SensorDummy3d::create(params, specs);
-
- // noise
- ASSERT_MATRIX_APPROX(S->computeNoiseCov(vector0), noise_cov_dummy, Constants::EPS);
-
- // factors
- ASSERT_EQ(S->getPriorFactorMap().size(), 0);
-
- // check
- checkSensor(S, 'P', p_state_3D, true, vector0, false, vector0);
- checkSensor(S, 'O', o_state_3D, true, vector0, false, vector0);
-}
-
-// 2D Initial guess
-TEST(SensorBase, makeshared_priors_POinitial_guess2D)
-{
- auto params = std::make_shared<ParamsSensorDummy>();
- params->name = "sensor_1";
- params->noise_p_std = noise_p_std;
- params->noise_o_std = noise_o_std;
-
- auto specs = SpecStateSensorComposite({{'P', SpecStateSensor("StatePoint2d", p_state_2D, "initial_guess")},
- {'O', SpecStateSensor("StateAngle", o_state_2D, "initial_guess")}});
-
- // create and apply priors
- auto S = SensorDummy2d::create(params, specs);
-
- // noise
- ASSERT_MATRIX_APPROX(S->computeNoiseCov(vector0), noise_cov_dummy, Constants::EPS);
-
- // factors
- ASSERT_EQ(S->getPriorFactorMap().size(), 0);
-
- // check
- checkSensor(S, 'P', p_state_2D, false, vector0, false, vector0);
- checkSensor(S, 'O', o_state_2D, false, vector0, false, vector0);
-}
-
-// 3D Initial guess
-TEST(SensorBase, makeshared_priors_POinitial_guess3D)
-{
- auto params = std::make_shared<ParamsSensorDummy>();
- params->name = "sensor_1";
- params->noise_p_std = noise_p_std;
- params->noise_o_std = noise_o_std;
-
- auto specs = SpecStateSensorComposite({{'P', SpecStateSensor("StatePoint3d", p_state_3D, "initial_guess")},
- {'O', SpecStateSensor("StateQuaternion", o_state_3D, "initial_guess")}});
-
- // create and apply priors
- auto S = SensorDummy3d::create(params, specs);
-
- // noise
- ASSERT_MATRIX_APPROX(S->computeNoiseCov(vector0), noise_cov_dummy, Constants::EPS);
-
- // factors
- ASSERT_EQ(S->getPriorFactorMap().size(), 0);
-
- // check
- checkSensor(S, 'P', p_state_3D, false, vector0, false, vector0);
- checkSensor(S, 'O', o_state_3D, false, vector0, false, vector0);
-}
-
-// 2D Factor
-TEST(SensorBase, makeshared_priors_POfactor2D)
-{
- auto params = std::make_shared<ParamsSensorDummy>();
- params->name = "sensor_1";
- params->noise_p_std = noise_p_std;
- params->noise_o_std = noise_o_std;
-
- auto specs = SpecStateSensorComposite({{'P', SpecStateSensor("StatePoint2d", p_state_2D, "factor", p_std_2D)},
- {'O', SpecStateSensor("StateAngle", o_state_2D, "factor", o_std_2D)}});
-
- // create and apply priors
- auto S = SensorDummy2d::create(params, specs);
-
- // noise
- ASSERT_MATRIX_APPROX(S->computeNoiseCov(vector0), noise_cov_dummy, Constants::EPS);
-
- // factors
- ASSERT_EQ(S->getPriorFactorMap().size(), 2);
-
- // check
- checkSensor(S, 'P', p_state_2D, false, p_std_2D, false, vector0);
- checkSensor(S, 'O', o_state_2D, false, o_std_2D, false, vector0);
-}
-
-// 3D Factor
-TEST(SensorBase, makeshared_priors_POfactor3D)
-{
- auto params = std::make_shared<ParamsSensorDummy>();
- params->name = "sensor_1";
- params->noise_p_std = noise_p_std;
- params->noise_o_std = noise_o_std;
-
- auto specs = SpecStateSensorComposite({{'P', SpecStateSensor("StatePoint3d", p_state_3D, "factor", p_std_3D)},
- {'O', SpecStateSensor("StateQuaternion", o_state_3D, "factor", o_std_3D)}});
-
- // create and apply priors
- auto S = SensorDummy3d::create(params, specs);
-
- // noise
- ASSERT_MATRIX_APPROX(S->computeNoiseCov(vector0), noise_cov_dummy, Constants::EPS);
-
- // factors
- ASSERT_EQ(S->getPriorFactorMap().size(), 2);
-
- // check
- checkSensor(S, 'P', p_state_3D, false, p_std_3D, false, vector0);
- checkSensor(S, 'O', o_state_3D, false, o_std_3D, false, vector0);
-}
-
-// 2D Initial guess dynamic
-TEST(SensorBase, makeshared_priors_POinitial_guess_dynamic2D)
-{
- auto params = std::make_shared<ParamsSensorDummy>();
- params->name = "sensor_1";
- params->noise_p_std = noise_p_std;
- params->noise_o_std = noise_o_std;
-
- auto specs =
- SpecStateSensorComposite({{'P', SpecStateSensor("StatePoint2d", p_state_2D, "initial_guess", vector0, true)},
- {'O', SpecStateSensor("StateAngle", o_state_2D, "initial_guess", vector0, true)}});
-
- // create and apply priors
- auto S = SensorDummy2d::create(params, specs);
-
- // noise
- ASSERT_MATRIX_APPROX(S->computeNoiseCov(vector0), noise_cov_dummy, Constants::EPS);
-
- // factors
- ASSERT_EQ(S->getPriorFactorMap().size(), 0);
-
- // check
- checkSensor(S, 'P', p_state_2D, false, vector0, true, vector0);
- checkSensor(S, 'O', o_state_2D, false, vector0, true, vector0);
-}
-
-// 3D Initial guess dynamic
-TEST(SensorBase, makeshared_priors_POinitial_guess_dynamic3D)
-{
- auto params = std::make_shared<ParamsSensorDummy>();
- params->name = "sensor_1";
- params->noise_p_std = noise_p_std;
- params->noise_o_std = noise_o_std;
-
- auto specs = SpecStateSensorComposite(
- {{'P', SpecStateSensor("StatePoint3d", p_state_3D, "initial_guess", vector0, true)},
- {'O', SpecStateSensor("StateQuaternion", o_state_3D, "initial_guess", vector0, true)}});
-
- // create and apply priors
- auto S = SensorDummy3d::create(params, specs);
-
- // noise
- ASSERT_MATRIX_APPROX(S->computeNoiseCov(vector0), noise_cov_dummy, Constants::EPS);
-
- // factors
- ASSERT_EQ(S->getPriorFactorMap().size(), 0);
-
- // check
- checkSensor(S, 'P', p_state_3D, false, vector0, true, vector0);
- checkSensor(S, 'O', o_state_3D, false, vector0, true, vector0);
-}
-
-// 2D Initial guess dynamic drift
-TEST(SensorBase, makeshared_priors_POinitial_guess_dynamic2D_drift)
-{
- auto params = std::make_shared<ParamsSensorDummy>();
- params->name = "sensor_1";
- params->noise_p_std = noise_p_std;
- params->noise_o_std = noise_o_std;
-
- auto specs = SpecStateSensorComposite(
- {{'P', SpecStateSensor("StatePoint2d", p_state_2D, "initial_guess", vector0, true, p_std_2D)},
- {'O', SpecStateSensor("StateAngle", o_state_2D, "initial_guess", vector0, true, o_std_2D)}});
-
- // create and apply priors
- auto S = SensorDummy2d::create(params, specs);
-
- // noise
- ASSERT_MATRIX_APPROX(S->computeNoiseCov(vector0), noise_cov_dummy, Constants::EPS);
-
- // factors
- ASSERT_EQ(S->getPriorFactorMap().size(), 0);
-
- // check
- checkSensor(S, 'P', p_state_2D, false, vector0, true, p_std_2D);
- checkSensor(S, 'O', o_state_2D, false, vector0, true, o_std_2D);
-}
-
-// 3D Initial guess dynamic drift
-TEST(SensorBase, makeshared_priors_POinitial_guess_dynamic3D_drift)
-{
- auto params = std::make_shared<ParamsSensorDummy>();
- params->name = "sensor_1";
- params->noise_p_std = noise_p_std;
- params->noise_o_std = noise_o_std;
-
- auto specs = SpecStateSensorComposite(
- {{'P', SpecStateSensor("StatePoint3d", p_state_3D, "initial_guess", vector0, true, p_std_3D)},
- {'O', SpecStateSensor("StateQuaternion", o_state_3D, "initial_guess", vector0, true, o_std_3D)}});
-
- // create and apply priors
- auto S = SensorDummy3d::create(params, specs);
-
- // noise
- ASSERT_MATRIX_APPROX(S->computeNoiseCov(vector0), noise_cov_dummy, Constants::EPS);
-
- // factors
- ASSERT_EQ(S->getPriorFactorMap().size(), 0);
-
- // check
- checkSensor(S, 'P', p_state_3D, false, vector0, true, p_std_3D);
- checkSensor(S, 'O', o_state_3D, false, vector0, true, o_std_3D);
-}
-
-// 3D POIA mixed
-TEST(SensorBase, makeshared_priors_POIA_mixed)
-{
- auto params = std::make_shared<ParamsSensorDummyPoia>();
- params->name = "sensor_1";
- params->noise_p_std = noise_p_std;
- params->noise_o_std = noise_o_std;
-
- VectorXd i_state_3D = VectorXd::Random(5);
-
- auto specs = SpecStateSensorComposite(
- {{'P', SpecStateSensor("StatePoint3d", p_state_3D, "fix", vector0, true)},
- {'O', SpecStateSensor("StateQuaternion", o_state_3D, "factor", o_std_3D, true, o_std_3D)},
- {'I', SpecStateSensor("StateParams5", i_state_3D, "initial_guess")},
- {'A', SpecStateSensor("StateQuaternion", o_state_3D, "factor", o_std_3D)}});
-
- // create and apply priors
- auto S = SensorDummyPoia3d::create(params, specs);
-
- // noise
- ASSERT_MATRIX_APPROX(S->computeNoiseCov(vector0), noise_cov_dummy, Constants::EPS);
-
- // factors
- ASSERT_EQ(S->getPriorFactorMap().size(), 2);
-
- // check
- checkSensor(S, 'P', p_state_3D, true, vector0, true, vector0);
- checkSensor(S, 'O', o_state_3D, false, o_std_3D, true, o_std_3D);
- checkSensor(S, 'I', i_state_3D, false, vector0, false, vector0);
- checkSensor(S, 'A', o_state_3D, false, o_std_3D, false, vector0);
-}
-
-//////////////////////////////////////////////////////////////////////////////////////////////////////
-//////////////////////////////////////// FactorySensorNode ///////////////////////////////////////////
-//////////////////////////////////////////////////////////////////////////////////////////////////////
-TEST(SensorBase, factory_node)
-{
- std::vector<int> dims({2, 3});
- std::vector<std::string> modes({"fix", "initial_guess", "factor"});
- std::vector<bool> dynamics({false, true});
- std::vector<bool> drifts({false, true});
- std::vector<bool> wrongs({false, true});
-
- VectorXd p_state(4), o_state(4), po_std(4), i_state(5), i_std(5);
- p_state << 1, 2, 3, 4;
- o_state << 1, 0, 0, 0;
- po_std << 0.1, 0.2, 0.3, 0.4;
- i_state << 1, 2, 3, 4, 5;
- i_std << 0.1, 0.2, 0.3, 0.4, 0.5;
-
- // P & O
- for (auto dim : dims)
- for (auto mode : modes)
- for (auto dynamic : dynamics)
- for (auto drift : drifts)
- for (auto wrong : wrongs)
+ for (int dim : {2, 3})
+ for (std::string mode : {"fix", "initial_guess", "factor"})
+ for (bool dynamic : {false, true})
+ for (bool drift : {false, true})
+ {
+ // skip nonsense combinations
+ if (not dynamic and drift) continue;
+
+ std::string name = "sensor_PO_" + toString(dim) + "D_" + mode + (dynamic ? "_dynamic" : "") +
+ (drift ? "_drift" : "");
+
+ WOLF_INFO("---- Sensor ", name, "...");
+
+ YAML::Node params;
+ params["type"] = "SensorDummy" + toString(dim) + "d";
+ params["name"] = name;
+ params["noise_p_std"] = noise_p_std;
+ params["noise_o_std"] = noise_o_std;
+ params["states"]["keys"] = "PO";
+
+ params["states"]["P"]["type"] = dim == 2 ? "StatePoint2d" : "StatePoint3d";
+ params["states"]["P"]["state"] = dim == 2 ? p_state_2D : p_state_3D;
+ params["states"]["P"]["mode"] = mode;
+ if (mode == "factor") params["states"]["P"]["noise_std"] = (dim == 2 ? p_std_2D : p_std_3D);
+ params["states"]["P"]["dynamic"] = dynamic;
+ if (drift) params["states"]["P"]["drift_std"] = (dim == 2 ? p_std_2D : p_std_3D);
+
+ params["states"]["O"]["type"] = dim == 2 ? "StateAngle" : "StateQuaternion";
+ params["states"]["O"]["state"] = dim == 2 ? o_state_2D : o_state_3D;
+ params["states"]["O"]["mode"] = mode;
+ if (mode == "factor") params["states"]["O"]["noise_std"] = dim == 2 ? o_std_2D : o_std_3D;
+ params["states"]["O"]["dynamic"] = dynamic;
+ if (drift) params["states"]["O"]["drift_std"] = dim == 2 ? o_std_2D : o_std_3D;
+
+ // create from node ------------------------------------------------------------------------
+ auto S = dim == 2
+ ? std::static_pointer_cast<SensorBase>(SensorDummy2d::create(params, {wolf_dir}))
+ : std::static_pointer_cast<SensorBase>(SensorDummy3d::create(params, {wolf_dir}));
+
+ // noise
+ ASSERT_MATRIX_APPROX(S->computeNoiseCov(vector0), noise_cov_dummy, Constants::EPS);
+
+ // checks
+ checkSensor(S,
+ 'P',
+ dim == 2 ? p_state_2D : p_state_3D,
+ mode == "fix",
+ mode == "factor" ? dim == 2 ? p_std_2D : p_std_3D : vector0,
+ dynamic,
+ drift ? dim == 2 ? p_std_2D : p_std_3D : vector0);
+ checkSensor(S,
+ 'O',
+ dim == 2 ? o_state_2D : o_state_3D,
+ mode == "fix",
+ mode == "factor" ? dim == 2 ? o_std_2D : o_std_3D : vector0,
+ dynamic,
+ drift ? dim == 2 ? o_std_2D : o_std_3D : vector0);
+
+ // emplace ------------------------------------------------------------------------
+ auto S2 = dim == 2 ? std::static_pointer_cast<SensorBase>(SensorBase::emplace<SensorDummy2d>(
+ problem_2D->getHardware(), params, {wolf_dir}))
+ : std::static_pointer_cast<SensorBase>(SensorBase::emplace<SensorDummy3d>(
+ problem_3D->getHardware(), params, {wolf_dir}));
+
+ // noise
+ ASSERT_MATRIX_APPROX(S2->computeNoiseCov(vector0), noise_cov_dummy, Constants::EPS);
+
+ // checks
+ checkSensor(S2,
+ 'P',
+ dim == 2 ? p_state_2D : p_state_3D,
+ mode == "fix",
+ mode == "factor" ? dim == 2 ? p_std_2D : p_std_3D : vector0,
+ dynamic,
+ drift ? dim == 2 ? p_std_2D : p_std_3D : vector0);
+ checkSensor(S2,
+ 'O',
+ dim == 2 ? o_state_2D : o_state_3D,
+ mode == "fix",
+ mode == "factor" ? dim == 2 ? o_std_2D : o_std_3D : vector0,
+ dynamic,
+ drift ? dim == 2 ? o_std_2D : o_std_3D : vector0);
+
+ // Factories
+ for (bool wrong : {false, true})
{
- // nonsense combination
- if (not dynamic and drift) continue;
-
- std::string name = "sensor_PO_" + toString(dim) + "D_" + mode + (dynamic ? "_dynamic" : "") +
- (drift ? "_drift" : "") + (wrong ? "_wrong" : "");
- // Yaml server
- YamlServer server({wolf_dir}, wolf_dir + "/test/yaml/sensor_tests/" + name + ".yaml");
+ auto yaml_file =
+ wolf_dir + "/test/yaml/sensor_tests/" + name + (wrong ? "_wrong" : "") + ".yaml";
- WOLF_INFO("Creating sensor from ", name, ".yaml");
+ WOLF_DEBUG("yaml_file: ", yaml_file);
- auto valid = server.applySchema("SensorDummy" + toString(dim) + "d");
- WOLF_WARN_COND(not valid and not wrong, server.getLog());
- WOLF_WARN_COND(valid and wrong, "Schema didn't detect any problem in a wrong yaml.");
+ // factory node ------------------------------------------------------------------------
+ auto params_node = YAML::LoadFile(yaml_file);
- // CORRECT YAML
- if (not wrong)
+ try
{
- ASSERT_TRUE(valid);
-
- auto S = FactorySensorNode::create("SensorDummy" + toString(dim) + "d", server.getNode());
-
- auto p_size = dim;
- auto o_size = dim == 2 ? 1 : 4;
- auto p_size_std = mode == "factor" ? dim : 0;
- auto o_size_std = mode == "factor" ? (dim == 2 ? 1 : 3) : 0;
- auto p_size_std_drift = drift ? dim : 0;
- auto o_size_std_drift = drift ? (dim == 2 ? 1 : 3) : 0;
+ auto S3 = FactorySensorNode::create(
+ "SensorDummy" + toString(dim) + "d", params_node, {wolf_dir});
// noise
- ASSERT_MATRIX_APPROX(S->computeNoiseCov(vector0), noise_cov_dummy, Constants::EPS);
-
- // factors
- ASSERT_EQ(S->getPriorFactorMap().size(), mode == "factor" ? 2 : 0);
+ ASSERT_MATRIX_APPROX(S3->computeNoiseCov(vector0), noise_cov_dummy, Constants::EPS);
- // check
- checkSensor(S,
+ // checks
+ checkSensor(S3,
'P',
- p_state.head(p_size),
+ dim == 2 ? p_state_2D_yaml : p_state_3D_yaml,
mode == "fix",
- po_std.head(p_size_std),
+ mode == "factor" ? dim == 2 ? p_std_2D_yaml : p_std_3D_yaml : vector0,
dynamic,
- po_std.head(p_size_std_drift));
- checkSensor(S,
+ drift ? dim == 2 ? p_std_2D_yaml : p_std_3D_yaml : vector0);
+ checkSensor(S3,
'O',
- o_state.head(o_size),
+ dim == 2 ? o_state_2D_yaml : o_state_3D_yaml,
mode == "fix",
- po_std.head(o_size_std),
+ mode == "factor" ? dim == 2 ? o_std_2D_yaml : o_std_3D_yaml : vector0,
dynamic,
- po_std.head(o_size_std_drift));
+ drift ? dim == 2 ? o_std_2D_yaml : o_std_3D_yaml : vector0);
}
- // INCORRECT YAML
- else
+ catch (const std::exception& e)
{
- // either is not valid (schema) or it throws an error
- if (valid)
- {
- ASSERT_THROW(
- FactorySensorNode::create("SensorDummy" + toString(dim) + "d", server.getNode()),
- std::runtime_error);
- }
+ WOLF_WARN_COND(not wrong, "Unexpected failure, error message:\n", e.what());
+ WOLF_DEBUG_COND(wrong, "Failed as expected with error message:\n", e.what());
+ ASSERT_TRUE(wrong);
}
- }
-
- // POIA - 3D - CORRECT YAML
- {
- WOLF_INFO("Creating sensor from name sensor_POIA_3D.yaml");
-
- // Yaml server
- YamlServer server({wolf_dir}, wolf_dir + "/test/yaml/sensor_tests/sensor_POIA_3D.yaml");
-
- auto valid = server.applySchema("SensorDummyPoia3d");
- WOLF_WARN_COND(not valid, server.getLog());
- ASSERT_TRUE(valid);
- // create sensor
- auto S = FactorySensorNode::create("SensorDummyPoia3d", server.getNode());
- WOLF_INFO("created");
-
- // noise
- ASSERT_MATRIX_APPROX(S->computeNoiseCov(vector0), noise_cov_dummy, Constants::EPS);
-
- // factors
- ASSERT_EQ(S->getPriorFactorMap().size(), 2);
-
- // check
- checkSensor(S, 'P', p_state.head(3), false, po_std.head(3), true, vector0);
- checkSensor(S, 'O', o_state.head(4), true, vector0, false, vector0);
- checkSensor(S, 'I', i_state, false, vector0, true, i_std);
- checkSensor(S, 'A', o_state.head(4), false, po_std.head(3), true, po_std.head(3));
- }
- // POIA - 3D - INCORRECT YAML
- {
- WOLF_INFO("Creating sensor from name sensor_POIA_3D_wrong.yaml");
-
- // Yaml server
- YamlServer server({wolf_dir}, wolf_dir + "/test/yaml/sensor_tests/sensor_POIA_3D_wrong.yaml");
-
- auto valid = server.applySchema("SensorDummyPoia3d");
-
- // create sensor
- if (valid)
- {
- ASSERT_THROW(FactorySensorNode::create("SensorDummyPoia3d", server.getNode()), std::runtime_error);
- }
- }
-}
-
-//////////////////////////////////////////////////////////////////////////////////////////////////////
-////////////////////////////////////////// FactorySensorFile /////////////////////////////////////////
-//////////////////////////////////////////////////////////////////////////////////////////////////////
-TEST(SensorBase, factory_file)
-{
- std::vector<int> dims({2, 3});
- std::vector<std::string> modes({"fix", "initial_guess", "factor"});
- std::vector<bool> dynamics({false, true});
- std::vector<bool> drifts({false, true});
- std::vector<bool> wrongs({false, true});
-
- VectorXd p_state(4), o_state(4), po_std(4), i_state(5), i_std(5);
- p_state << 1, 2, 3, 4;
- o_state << 1, 0, 0, 0;
- po_std << 0.1, 0.2, 0.3, 0.4;
- i_state << 1, 2, 3, 4, 5;
- i_std << 0.1, 0.2, 0.3, 0.4, 0.5;
-
- // P & O
- for (auto dim : dims)
- for (auto mode : modes)
- for (auto dynamic : dynamics)
- for (auto drift : drifts)
- for (auto wrong : wrongs)
- {
- // nonsense combination
- if (not dynamic and drift) continue;
-
- std::string SensorType = "SensorDummy" + toString(dim) + "d";
- std::string yaml_filepath = wolf_dir + "/test/yaml/sensor_tests/" + "sensor_PO_" +
- toString(dim) + "D_" + mode + (dynamic ? "_dynamic" : "") +
- (drift ? "_drift" : "") + (wrong ? "_wrong" : "") + ".yaml";
-
- WOLF_INFO("Creating sensor ", SensorType, " from ", yaml_filepath);
-
- // CORRECT YAML
- if (not wrong)
+ // factory file ------------------------------------------------------------------------
+ try
{
- auto S = FactorySensorFile::create(SensorType, yaml_filepath, {wolf_dir});
-
- auto p_size = dim;
- auto o_size = dim == 2 ? 1 : 4;
- auto p_size_std = mode == "factor" ? dim : 0;
- auto o_size_std = mode == "factor" ? (dim == 2 ? 1 : 3) : 0;
- auto p_size_std_drift = drift ? dim : 0;
- auto o_size_std_drift = drift ? (dim == 2 ? 1 : 3) : 0;
+ auto S4 =
+ FactorySensorFile::create("SensorDummy" + toString(dim) + "d", yaml_file, {wolf_dir});
// noise
- ASSERT_MATRIX_APPROX(S->computeNoiseCov(vector0), noise_cov_dummy, Constants::EPS);
+ ASSERT_MATRIX_APPROX(S4->computeNoiseCov(vector0), noise_cov_dummy, Constants::EPS);
- // factors
- ASSERT_EQ(S->getPriorFactorMap().size(), mode == "factor" ? 2 : 0);
-
- // check
- checkSensor(S,
+ // checks
+ checkSensor(S4,
'P',
- p_state.head(p_size),
+ dim == 2 ? p_state_2D_yaml : p_state_3D_yaml,
mode == "fix",
- po_std.head(p_size_std),
+ mode == "factor" ? dim == 2 ? p_std_2D_yaml : p_std_3D_yaml : vector0,
dynamic,
- po_std.head(p_size_std_drift));
- checkSensor(S,
+ drift ? dim == 2 ? p_std_2D_yaml : p_std_3D_yaml : vector0);
+ checkSensor(S4,
'O',
- o_state.head(o_size),
+ dim == 2 ? o_state_2D_yaml : o_state_3D_yaml,
mode == "fix",
- po_std.head(o_size_std),
+ mode == "factor" ? dim == 2 ? o_std_2D_yaml : o_std_3D_yaml : vector0,
dynamic,
- po_std.head(o_size_std_drift));
+ drift ? dim == 2 ? o_std_2D_yaml : o_std_3D_yaml : vector0);
}
- // INCORRECT YAML
- else
+ catch (const std::exception& e)
{
- ASSERT_THROW(FactorySensorFile::create(SensorType, yaml_filepath, {wolf_dir}),
- std::runtime_error);
+ WOLF_WARN_COND(not wrong, "Unexpected failure, error message:\n", e.what());
+ WOLF_DEBUG_COND(wrong, "Failed as expected with error message:\n", e.what());
+ ASSERT_TRUE(wrong);
}
}
+ }
+}
- // POIA - 3D - CORRECT YAML
- {
- WOLF_INFO("Creating sensor from name sensor_POIA_3D.yaml");
+TEST(SensorBase, sensor_dummy_poia)
+{
+ YAML::Node params;
+ params["type"] = "SensorDummyPoia3d";
+ params["name"] = "cool sensor";
+ params["noise_p_std"] = noise_p_std;
+ params["noise_o_std"] = noise_o_std;
+ params["states"]["keys"] = "POIA";
+
+ params["states"]["P"]["type"] = "StatePoint3d";
+ params["states"]["P"]["state"] = p_state_3D_yaml;
+ params["states"]["P"]["mode"] = "factor";
+ params["states"]["P"]["noise_std"] = p_std_3D_yaml;
+ params["states"]["P"]["dynamic"] = true;
+
+ params["states"]["O"]["type"] = "StateQuaternion";
+ params["states"]["O"]["state"] = o_state_3D_yaml;
+ params["states"]["O"]["mode"] = "fix";
+ params["states"]["O"]["dynamic"] = false;
+
+ params["states"]["I"]["type"] = "StateParams5";
+ params["states"]["I"]["state"] = i_state_yaml;
+ params["states"]["I"]["mode"] = "initial_guess";
+ params["states"]["I"]["dynamic"] = true;
+ params["states"]["I"]["drift_std"] = i_std_yaml;
+
+ params["states"]["A"]["type"] = "StateQuaternion";
+ params["states"]["A"]["state"] = a_state_yaml;
+ params["states"]["A"]["mode"] = "factor";
+ params["states"]["A"]["noise_std"] = a_std_yaml;
+ params["states"]["A"]["dynamic"] = true;
+ params["states"]["A"]["drift_std"] = a_std_yaml;
+
+ // create from node ------------------------------------------------------------------------
+ auto S = SensorDummyPoia3d::create(params, {wolf_dir});
- // create sensor
- auto S = FactorySensorFile::create("SensorDummyPoia3d",
- wolf_dir + "/test/yaml/sensor_tests/sensor_POIA_3D.yaml",
- {wolf_dir});
+ // noise
+ ASSERT_MATRIX_APPROX(S->computeNoiseCov(vector0), noise_cov_dummy, Constants::EPS);
- // noise
- ASSERT_MATRIX_APPROX(S->computeNoiseCov(vector0), noise_cov_dummy, Constants::EPS);
+ // checks
+ checkSensor(S, 'P', p_state_3D_yaml, false, p_std_3D_yaml, true, vector0);
+ checkSensor(S, 'O', o_state_3D_yaml, true, vector0, false, vector0);
+ checkSensor(S, 'I', i_state_yaml, false, vector0, true, i_std_yaml);
+ checkSensor(S, 'A', a_state_yaml, false, a_std_yaml, true, a_std_yaml);
- // factors
- ASSERT_EQ(S->getPriorFactorMap().size(), 2);
+ // emplace ------------------------------------------------------------------------
+ auto S2 = SensorBase::emplace<SensorDummyPoia3d>(problem_3D->getHardware(), params, {wolf_dir});
- // check
- checkSensor(S, 'P', p_state.head(3), false, po_std.head(3), true, vector0);
- checkSensor(S, 'O', o_state.head(4), true, vector0, false, vector0);
- checkSensor(S, 'I', i_state, false, vector0, true, i_std);
- checkSensor(S, 'A', o_state.head(4), false, po_std.head(3), true, po_std.head(3));
- }
- // POIA - 3D - INCORRECT YAML
+ // noise
+ ASSERT_MATRIX_APPROX(S2->computeNoiseCov(vector0), noise_cov_dummy, Constants::EPS);
+
+ // checks
+ checkSensor(S2, 'P', p_state_3D_yaml, false, p_std_3D_yaml, true, vector0);
+ checkSensor(S2, 'O', o_state_3D_yaml, true, vector0, false, vector0);
+ checkSensor(S2, 'I', i_state_yaml, false, vector0, true, i_std_yaml);
+ checkSensor(S2, 'A', a_state_yaml, false, a_std_yaml, true, a_std_yaml);
+
+ // Factories
+ for (bool wrong : {false, true})
{
- WOLF_INFO("Creating sensor from name sensor_POIA_3D_wrong.yaml");
+ auto yaml_file = wolf_dir + "/test/yaml/sensor_tests/sensor_POIA_3D" + (wrong ? "_wrong" : "") + ".yaml";
+
+ WOLF_DEBUG("yaml_file: ", yaml_file);
+
+ // factory node ------------------------------------------------------------------------
+ auto params_node = YAML::LoadFile(yaml_file);
+
+ try
+ {
+ auto S3 = FactorySensorNode::create("SensorDummyPoia3d", params_node, {wolf_dir});
+
+ // noise
+ ASSERT_MATRIX_APPROX(S3->computeNoiseCov(vector0), noise_cov_dummy, Constants::EPS);
- // create sensor
- ASSERT_THROW(FactorySensorFile::create("SensorDummyPoia3d",
- wolf_dir + "/test/yaml/sensor_tests/sensor_POIA_3D_wrong.yaml",
- {wolf_dir}),
- std::runtime_error);
+ // checks
+ checkSensor(S3, 'P', p_state_3D_yaml, false, p_std_3D_yaml, true, vector0);
+ checkSensor(S3, 'O', o_state_3D_yaml, true, vector0, false, vector0);
+ checkSensor(S3, 'I', i_state_yaml, false, vector0, true, i_std_yaml);
+ checkSensor(S3, 'A', a_state_yaml, false, a_std_yaml, true, a_std_yaml);
+ }
+ catch (const std::exception& e)
+ {
+ WOLF_WARN_COND(not wrong, "Unexpected failure, error message:\n", e.what());
+ WOLF_DEBUG_COND(wrong, "Failed as expected with error message:\n", e.what());
+ ASSERT_TRUE(wrong);
+ }
+
+ // factory file ------------------------------------------------------------------------
+ try
+ {
+ auto S4 = FactorySensorFile::create("SensorDummyPoia3d", yaml_file, {wolf_dir});
+
+ // noise
+ ASSERT_MATRIX_APPROX(S4->computeNoiseCov(vector0), noise_cov_dummy, Constants::EPS);
+
+ // checks
+ checkSensor(S4, 'P', p_state_3D_yaml, false, p_std_3D_yaml, true, vector0);
+ checkSensor(S4, 'O', o_state_3D_yaml, true, vector0, false, vector0);
+ checkSensor(S4, 'I', i_state_yaml, false, vector0, true, i_std_yaml);
+ checkSensor(S4, 'A', a_state_yaml, false, a_std_yaml, true, a_std_yaml);
+ }
+ catch (const std::exception& e)
+ {
+ WOLF_WARN_COND(not wrong, "Unexpected failure, error message:\n", e.what());
+ WOLF_DEBUG_COND(wrong, "Failed as expected with error message:\n", e.what());
+ ASSERT_TRUE(wrong);
+ }
}
}
diff --git a/test/gtest_sensor_diff_drive.cpp b/test/gtest_sensor_diff_drive.cpp
index 55259840cc26b9d640f737ab295b160b76d210c2..9ad36f3918ffa7163abd8ef890aa9769495010eb 100644
--- a/test/gtest_sensor_diff_drive.cpp
+++ b/test/gtest_sensor_diff_drive.cpp
@@ -33,15 +33,28 @@ Vector2d p_state = (Vector2d() << 1, 2).finished();
Vector1d o_state = (Vector1d() << 3).finished();
Vector3d i_state = (Vector3d() << 0.1, 0.2, 0.3).finished();
-TEST(SensorDiffDrive, constructor_priors)
+TEST(SensorDiffDrive, create)
{
- auto param = std::make_shared<ParamsSensorDiffDrive>();
-
- SpecStateSensorComposite priors{{'P', SpecStateSensor("StatePoint2d", p_state, "fix")}, // default not dynamic
- {'O', SpecStateSensor("StateAngle", o_state, "fix")}, // default not dynamic
- {'I', SpecStateSensor("StateParams3", i_state, "fix")}}; // default not dynamic
-
- auto sen = SensorDiffDrive::create(param, priors);
+ YAML::Node param;
+
+ param["states"]["P"]["type"] = "StatePoint2d";
+ param["states"]["P"]["state"] = p_state;
+ param["states"]["P"]["mode"] = "fix";
+ param["states"]["P"]["dynamic"] = false;
+ param["states"]["O"]["type"] = "StateAngle";
+ param["states"]["O"]["state"] = o_state;
+ param["states"]["O"]["mode"] = "fix";
+ param["states"]["O"]["dynamic"] = false;
+ param["states"]["I"]["type"] = "StateParams3";
+ param["states"]["I"]["state"] = i_state;
+ param["states"]["I"]["mode"] = "fix";
+ param["states"]["I"]["dynamic"] = false;
+
+ param["name"] = "just a sensor";
+ param["ticks_per_wheel_revolution"] = 4;
+ param["ticks_std_factor"] = 2;
+
+ auto sen = SensorDiffDrive::create(param, {wolf_dir});
ASSERT_NE(sen, nullptr);
@@ -52,7 +65,7 @@ TEST(SensorDiffDrive, constructor_priors)
ASSERT_MATRIX_APPROX(sen->getO()->getState(), o_state, Constants::EPS);
}
-TEST(SensorDiffDrive, factory)
+TEST(SensorDiffDrive, factory_node)
{
yaml_schema_cpp::YamlServer server({wolf_dir}, wolf_dir + "/test/yaml/sensor_diff_drive.yaml");
@@ -60,7 +73,7 @@ TEST(SensorDiffDrive, factory)
WOLF_WARN_COND(not valid, server.getLog());
ASSERT_TRUE(valid);
- auto sb = FactorySensorNode::create("SensorDiffDrive", server.getNode());
+ auto sb = FactorySensorNode::create("SensorDiffDrive", server.getNode(), {wolf_dir});
SensorDiffDrivePtr sen = std::dynamic_pointer_cast<SensorDiffDrive>(sb);
@@ -75,10 +88,9 @@ TEST(SensorDiffDrive, factory)
TEST(SensorDiffDrive, factory_yaml)
{
- auto sb = FactorySensorFile::create(
- "SensorDiffDrive", wolf_dir + "/test/yaml/sensor_diff_drive.yaml", {wolf_dir});
+ auto sb = FactorySensorFile::create("SensorDiffDrive", wolf_dir + "/test/yaml/sensor_diff_drive.yaml", {wolf_dir});
- SensorDiffDrivePtr sen = std::dynamic_pointer_cast<SensorDiffDrive>(sb);
+ SensorDiffDrivePtr sen = std::static_pointer_cast<SensorDiffDrive>(sb);
ASSERT_NE(sen, nullptr);
@@ -91,34 +103,26 @@ TEST(SensorDiffDrive, factory_yaml)
TEST(SensorDiffDrive, getParams)
{
- auto param = std::make_shared<ParamsSensorDiffDrive>();
- param->ticks_per_wheel_revolution = 400;
- param->ticks_std_factor = 2;
+ YAML::Node param = YAML::LoadFile(wolf_dir + "/test/yaml/sensor_diff_drive.yaml");
+ param["ticks_per_wheel_revolution"] = 400;
+ param["ticks_std_factor"] = 2;
- SpecStateSensorComposite priors{{'P', SpecStateSensor("StatePoint2d", p_state, "fix")}, // default not dynamic
- {'O', SpecStateSensor("StateAngle", o_state, "fix")}, // default not dynamic
- {'I', SpecStateSensor("StateParams3", i_state, "fix")}}; // default not dynamic
-
- auto sen = std::static_pointer_cast<SensorDiffDrive>(SensorDiffDrive::create(param, priors));
+ auto sen = std::static_pointer_cast<SensorDiffDrive>(SensorDiffDrive::create(param, {wolf_dir}));
ASSERT_NE(sen->getParams(), nullptr);
- ASSERT_NEAR(sen->getParams()->ticks_per_wheel_revolution, 400, Constants::EPS);
+ ASSERT_NEAR(sen->getTicksPerWheelRevolution(), 400, Constants::EPS);
ASSERT_NEAR(sen->getRadiansPerTick(), 2 * M_PI / 400, Constants::EPS);
- ASSERT_NEAR(sen->getParams()->ticks_std_factor, 2, Constants::EPS);
+ ASSERT_NEAR(sen->getTicksStdFactor(), 2, Constants::EPS);
}
TEST(SensorDiffDrive, computeNoiseCov)
{
- auto param = std::make_shared<ParamsSensorDiffDrive>();
- param->ticks_per_wheel_revolution = 400;
- param->ticks_std_factor = 2;
-
- SpecStateSensorComposite priors{{'P', SpecStateSensor("StatePoint2d", p_state, "fix")}, // default not dynamic
- {'O', SpecStateSensor("StateAngle", o_state, "fix")}, // default not dynamic
- {'I', SpecStateSensor("StateParams3", i_state, "fix")}}; // default not dynamic
+ YAML::Node param = YAML::LoadFile(wolf_dir + "/test/yaml/sensor_diff_drive.yaml");
+ param["ticks_per_wheel_revolution"] = 400;
+ param["ticks_std_factor"] = 2;
- auto sen = SensorDiffDrive::create(param, priors);
+ auto sen = SensorDiffDrive::create(param, {wolf_dir});
ASSERT_MATRIX_APPROX(sen->computeNoiseCov(Eigen::Vector2d::Zero()), Eigen::Matrix2d::Zero(), Constants::EPS);
ASSERT_MATRIX_APPROX(
diff --git a/test/gtest_sensor_odom.cpp b/test/gtest_sensor_odom.cpp
index 6c515024d26c6c6b01eae9f73258911d849e94b6..0e038e700895fad567a295515e24e625bc259cfa 100644
--- a/test/gtest_sensor_odom.cpp
+++ b/test/gtest_sensor_odom.cpp
@@ -28,27 +28,20 @@ using namespace yaml_schema_cpp;
std::string wolf_dir = _WOLF_CODE_DIR;
-VectorXd vector0 = VectorXd(0);
-VectorXd p_state_2D = VectorXd::Random(2);
-VectorXd p_state_3D = VectorXd::Random(3);
-VectorXd o_state_2D = VectorXd::Random(1);
-VectorXd o_state_3D = VectorXd::Random(4).normalized();
-VectorXd p_std_2D = VectorXd::Random(2).cwiseAbs();
-VectorXd p_std_3D = VectorXd::Random(3).cwiseAbs();
-VectorXd o_std_2D = VectorXd::Random(1).cwiseAbs();
-VectorXd o_std_3D = VectorXd::Random(3).cwiseAbs();
-MatrixXd p_cov_2D = p_std_2D.cwiseAbs2().asDiagonal();
-MatrixXd p_cov_3D = p_std_3D.cwiseAbs2().asDiagonal();
-MatrixXd o_cov_2D = o_std_2D.cwiseAbs2().asDiagonal();
-MatrixXd o_cov_3D = o_std_3D.cwiseAbs2().asDiagonal();
-double k_disp_to_disp = 0.1;
-double k_disp_to_rot = 0.2;
-double k_rot_to_rot = 0.3;
-double min_disp_var = 0.01;
-double min_rot_var = 0.02;
-double noise_p_std = 0.1;
-double noise_o_std = 0.01;
-MatrixXd noise_cov_dummy = Vector2d(noise_p_std, noise_o_std).cwiseAbs2().asDiagonal();
+ProblemPtr problem_2D = Problem::create("PO", 2);
+ProblemPtr problem_3D = Problem::create("PO", 3);
+VectorXd vector0 = VectorXd(0);
+VectorXd p_state_2D = (VectorXd(2) << 1, 2).finished();
+VectorXd p_state_3D = (VectorXd(3) << 1, 2, 3).finished();
+VectorXd o_state_2D = (VectorXd(1) << 3).finished();
+VectorXd o_state_3D = (VectorXd(4) << 1, 0, 0, 0).finished();
+double k_disp_to_disp = 0.1;
+double k_disp_to_rot = 0.2;
+double k_rot_to_rot = 0.3;
+double min_disp_var = 0.01;
+double min_rot_var = 0.02;
+double noise_p_std = 0.1;
+double noise_o_std = 0.01;
void checkSensor(SensorBasePtr S,
char _key,
@@ -92,409 +85,74 @@ void checkSensor(SensorBasePtr S,
}
}
-//////////////////////////////////////////////////////////////////////////////////////////////////////
-////////////////////////////////// CONSTRUCTOR WITH PRIORS AND PARAMS ////////////////////////////////
-//////////////////////////////////////////////////////////////////////////////////////////////////////
-// 2D Fix
-TEST(SensorOdom, create_priors_fix_2D)
+TEST(SensorOdom, creators_and_factories)
{
- auto params = std::make_shared<ParamsSensorOdom>();
- params->name = "sensor_1";
- params->k_disp_to_disp = k_disp_to_disp;
- params->k_disp_to_rot = k_disp_to_rot;
- params->k_rot_to_rot = k_rot_to_rot;
- params->min_disp_var = min_disp_var;
- params->min_rot_var = min_rot_var;
-
- auto specs = SpecStateSensorComposite({{'P', SpecStateSensor("StatePoint2d", p_state_2D, "fix")},
- {'O', SpecStateSensor("StateAngle", o_state_2D, "fix")}});
- auto S = SensorOdom2d::create(params, specs);
-
- // checks
- checkSensor(S, 'P', p_state_2D, true, vector0, false, vector0);
- checkSensor(S, 'O', o_state_2D, true, vector0, false, vector0);
-}
-
-// 3D Fix
-TEST(SensorOdom, create_priors_fix_3D)
-{
- auto params = std::make_shared<ParamsSensorOdom>();
- params->name = "sensor_1";
- params->k_disp_to_disp = k_disp_to_disp;
- params->k_disp_to_rot = k_disp_to_rot;
- params->k_rot_to_rot = k_rot_to_rot;
- params->min_disp_var = min_disp_var;
- params->min_rot_var = min_rot_var;
-
- auto specs = SpecStateSensorComposite({{'P', SpecStateSensor("StatePoint3d", p_state_3D, "fix")},
- {'O', SpecStateSensor("StateQuaternion", o_state_3D, "fix")}});
- auto S = SensorOdom3d::create(params, specs);
-
- // factors
- ASSERT_EQ(S->getPriorFactorMap().size(), 0);
-
- // check
- checkSensor(S, 'P', p_state_3D, true, vector0, false, vector0);
- checkSensor(S, 'O', o_state_3D, true, vector0, false, vector0);
-}
-
-// 2D Initial guess
-TEST(SensorOdom, create_priors_initial_guess_2D)
-{
- auto params = std::make_shared<ParamsSensorOdom>();
- params->name = "sensor_1";
- params->k_disp_to_disp = k_disp_to_disp;
- params->k_disp_to_rot = k_disp_to_rot;
- params->k_rot_to_rot = k_rot_to_rot;
- params->min_disp_var = min_disp_var;
- params->min_rot_var = min_rot_var;
-
- auto specs = SpecStateSensorComposite({{'P', SpecStateSensor("StatePoint2d", p_state_2D, "initial_guess")},
- {'O', SpecStateSensor("StateAngle", o_state_2D, "initial_guess")}});
- auto S = SensorOdom2d::create(params, specs);
-
- // factors
- ASSERT_EQ(S->getPriorFactorMap().size(), 0);
-
- // check
- checkSensor(S, 'P', p_state_2D, false, vector0, false, vector0);
- checkSensor(S, 'O', o_state_2D, false, vector0, false, vector0);
-}
-
-// 3D Initial guess
-TEST(SensorOdom, create_priors_initial_guess_3D)
-{
- auto params = std::make_shared<ParamsSensorOdom>();
- params->name = "sensor_1";
- params->k_disp_to_disp = k_disp_to_disp;
- params->k_disp_to_rot = k_disp_to_rot;
- params->k_rot_to_rot = k_rot_to_rot;
- params->min_disp_var = min_disp_var;
- params->min_rot_var = min_rot_var;
-
- auto specs = SpecStateSensorComposite({{'P', SpecStateSensor("StatePoint3d", p_state_3D, "initial_guess")},
- {'O', SpecStateSensor("StateQuaternion", o_state_3D, "initial_guess")}});
- auto S = SensorOdom3d::create(params, specs);
-
- // factors
- ASSERT_EQ(S->getPriorFactorMap().size(), 0);
-
- // check
- checkSensor(S, 'P', p_state_3D, false, vector0, false, vector0);
- checkSensor(S, 'O', o_state_3D, false, vector0, false, vector0);
-}
-
-// 2D Factor
-TEST(SensorOdom, create_priors_factor_2D)
-{
- auto params = std::make_shared<ParamsSensorOdom>();
- params->name = "sensor_1";
- params->k_disp_to_disp = k_disp_to_disp;
- params->k_disp_to_rot = k_disp_to_rot;
- params->k_rot_to_rot = k_rot_to_rot;
- params->min_disp_var = min_disp_var;
- params->min_rot_var = min_rot_var;
-
- auto specs = SpecStateSensorComposite({{'P', SpecStateSensor("StatePoint2d", p_state_2D, "factor", p_std_2D)},
- {'O', SpecStateSensor("StateAngle", o_state_2D, "factor", o_std_2D)}});
- auto S = SensorOdom2d::create(params, specs);
-
- // factors
- ASSERT_EQ(S->getPriorFactorMap().size(), 2);
-
- // check
- checkSensor(S, 'P', p_state_2D, false, p_std_2D, false, vector0);
- checkSensor(S, 'O', o_state_2D, false, o_std_2D, false, vector0);
-}
-
-// 3D Factor
-TEST(SensorOdom, create_priors_factor_3D)
-{
- auto params = std::make_shared<ParamsSensorOdom>();
- params->name = "sensor_1";
- params->k_disp_to_disp = k_disp_to_disp;
- params->k_disp_to_rot = k_disp_to_rot;
- params->k_rot_to_rot = k_rot_to_rot;
- params->min_disp_var = min_disp_var;
- params->min_rot_var = min_rot_var;
-
- auto specs = SpecStateSensorComposite({{'P', SpecStateSensor("StatePoint3d", p_state_3D, "factor", p_std_3D)},
- {'O', SpecStateSensor("StateQuaternion", o_state_3D, "factor", o_std_3D)}});
- auto S = SensorOdom3d::create(params, specs);
-
- // factors
- ASSERT_EQ(S->getPriorFactorMap().size(), 2);
-
- // check
- checkSensor(S, 'P', p_state_3D, false, p_std_3D, false, vector0);
- checkSensor(S, 'O', o_state_3D, false, o_std_3D, false, vector0);
-}
-
-// 2D Initial guess dynamic
-TEST(SensorOdom, create_priors_initial_guess_dynamic_2D)
-{
- auto params = std::make_shared<ParamsSensorOdom>();
- params->name = "sensor_1";
- params->k_disp_to_disp = k_disp_to_disp;
- params->k_disp_to_rot = k_disp_to_rot;
- params->k_rot_to_rot = k_rot_to_rot;
- params->min_disp_var = min_disp_var;
- params->min_rot_var = min_rot_var;
-
- auto specs =
- SpecStateSensorComposite({{'P', SpecStateSensor("StatePoint2d", p_state_2D, "initial_guess", vector0, true)},
- {'O', SpecStateSensor("StateAngle", o_state_2D, "initial_guess", vector0, true)}});
- auto S = SensorOdom2d::create(params, specs);
-
- // factors
- ASSERT_EQ(S->getPriorFactorMap().size(), 0);
-
- // check
- checkSensor(S, 'P', p_state_2D, false, vector0, true, vector0);
- checkSensor(S, 'O', o_state_2D, false, vector0, true, vector0);
-}
-
-// 3D Initial guess dynamic
-TEST(SensorOdom, create_priors_initial_guess_dynamic_3D)
-{
- auto params = std::make_shared<ParamsSensorOdom>();
- params->name = "sensor_1";
- params->k_disp_to_disp = k_disp_to_disp;
- params->k_disp_to_rot = k_disp_to_rot;
- params->k_rot_to_rot = k_rot_to_rot;
- params->min_disp_var = min_disp_var;
- params->min_rot_var = min_rot_var;
-
- auto specs = SpecStateSensorComposite(
- {{'P', SpecStateSensor("StatePoint3d", p_state_3D, "initial_guess", vector0, true)},
- {'O', SpecStateSensor("StateQuaternion", o_state_3D, "initial_guess", vector0, true)}});
- auto S = SensorOdom3d::create(params, specs);
-
- // factors
- ASSERT_EQ(S->getPriorFactorMap().size(), 0);
-
- // check
- checkSensor(S, 'P', p_state_3D, false, vector0, true, vector0);
- checkSensor(S, 'O', o_state_3D, false, vector0, true, vector0);
-}
-
-// 2D Initial guess dynamic drift
-TEST(SensorOdom, create_priors_initial_guess_dynamic_drift_2D)
-{
- auto params = std::make_shared<ParamsSensorOdom>();
- params->name = "sensor_1";
- params->k_disp_to_disp = k_disp_to_disp;
- params->k_disp_to_rot = k_disp_to_rot;
- params->k_rot_to_rot = k_rot_to_rot;
- params->min_disp_var = min_disp_var;
- params->min_rot_var = min_rot_var;
-
- auto specs = SpecStateSensorComposite(
- {{'P', SpecStateSensor("StatePoint2d", p_state_2D, "initial_guess", vector0, true, p_std_2D)},
- {'O', SpecStateSensor("StateAngle", o_state_2D, "initial_guess", vector0, true, o_std_2D)}});
- auto S = SensorOdom2d::create(params, specs);
-
- // factors
- ASSERT_EQ(S->getPriorFactorMap().size(), 0);
-
- // check
- checkSensor(S, 'P', p_state_2D, false, vector0, true, p_std_2D);
- checkSensor(S, 'O', o_state_2D, false, vector0, true, o_std_2D);
-}
-
-// 3D Initial guess dynamic drift
-TEST(SensorOdom, create_priors_initial_guess_dynamic_drift_3D)
-{
- auto params = std::make_shared<ParamsSensorOdom>();
- params->name = "sensor_1";
- params->k_disp_to_disp = k_disp_to_disp;
- params->k_disp_to_rot = k_disp_to_rot;
- params->k_rot_to_rot = k_rot_to_rot;
- params->min_disp_var = min_disp_var;
- params->min_rot_var = min_rot_var;
-
- auto specs = SpecStateSensorComposite(
- {{'P', SpecStateSensor("StatePoint3d", p_state_3D, "initial_guess", vector0, true, p_std_3D)},
- {'O', SpecStateSensor("StateQuaternion", o_state_3D, "initial_guess", vector0, true, o_std_3D)}});
- auto S = SensorOdom3d::create(params, specs);
-
- // factors
- ASSERT_EQ(S->getPriorFactorMap().size(), 0);
-
- // check
- checkSensor(S, 'P', p_state_3D, false, vector0, true, p_std_3D);
- checkSensor(S, 'O', o_state_3D, false, vector0, true, o_std_3D);
-}
-
-//////////////////////////////////////////////////////////////////////////////////////////////////////
-//////////////////////////////////////////// FactorySensorNode ///////////////////////////////////////////
-//////////////////////////////////////////////////////////////////////////////////////////////////////
-TEST(SensorOdom, factory)
-{
- std::vector<int> dims({2, 3});
- std::vector<std::string> modes({"fix", "initial_guess", "factor"});
- std::vector<bool> dynamics({false, true});
- std::vector<bool> drifts({false, true});
- std::vector<bool> wrongs({false, true});
-
- VectorXd p_state(4), o_state(4), po_std(4);
- p_state << 1, 2, 3, 4;
- o_state << 1, 0, 0, 0;
- po_std << 0.1, 0.2, 0.3, 0.4;
-
- // P & O
- for (auto dim : dims)
- for (auto mode : modes)
- for (auto dynamic : dynamics)
- for (auto drift : drifts)
- for (auto wrong : wrongs)
- {
- // nonsense combination
- if (not dynamic and drift) continue;
-
- std::string name = "sensor_PO_" + toString(dim) + "D_" + mode + (dynamic ? "_dynamic" : "") +
- (drift ? "_drift" : "") + (wrong ? "_wrong" : "");
- // Yaml server
- YamlServer server({wolf_dir}, wolf_dir + "/test/yaml/sensor_tests/" + name + ".yaml");
-
- WOLF_INFO("Creating sensor from ", name, ".yaml");
-
- auto valid = server.applySchema("SensorOdom" + toString(dim) + "d");
- WOLF_WARN_COND(not valid and not wrong, server.getLog());
- WOLF_WARN_COND(valid and wrong, "Schema didn't detect any problem in a wrong yaml.");
-
- // CORRECT YAML
- if (not wrong)
- {
- auto S = FactorySensorNode::create("SensorOdom" + toString(dim) + "d", server.getNode());
-
- auto p_size = dim;
- auto o_size = dim == 2 ? 1 : 4;
- auto p_size_std = mode == "factor" ? dim : 0;
- auto o_size_std = mode == "factor" ? (dim == 2 ? 1 : 3) : 0;
- auto p_size_std_drift = drift ? dim : 0;
- auto o_size_std_drift = drift ? (dim == 2 ? 1 : 3) : 0;
-
- // factors
- ASSERT_EQ(S->getPriorFactorMap().size(), mode == "factor" ? 2 : 0);
-
- // check
- checkSensor(S,
- 'P',
- p_state.head(p_size),
- mode == "fix",
- po_std.head(p_size_std),
- dynamic,
- po_std.head(p_size_std_drift));
- checkSensor(S,
- 'O',
- o_state.head(o_size),
- mode == "fix",
- po_std.head(o_size_std),
- dynamic,
- po_std.head(o_size_std_drift));
- }
- // INCORRECT YAML
- else
- {
- ASSERT_THROW(
- FactorySensorNode::create("SensorOdom" + toString(dim) + "d", server.getNode()),
- std::runtime_error);
- }
- }
-}
-
-//////////////////////////////////////////////////////////////////////////////////////////////////////
-////////////////////////////////////////// FactorySensorFile /////////////////////////////////////////
-//////////////////////////////////////////////////////////////////////////////////////////////////////
-TEST(SensorOdom, factory_yaml)
-{
- std::vector<int> dims({2, 3});
- std::vector<std::string> modes({"fix", "initial_guess", "factor"});
- std::vector<bool> dynamics({false, true});
- std::vector<bool> drifts({false, true});
- std::vector<bool> wrongs({false, true});
-
- VectorXd p_state(4), o_state(4), po_std(4);
- p_state << 1, 2, 3, 4;
- o_state << 1, 0, 0, 0;
- po_std << 0.1, 0.2, 0.3, 0.4;
-
- // P & O
- for (auto dim : dims)
- for (auto mode : modes)
- for (auto dynamic : dynamics)
- for (auto drift : drifts)
- for (auto wrong : wrongs)
- {
- // nonsense combination
- if (not dynamic and drift) continue;
-
- std::string yaml_filepath = wolf_dir + "/test/yaml/sensor_tests/" + "sensor_PO_" +
- toString(dim) + "D_" + mode + (dynamic ? "_dynamic" : "") +
- (drift ? "_drift" : "") + (wrong ? "_wrong" : "") + ".yaml";
-
- WOLF_INFO("Creating sensor from ", yaml_filepath);
-
- // CORRECT YAML
- if (not wrong)
- {
- auto S = FactorySensorFile::create(
- "SensorOdom" + toString(dim) + "d", yaml_filepath, {wolf_dir});
-
- auto p_size = dim;
- auto o_size = dim == 2 ? 1 : 4;
- auto p_size_std = mode == "factor" ? dim : 0;
- auto o_size_std = mode == "factor" ? (dim == 2 ? 1 : 3) : 0;
- auto p_size_std_drift = drift ? dim : 0;
- auto o_size_std_drift = drift ? (dim == 2 ? 1 : 3) : 0;
-
- // factors
- ASSERT_EQ(S->getPriorFactorMap().size(), mode == "factor" ? 2 : 0);
-
- // check
- checkSensor(S,
- 'P',
- p_state.head(p_size),
- mode == "fix",
- po_std.head(p_size_std),
- dynamic,
- po_std.head(p_size_std_drift));
- checkSensor(S,
- 'O',
- o_state.head(o_size),
- mode == "fix",
- po_std.head(o_size_std),
- dynamic,
- po_std.head(o_size_std_drift));
- }
- // INCORRECT YAML
- else
- {
- ASSERT_THROW(FactorySensorFile::create(
- "SensorOdom" + toString(dim) + "d", yaml_filepath, {wolf_dir}),
- std::runtime_error);
- }
- }
+ for (int dim : {2, 3})
+ {
+ std::string name = "sensor_odom_" + toString(dim) + "d";
+
+ YAML::Node params;
+ params["type"] = "SensorOdom" + toString(dim) + "d";
+ params["name"] = name;
+ params["noise_p_std"] = noise_p_std;
+ params["noise_o_std"] = noise_o_std;
+ params["k_disp_to_disp"] = k_disp_to_disp;
+ params["k_disp_to_rot"] = k_disp_to_rot;
+ params["k_rot_to_rot"] = k_rot_to_rot;
+ params["min_disp_var"] = min_disp_var;
+ params["min_rot_var"] = min_rot_var;
+ params["states"]["keys"] = "PO";
+
+ params["states"]["P"]["type"] = dim == 2 ? "StatePoint2d" : "StatePoint3d";
+ params["states"]["P"]["state"] = dim == 2 ? p_state_2D : p_state_3D;
+ params["states"]["P"]["mode"] = "fix";
+ params["states"]["P"]["dynamic"] = false;
+
+ params["states"]["O"]["type"] = dim == 2 ? "StateAngle" : "StateQuaternion";
+ params["states"]["O"]["state"] = dim == 2 ? o_state_2D : o_state_3D;
+ params["states"]["O"]["mode"] = "fix";
+ params["states"]["O"]["dynamic"] = false;
+
+ // create from node ------------------------------------------------------------------------
+ auto S = dim == 2 ? std::static_pointer_cast<SensorBase>(SensorOdom2d::create(params, {wolf_dir}))
+ : std::static_pointer_cast<SensorBase>(SensorOdom3d::create(params, {wolf_dir}));
+
+ // checks
+ checkSensor(S, 'P', dim == 2 ? p_state_2D : p_state_3D, true, vector0, false, vector0);
+ checkSensor(S, 'O', dim == 2 ? o_state_2D : o_state_3D, true, vector0, false, vector0);
+
+ // emplace ------------------------------------------------------------------------
+ auto S2 = dim == 2 ? std::static_pointer_cast<SensorBase>(
+ SensorBase::emplace<SensorOdom2d>(problem_2D->getHardware(), params, {wolf_dir}))
+ : std::static_pointer_cast<SensorBase>(
+ SensorBase::emplace<SensorOdom3d>(problem_3D->getHardware(), params, {wolf_dir}));
+
+ // checks
+ checkSensor(S2, 'P', dim == 2 ? p_state_2D : p_state_3D, true, vector0, false, vector0);
+ checkSensor(S2, 'O', dim == 2 ? o_state_2D : o_state_3D, true, vector0, false, vector0);
+
+ // factory node ------------------------------------------------------------------------
+ auto params_node = YAML::LoadFile(wolf_dir + "/test/yaml/" + name + ".yaml");
+
+ auto S3 = FactorySensorNode::create("SensorOdom" + toString(dim) + "d", params_node, {wolf_dir});
+
+ // checks
+ checkSensor(S3, 'P', dim == 2 ? p_state_2D : p_state_3D, true, vector0, false, vector0);
+ checkSensor(S3, 'O', dim == 2 ? o_state_2D : o_state_3D, true, vector0, false, vector0);
+
+ // factory file ------------------------------------------------------------------------
+ auto S4 = FactorySensorFile::create(
+ "SensorOdom" + toString(dim) + "d", wolf_dir + "/test/yaml/" + name + ".yaml", {wolf_dir});
+
+ // checks
+ checkSensor(S4, 'P', dim == 2 ? p_state_2D : p_state_3D, true, vector0, false, vector0);
+ checkSensor(S4, 'O', dim == 2 ? o_state_2D : o_state_3D, true, vector0, false, vector0);
+ }
}
-//////////////////////////////////////////////////////////////////////////////////////////////////////
-////////////////////////////////////////// COMPUTE NOISE COV /////////////////////////////////////////
-//////////////////////////////////////////////////////////////////////////////////////////////////////
TEST(SensorOdom, compute_noise_cov_3D)
{
- auto params = std::make_shared<ParamsSensorOdom>();
- params->name = "sensor_1";
- params->k_disp_to_disp = k_disp_to_disp;
- params->k_disp_to_rot = k_disp_to_rot;
- params->k_rot_to_rot = k_rot_to_rot;
- params->min_disp_var = min_disp_var;
- params->min_rot_var = min_rot_var;
-
- auto S = SensorOdom3d::create(
- params,
- SpecStateSensorComposite({{'P', SpecStateSensor("StatePoint3d", p_state_3D, "fix")}, // default not dynamic
- {'O', SpecStateSensor("StateQuaternion", o_state_3D, "fix")}}));
+ auto S = SensorOdom3d::create(wolf_dir + "/test/yaml/sensor_odom_3d.yaml", {wolf_dir});
Vector6d disp_elements, rot_elements;
disp_elements << 1, 1, 1, 0, 0, 0;
diff --git a/test/gtest_sensor_pose.cpp b/test/gtest_sensor_pose.cpp
index 6fe7f616e42b8365fa0e7560c179ba85adf74374..f6231b8a1e49ccb089cf3060830ad48fa2ad0b7c 100644
--- a/test/gtest_sensor_pose.cpp
+++ b/test/gtest_sensor_pose.cpp
@@ -22,147 +22,127 @@
#include "core/sensor/factory_sensor.h"
#include "core/utils/utils_gtest.h"
-#include <cstdio>
-
using namespace wolf;
using namespace Eigen;
using namespace yaml_schema_cpp;
std::string wolf_dir = _WOLF_CODE_DIR;
-Vector2d p_state_2D = (Vector2d() << 1, 2).finished();
-Vector3d p_state_3D = (Vector3d() << 1, 2, 3).finished();
-Vector1d o_state_2D = (Vector1d() << 3).finished();
-Vector4d o_state_3D = (Vector4d() << .5, .5, .5, .5).finished();
-Vector3d std_noise_2D = (Vector3d() << 0.1, 0.2, 0.3).finished();
-Vector6d std_noise_3D = (Vector6d() << 0.1, 0.2, 0.3, 0.4, 0.5, 0.6).finished();
-Matrix3d noise_cov_2D = std_noise_2D.cwiseAbs2().asDiagonal();
-Matrix6d noise_cov_3D = std_noise_3D.cwiseAbs2().asDiagonal();
-
-TEST(Pose, constructor_priors_2D)
-{
- auto param = std::make_shared<ParamsSensorPose>();
- param->name = "a not so cool sensor";
- param->std_noise = std_noise_2D;
-
- SpecStateSensorComposite priors{{'P', SpecStateSensor("StatePoint2d", p_state_2D, "fix")}, // default not dynamic
- {'O', SpecStateSensor("StateAngle", o_state_2D, "fix")}}; // default not dynamic
-
- auto sen = SensorPose2d::create(param, priors);
-
- ASSERT_NE(sen, nullptr);
-
- ASSERT_TRUE(sen->getP()->isFixed());
- ASSERT_TRUE(sen->getO()->isFixed());
-
- ASSERT_MATRIX_APPROX(sen->getP()->getState(), p_state_2D, Constants::EPS);
- ASSERT_MATRIX_APPROX(sen->getO()->getState(), o_state_2D, Constants::EPS);
-
- ASSERT_MATRIX_APPROX(sen->computeNoiseCov(VectorXd(0)), noise_cov_2D, Constants::EPS)
-}
-
-TEST(Pose, constructor_priors_3D)
-{
- auto param = std::make_shared<ParamsSensorPose>();
- param->name = "a not so cool sensor";
- param->std_noise = std_noise_3D;
-
- SpecStateSensorComposite priors{
- {'P', SpecStateSensor("StatePoint3d", p_state_3D, "fix")}, // default not dynamic
- {'O', SpecStateSensor("StateQuaternion", o_state_3D, "fix")}}; // default not dynamic
-
- auto sen = SensorPose3d::create(param, priors);
-
- ASSERT_NE(sen, nullptr);
-
- ASSERT_TRUE(sen->getP()->isFixed());
- ASSERT_TRUE(sen->getO()->isFixed());
-
- ASSERT_MATRIX_APPROX(sen->getP()->getState(), p_state_3D, Constants::EPS);
- ASSERT_MATRIX_APPROX(sen->getO()->getState(), o_state_3D, Constants::EPS);
-
- ASSERT_MATRIX_APPROX(sen->computeNoiseCov(VectorXd(0)), noise_cov_3D, Constants::EPS)
-}
-
-TEST(Pose, factory_2D)
-{
- YamlServer server({wolf_dir}, wolf_dir + "/test/yaml/sensor_pose_2d.yaml");
-
- ASSERT_TRUE(server.applySchema("SensorPose2d"));
-
- auto sb = FactorySensorNode::create("SensorPose2d", server.getNode());
-
- SensorPose2dPtr sen = std::dynamic_pointer_cast<SensorPose2d>(sb);
-
- ASSERT_NE(sen, nullptr);
-
- ASSERT_TRUE(sen->getP()->isFixed());
- ASSERT_TRUE(sen->getO()->isFixed());
-
- ASSERT_MATRIX_APPROX(sen->getP()->getState(), p_state_2D, Constants::EPS);
- ASSERT_MATRIX_APPROX(sen->getO()->getState(), o_state_2D, Constants::EPS);
-
- ASSERT_MATRIX_APPROX(sen->computeNoiseCov(VectorXd(0)), noise_cov_2D, Constants::EPS)
-}
-
-TEST(Pose, factory_3D)
+ProblemPtr problem_2D = Problem::create("PO", 2);
+ProblemPtr problem_3D = Problem::create("PO", 3);
+VectorXd vector0 = VectorXd(0);
+VectorXd p_state_2D = (VectorXd(2) << 1, 2).finished();
+VectorXd p_state_3D = (VectorXd(3) << 1, 2, 3).finished();
+VectorXd o_state_2D = (VectorXd(1) << 3).finished();
+VectorXd o_state_3D = (VectorXd(4) << 0.5, 0.5, 0.5, 0.5).finished();
+VectorXd std_noise_2D = (VectorXd(3) << 0.1, 0.2, 0.3).finished();
+VectorXd std_noise_3D = (VectorXd(6) << 0.1, 0.2, 0.3, 0.4, 0.5, 0.6).finished();
+
+void checkSensor(SensorBasePtr S,
+ char _key,
+ const VectorXd& _state,
+ bool _fixed,
+ const VectorXd& _noise_std,
+ bool _dynamic,
+ const VectorXd& _drift_std)
{
- YamlServer server({wolf_dir}, wolf_dir + "/test/yaml/sensor_pose_3d.yaml");
-
- ASSERT_TRUE(server.applySchema("SensorPose3d"));
-
- auto sb = FactorySensorNode::create("SensorPose3d", server.getNode());
-
- SensorPose3dPtr sen = std::dynamic_pointer_cast<SensorPose3d>(sb);
-
- ASSERT_NE(sen, nullptr);
-
- ASSERT_TRUE(sen->getP()->isFixed());
- ASSERT_TRUE(sen->getO()->isFixed());
-
- ASSERT_MATRIX_APPROX(sen->getP()->getState(), p_state_3D, Constants::EPS);
- ASSERT_MATRIX_APPROX(sen->getO()->getState(), o_state_3D, Constants::EPS);
-
- ASSERT_MATRIX_APPROX(sen->computeNoiseCov(VectorXd(0)), noise_cov_3D, Constants::EPS)
+ MatrixXd drift_cov = _drift_std.cwiseAbs2().asDiagonal();
+
+ // state
+ ASSERT_MATRIX_APPROX(_state, S->getStateBlockDynamic(_key)->getState(), Constants::EPS);
+ // fixed
+ ASSERT_EQ(S->getStateBlockDynamic(_key)->isFixed(), _fixed);
+ // dynamic
+ ASSERT_EQ(S->isStateBlockDynamic(_key), _dynamic);
+ // drift
+ ASSERT_EQ(_drift_std.size(), S->getDriftStd(_key).size());
+ ASSERT_EQ(drift_cov.size(), S->getDriftCov(_key).size());
+ if (_drift_std.size() != 0)
+ {
+ ASSERT_MATRIX_APPROX(_drift_std, S->getDriftStd(_key), Constants::EPS);
+ ASSERT_MATRIX_APPROX(drift_cov, S->getDriftCov(_key), Constants::EPS);
+ }
+ // factor
+ if (_noise_std.size() != 0)
+ {
+ MatrixXd noise_sqrt_info_up = _noise_std.cwiseInverse().asDiagonal();
+ ASSERT_TRUE(S->getPriorFactor(_key) != nullptr);
+ ASSERT_EQ(_state.size(), S->getPriorFactor(_key)->getMeasurement().size());
+ ASSERT_MATRIX_APPROX(_state, S->getPriorFactor(_key)->getMeasurement(), Constants::EPS);
+ ASSERT_EQ(noise_sqrt_info_up.size(),
+ S->getPriorFactor(_key)->getMeasurementSquareRootInformationUpper().size());
+ ASSERT_MATRIX_APPROX(
+ noise_sqrt_info_up, S->getPriorFactor(_key)->getMeasurementSquareRootInformationUpper(), Constants::EPS);
+ }
+ else
+ {
+ ASSERT_TRUE(S->getPriorFactor(_key) == nullptr);
+ }
}
-TEST(Pose, factory_yaml_2D)
+TEST(SensorPose, creators_and_factories)
{
- auto sb = FactorySensorFile::create(
- "SensorPose2d", wolf_dir + "/test/yaml/sensor_pose_2d.yaml", {wolf_dir});
-
- SensorPose2dPtr sen = std::dynamic_pointer_cast<SensorPose2d>(sb);
-
- ASSERT_NE(sen, nullptr);
-
- ASSERT_TRUE(sen->getP()->isFixed());
- ASSERT_TRUE(sen->getO()->isFixed());
-
- ASSERT_MATRIX_APPROX(sen->getP()->getState(), p_state_2D, Constants::EPS);
- ASSERT_MATRIX_APPROX(sen->getO()->getState(), o_state_2D, Constants::EPS);
-
- ASSERT_MATRIX_APPROX(sen->computeNoiseCov(VectorXd(0)), noise_cov_2D, Constants::EPS)
-}
-
-TEST(Pose, factory_yaml_3D)
-{
- auto sb = FactorySensorFile::create(
- "SensorPose3d", wolf_dir + "/test/yaml/sensor_pose_3d.yaml", {wolf_dir});
-
- SensorPose3dPtr sen = std::dynamic_pointer_cast<SensorPose3d>(sb);
-
- ASSERT_NE(sen, nullptr);
-
- ASSERT_TRUE(sen->getP()->isFixed());
- ASSERT_TRUE(sen->getO()->isFixed());
-
- ASSERT_MATRIX_APPROX(sen->getP()->getState(), p_state_3D, Constants::EPS);
- ASSERT_MATRIX_APPROX(sen->getO()->getState(), o_state_3D, Constants::EPS);
-
- ASSERT_MATRIX_APPROX(sen->computeNoiseCov(VectorXd(0)), noise_cov_3D, Constants::EPS)
+ for (int dim : {2, 3})
+ {
+ std::string name = "sensor_pose_" + toString(dim) + "d";
+
+ YAML::Node params;
+ params["type"] = "SensorPose" + toString(dim) + "d";
+ params["name"] = name;
+ params["std_noise"] = dim == 2 ? std_noise_2D : std_noise_3D;
+ params["states"]["keys"] = "PO";
+
+ params["states"]["P"]["type"] = dim == 2 ? "StatePoint2d" : "StatePoint3d";
+ params["states"]["P"]["state"] = dim == 2 ? p_state_2D : p_state_3D;
+ params["states"]["P"]["mode"] = "fix";
+ params["states"]["P"]["dynamic"] = false;
+
+ params["states"]["O"]["type"] = dim == 2 ? "StateAngle" : "StateQuaternion";
+ params["states"]["O"]["state"] = dim == 2 ? o_state_2D : o_state_3D;
+ params["states"]["O"]["mode"] = "fix";
+ params["states"]["O"]["dynamic"] = false;
+
+ // create from node ------------------------------------------------------------------------
+ auto S = dim == 2 ? std::static_pointer_cast<SensorBase>(SensorPose2d::create(params, {wolf_dir}))
+ : std::static_pointer_cast<SensorBase>(SensorPose3d::create(params, {wolf_dir}));
+
+ // checks
+ checkSensor(S, 'P', dim == 2 ? p_state_2D : p_state_3D, true, vector0, false, vector0);
+ checkSensor(S, 'O', dim == 2 ? o_state_2D : o_state_3D, true, vector0, false, vector0);
+ ASSERT_MATRIX_APPROX(S->computeNoiseCov(Vector1d::Zero()),
+ MatrixXd((dim == 2 ? std_noise_2D : std_noise_3D).cwiseAbs2().asDiagonal()),
+ 1e-9);
+
+ // emplace ------------------------------------------------------------------------
+ auto S2 = dim == 2 ? std::static_pointer_cast<SensorBase>(
+ SensorBase::emplace<SensorPose2d>(problem_2D->getHardware(), params, {wolf_dir}))
+ : std::static_pointer_cast<SensorBase>(
+ SensorBase::emplace<SensorPose3d>(problem_3D->getHardware(), params, {wolf_dir}));
+
+ // checks
+ checkSensor(S2, 'P', dim == 2 ? p_state_2D : p_state_3D, true, vector0, false, vector0);
+ checkSensor(S2, 'O', dim == 2 ? o_state_2D : o_state_3D, true, vector0, false, vector0);
+
+ // factory node ------------------------------------------------------------------------
+ auto params_node = YAML::LoadFile(wolf_dir + "/test/yaml/" + name + ".yaml");
+
+ auto S3 = FactorySensorNode::create("SensorPose" + toString(dim) + "d", params_node, {wolf_dir});
+
+ // checks
+ checkSensor(S3, 'P', dim == 2 ? p_state_2D : p_state_3D, true, vector0, false, vector0);
+ checkSensor(S3, 'O', dim == 2 ? o_state_2D : o_state_3D, true, vector0, false, vector0);
+
+ // factory file ------------------------------------------------------------------------
+ auto S4 = FactorySensorFile::create(
+ "SensorPose" + toString(dim) + "d", wolf_dir + "/test/yaml/" + name + ".yaml", {wolf_dir});
+
+ // checks
+ checkSensor(S4, 'P', dim == 2 ? p_state_2D : p_state_3D, true, vector0, false, vector0);
+ checkSensor(S4, 'O', dim == 2 ? o_state_2D : o_state_3D, true, vector0, false, vector0);
+ }
}
-int main(int argc, char **argv)
+int main(int argc, char** argv)
{
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
diff --git a/test/gtest_shared_from_this.cpp b/test/gtest_shared_from_this.cpp
index 82dda2203ca4a0c288a28162c2ebcd22976548a7..6859e3625787787b4841dd729941e2c516fc47dd 100644
--- a/test/gtest_shared_from_this.cpp
+++ b/test/gtest_shared_from_this.cpp
@@ -25,52 +25,51 @@ class CChildBase;
class CParentBase : public wolf::NodeBase
{
- public:
+ public:
+ std::list<std::shared_ptr<CChildBase> > child_list_;
- std::list<std::shared_ptr<CChildBase> > child_list_;
+ CParentBase() : NodeBase(""){};
- CParentBase() :
- NodeBase("")
- {};
+ ~CParentBase() override{};
- ~CParentBase() override{};
+ virtual void addChild(std::shared_ptr<CChildBase> _child_ptr) final
+ {
+ child_list_.push_back(_child_ptr);
+ }
- virtual void addChild(std::shared_ptr<CChildBase> _child_ptr) final
- {
- child_list_.push_back(_child_ptr);
- }
-
- bool hasChildren() const override {return false;};
+ bool hasChildren() const override
+ {
+ return false;
+ };
};
class CParentDerived : public CParentBase
{
- public:
-
- CParentDerived(){};
+ public:
+ CParentDerived(){};
};
class CChildBase : public wolf::NodeBase, public std::enable_shared_from_this<CChildBase>
{
- public:
- std::shared_ptr<CParentBase> parent_ptr_;
-
- CChildBase(std::shared_ptr<CParentBase> _parent_ptr) :
- NodeBase(""),
- parent_ptr_(_parent_ptr)
- {
- auto wptr = std::shared_ptr<CChildBase>( this, [](CChildBase*){} );
-
- parent_ptr_->addChild(shared_from_this());
- };
- bool hasChildren() const override {return false;};
+ public:
+ std::shared_ptr<CParentBase> parent_ptr_;
+
+ CChildBase(std::shared_ptr<CParentBase> _parent_ptr) : NodeBase(""), parent_ptr_(_parent_ptr)
+ {
+ auto wptr = std::shared_ptr<CChildBase>(this, [](CChildBase *) {});
+
+ parent_ptr_->addChild(shared_from_this());
+ };
+ bool hasChildren() const override
+ {
+ return false;
+ };
};
class CChildDerived : public CChildBase
{
- public:
-
- CChildDerived(std::shared_ptr<CParentBase> _parent_ptr) : CChildBase(_parent_ptr){};
+ public:
+ CChildDerived(std::shared_ptr<CParentBase> _parent_ptr) : CChildBase(_parent_ptr){};
};
TEST(TestTest, SingleTest)
@@ -92,16 +91,17 @@ TEST(TestTest, SingleTest)
ASSERT_EQ(child_derived_ptr->getName(), parent_derived_ptr->child_list_.front()->getName());
ASSERT_EQ(parent_derived_ptr->getName(), child_derived_ptr->parent_ptr_->getName());
- //std::cout << "parent_derived_ptr->getName() " << parent_derived_ptr->getName() << std::endl;
- //std::cout << "child_derived_ptr->getName() " << child_derived_ptr->getName() << std::endl;
- //std::cout << "child_derived_ptr->parent_ptr_->getName() " << child_derived_ptr->parent_ptr_->getName() << std::endl;
- //std::cout << "parent_derived_ptr->child_list_.front()->getName() " << parent_derived_ptr->child_list_.front()->getName() << std::endl;
+ // std::cout << "parent_derived_ptr->getName() " << parent_derived_ptr->getName() << std::endl;
+ // std::cout << "child_derived_ptr->getName() " << child_derived_ptr->getName() << std::endl;
+ // std::cout << "child_derived_ptr->parent_ptr_->getName() " << child_derived_ptr->parent_ptr_->getName() <<
+ // std::endl; std::cout << "parent_derived_ptr->child_list_.front()->getName() " <<
+ // parent_derived_ptr->child_list_.front()->getName() << std::endl;
-// PRINTF("All good at TestTest::DummyTestExample !\n");
+ // PRINTF("All good at TestTest::DummyTestExample !\n");
}
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_solver_ceres.cpp b/test/gtest_solver_ceres.cpp
index a01a0f0eeac8e6fc34ca5120d88b80d59ce40333..95da75d48493337c936f94a5330eb2c8ac6bf2c0 100644
--- a/test/gtest_solver_ceres.cpp
+++ b/test/gtest_solver_ceres.cpp
@@ -47,6 +47,24 @@ using namespace Eigen;
* (modifications should be applied to both tests)
*/
+std::string wolf_dir = _WOLF_CODE_DIR;
+
+class SolverCeresTest : public testing::Test
+{
+ public:
+ ProblemPtr problem;
+ SolverManagerPtr solver_ptr;
+
+ std::string wolf_dir = _WOLF_CODE_DIR;
+
+ void SetUp() override
+ {
+ problem = Problem::create("PO", 3);
+ solver_ptr =
+ FactorySolverFile::create("SolverCeres", problem, wolf_dir + "/test/yaml/solver_ceres.yaml", {wolf_dir});
+ }
+};
+
NodeStateBlocksPtr createStateBlock()
{
auto node = std::make_shared<NodeStateBlocksDummy>();
@@ -65,13 +83,44 @@ FactorBasePtr createFactor(NodeStateBlocksPtr node_ptr)
nullptr,
false);
}
-TEST(SolverCeres, Create)
+
+TEST(SolverCeresTestFactories, FactoryNode)
{
- ProblemPtr P = Problem::create("PO", 3);
- auto solver_ptr = std::make_shared<SolverCeres>(P);
+ auto problem = Problem::create("PO", 3);
+ YAML::Node params;
+ params["period"] = 0;
+ params["verbose"] = 0;
+ params["compute_cov"] = false;
+ params["period"] = 1;
+ params["verbose"] = 2;
+ params["interrupt_on_problem_change"] = false;
+ params["max_num_iterations"] = 1000;
+ params["n_threads"] = 2;
+ params["function_tolerance"] = 0.000001;
+ params["gradient_tolerance"] = 0.0000000001;
+ params["minimizer"] = "LEVENBERG_MARQUARDT";
+ params["use_nonmonotonic_steps"] = false;
+ params["max_consecutive_nonmonotonic_steps"] = 5;
+ params["compute_cov"] = true;
+ params["cov_period"] = 1;
+ params["cov_enum"] = 2;
+ auto solver_ptr = FactorySolverNode::create("SolverCeres", problem, params, {wolf_dir});
// check pointers
- EXPECT_EQ(P, solver_ptr->getProblem());
+ EXPECT_EQ(problem, solver_ptr->getProblem());
+
+ // run solver check
+ EXPECT_TRUE(solver_ptr->check());
+}
+
+TEST(SolverCeresTestFactories, FactoryFile)
+{
+ auto problem = Problem::create("PO", 3);
+ auto solver_ptr =
+ FactorySolverFile::create("SolverCeres", problem, wolf_dir + "/test/yaml/solver_ceres.yaml", {wolf_dir});
+
+ // check pointers
+ EXPECT_EQ(problem, solver_ptr->getProblem());
// run solver check
EXPECT_TRUE(solver_ptr->check());
@@ -79,29 +128,27 @@ TEST(SolverCeres, Create)
////////////////////////////////////////////////////////
// FLOATING STATE BLOCKS
-TEST(SolverCeres, FloatingStateBlock_Add)
+TEST_F(SolverCeresTest, FloatingStateBlock_Add)
{
- ProblemPtr P = Problem::create("PO", 3);
- auto solver_ptr = std::make_shared<SolverCeres>(P);
-
// Create State block
- auto node_ptr = createStateBlock(); auto sb_ptr = node_ptr->getStateBlock('I');
+ auto node_ptr = createStateBlock();
+ auto sb_ptr = node_ptr->getStateBlock('I');
// notify stateblock
- P->notifyStateBlock(sb_ptr,ADD);
+ problem->notifyStateBlock(sb_ptr, ADD);
// check notifications
Notification notif;
- EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_TRUE(problem->getStateBlockNotification(sb_ptr, notif));
EXPECT_EQ(ADD, notif);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// update solver
solver_ptr->update();
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// check stateblock
EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
@@ -109,44 +156,42 @@ TEST(SolverCeres, FloatingStateBlock_Add)
EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverCeres, FloatingStateBlock_DoubleAdd)
+TEST_F(SolverCeresTest, FloatingStateBlock_DoubleAdd)
{
- ProblemPtr P = Problem::create("PO", 3);
- auto solver_ptr = std::make_shared<SolverCeres>(P);
-
// Create State block
- auto node_ptr = createStateBlock(); auto sb_ptr = node_ptr->getStateBlock('I');
+ auto node_ptr = createStateBlock();
+ auto sb_ptr = node_ptr->getStateBlock('I');
// notify stateblock
- P->notifyStateBlock(sb_ptr,ADD);
+ problem->notifyStateBlock(sb_ptr, ADD);
// check notifications
Notification notif;
- EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_TRUE(problem->getStateBlockNotification(sb_ptr, notif));
EXPECT_EQ(ADD, notif);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// update solver
solver_ptr->update();
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// notify stateblock again
- P->notifyStateBlock(sb_ptr,ADD);
+ problem->notifyStateBlock(sb_ptr, ADD);
// check notifications
- EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_TRUE(problem->getStateBlockNotification(sb_ptr, notif));
EXPECT_EQ(ADD, notif);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// update solver
solver_ptr->update();
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// check stateblock
EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
@@ -154,22 +199,20 @@ TEST(SolverCeres, FloatingStateBlock_DoubleAdd)
EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverCeres, FloatingStateBlock_AddFix)
+TEST_F(SolverCeresTest, FloatingStateBlock_AddFix)
{
- ProblemPtr P = Problem::create("PO", 3);
- auto solver_ptr = std::make_shared<SolverCeres>(P);
-
// Create State block
- auto node_ptr = createStateBlock(); auto sb_ptr = node_ptr->getStateBlock('I');
+ auto node_ptr = createStateBlock();
+ auto sb_ptr = node_ptr->getStateBlock('I');
// notify stateblock
- P->notifyStateBlock(sb_ptr,ADD);
+ problem->notifyStateBlock(sb_ptr, ADD);
// check notifications
Notification notif;
- EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_TRUE(problem->getStateBlockNotification(sb_ptr, notif));
EXPECT_EQ(ADD, notif);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// check flags
EXPECT_FALSE(sb_ptr->stateUpdated());
@@ -180,8 +223,8 @@ TEST(SolverCeres, FloatingStateBlock_AddFix)
solver_ptr->update();
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// check flags
EXPECT_FALSE(sb_ptr->stateUpdated());
@@ -197,8 +240,8 @@ TEST(SolverCeres, FloatingStateBlock_AddFix)
sb_ptr->fix();
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// check flags
EXPECT_FALSE(sb_ptr->stateUpdated());
@@ -209,8 +252,8 @@ TEST(SolverCeres, FloatingStateBlock_AddFix)
solver_ptr->update();
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// check flags
EXPECT_FALSE(sb_ptr->stateUpdated());
@@ -223,22 +266,20 @@ TEST(SolverCeres, FloatingStateBlock_AddFix)
EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverCeres, FloatingStateBlock_AddFixed)
+TEST_F(SolverCeresTest, FloatingStateBlock_AddFixed)
{
- ProblemPtr P = Problem::create("PO", 3);
- auto solver_ptr = std::make_shared<SolverCeres>(P);
-
// Create State block
- auto node_ptr = createStateBlock(); auto sb_ptr = node_ptr->getStateBlock('I');
+ auto node_ptr = createStateBlock();
+ auto sb_ptr = node_ptr->getStateBlock('I');
// notify stateblock
- P->notifyStateBlock(sb_ptr,ADD);
+ problem->notifyStateBlock(sb_ptr, ADD);
// check notifications
Notification notif;
- EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_TRUE(problem->getStateBlockNotification(sb_ptr, notif));
EXPECT_EQ(ADD, notif);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// check flags
EXPECT_FALSE(sb_ptr->stateUpdated());
@@ -257,8 +298,8 @@ TEST(SolverCeres, FloatingStateBlock_AddFixed)
solver_ptr->update();
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// check flags
EXPECT_FALSE(sb_ptr->stateUpdated());
@@ -272,22 +313,20 @@ TEST(SolverCeres, FloatingStateBlock_AddFixed)
EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverCeres, FloatingStateBlock_AddUpdateLocalParam)
+TEST_F(SolverCeresTest, FloatingStateBlock_AddUpdateLocalParam)
{
- ProblemPtr P = Problem::create("PO", 3);
- auto solver_ptr = std::make_shared<SolverCeres>(P);
-
// Create State block
- auto node_ptr = createStateBlock(); auto sb_ptr = node_ptr->getStateBlock('I');
+ auto node_ptr = createStateBlock();
+ auto sb_ptr = node_ptr->getStateBlock('I');
// notify stateblock
- P->notifyStateBlock(sb_ptr,ADD);
+ problem->notifyStateBlock(sb_ptr, ADD);
// check notifications
Notification notif;
- EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_TRUE(problem->getStateBlockNotification(sb_ptr, notif));
EXPECT_EQ(ADD, notif);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// check flags
EXPECT_FALSE(sb_ptr->stateUpdated());
@@ -308,16 +347,16 @@ TEST(SolverCeres, FloatingStateBlock_AddUpdateLocalParam)
solver_ptr->update();
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// update solver manager
solver_ptr->update();
solver_ptr->update();
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// check flags
EXPECT_FALSE(sb_ptr->stateUpdated());
@@ -326,19 +365,17 @@ TEST(SolverCeres, FloatingStateBlock_AddUpdateLocalParam)
// check stateblock fixed
EXPECT_TRUE(solver_ptr->isStateBlockFixed(sb_ptr));
- EXPECT_TRUE(solver_ptr->hasThisLocalParametrization(sb_ptr,local_ptr));
+ EXPECT_TRUE(solver_ptr->hasThisLocalParametrization(sb_ptr, local_ptr));
EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
EXPECT_TRUE(solver_ptr->isStateBlockFloating(sb_ptr));
EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverCeres, FloatingStateBlock_AddLocalParamRemoveLocalParam)
+TEST_F(SolverCeresTest, FloatingStateBlock_AddLocalParamRemoveLocalParam)
{
- ProblemPtr P = Problem::create("PO", 3);
- auto solver_ptr = std::make_shared<SolverCeres>(P);
-
// Create State block
- auto node_ptr = createStateBlock(); auto sb_ptr = node_ptr->getStateBlock('I');
+ auto node_ptr = createStateBlock();
+ auto sb_ptr = node_ptr->getStateBlock('I');
// Local param
LocalParametrizationBasePtr local_ptr = std::make_shared<LocalParametrizationQuaternionLocal>();
@@ -350,23 +387,23 @@ TEST(SolverCeres, FloatingStateBlock_AddLocalParamRemoveLocalParam)
EXPECT_TRUE(sb_ptr->localParamUpdated());
// notify stateblock
- P->notifyStateBlock(sb_ptr,ADD);
+ problem->notifyStateBlock(sb_ptr, ADD);
// check notifications
Notification notif;
- EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_TRUE(problem->getStateBlockNotification(sb_ptr, notif));
EXPECT_EQ(ADD, notif);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// update solver manager
solver_ptr->update();
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// check stateblock localparam
- EXPECT_TRUE(solver_ptr->hasThisLocalParametrization(sb_ptr,local_ptr));
+ EXPECT_TRUE(solver_ptr->hasThisLocalParametrization(sb_ptr, local_ptr));
// check flags
EXPECT_FALSE(sb_ptr->stateUpdated());
@@ -385,8 +422,8 @@ TEST(SolverCeres, FloatingStateBlock_AddLocalParamRemoveLocalParam)
solver_ptr->update();
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// check stateblock localparam
EXPECT_FALSE(solver_ptr->hasLocalParametrization(sb_ptr));
@@ -395,183 +432,173 @@ TEST(SolverCeres, FloatingStateBlock_AddLocalParamRemoveLocalParam)
EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverCeres, FloatingStateBlock_Remove)
+TEST_F(SolverCeresTest, FloatingStateBlock_Remove)
{
- ProblemPtr P = Problem::create("PO", 3);
- auto solver_ptr = std::make_shared<SolverCeres>(P);
-
// Create State block
- auto node_ptr = createStateBlock(); auto sb_ptr = node_ptr->getStateBlock('I');
+ auto node_ptr = createStateBlock();
+ auto sb_ptr = node_ptr->getStateBlock('I');
// notify stateblock
- P->notifyStateBlock(sb_ptr,ADD);
+ problem->notifyStateBlock(sb_ptr, ADD);
// check notifications
Notification notif;
- EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_TRUE(problem->getStateBlockNotification(sb_ptr, notif));
EXPECT_EQ(ADD, notif);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// update solver
solver_ptr->update();
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// remove state_block
- P->notifyStateBlock(sb_ptr,REMOVE);
+ problem->notifyStateBlock(sb_ptr, REMOVE);
// check notifications
- EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_TRUE(problem->getStateBlockNotification(sb_ptr, notif));
EXPECT_EQ(REMOVE, notif);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// update solver
solver_ptr->update();
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// check stateblock
EXPECT_FALSE(solver_ptr->isStateBlockRegistered(sb_ptr));
EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverCeres, FloatingStateBlock_AddRemove)
+TEST_F(SolverCeresTest, FloatingStateBlock_AddRemove)
{
- ProblemPtr P = Problem::create("PO", 3);
- auto solver_ptr = std::make_shared<SolverCeres>(P);
-
// Create State block
- auto node_ptr = createStateBlock(); auto sb_ptr = node_ptr->getStateBlock('I');
+ auto node_ptr = createStateBlock();
+ auto sb_ptr = node_ptr->getStateBlock('I');
// notify stateblock
- P->notifyStateBlock(sb_ptr,ADD);
+ problem->notifyStateBlock(sb_ptr, ADD);
// check notifications
Notification notif;
- EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_TRUE(problem->getStateBlockNotification(sb_ptr, notif));
EXPECT_EQ(ADD, notif);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// remove state_block
- P->notifyStateBlock(sb_ptr,REMOVE); // should cancell out the ADD
+ problem->notifyStateBlock(sb_ptr, REMOVE); // should cancell out the ADD
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// update solver
solver_ptr->update();
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// check state block
EXPECT_FALSE(solver_ptr->isStateBlockRegistered(sb_ptr));
EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverCeres, FloatingStateBlock_AddRemoveAdd)
+TEST_F(SolverCeresTest, FloatingStateBlock_AddRemoveAdd)
{
- ProblemPtr P = Problem::create("PO", 3);
- auto solver_ptr = std::make_shared<SolverCeres>(P);
-
// Create State block
- auto node_ptr = createStateBlock(); auto sb_ptr = node_ptr->getStateBlock('I');
+ auto node_ptr = createStateBlock();
+ auto sb_ptr = node_ptr->getStateBlock('I');
// notify stateblock
- P->notifyStateBlock(sb_ptr,ADD);
+ problem->notifyStateBlock(sb_ptr, ADD);
// check notifications
Notification notif;
- EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_TRUE(problem->getStateBlockNotification(sb_ptr, notif));
EXPECT_EQ(ADD, notif);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// remove state_block
- P->notifyStateBlock(sb_ptr,REMOVE); // should cancell out the ADD
+ problem->notifyStateBlock(sb_ptr, REMOVE); // should cancell out the ADD
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// notify stateblock
- P->notifyStateBlock(sb_ptr,ADD); // again ADD in notification list
+ problem->notifyStateBlock(sb_ptr, ADD); // again ADD in notification list
// check notifications
- EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_TRUE(problem->getStateBlockNotification(sb_ptr, notif));
EXPECT_EQ(ADD, notif);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// update solver
solver_ptr->update();
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// check state block
EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverCeres, FloatingStateBlock_DoubleRemove)
+TEST_F(SolverCeresTest, FloatingStateBlock_DoubleRemove)
{
- ProblemPtr P = Problem::create("PO", 3);
- auto solver_ptr = std::make_shared<SolverCeres>(P);
-
// Create State block
- auto node_ptr = createStateBlock(); auto sb_ptr = node_ptr->getStateBlock('I');
+ auto node_ptr = createStateBlock();
+ auto sb_ptr = node_ptr->getStateBlock('I');
// notify stateblock
- P->notifyStateBlock(sb_ptr,ADD);
+ problem->notifyStateBlock(sb_ptr, ADD);
// update solver
solver_ptr->update();
// remove state_block
- P->notifyStateBlock(sb_ptr,REMOVE);
+ problem->notifyStateBlock(sb_ptr, REMOVE);
// update solver
solver_ptr->update();
// remove state_block
- P->notifyStateBlock(sb_ptr,REMOVE);
+ problem->notifyStateBlock(sb_ptr, REMOVE);
// check notifications
Notification notif;
- EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_TRUE(problem->getStateBlockNotification(sb_ptr, notif));
EXPECT_EQ(REMOVE, notif);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// update solver manager
solver_ptr->update();
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// checks
EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverCeres, FloatingStateBlock_AddUpdated)
+TEST_F(SolverCeresTest, FloatingStateBlock_AddUpdated)
{
- ProblemPtr P = Problem::create("PO", 3);
- auto solver_ptr = std::make_shared<SolverCeres>(P);
-
// Create State block
- auto node_ptr = createStateBlock(); auto sb_ptr = node_ptr->getStateBlock('I');
+ auto node_ptr = createStateBlock();
+ auto sb_ptr = node_ptr->getStateBlock('I');
// Fix
sb_ptr->fix();
// Set State
- sb_ptr->setState(2*sb_ptr->getState());
+ sb_ptr->setState(2 * sb_ptr->getState());
// Check flags have been set true
EXPECT_TRUE(sb_ptr->fixUpdated());
@@ -580,30 +607,31 @@ TEST(SolverCeres, FloatingStateBlock_AddUpdated)
// == When an ADD is notified: a ADD notification should be stored ==
// notify state block
- P->notifyStateBlock(sb_ptr,ADD);
+ problem->notifyStateBlock(sb_ptr, ADD);
// check notifications
// check notifications
Notification notif;
- EXPECT_EQ(P->getStateBlockNotificationMapSize(),1);
- EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 1);
+ EXPECT_TRUE(problem->getStateBlockNotification(sb_ptr, notif));
EXPECT_EQ(notif, ADD);
// == Insert OTHER notifications ==
// Set State --> FLAG
- sb_ptr->setState(2*sb_ptr->getState());
+ sb_ptr->setState(2 * sb_ptr->getState());
// Fix --> FLAG
sb_ptr->unfix();
// Check flag has been set true
EXPECT_TRUE(sb_ptr->fixUpdated());
- EXPECT_EQ(P->getStateBlockNotificationMapSize(),1); // No new notifications (fix and set state are flags in sb)
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(),
+ 1); // No new notifications (fix and set state are flags in sb)
// == consume empties the notification map ==
- solver_ptr->update(); // it calls P->consumeStateBlockNotificationMap();
- EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ solver_ptr->update(); // it calls problem->consumeStateBlockNotificationMap();
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
// Check flags have been reset
EXPECT_FALSE(sb_ptr->fixUpdated());
EXPECT_FALSE(sb_ptr->stateUpdated());
@@ -611,55 +639,54 @@ TEST(SolverCeres, FloatingStateBlock_AddUpdated)
// == When an REMOVE is notified: a REMOVE notification should be stored ==
// remove state_block
- P->notifyStateBlock(sb_ptr,REMOVE);
+ problem->notifyStateBlock(sb_ptr, REMOVE);
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(),1);
- EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 1);
+ EXPECT_TRUE(problem->getStateBlockNotification(sb_ptr, notif));
EXPECT_EQ(notif, REMOVE);
// == REMOVE + ADD: notification map should be empty ==
- P->notifyStateBlock(sb_ptr,ADD);
- EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ problem->notifyStateBlock(sb_ptr, ADD);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
// == UPDATES should clear the list of notifications ==
// notify state block
- P->notifyStateBlock(sb_ptr,ADD);
+ problem->notifyStateBlock(sb_ptr, ADD);
// update solver
solver_ptr->update();
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(),0); // After solver_manager->update, notifications should be empty
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(),
+ 0); // After solver_manager->update, notifications should be empty
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// == ADD + REMOVE: notification map should be empty ==
- P->notifyStateBlock(sb_ptr,ADD);
- P->notifyStateBlock(sb_ptr,REMOVE);
- EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ problem->notifyStateBlock(sb_ptr, ADD);
+ problem->notifyStateBlock(sb_ptr, REMOVE);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
}
////////////////////////////////////////////////////////
// STATE BLOCKS AND FACTOR
-TEST(SolverCeres, StateBlockAndFactor_Add)
+TEST_F(SolverCeresTest, StateBlockAndFactor_Add)
{
- ProblemPtr P = Problem::create("PO", 3);
- auto solver_ptr = std::make_shared<SolverCeres>(P);
-
// Create State block
- auto node_ptr = createStateBlock(); auto sb_ptr = node_ptr->getStateBlock('I');
+ auto node_ptr = createStateBlock();
+ auto sb_ptr = node_ptr->getStateBlock('I');
// Create Factor
auto fac_ptr = createFactor(node_ptr);
// notify stateblock (floating for the moment)
- P->notifyStateBlock(sb_ptr,ADD);
+ problem->notifyStateBlock(sb_ptr, ADD);
// check notifications
Notification notif;
- EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_TRUE(problem->getStateBlockNotification(sb_ptr, notif));
EXPECT_EQ(ADD, notif);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// update solver
solver_ptr->update();
@@ -669,19 +696,19 @@ TEST(SolverCeres, StateBlockAndFactor_Add)
EXPECT_TRUE(solver_ptr->isStateBlockFloating(sb_ptr));
// notify factor (state block now not floating)
- P->notifyFactor(fac_ptr,ADD);
+ problem->notifyFactor(fac_ptr, ADD);
// check notifications
- EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_TRUE(problem->getFactorNotification(fac_ptr, notif));
EXPECT_EQ(ADD, notif);
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
// update solver
solver_ptr->update();
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// checks
EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
@@ -690,28 +717,26 @@ TEST(SolverCeres, StateBlockAndFactor_Add)
EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverCeres, StateBlockAndFactor_DoubleAdd)
+TEST_F(SolverCeresTest, StateBlockAndFactor_DoubleAdd)
{
- ProblemPtr P = Problem::create("PO", 3);
- auto solver_ptr = std::make_shared<SolverCeres>(P);
-
// Create State block
- auto node_ptr = createStateBlock(); auto sb_ptr = node_ptr->getStateBlock('I');
+ auto node_ptr = createStateBlock();
+ auto sb_ptr = node_ptr->getStateBlock('I');
// Create Factor
auto fac_ptr = createFactor(node_ptr);
// notify stateblock
- P->notifyStateBlock(sb_ptr,ADD);
+ problem->notifyStateBlock(sb_ptr, ADD);
// notify factor
- P->notifyFactor(fac_ptr,ADD);
+ problem->notifyFactor(fac_ptr, ADD);
// check notifications
Notification notif;
- EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_TRUE(problem->getStateBlockNotification(sb_ptr, notif));
EXPECT_EQ(ADD, notif);
- EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_TRUE(problem->getFactorNotification(fac_ptr, notif));
EXPECT_EQ(ADD, notif);
// update solver
@@ -719,27 +744,27 @@ TEST(SolverCeres, StateBlockAndFactor_DoubleAdd)
solver_ptr->update();
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// notify stateblock again
- P->notifyStateBlock(sb_ptr,ADD);
+ problem->notifyStateBlock(sb_ptr, ADD);
// notify factor
- P->notifyFactor(fac_ptr,ADD);
+ problem->notifyFactor(fac_ptr, ADD);
// check notifications
- EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_TRUE(problem->getStateBlockNotification(sb_ptr, notif));
EXPECT_EQ(ADD, notif);
- EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_TRUE(problem->getFactorNotification(fac_ptr, notif));
EXPECT_EQ(ADD, notif);
// update solver
solver_ptr->update();
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// check stateblock
EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
@@ -748,28 +773,26 @@ TEST(SolverCeres, StateBlockAndFactor_DoubleAdd)
EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverCeres, StateBlockAndFactor_Fix)
+TEST_F(SolverCeresTest, StateBlockAndFactor_Fix)
{
- ProblemPtr P = Problem::create("PO", 3);
- auto solver_ptr = std::make_shared<SolverCeres>(P);
-
// Create State block
- auto node_ptr = createStateBlock(); auto sb_ptr = node_ptr->getStateBlock('I');
+ auto node_ptr = createStateBlock();
+ auto sb_ptr = node_ptr->getStateBlock('I');
// Create Factor
auto fac_ptr = createFactor(node_ptr);
// notify stateblock
- P->notifyStateBlock(sb_ptr,ADD);
+ problem->notifyStateBlock(sb_ptr, ADD);
// notify factor
- P->notifyFactor(fac_ptr,ADD);
+ problem->notifyFactor(fac_ptr, ADD);
// check notifications
Notification notif;
- EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_TRUE(problem->getStateBlockNotification(sb_ptr, notif));
EXPECT_EQ(ADD, notif);
- EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_TRUE(problem->getFactorNotification(fac_ptr, notif));
EXPECT_EQ(ADD, notif);
// check flags
@@ -781,8 +804,8 @@ TEST(SolverCeres, StateBlockAndFactor_Fix)
solver_ptr->update();
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// check flags
EXPECT_FALSE(sb_ptr->stateUpdated());
@@ -802,15 +825,15 @@ TEST(SolverCeres, StateBlockAndFactor_Fix)
EXPECT_FALSE(sb_ptr->localParamUpdated());
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// update solver manager
solver_ptr->update();
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// check flags
EXPECT_FALSE(sb_ptr->stateUpdated());
@@ -822,28 +845,26 @@ TEST(SolverCeres, StateBlockAndFactor_Fix)
EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverCeres, StateBlockAndFactor_AddFixed)
+TEST_F(SolverCeresTest, StateBlockAndFactor_AddFixed)
{
- ProblemPtr P = Problem::create("PO", 3);
- auto solver_ptr = std::make_shared<SolverCeres>(P);
-
// Create State block
- auto node_ptr = createStateBlock(); auto sb_ptr = node_ptr->getStateBlock('I');
+ auto node_ptr = createStateBlock();
+ auto sb_ptr = node_ptr->getStateBlock('I');
// Create Factor
auto fac_ptr = createFactor(node_ptr);
// notify stateblock
- P->notifyStateBlock(sb_ptr,ADD);
+ problem->notifyStateBlock(sb_ptr, ADD);
// notify factor
- P->notifyFactor(fac_ptr,ADD);
+ problem->notifyFactor(fac_ptr, ADD);
// check notifications
Notification notif;
- EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_TRUE(problem->getStateBlockNotification(sb_ptr, notif));
EXPECT_EQ(ADD, notif);
- EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_TRUE(problem->getFactorNotification(fac_ptr, notif));
EXPECT_EQ(ADD, notif);
// check flags
@@ -863,8 +884,8 @@ TEST(SolverCeres, StateBlockAndFactor_AddFixed)
solver_ptr->update();
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// check flags
EXPECT_FALSE(sb_ptr->stateUpdated());
@@ -877,28 +898,26 @@ TEST(SolverCeres, StateBlockAndFactor_AddFixed)
EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverCeres, StateBlockAndFactor_UpdateLocalParam)
+TEST_F(SolverCeresTest, StateBlockAndFactor_UpdateLocalParam)
{
- ProblemPtr P = Problem::create("PO", 3);
- auto solver_ptr = std::make_shared<SolverCeres>(P);
-
// Create State block
- auto node_ptr = createStateBlock(); auto sb_ptr = node_ptr->getStateBlock('I');
+ auto node_ptr = createStateBlock();
+ auto sb_ptr = node_ptr->getStateBlock('I');
// Create Factor
auto fac_ptr = createFactor(node_ptr);
// notify stateblock
- P->notifyStateBlock(sb_ptr,ADD);
+ problem->notifyStateBlock(sb_ptr, ADD);
// notify factor
- P->notifyFactor(fac_ptr,ADD);
+ problem->notifyFactor(fac_ptr, ADD);
// check notifications
Notification notif;
- EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_TRUE(problem->getStateBlockNotification(sb_ptr, notif));
EXPECT_EQ(ADD, notif);
- EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_TRUE(problem->getFactorNotification(fac_ptr, notif));
EXPECT_EQ(ADD, notif);
// check flags
@@ -922,8 +941,8 @@ TEST(SolverCeres, StateBlockAndFactor_UpdateLocalParam)
solver_ptr->update();
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// check flags
EXPECT_FALSE(sb_ptr->stateUpdated());
@@ -933,17 +952,15 @@ TEST(SolverCeres, StateBlockAndFactor_UpdateLocalParam)
// check stateblock fixed
EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
EXPECT_TRUE(solver_ptr->isStateBlockFixed(sb_ptr));
- EXPECT_TRUE(solver_ptr->hasThisLocalParametrization(sb_ptr,local_ptr));
+ EXPECT_TRUE(solver_ptr->hasThisLocalParametrization(sb_ptr, local_ptr));
EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverCeres, StateBlockAndFactor_AddLocalParamRemoveLocalParam)
+TEST_F(SolverCeresTest, StateBlockAndFactor_AddLocalParamRemoveLocalParam)
{
- ProblemPtr P = Problem::create("PO", 3);
- auto solver_ptr = std::make_shared<SolverCeres>(P);
-
// Create State block
- auto node_ptr = createStateBlock(); auto sb_ptr = node_ptr->getStateBlock('I');
+ auto node_ptr = createStateBlock();
+ auto sb_ptr = node_ptr->getStateBlock('I');
// Create Factor
auto fac_ptr = createFactor(node_ptr);
@@ -958,29 +975,29 @@ TEST(SolverCeres, StateBlockAndFactor_AddLocalParamRemoveLocalParam)
EXPECT_TRUE(sb_ptr->localParamUpdated());
// notify stateblock
- P->notifyStateBlock(sb_ptr,ADD);
+ problem->notifyStateBlock(sb_ptr, ADD);
// notify factor
- P->notifyFactor(fac_ptr,ADD);
+ problem->notifyFactor(fac_ptr, ADD);
// check notifications
Notification notif;
- EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_TRUE(problem->getStateBlockNotification(sb_ptr, notif));
EXPECT_EQ(ADD, notif);
- EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_TRUE(problem->getFactorNotification(fac_ptr, notif));
EXPECT_EQ(ADD, notif);
// update solver manager
solver_ptr->update();
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// check solver
EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
EXPECT_TRUE(solver_ptr->hasLocalParametrization(sb_ptr));
- EXPECT_TRUE(solver_ptr->hasThisLocalParametrization(sb_ptr,local_ptr));
+ EXPECT_TRUE(solver_ptr->hasThisLocalParametrization(sb_ptr, local_ptr));
EXPECT_TRUE(solver_ptr->check());
// check flags
@@ -1005,52 +1022,50 @@ TEST(SolverCeres, StateBlockAndFactor_AddLocalParamRemoveLocalParam)
EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverCeres, StateBlockAndFactor_RemoveStateBlock)
+TEST_F(SolverCeresTest, StateBlockAndFactor_RemoveStateBlock)
{
- ProblemPtr P = Problem::create("PO", 3);
- auto solver_ptr = std::make_shared<SolverCeres>(P);
-
// Create State block
- auto node_ptr = createStateBlock(); auto sb_ptr = node_ptr->getStateBlock('I');
+ auto node_ptr = createStateBlock();
+ auto sb_ptr = node_ptr->getStateBlock('I');
// Create Factor
auto fac_ptr = createFactor(node_ptr);
// notify stateblock
- P->notifyStateBlock(sb_ptr,ADD);
+ problem->notifyStateBlock(sb_ptr, ADD);
// notify factor
- P->notifyFactor(fac_ptr,ADD);
+ problem->notifyFactor(fac_ptr, ADD);
// check notifications
Notification notif;
- EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_TRUE(problem->getStateBlockNotification(sb_ptr, notif));
EXPECT_EQ(ADD, notif);
- EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_TRUE(problem->getFactorNotification(fac_ptr, notif));
EXPECT_EQ(ADD, notif);
// update solver
solver_ptr->update();
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// remove state_block
- P->notifyStateBlock(sb_ptr,REMOVE);
+ problem->notifyStateBlock(sb_ptr, REMOVE);
// check notifications
- EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_TRUE(problem->getStateBlockNotification(sb_ptr, notif));
EXPECT_EQ(REMOVE, notif);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// update solver
- solver_ptr->update(); // there is a factor involved, removal posponed (REMOVE in notification list)
+ solver_ptr->update(); // there is a factor involved, removal posponed (REMOVE in notification list)
// check notifications
- EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_TRUE(problem->getStateBlockNotification(sb_ptr, notif));
EXPECT_EQ(REMOVE, notif);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// checks
EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
@@ -1058,51 +1073,49 @@ TEST(SolverCeres, StateBlockAndFactor_RemoveStateBlock)
EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverCeres, StateBlockAndFactor_RemoveFactor)
+TEST_F(SolverCeresTest, StateBlockAndFactor_RemoveFactor)
{
- ProblemPtr P = Problem::create("PO", 3);
- auto solver_ptr = std::make_shared<SolverCeres>(P);
-
// Create State block
- auto node_ptr = createStateBlock(); auto sb_ptr = node_ptr->getStateBlock('I');
+ auto node_ptr = createStateBlock();
+ auto sb_ptr = node_ptr->getStateBlock('I');
// Create Factor
auto fac_ptr = createFactor(node_ptr);
// notify stateblock
- P->notifyStateBlock(sb_ptr,ADD);
+ problem->notifyStateBlock(sb_ptr, ADD);
// notify factor
- P->notifyFactor(fac_ptr,ADD);
+ problem->notifyFactor(fac_ptr, ADD);
// check notifications
Notification notif;
- EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_TRUE(problem->getStateBlockNotification(sb_ptr, notif));
EXPECT_EQ(ADD, notif);
- EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_TRUE(problem->getFactorNotification(fac_ptr, notif));
EXPECT_EQ(ADD, notif);
// update solver
solver_ptr->update();
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// remove state_block
- P->notifyFactor(fac_ptr,REMOVE); // state block should be automatically stored as floating
+ problem->notifyFactor(fac_ptr, REMOVE); // state block should be automatically stored as floating
// check notifications
- EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_TRUE(problem->getFactorNotification(fac_ptr, notif));
EXPECT_EQ(REMOVE, notif);
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
// update solver
solver_ptr->update();
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// checks
EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
@@ -1111,44 +1124,43 @@ TEST(SolverCeres, StateBlockAndFactor_RemoveFactor)
EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverCeres, StateBlockAndFactor_AddRemoveStateBlock)
+TEST_F(SolverCeresTest, StateBlockAndFactor_AddRemoveStateBlock)
{
- ProblemPtr P = Problem::create("PO", 3);
- auto solver_ptr = std::make_shared<SolverCeres>(P);
-
// Create State block
- auto node_ptr = createStateBlock(); auto sb_ptr = node_ptr->getStateBlock('I');
+ auto node_ptr = createStateBlock();
+ auto sb_ptr = node_ptr->getStateBlock('I');
// Create Factor
auto fac_ptr = createFactor(node_ptr);
// notify stateblock
- P->notifyStateBlock(sb_ptr,ADD);
+ problem->notifyStateBlock(sb_ptr, ADD);
// notify factor
- P->notifyFactor(fac_ptr,ADD);
+ problem->notifyFactor(fac_ptr, ADD);
// check notifications
Notification notif;
- EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_TRUE(problem->getStateBlockNotification(sb_ptr, notif));
EXPECT_EQ(ADD, notif);
- EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_TRUE(problem->getFactorNotification(fac_ptr, notif));
EXPECT_EQ(ADD, notif);
// remove state_block
- P->notifyStateBlock(sb_ptr,REMOVE); // cancells out ADD notification
+ problem->notifyStateBlock(sb_ptr, REMOVE); // cancells out ADD notification
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
- EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_TRUE(problem->getFactorNotification(fac_ptr, notif));
EXPECT_EQ(ADD, notif);
// update solver
- solver_ptr->update(); // factor ADD notification is not executed since state block involved is missing (ADD notification added)
+ solver_ptr->update(); // factor ADD notification is not executed since state block involved is missing (ADD
+ // notification added)
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
- EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_TRUE(problem->getFactorNotification(fac_ptr, notif));
EXPECT_EQ(ADD, notif);
// check state block
@@ -1157,44 +1169,42 @@ TEST(SolverCeres, StateBlockAndFactor_AddRemoveStateBlock)
EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverCeres, StateBlockAndFactor_AddRemoveFactor)
+TEST_F(SolverCeresTest, StateBlockAndFactor_AddRemoveFactor)
{
- ProblemPtr P = Problem::create("PO", 3);
- auto solver_ptr = std::make_shared<SolverCeres>(P);
-
// Create State block
- auto node_ptr = createStateBlock(); auto sb_ptr = node_ptr->getStateBlock('I');
+ auto node_ptr = createStateBlock();
+ auto sb_ptr = node_ptr->getStateBlock('I');
// Create Factor
auto fac_ptr = createFactor(node_ptr);
// notify stateblock
- P->notifyStateBlock(sb_ptr,ADD);
+ problem->notifyStateBlock(sb_ptr, ADD);
// notify factor
- P->notifyFactor(fac_ptr,ADD);
+ problem->notifyFactor(fac_ptr, ADD);
// check notifications
Notification notif;
- EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_TRUE(problem->getStateBlockNotification(sb_ptr, notif));
EXPECT_EQ(ADD, notif);
- EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_TRUE(problem->getFactorNotification(fac_ptr, notif));
EXPECT_EQ(ADD, notif);
// remove state_block
- P->notifyFactor(fac_ptr,REMOVE); // cancells out ADD notification
+ problem->notifyFactor(fac_ptr, REMOVE); // cancells out ADD notification
// check notifications
- EXPECT_EQ(P->getFactorNotificationMapSize(),0);
- EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
+ EXPECT_TRUE(problem->getStateBlockNotification(sb_ptr, notif));
EXPECT_EQ(ADD, notif);
// update solver
- solver_ptr->update(); // state block should be automatically stored as floating
+ solver_ptr->update(); // state block should be automatically stored as floating
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// checks
EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
@@ -1203,61 +1213,59 @@ TEST(SolverCeres, StateBlockAndFactor_AddRemoveFactor)
EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverCeres, StateBlockAndFactor_DoubleRemoveStateBlock)
+TEST_F(SolverCeresTest, StateBlockAndFactor_DoubleRemoveStateBlock)
{
- ProblemPtr P = Problem::create("PO", 3);
- auto solver_ptr = std::make_shared<SolverCeres>(P);
-
// Create State block
- auto node_ptr = createStateBlock(); auto sb_ptr = node_ptr->getStateBlock('I');
+ auto node_ptr = createStateBlock();
+ auto sb_ptr = node_ptr->getStateBlock('I');
// Create Factor
auto fac_ptr = createFactor(node_ptr);
// notify stateblock
- P->notifyStateBlock(sb_ptr,ADD);
+ problem->notifyStateBlock(sb_ptr, ADD);
// notify factor
- P->notifyFactor(fac_ptr,ADD);
+ problem->notifyFactor(fac_ptr, ADD);
// check notifications
Notification notif;
- EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_TRUE(problem->getStateBlockNotification(sb_ptr, notif));
EXPECT_EQ(ADD, notif);
- EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_TRUE(problem->getFactorNotification(fac_ptr, notif));
EXPECT_EQ(ADD, notif);
// remove state_block
- P->notifyStateBlock(sb_ptr,REMOVE); // cancells out the ADD
+ problem->notifyStateBlock(sb_ptr, REMOVE); // cancells out the ADD
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
- EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_TRUE(problem->getFactorNotification(fac_ptr, notif));
EXPECT_EQ(ADD, notif);
// update solver
- solver_ptr->update(); // factor ADD is posponed
+ solver_ptr->update(); // factor ADD is posponed
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
- EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_TRUE(problem->getFactorNotification(fac_ptr, notif));
EXPECT_EQ(ADD, notif);
// remove state_block
- P->notifyStateBlock(sb_ptr,REMOVE);
+ problem->notifyStateBlock(sb_ptr, REMOVE);
// check notifications
- EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_TRUE(problem->getStateBlockNotification(sb_ptr, notif));
EXPECT_EQ(REMOVE, notif);
- EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_TRUE(problem->getFactorNotification(fac_ptr, notif));
EXPECT_EQ(ADD, notif);
// update solver manager
- solver_ptr->update(); // repeated REMOVE should be ignored
+ solver_ptr->update(); // repeated REMOVE should be ignored
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
- EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_TRUE(problem->getFactorNotification(fac_ptr, notif));
EXPECT_EQ(ADD, notif);
// check state block
@@ -1266,55 +1274,53 @@ TEST(SolverCeres, StateBlockAndFactor_DoubleRemoveStateBlock)
EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverCeres, StateBlockAndFactor_DoubleRemoveFactor)
+TEST_F(SolverCeresTest, StateBlockAndFactor_DoubleRemoveFactor)
{
- ProblemPtr P = Problem::create("PO", 3);
- auto solver_ptr = std::make_shared<SolverCeres>(P);
-
// Create State block
- auto node_ptr = createStateBlock(); auto sb_ptr = node_ptr->getStateBlock('I');
+ auto node_ptr = createStateBlock();
+ auto sb_ptr = node_ptr->getStateBlock('I');
// Create Factor
auto fac_ptr = createFactor(node_ptr);
// notify stateblock
- P->notifyStateBlock(sb_ptr,ADD);
+ problem->notifyStateBlock(sb_ptr, ADD);
// notify factor
- P->notifyFactor(fac_ptr,ADD);
+ problem->notifyFactor(fac_ptr, ADD);
// check notifications
Notification notif;
- EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_TRUE(problem->getStateBlockNotification(sb_ptr, notif));
EXPECT_EQ(ADD, notif);
- EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_TRUE(problem->getFactorNotification(fac_ptr, notif));
EXPECT_EQ(ADD, notif);
// remove state_block
- P->notifyFactor(fac_ptr,REMOVE); // cancells out ADD
+ problem->notifyFactor(fac_ptr, REMOVE); // cancells out ADD
// check notifications
- EXPECT_EQ(P->getFactorNotificationMapSize(),0);
- EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
+ EXPECT_TRUE(problem->getStateBlockNotification(sb_ptr, notif));
EXPECT_EQ(ADD, notif);
// update solver
- solver_ptr->update(); // state block should be automatically stored as floating
+ solver_ptr->update(); // state block should be automatically stored as floating
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
- EXPECT_EQ(P->getFactorNotificationMapSize(),0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// remove state_block
- P->notifyFactor(fac_ptr,REMOVE);
+ problem->notifyFactor(fac_ptr, REMOVE);
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
- EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_TRUE(problem->getFactorNotification(fac_ptr, notif));
EXPECT_EQ(REMOVE, notif);
// update solver
- solver_ptr->update(); // repeated REMOVE should be ignored
+ solver_ptr->update(); // repeated REMOVE should be ignored
// checks
EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
@@ -1323,13 +1329,11 @@ TEST(SolverCeres, StateBlockAndFactor_DoubleRemoveFactor)
EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverCeres, StateBlockAndFactor_AddUpdatedStateBlock)
+TEST_F(SolverCeresTest, StateBlockAndFactor_AddUpdatedStateBlock)
{
- ProblemPtr P = Problem::create("PO", 3);
- auto solver_ptr = std::make_shared<SolverCeres>(P);
-
// Create State block
- auto node_ptr = createStateBlock(); auto sb_ptr = node_ptr->getStateBlock('I');
+ auto node_ptr = createStateBlock();
+ auto sb_ptr = node_ptr->getStateBlock('I');
// Create Factor
auto fac_ptr = createFactor(node_ptr);
@@ -1338,7 +1342,7 @@ TEST(SolverCeres, StateBlockAndFactor_AddUpdatedStateBlock)
sb_ptr->fix();
// Change State
- sb_ptr->setState(2*sb_ptr->getState());
+ sb_ptr->setState(2 * sb_ptr->getState());
// Check flags have been set true
EXPECT_TRUE(sb_ptr->fixUpdated());
@@ -1347,33 +1351,34 @@ TEST(SolverCeres, StateBlockAndFactor_AddUpdatedStateBlock)
// == When an ADD is notified: a ADD notification should be stored ==
// notify state block
- P->notifyStateBlock(sb_ptr,ADD);
+ problem->notifyStateBlock(sb_ptr, ADD);
// notify factor
- P->notifyFactor(fac_ptr,ADD);
+ problem->notifyFactor(fac_ptr, ADD);
// check notifications
Notification notif;
- EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_TRUE(problem->getStateBlockNotification(sb_ptr, notif));
EXPECT_EQ(ADD, notif);
- EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_TRUE(problem->getFactorNotification(fac_ptr, notif));
EXPECT_EQ(ADD, notif);
// == Insert OTHER notifications ==
// Set State --> FLAG
- sb_ptr->setState(2*sb_ptr->getState());
+ sb_ptr->setState(2 * sb_ptr->getState());
// Fix --> FLAG
sb_ptr->unfix();
// Check flag has been set true
EXPECT_TRUE(sb_ptr->fixUpdated());
- EXPECT_EQ(P->getStateBlockNotificationMapSize(),1); // No new notifications (fix and set state are flags in sb)
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(),
+ 1); // No new notifications (fix and set state are flags in sb)
// == consume empties the notification map ==
- solver_ptr->update(); // it calls P->consumeStateBlockNotificationMap();
- EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ solver_ptr->update(); // it calls problem->consumeStateBlockNotificationMap();
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
// Check flags have been reset
EXPECT_FALSE(sb_ptr->fixUpdated());
EXPECT_FALSE(sb_ptr->stateUpdated());
@@ -1381,59 +1386,58 @@ TEST(SolverCeres, StateBlockAndFactor_AddUpdatedStateBlock)
// == When an REMOVE is notified: a REMOVE notification should be stored ==
// remove state_block
- P->notifyStateBlock(sb_ptr,REMOVE);
+ problem->notifyStateBlock(sb_ptr, REMOVE);
- EXPECT_EQ(P->getStateBlockNotificationMapSize(),1);
- EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 1);
+ EXPECT_TRUE(problem->getStateBlockNotification(sb_ptr, notif));
EXPECT_EQ(notif, REMOVE);
// == REMOVE + ADD: notification map should be empty ==
- P->notifyStateBlock(sb_ptr,ADD);
- EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ problem->notifyStateBlock(sb_ptr, ADD);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
// == UPDATES should clear the list of notifications ==
// notify state block
- P->notifyStateBlock(sb_ptr,ADD);
+ problem->notifyStateBlock(sb_ptr, ADD);
// update solver
solver_ptr->update();
- EXPECT_EQ(P->getStateBlockNotificationMapSize(),0); // After solver_manager->update, notifications should be empty
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(),
+ 0); // After solver_manager->update, notifications should be empty
// == ADD + REMOVE: notification map should be empty ==
- P->notifyStateBlock(sb_ptr,ADD);
- P->notifyStateBlock(sb_ptr,REMOVE);
- EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ problem->notifyStateBlock(sb_ptr, ADD);
+ problem->notifyStateBlock(sb_ptr, REMOVE);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
}
////////////////////////////////////////////////////////
// ONLY FACTOR
-TEST(SolverCeres, OnlyFactor_Add)
+TEST_F(SolverCeresTest, OnlyFactor_Add)
{
- ProblemPtr P = Problem::create("PO", 3);
- auto solver_ptr = std::make_shared<SolverCeres>(P);
-
// Create State block
- auto node_ptr = createStateBlock(); auto sb_ptr = node_ptr->getStateBlock('I');
+ auto node_ptr = createStateBlock();
+ auto sb_ptr = node_ptr->getStateBlock('I');
// Create Factor
auto fac_ptr = createFactor(node_ptr);
// notify factor (state block has not been notified)
- P->notifyFactor(fac_ptr,ADD);
+ problem->notifyFactor(fac_ptr, ADD);
// check notifications
Notification notif;
- EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
- EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_TRUE(problem->getFactorNotification(fac_ptr, notif));
EXPECT_EQ(ADD, notif);
// update solver
- solver_ptr->update(); // factor ADD should be posponed (in the notification list again)
+ solver_ptr->update(); // factor ADD should be posponed (in the notification list again)
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
- EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_TRUE(problem->getFactorNotification(fac_ptr, notif));
EXPECT_EQ(ADD, notif);
// checks
@@ -1442,47 +1446,45 @@ TEST(SolverCeres, OnlyFactor_Add)
EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverCeres, OnlyFactor_Remove)
+TEST_F(SolverCeresTest, OnlyFactor_Remove)
{
- ProblemPtr P = Problem::create("PO", 3);
- auto solver_ptr = std::make_shared<SolverCeres>(P);
-
// Create State block
- auto node_ptr = createStateBlock(); auto sb_ptr = node_ptr->getStateBlock('I');
+ auto node_ptr = createStateBlock();
+ auto sb_ptr = node_ptr->getStateBlock('I');
// Create Factor
auto fac_ptr = createFactor(node_ptr);
// notify factor (state block has not been notified)
- P->notifyFactor(fac_ptr,ADD);
+ problem->notifyFactor(fac_ptr, ADD);
// check notifications
Notification notif;
- EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
- EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_TRUE(problem->getFactorNotification(fac_ptr, notif));
EXPECT_EQ(ADD, notif);
// update solver
- solver_ptr->update(); // ADD factor should be posponed (in the notification list again)
+ solver_ptr->update(); // ADD factor should be posponed (in the notification list again)
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
- EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_TRUE(problem->getFactorNotification(fac_ptr, notif));
EXPECT_EQ(ADD, notif);
// notify factor
- P->notifyFactor(fac_ptr,REMOVE);
+ problem->notifyFactor(fac_ptr, REMOVE);
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
- EXPECT_EQ(P->getFactorNotificationMapSize(),0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// update solver
- solver_ptr->update(); // nothing to update
+ solver_ptr->update(); // nothing to update
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
- EXPECT_EQ(P->getFactorNotificationMapSize(),0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// checks
EXPECT_FALSE(solver_ptr->isStateBlockRegistered(sb_ptr));
@@ -1490,39 +1492,37 @@ TEST(SolverCeres, OnlyFactor_Remove)
EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverCeres, OnlyFactor_AddRemove)
+TEST_F(SolverCeresTest, OnlyFactor_AddRemove)
{
- ProblemPtr P = Problem::create("PO", 3);
- auto solver_ptr = std::make_shared<SolverCeres>(P);
-
// Create State block
- auto node_ptr = createStateBlock(); auto sb_ptr = node_ptr->getStateBlock('I');
+ auto node_ptr = createStateBlock();
+ auto sb_ptr = node_ptr->getStateBlock('I');
// Create Factor
auto fac_ptr = createFactor(node_ptr);
// notify factor (state block has not been notified)
- P->notifyFactor(fac_ptr,ADD);
+ problem->notifyFactor(fac_ptr, ADD);
// check notifications
Notification notif;
- EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
- EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_TRUE(problem->getFactorNotification(fac_ptr, notif));
EXPECT_EQ(ADD, notif);
// notify factor
- P->notifyFactor(fac_ptr,REMOVE);
+ problem->notifyFactor(fac_ptr, REMOVE);
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
- EXPECT_EQ(P->getFactorNotificationMapSize(),0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// update solver
- solver_ptr->update(); // nothing to update
+ solver_ptr->update(); // nothing to update
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
- EXPECT_EQ(P->getFactorNotificationMapSize(),0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// checks
EXPECT_FALSE(solver_ptr->isStateBlockRegistered(sb_ptr));
@@ -1532,7 +1532,6 @@ TEST(SolverCeres, OnlyFactor_AddRemove)
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_solver_ceres_multithread.cpp b/test/gtest_solver_ceres_multithread.cpp
index 8d670a40a8656f5413db56d33f26c9d782670c3a..360548f06683ee71cf3f5b15250c55726beed12a 100644
--- a/test/gtest_solver_ceres_multithread.cpp
+++ b/test/gtest_solver_ceres_multithread.cpp
@@ -46,11 +46,14 @@ using namespace Eigen;
* (modifications should be applied to both tests)
*/
+std::string wolf_dir = _WOLF_CODE_DIR;
+
TEST(SolverCeresMultithread, MultiThreadingTruncatedNotifications)
{
- double Dt = 5.0;
- ProblemPtr P = Problem::create("PO", 2);
- auto solver_ptr = std::make_shared<SolverCeres>(P);
+ double Dt = 5.0;
+ ProblemPtr P = Problem::create("PO", 2);
+ auto solver_ptr =
+ FactorySolverFile::create("SolverCeres", P, wolf_dir + "/test/yaml/solver_ceres.yaml", {wolf_dir});
// loop updating (without sleep)
std::thread t([&]() {
diff --git a/test/gtest_solver_manager.cpp b/test/gtest_solver_manager.cpp
index 47721f1179d4dd5b04ee70928266eb7dc3679510..55d4ae764c2bbb687bd2ccf19ce2a5bada6eae71 100644
--- a/test/gtest_solver_manager.cpp
+++ b/test/gtest_solver_manager.cpp
@@ -17,12 +17,6 @@
//
// 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/>.
-/*
- * gtest_solver_manager.cpp
- *
- * Created on: Jun, 2018
- * Author: jvallve
- */
#include "core/utils/utils_gtest.h"
@@ -34,7 +28,7 @@
#include "core/factor/factor_block_absolute.h"
#include "core/state_block/local_parametrization_quaternion.h"
#include "dummy/node_state_blocks_dummy.h"
-#include "dummy/solver_manager_dummy.h"
+#include "dummy/solver_dummy.h"
#include "core/state_block/state_block_derived.h"
#include "core/state_block/state_angle.h"
@@ -49,6 +43,24 @@ using namespace Eigen;
* (modifications should be applied to both tests)
*/
+std::string wolf_dir = _WOLF_CODE_DIR;
+
+class SolverManagerTest : public testing::Test
+{
+ public:
+ ProblemPtr problem;
+ SolverManagerPtr solver_ptr;
+
+ std::string wolf_dir = _WOLF_CODE_DIR;
+
+ void SetUp() override
+ {
+ problem = Problem::create("PO", 3);
+ solver_ptr =
+ FactorySolverFile::create("SolverDummy", problem, wolf_dir + "/test/yaml/solver_dummy.yaml", {wolf_dir});
+ }
+};
+
NodeStateBlocksPtr createStateBlock()
{
auto node = std::make_shared<NodeStateBlocksDummy>();
@@ -68,13 +80,30 @@ FactorBasePtr createFactor(NodeStateBlocksPtr node_ptr)
false);
}
-TEST(SolverManager, Create)
+TEST(SolverManagerFactories, FactoryNode)
{
- ProblemPtr P = Problem::create("PO", 3);
- auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
+ auto problem = Problem::create("PO", 3);
+ YAML::Node params;
+ params["period"] = 0;
+ params["verbose"] = 0;
+ params["compute_cov"] = false;
+ auto solver_ptr = FactorySolverNode::create("SolverDummy", problem, params, {wolf_dir});
// check pointers
- EXPECT_EQ(P, solver_ptr->getProblem());
+ EXPECT_EQ(problem, solver_ptr->getProblem());
+
+ // run solver check
+ EXPECT_TRUE(solver_ptr->check());
+}
+
+TEST(SolverManagerFactories, FactoryFile)
+{
+ auto problem = Problem::create("PO", 3);
+ auto solver_ptr =
+ FactorySolverFile::create("SolverDummy", problem, wolf_dir + "/test/yaml/solver_dummy.yaml", {wolf_dir});
+
+ // check pointers
+ EXPECT_EQ(problem, solver_ptr->getProblem());
// run solver check
EXPECT_TRUE(solver_ptr->check());
@@ -82,30 +111,28 @@ TEST(SolverManager, Create)
////////////////////////////////////////////////////////
// FLOATING STATE BLOCKS
-TEST(SolverManager, FloatingStateBlock_Add)
+TEST_F(SolverManagerTest, FloatingStateBlock_Add)
{
- ProblemPtr P = Problem::create("PO", 3);
- auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
-
// Create State block
auto node_ptr = createStateBlock();
auto sb_ptr = node_ptr->getStateBlock('I');
// notify stateblock
- P->notifyStateBlock(sb_ptr, ADD);
+ problem->notifyStateBlock(sb_ptr, ADD);
// check notifications
Notification notif;
- EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_TRUE(problem->getStateBlockNotification(sb_ptr, notif));
EXPECT_EQ(ADD, notif);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// update solver
+ WOLF_INFO(solver_ptr->getPeriod())
solver_ptr->update();
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// check stateblock
EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
@@ -113,45 +140,42 @@ TEST(SolverManager, FloatingStateBlock_Add)
EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverManager, FloatingStateBlock_DoubleAdd)
+TEST_F(SolverManagerTest, FloatingStateBlock_DoubleAdd)
{
- ProblemPtr P = Problem::create("PO", 3);
- auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
-
// Create State block
auto node_ptr = createStateBlock();
auto sb_ptr = node_ptr->getStateBlock('I');
// notify stateblock
- P->notifyStateBlock(sb_ptr, ADD);
+ problem->notifyStateBlock(sb_ptr, ADD);
// check notifications
Notification notif;
- EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_TRUE(problem->getStateBlockNotification(sb_ptr, notif));
EXPECT_EQ(ADD, notif);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// update solver
solver_ptr->update();
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// notify stateblock again
- P->notifyStateBlock(sb_ptr, ADD);
+ problem->notifyStateBlock(sb_ptr, ADD);
// check notifications
- EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_TRUE(problem->getStateBlockNotification(sb_ptr, notif));
EXPECT_EQ(ADD, notif);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// update solver
solver_ptr->update();
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// check stateblock
EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
@@ -159,23 +183,20 @@ TEST(SolverManager, FloatingStateBlock_DoubleAdd)
EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverManager, FloatingStateBlock_AddFix)
+TEST_F(SolverManagerTest, FloatingStateBlock_AddFix)
{
- ProblemPtr P = Problem::create("PO", 3);
- auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
-
// Create State block
auto node_ptr = createStateBlock();
auto sb_ptr = node_ptr->getStateBlock('I');
// notify stateblock
- P->notifyStateBlock(sb_ptr, ADD);
+ problem->notifyStateBlock(sb_ptr, ADD);
// check notifications
Notification notif;
- EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_TRUE(problem->getStateBlockNotification(sb_ptr, notif));
EXPECT_EQ(ADD, notif);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// check flags
EXPECT_FALSE(sb_ptr->stateUpdated());
@@ -186,8 +207,8 @@ TEST(SolverManager, FloatingStateBlock_AddFix)
solver_ptr->update();
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// check flags
EXPECT_FALSE(sb_ptr->stateUpdated());
@@ -203,8 +224,8 @@ TEST(SolverManager, FloatingStateBlock_AddFix)
sb_ptr->fix();
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// check flags
EXPECT_FALSE(sb_ptr->stateUpdated());
@@ -215,8 +236,8 @@ TEST(SolverManager, FloatingStateBlock_AddFix)
solver_ptr->update();
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// check flags
EXPECT_FALSE(sb_ptr->stateUpdated());
@@ -229,23 +250,20 @@ TEST(SolverManager, FloatingStateBlock_AddFix)
EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverManager, FloatingStateBlock_AddFixed)
+TEST_F(SolverManagerTest, FloatingStateBlock_AddFixed)
{
- ProblemPtr P = Problem::create("PO", 3);
- auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
-
// Create State block
auto node_ptr = createStateBlock();
auto sb_ptr = node_ptr->getStateBlock('I');
// notify stateblock
- P->notifyStateBlock(sb_ptr, ADD);
+ problem->notifyStateBlock(sb_ptr, ADD);
// check notifications
Notification notif;
- EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_TRUE(problem->getStateBlockNotification(sb_ptr, notif));
EXPECT_EQ(ADD, notif);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// check flags
EXPECT_FALSE(sb_ptr->stateUpdated());
@@ -264,8 +282,8 @@ TEST(SolverManager, FloatingStateBlock_AddFixed)
solver_ptr->update();
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// check flags
EXPECT_FALSE(sb_ptr->stateUpdated());
@@ -279,23 +297,20 @@ TEST(SolverManager, FloatingStateBlock_AddFixed)
EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverManager, FloatingStateBlock_AddUpdateLocalParam)
+TEST_F(SolverManagerTest, FloatingStateBlock_AddUpdateLocalParam)
{
- ProblemPtr P = Problem::create("PO", 3);
- auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
-
// Create State block
auto node_ptr = createStateBlock();
auto sb_ptr = node_ptr->getStateBlock('I');
// notify stateblock
- P->notifyStateBlock(sb_ptr, ADD);
+ problem->notifyStateBlock(sb_ptr, ADD);
// check notifications
Notification notif;
- EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_TRUE(problem->getStateBlockNotification(sb_ptr, notif));
EXPECT_EQ(ADD, notif);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// check flags
EXPECT_FALSE(sb_ptr->stateUpdated());
@@ -316,16 +331,16 @@ TEST(SolverManager, FloatingStateBlock_AddUpdateLocalParam)
solver_ptr->update();
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// update solver manager
solver_ptr->update();
solver_ptr->update();
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// check flags
EXPECT_FALSE(sb_ptr->stateUpdated());
@@ -340,11 +355,8 @@ TEST(SolverManager, FloatingStateBlock_AddUpdateLocalParam)
EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverManager, FloatingStateBlock_AddLocalParamRemoveLocalParam)
+TEST_F(SolverManagerTest, FloatingStateBlock_AddLocalParamRemoveLocalParam)
{
- ProblemPtr P = Problem::create("PO", 3);
- auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
-
// Create State block
auto node_ptr = createStateBlock();
auto sb_ptr = node_ptr->getStateBlock('I');
@@ -359,20 +371,20 @@ TEST(SolverManager, FloatingStateBlock_AddLocalParamRemoveLocalParam)
EXPECT_TRUE(sb_ptr->localParamUpdated());
// notify stateblock
- P->notifyStateBlock(sb_ptr, ADD);
+ problem->notifyStateBlock(sb_ptr, ADD);
// check notifications
Notification notif;
- EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_TRUE(problem->getStateBlockNotification(sb_ptr, notif));
EXPECT_EQ(ADD, notif);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// update solver manager
solver_ptr->update();
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// check stateblock localparam
EXPECT_TRUE(solver_ptr->hasThisLocalParametrization(sb_ptr, local_ptr));
@@ -394,8 +406,8 @@ TEST(SolverManager, FloatingStateBlock_AddLocalParamRemoveLocalParam)
solver_ptr->update();
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// check stateblock localparam
EXPECT_FALSE(solver_ptr->hasLocalParametrization(sb_ptr));
@@ -404,179 +416,164 @@ TEST(SolverManager, FloatingStateBlock_AddLocalParamRemoveLocalParam)
EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverManager, FloatingStateBlock_Remove)
+TEST_F(SolverManagerTest, FloatingStateBlock_Remove)
{
- ProblemPtr P = Problem::create("PO", 3);
- auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
-
// Create State block
auto node_ptr = createStateBlock();
auto sb_ptr = node_ptr->getStateBlock('I');
// notify stateblock
- P->notifyStateBlock(sb_ptr, ADD);
+ problem->notifyStateBlock(sb_ptr, ADD);
// check notifications
Notification notif;
- EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_TRUE(problem->getStateBlockNotification(sb_ptr, notif));
EXPECT_EQ(ADD, notif);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// update solver
solver_ptr->update();
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// remove state_block
- P->notifyStateBlock(sb_ptr, REMOVE);
+ problem->notifyStateBlock(sb_ptr, REMOVE);
// check notifications
- EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_TRUE(problem->getStateBlockNotification(sb_ptr, notif));
EXPECT_EQ(REMOVE, notif);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// update solver
solver_ptr->update();
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// check stateblock
EXPECT_FALSE(solver_ptr->isStateBlockRegistered(sb_ptr));
EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverManager, FloatingStateBlock_AddRemove)
+TEST_F(SolverManagerTest, FloatingStateBlock_AddRemove)
{
- ProblemPtr P = Problem::create("PO", 3);
- auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
-
// Create State block
auto node_ptr = createStateBlock();
auto sb_ptr = node_ptr->getStateBlock('I');
// notify stateblock
- P->notifyStateBlock(sb_ptr, ADD);
+ problem->notifyStateBlock(sb_ptr, ADD);
// check notifications
Notification notif;
- EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_TRUE(problem->getStateBlockNotification(sb_ptr, notif));
EXPECT_EQ(ADD, notif);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// remove state_block
- P->notifyStateBlock(sb_ptr, REMOVE); // should cancel out the ADD
+ problem->notifyStateBlock(sb_ptr, REMOVE); // should cancel out the ADD
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// update solver
solver_ptr->update();
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// check state block
EXPECT_FALSE(solver_ptr->isStateBlockRegistered(sb_ptr));
EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverManager, FloatingStateBlock_AddRemoveAdd)
+TEST_F(SolverManagerTest, FloatingStateBlock_AddRemoveAdd)
{
- ProblemPtr P = Problem::create("PO", 3);
- auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
-
// Create State block
auto node_ptr = createStateBlock();
auto sb_ptr = node_ptr->getStateBlock('I');
// notify stateblock
- P->notifyStateBlock(sb_ptr, ADD);
+ problem->notifyStateBlock(sb_ptr, ADD);
// check notifications
Notification notif;
- EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_TRUE(problem->getStateBlockNotification(sb_ptr, notif));
EXPECT_EQ(ADD, notif);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// remove state_block
- P->notifyStateBlock(sb_ptr, REMOVE); // should cancell out the ADD
+ problem->notifyStateBlock(sb_ptr, REMOVE); // should cancell out the ADD
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// notify stateblock
- P->notifyStateBlock(sb_ptr, ADD); // again ADD in notification list
+ problem->notifyStateBlock(sb_ptr, ADD); // again ADD in notification list
// check notifications
- EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_TRUE(problem->getStateBlockNotification(sb_ptr, notif));
EXPECT_EQ(ADD, notif);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// update solver
solver_ptr->update();
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// check state block
EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverManager, FloatingStateBlock_DoubleRemove)
+TEST_F(SolverManagerTest, FloatingStateBlock_DoubleRemove)
{
- ProblemPtr P = Problem::create("PO", 3);
- auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
-
// Create State block
auto node_ptr = createStateBlock();
auto sb_ptr = node_ptr->getStateBlock('I');
// notify stateblock
- P->notifyStateBlock(sb_ptr, ADD);
+ problem->notifyStateBlock(sb_ptr, ADD);
// update solver
solver_ptr->update();
// remove state_block
- P->notifyStateBlock(sb_ptr, REMOVE);
+ problem->notifyStateBlock(sb_ptr, REMOVE);
// update solver
solver_ptr->update();
// remove state_block
- P->notifyStateBlock(sb_ptr, REMOVE);
+ problem->notifyStateBlock(sb_ptr, REMOVE);
// check notifications
Notification notif;
- EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_TRUE(problem->getStateBlockNotification(sb_ptr, notif));
EXPECT_EQ(REMOVE, notif);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// update solver manager
solver_ptr->update();
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// checks
EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverManager, FloatingStateBlock_AddUpdated)
+TEST_F(SolverManagerTest, FloatingStateBlock_AddUpdated)
{
- ProblemPtr P = Problem::create("PO", 3);
- auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
-
// Create State block
auto node_ptr = createStateBlock();
auto sb_ptr = node_ptr->getStateBlock('I');
@@ -594,13 +591,13 @@ TEST(SolverManager, FloatingStateBlock_AddUpdated)
// == When an ADD is notified: a ADD notification should be stored ==
// notify state block
- P->notifyStateBlock(sb_ptr, ADD);
+ problem->notifyStateBlock(sb_ptr, ADD);
// check notifications
// check notifications
Notification notif;
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 1);
- EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 1);
+ EXPECT_TRUE(problem->getStateBlockNotification(sb_ptr, notif));
EXPECT_EQ(notif, ADD);
// == Insert OTHER notifications ==
@@ -613,11 +610,12 @@ TEST(SolverManager, FloatingStateBlock_AddUpdated)
// Check flag has been set true
EXPECT_TRUE(sb_ptr->fixUpdated());
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 1); // No new notifications (fix and set state are flags in sb)
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(),
+ 1); // No new notifications (fix and set state are flags in sb)
// == consume empties the notification map ==
- solver_ptr->update(); // it calls P->consumeStateBlockNotificationMap();
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ solver_ptr->update(); // it calls problem->consumeStateBlockNotificationMap();
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
// Check flags have been reset
EXPECT_FALSE(sb_ptr->fixUpdated());
EXPECT_FALSE(sb_ptr->stateUpdated());
@@ -625,42 +623,39 @@ TEST(SolverManager, FloatingStateBlock_AddUpdated)
// == When an REMOVE is notified: a REMOVE notification should be stored ==
// remove state_block
- P->notifyStateBlock(sb_ptr, REMOVE);
+ problem->notifyStateBlock(sb_ptr, REMOVE);
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 1);
- EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 1);
+ EXPECT_TRUE(problem->getStateBlockNotification(sb_ptr, notif));
EXPECT_EQ(notif, REMOVE);
// == REMOVE + ADD: notification map should be empty ==
- P->notifyStateBlock(sb_ptr, ADD);
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ problem->notifyStateBlock(sb_ptr, ADD);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
// == UPDATES should clear the list of notifications ==
// notify state block
- P->notifyStateBlock(sb_ptr, ADD);
+ problem->notifyStateBlock(sb_ptr, ADD);
// update solver
solver_ptr->update();
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(),
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(),
0); // After solver_manager->update, notifications should be empty
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// == ADD + REMOVE: notification map should be empty ==
- P->notifyStateBlock(sb_ptr, ADD);
- P->notifyStateBlock(sb_ptr, REMOVE);
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ problem->notifyStateBlock(sb_ptr, ADD);
+ problem->notifyStateBlock(sb_ptr, REMOVE);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
}
////////////////////////////////////////////////////////
// STATE BLOCKS AND FACTOR
-TEST(SolverManager, StateBlockAndFactor_Add)
+TEST_F(SolverManagerTest, StateBlockAndFactor_Add)
{
- ProblemPtr P = Problem::create("PO", 3);
- auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
-
// Create State block
auto node_ptr = createStateBlock();
auto sb_ptr = node_ptr->getStateBlock('I');
@@ -669,13 +664,13 @@ TEST(SolverManager, StateBlockAndFactor_Add)
auto fac_ptr = createFactor(node_ptr);
// notify stateblock (floating for the moment)
- P->notifyStateBlock(sb_ptr, ADD);
+ problem->notifyStateBlock(sb_ptr, ADD);
// check notifications
Notification notif;
- EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_TRUE(problem->getStateBlockNotification(sb_ptr, notif));
EXPECT_EQ(ADD, notif);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// update solver
solver_ptr->update();
@@ -685,19 +680,19 @@ TEST(SolverManager, StateBlockAndFactor_Add)
EXPECT_TRUE(solver_ptr->isStateBlockFloating(sb_ptr));
// notify factor (state block now not floating)
- P->notifyFactor(fac_ptr, ADD);
+ problem->notifyFactor(fac_ptr, ADD);
// check notifications
- EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_TRUE(problem->getFactorNotification(fac_ptr, notif));
EXPECT_EQ(ADD, notif);
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
// update solver
solver_ptr->update();
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// checks
EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
@@ -706,11 +701,8 @@ TEST(SolverManager, StateBlockAndFactor_Add)
EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverManager, StateBlockAndFactor_DoubleAdd)
+TEST_F(SolverManagerTest, StateBlockAndFactor_DoubleAdd)
{
- ProblemPtr P = Problem::create("PO", 3);
- auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
-
// Create State block
auto node_ptr = createStateBlock();
auto sb_ptr = node_ptr->getStateBlock('I');
@@ -719,16 +711,16 @@ TEST(SolverManager, StateBlockAndFactor_DoubleAdd)
auto fac_ptr = createFactor(node_ptr);
// notify stateblock
- P->notifyStateBlock(sb_ptr, ADD);
+ problem->notifyStateBlock(sb_ptr, ADD);
// notify factor
- P->notifyFactor(fac_ptr, ADD);
+ problem->notifyFactor(fac_ptr, ADD);
// check notifications
Notification notif;
- EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_TRUE(problem->getStateBlockNotification(sb_ptr, notif));
EXPECT_EQ(ADD, notif);
- EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_TRUE(problem->getFactorNotification(fac_ptr, notif));
EXPECT_EQ(ADD, notif);
// update solver
@@ -736,27 +728,27 @@ TEST(SolverManager, StateBlockAndFactor_DoubleAdd)
solver_ptr->update();
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// notify stateblock again
- P->notifyStateBlock(sb_ptr, ADD);
+ problem->notifyStateBlock(sb_ptr, ADD);
// notify factor
- P->notifyFactor(fac_ptr, ADD);
+ problem->notifyFactor(fac_ptr, ADD);
// check notifications
- EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_TRUE(problem->getStateBlockNotification(sb_ptr, notif));
EXPECT_EQ(ADD, notif);
- EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_TRUE(problem->getFactorNotification(fac_ptr, notif));
EXPECT_EQ(ADD, notif);
// update solver
solver_ptr->update();
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// check stateblock
EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
@@ -765,11 +757,8 @@ TEST(SolverManager, StateBlockAndFactor_DoubleAdd)
EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverManager, StateBlockAndFactor_Fix)
+TEST_F(SolverManagerTest, StateBlockAndFactor_Fix)
{
- ProblemPtr P = Problem::create("PO", 3);
- auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
-
// Create State block
auto node_ptr = createStateBlock();
auto sb_ptr = node_ptr->getStateBlock('I');
@@ -778,16 +767,16 @@ TEST(SolverManager, StateBlockAndFactor_Fix)
auto fac_ptr = createFactor(node_ptr);
// notify stateblock
- P->notifyStateBlock(sb_ptr, ADD);
+ problem->notifyStateBlock(sb_ptr, ADD);
// notify factor
- P->notifyFactor(fac_ptr, ADD);
+ problem->notifyFactor(fac_ptr, ADD);
// check notifications
Notification notif;
- EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_TRUE(problem->getStateBlockNotification(sb_ptr, notif));
EXPECT_EQ(ADD, notif);
- EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_TRUE(problem->getFactorNotification(fac_ptr, notif));
EXPECT_EQ(ADD, notif);
// check flags
@@ -799,8 +788,8 @@ TEST(SolverManager, StateBlockAndFactor_Fix)
solver_ptr->update();
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// check flags
EXPECT_FALSE(sb_ptr->stateUpdated());
@@ -820,15 +809,15 @@ TEST(SolverManager, StateBlockAndFactor_Fix)
EXPECT_FALSE(sb_ptr->localParamUpdated());
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// update solver manager
solver_ptr->update();
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// check flags
EXPECT_FALSE(sb_ptr->stateUpdated());
@@ -840,11 +829,8 @@ TEST(SolverManager, StateBlockAndFactor_Fix)
EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverManager, StateBlockAndFactor_AddFixed)
+TEST_F(SolverManagerTest, StateBlockAndFactor_AddFixed)
{
- ProblemPtr P = Problem::create("PO", 3);
- auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
-
// Create State block
auto node_ptr = createStateBlock();
auto sb_ptr = node_ptr->getStateBlock('I');
@@ -853,16 +839,16 @@ TEST(SolverManager, StateBlockAndFactor_AddFixed)
auto fac_ptr = createFactor(node_ptr);
// notify stateblock
- P->notifyStateBlock(sb_ptr, ADD);
+ problem->notifyStateBlock(sb_ptr, ADD);
// notify factor
- P->notifyFactor(fac_ptr, ADD);
+ problem->notifyFactor(fac_ptr, ADD);
// check notifications
Notification notif;
- EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_TRUE(problem->getStateBlockNotification(sb_ptr, notif));
EXPECT_EQ(ADD, notif);
- EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_TRUE(problem->getFactorNotification(fac_ptr, notif));
EXPECT_EQ(ADD, notif);
// check flags
@@ -882,8 +868,8 @@ TEST(SolverManager, StateBlockAndFactor_AddFixed)
solver_ptr->update();
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// check flags
EXPECT_FALSE(sb_ptr->stateUpdated());
@@ -896,11 +882,8 @@ TEST(SolverManager, StateBlockAndFactor_AddFixed)
EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverManager, StateBlockAndFactor_UpdateLocalParam)
+TEST_F(SolverManagerTest, StateBlockAndFactor_UpdateLocalParam)
{
- ProblemPtr P = Problem::create("PO", 3);
- auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
-
// Create State block
auto node_ptr = createStateBlock();
auto sb_ptr = node_ptr->getStateBlock('I');
@@ -909,16 +892,16 @@ TEST(SolverManager, StateBlockAndFactor_UpdateLocalParam)
auto fac_ptr = createFactor(node_ptr);
// notify stateblock
- P->notifyStateBlock(sb_ptr, ADD);
+ problem->notifyStateBlock(sb_ptr, ADD);
// notify factor
- P->notifyFactor(fac_ptr, ADD);
+ problem->notifyFactor(fac_ptr, ADD);
// check notifications
Notification notif;
- EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_TRUE(problem->getStateBlockNotification(sb_ptr, notif));
EXPECT_EQ(ADD, notif);
- EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_TRUE(problem->getFactorNotification(fac_ptr, notif));
EXPECT_EQ(ADD, notif);
// check flags
@@ -942,8 +925,8 @@ TEST(SolverManager, StateBlockAndFactor_UpdateLocalParam)
solver_ptr->update();
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// check flags
EXPECT_FALSE(sb_ptr->stateUpdated());
@@ -957,11 +940,8 @@ TEST(SolverManager, StateBlockAndFactor_UpdateLocalParam)
EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverManager, StateBlockAndFactor_AddLocalParamRemoveLocalParam)
+TEST_F(SolverManagerTest, StateBlockAndFactor_AddLocalParamRemoveLocalParam)
{
- ProblemPtr P = Problem::create("PO", 3);
- auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
-
// Create State block
auto node_ptr = createStateBlock();
auto sb_ptr = node_ptr->getStateBlock('I');
@@ -979,24 +959,24 @@ TEST(SolverManager, StateBlockAndFactor_AddLocalParamRemoveLocalParam)
EXPECT_TRUE(sb_ptr->localParamUpdated());
// notify stateblock
- P->notifyStateBlock(sb_ptr, ADD);
+ problem->notifyStateBlock(sb_ptr, ADD);
// notify factor
- P->notifyFactor(fac_ptr, ADD);
+ problem->notifyFactor(fac_ptr, ADD);
// check notifications
Notification notif;
- EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_TRUE(problem->getStateBlockNotification(sb_ptr, notif));
EXPECT_EQ(ADD, notif);
- EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_TRUE(problem->getFactorNotification(fac_ptr, notif));
EXPECT_EQ(ADD, notif);
// update solver manager
solver_ptr->update();
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// check solver
EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
@@ -1026,11 +1006,8 @@ TEST(SolverManager, StateBlockAndFactor_AddLocalParamRemoveLocalParam)
EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverManager, StateBlockAndFactor_RemoveStateBlock)
+TEST_F(SolverManagerTest, StateBlockAndFactor_RemoveStateBlock)
{
- ProblemPtr P = Problem::create("PO", 3);
- auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
-
// Create State block
auto node_ptr = createStateBlock();
auto sb_ptr = node_ptr->getStateBlock('I');
@@ -1039,40 +1016,40 @@ TEST(SolverManager, StateBlockAndFactor_RemoveStateBlock)
auto fac_ptr = createFactor(node_ptr);
// notify stateblock
- P->notifyStateBlock(sb_ptr, ADD);
+ problem->notifyStateBlock(sb_ptr, ADD);
// notify factor
- P->notifyFactor(fac_ptr, ADD);
+ problem->notifyFactor(fac_ptr, ADD);
// check notifications
Notification notif;
- EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_TRUE(problem->getStateBlockNotification(sb_ptr, notif));
EXPECT_EQ(ADD, notif);
- EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_TRUE(problem->getFactorNotification(fac_ptr, notif));
EXPECT_EQ(ADD, notif);
// update solver
solver_ptr->update();
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// remove state_block
- P->notifyStateBlock(sb_ptr, REMOVE);
+ problem->notifyStateBlock(sb_ptr, REMOVE);
// check notifications
- EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_TRUE(problem->getStateBlockNotification(sb_ptr, notif));
EXPECT_EQ(REMOVE, notif);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// update solver
solver_ptr->update(); // there is a factor involved, removal posponed (REMOVE in notification list)
// check notifications
- EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_TRUE(problem->getStateBlockNotification(sb_ptr, notif));
EXPECT_EQ(REMOVE, notif);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// checks
EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
@@ -1080,11 +1057,8 @@ TEST(SolverManager, StateBlockAndFactor_RemoveStateBlock)
EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverManager, StateBlockAndFactor_RemoveFactor)
+TEST_F(SolverManagerTest, StateBlockAndFactor_RemoveFactor)
{
- ProblemPtr P = Problem::create("PO", 3);
- auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
-
// Create State block
auto node_ptr = createStateBlock();
auto sb_ptr = node_ptr->getStateBlock('I');
@@ -1093,39 +1067,39 @@ TEST(SolverManager, StateBlockAndFactor_RemoveFactor)
auto fac_ptr = createFactor(node_ptr);
// notify stateblock
- P->notifyStateBlock(sb_ptr, ADD);
+ problem->notifyStateBlock(sb_ptr, ADD);
// notify factor
- P->notifyFactor(fac_ptr, ADD);
+ problem->notifyFactor(fac_ptr, ADD);
// check notifications
Notification notif;
- EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_TRUE(problem->getStateBlockNotification(sb_ptr, notif));
EXPECT_EQ(ADD, notif);
- EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_TRUE(problem->getFactorNotification(fac_ptr, notif));
EXPECT_EQ(ADD, notif);
// update solver
solver_ptr->update();
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// remove state_block
- P->notifyFactor(fac_ptr, REMOVE); // state block should be automatically stored as floating
+ problem->notifyFactor(fac_ptr, REMOVE); // state block should be automatically stored as floating
// check notifications
- EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_TRUE(problem->getFactorNotification(fac_ptr, notif));
EXPECT_EQ(REMOVE, notif);
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
// update solver
solver_ptr->update();
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// checks
EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
@@ -1134,11 +1108,8 @@ TEST(SolverManager, StateBlockAndFactor_RemoveFactor)
EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverManager, StateBlockAndFactor_AddRemoveStateBlock)
+TEST_F(SolverManagerTest, StateBlockAndFactor_AddRemoveStateBlock)
{
- ProblemPtr P = Problem::create("PO", 3);
- auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
-
// Create State block
auto node_ptr = createStateBlock();
auto sb_ptr = node_ptr->getStateBlock('I');
@@ -1147,24 +1118,24 @@ TEST(SolverManager, StateBlockAndFactor_AddRemoveStateBlock)
auto fac_ptr = createFactor(node_ptr);
// notify stateblock
- P->notifyStateBlock(sb_ptr, ADD);
+ problem->notifyStateBlock(sb_ptr, ADD);
// notify factor
- P->notifyFactor(fac_ptr, ADD);
+ problem->notifyFactor(fac_ptr, ADD);
// check notifications
Notification notif;
- EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_TRUE(problem->getStateBlockNotification(sb_ptr, notif));
EXPECT_EQ(ADD, notif);
- EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_TRUE(problem->getFactorNotification(fac_ptr, notif));
EXPECT_EQ(ADD, notif);
// remove state_block
- P->notifyStateBlock(sb_ptr, REMOVE); // cancells out ADD notification
+ problem->notifyStateBlock(sb_ptr, REMOVE); // cancells out ADD notification
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_TRUE(problem->getFactorNotification(fac_ptr, notif));
EXPECT_EQ(ADD, notif);
// update solver
@@ -1172,8 +1143,8 @@ TEST(SolverManager, StateBlockAndFactor_AddRemoveStateBlock)
// notification added)
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_TRUE(problem->getFactorNotification(fac_ptr, notif));
EXPECT_EQ(ADD, notif);
// check state block
@@ -1182,11 +1153,8 @@ TEST(SolverManager, StateBlockAndFactor_AddRemoveStateBlock)
EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverManager, StateBlockAndFactor_AddRemoveFactor)
+TEST_F(SolverManagerTest, StateBlockAndFactor_AddRemoveFactor)
{
- ProblemPtr P = Problem::create("PO", 3);
- auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
-
// Create State block
auto node_ptr = createStateBlock();
auto sb_ptr = node_ptr->getStateBlock('I');
@@ -1195,32 +1163,32 @@ TEST(SolverManager, StateBlockAndFactor_AddRemoveFactor)
auto fac_ptr = createFactor(node_ptr);
// notify stateblock
- P->notifyStateBlock(sb_ptr, ADD);
+ problem->notifyStateBlock(sb_ptr, ADD);
// notify factor
- P->notifyFactor(fac_ptr, ADD);
+ problem->notifyFactor(fac_ptr, ADD);
// check notifications
Notification notif;
- EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_TRUE(problem->getStateBlockNotification(sb_ptr, notif));
EXPECT_EQ(ADD, notif);
- EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_TRUE(problem->getFactorNotification(fac_ptr, notif));
EXPECT_EQ(ADD, notif);
// remove state_block
- P->notifyFactor(fac_ptr, REMOVE); // cancells out ADD notification
+ problem->notifyFactor(fac_ptr, REMOVE); // cancells out ADD notification
// check notifications
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
- EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
+ EXPECT_TRUE(problem->getStateBlockNotification(sb_ptr, notif));
EXPECT_EQ(ADD, notif);
// update solver
solver_ptr->update(); // state block should be automatically stored as floating
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// checks
EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
@@ -1229,11 +1197,8 @@ TEST(SolverManager, StateBlockAndFactor_AddRemoveFactor)
EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverManager, StateBlockAndFactor_DoubleRemoveStateBlock)
+TEST_F(SolverManagerTest, StateBlockAndFactor_DoubleRemoveStateBlock)
{
- ProblemPtr P = Problem::create("PO", 3);
- auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
-
// Create State block
auto node_ptr = createStateBlock();
auto sb_ptr = node_ptr->getStateBlock('I');
@@ -1242,49 +1207,49 @@ TEST(SolverManager, StateBlockAndFactor_DoubleRemoveStateBlock)
auto fac_ptr = createFactor(node_ptr);
// notify stateblock
- P->notifyStateBlock(sb_ptr, ADD);
+ problem->notifyStateBlock(sb_ptr, ADD);
// notify factor
- P->notifyFactor(fac_ptr, ADD);
+ problem->notifyFactor(fac_ptr, ADD);
// check notifications
Notification notif;
- EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_TRUE(problem->getStateBlockNotification(sb_ptr, notif));
EXPECT_EQ(ADD, notif);
- EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_TRUE(problem->getFactorNotification(fac_ptr, notif));
EXPECT_EQ(ADD, notif);
// remove state_block
- P->notifyStateBlock(sb_ptr, REMOVE); // cancells out the ADD
+ problem->notifyStateBlock(sb_ptr, REMOVE); // cancells out the ADD
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_TRUE(problem->getFactorNotification(fac_ptr, notif));
EXPECT_EQ(ADD, notif);
// update solver
solver_ptr->update(); // factor ADD is posponed
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_TRUE(problem->getFactorNotification(fac_ptr, notif));
EXPECT_EQ(ADD, notif);
// remove state_block
- P->notifyStateBlock(sb_ptr, REMOVE);
+ problem->notifyStateBlock(sb_ptr, REMOVE);
// check notifications
- EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_TRUE(problem->getStateBlockNotification(sb_ptr, notif));
EXPECT_EQ(REMOVE, notif);
- EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_TRUE(problem->getFactorNotification(fac_ptr, notif));
EXPECT_EQ(ADD, notif);
// update solver manager
solver_ptr->update(); // repeated REMOVE should be ignored
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_TRUE(problem->getFactorNotification(fac_ptr, notif));
EXPECT_EQ(ADD, notif);
// check state block
@@ -1293,11 +1258,8 @@ TEST(SolverManager, StateBlockAndFactor_DoubleRemoveStateBlock)
EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverManager, StateBlockAndFactor_DoubleRemoveFactor)
+TEST_F(SolverManagerTest, StateBlockAndFactor_DoubleRemoveFactor)
{
- ProblemPtr P = Problem::create("PO", 3);
- auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
-
// Create State block
auto node_ptr = createStateBlock();
auto sb_ptr = node_ptr->getStateBlock('I');
@@ -1306,39 +1268,39 @@ TEST(SolverManager, StateBlockAndFactor_DoubleRemoveFactor)
auto fac_ptr = createFactor(node_ptr);
// notify stateblock
- P->notifyStateBlock(sb_ptr, ADD);
+ problem->notifyStateBlock(sb_ptr, ADD);
// notify factor
- P->notifyFactor(fac_ptr, ADD);
+ problem->notifyFactor(fac_ptr, ADD);
// check notifications
Notification notif;
- EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_TRUE(problem->getStateBlockNotification(sb_ptr, notif));
EXPECT_EQ(ADD, notif);
- EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_TRUE(problem->getFactorNotification(fac_ptr, notif));
EXPECT_EQ(ADD, notif);
// remove state_block
- P->notifyFactor(fac_ptr, REMOVE); // cancells out ADD
+ problem->notifyFactor(fac_ptr, REMOVE); // cancells out ADD
// check notifications
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
- EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
+ EXPECT_TRUE(problem->getStateBlockNotification(sb_ptr, notif));
EXPECT_EQ(ADD, notif);
// update solver
solver_ptr->update(); // state block should be automatically stored as floating
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// remove state_block
- P->notifyFactor(fac_ptr, REMOVE);
+ problem->notifyFactor(fac_ptr, REMOVE);
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_TRUE(problem->getFactorNotification(fac_ptr, notif));
EXPECT_EQ(REMOVE, notif);
// update solver
@@ -1351,11 +1313,8 @@ TEST(SolverManager, StateBlockAndFactor_DoubleRemoveFactor)
EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverManager, StateBlockAndFactor_AddUpdatedStateBlock)
+TEST_F(SolverManagerTest, StateBlockAndFactor_AddUpdatedStateBlock)
{
- ProblemPtr P = Problem::create("PO", 3);
- auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
-
// Create State block
auto node_ptr = createStateBlock();
auto sb_ptr = node_ptr->getStateBlock('I');
@@ -1376,16 +1335,16 @@ TEST(SolverManager, StateBlockAndFactor_AddUpdatedStateBlock)
// == When an ADD is notified: a ADD notification should be stored ==
// notify state block
- P->notifyStateBlock(sb_ptr, ADD);
+ problem->notifyStateBlock(sb_ptr, ADD);
// notify factor
- P->notifyFactor(fac_ptr, ADD);
+ problem->notifyFactor(fac_ptr, ADD);
// check notifications
Notification notif;
- EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_TRUE(problem->getStateBlockNotification(sb_ptr, notif));
EXPECT_EQ(ADD, notif);
- EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_TRUE(problem->getFactorNotification(fac_ptr, notif));
EXPECT_EQ(ADD, notif);
// == Insert OTHER notifications ==
@@ -1398,11 +1357,12 @@ TEST(SolverManager, StateBlockAndFactor_AddUpdatedStateBlock)
// Check flag has been set true
EXPECT_TRUE(sb_ptr->fixUpdated());
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 1); // No new notifications (fix and set state are flags in sb)
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(),
+ 1); // No new notifications (fix and set state are flags in sb)
// == consume empties the notification map ==
- solver_ptr->update(); // it calls P->consumeStateBlockNotificationMap();
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ solver_ptr->update(); // it calls problem->consumeStateBlockNotificationMap();
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
// Check flags have been reset
EXPECT_FALSE(sb_ptr->fixUpdated());
EXPECT_FALSE(sb_ptr->stateUpdated());
@@ -1410,39 +1370,36 @@ TEST(SolverManager, StateBlockAndFactor_AddUpdatedStateBlock)
// == When an REMOVE is notified: a REMOVE notification should be stored ==
// remove state_block
- P->notifyStateBlock(sb_ptr, REMOVE);
+ problem->notifyStateBlock(sb_ptr, REMOVE);
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 1);
- EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 1);
+ EXPECT_TRUE(problem->getStateBlockNotification(sb_ptr, notif));
EXPECT_EQ(notif, REMOVE);
// == REMOVE + ADD: notification map should be empty ==
- P->notifyStateBlock(sb_ptr, ADD);
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ problem->notifyStateBlock(sb_ptr, ADD);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
// == UPDATES should clear the list of notifications ==
// notify state block
- P->notifyStateBlock(sb_ptr, ADD);
+ problem->notifyStateBlock(sb_ptr, ADD);
// update solver
solver_ptr->update();
- EXPECT_EQ(P->getStateBlockNotificationMapSize(),
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(),
0); // After solver_manager->update, notifications should be empty
// == ADD + REMOVE: notification map should be empty ==
- P->notifyStateBlock(sb_ptr, ADD);
- P->notifyStateBlock(sb_ptr, REMOVE);
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ problem->notifyStateBlock(sb_ptr, ADD);
+ problem->notifyStateBlock(sb_ptr, REMOVE);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
}
////////////////////////////////////////////////////////
// ONLY FACTOR
-TEST(SolverManager, OnlyFactor_Add)
+TEST_F(SolverManagerTest, OnlyFactor_Add)
{
- ProblemPtr P = Problem::create("PO", 3);
- auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
-
// Create State block
auto node_ptr = createStateBlock();
auto sb_ptr = node_ptr->getStateBlock('I');
@@ -1451,20 +1408,20 @@ TEST(SolverManager, OnlyFactor_Add)
auto fac_ptr = createFactor(node_ptr);
// notify factor (state block has not been notified)
- P->notifyFactor(fac_ptr, ADD);
+ problem->notifyFactor(fac_ptr, ADD);
// check notifications
Notification notif;
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_TRUE(problem->getFactorNotification(fac_ptr, notif));
EXPECT_EQ(ADD, notif);
// update solver
solver_ptr->update(); // factor ADD should be posponed (in the notification list again)
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_TRUE(problem->getFactorNotification(fac_ptr, notif));
EXPECT_EQ(ADD, notif);
// checks
@@ -1473,11 +1430,8 @@ TEST(SolverManager, OnlyFactor_Add)
EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverManager, OnlyFactor_Remove)
+TEST_F(SolverManagerTest, OnlyFactor_Remove)
{
- ProblemPtr P = Problem::create("PO", 3);
- auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
-
// Create State block
auto node_ptr = createStateBlock();
auto sb_ptr = node_ptr->getStateBlock('I');
@@ -1486,35 +1440,35 @@ TEST(SolverManager, OnlyFactor_Remove)
auto fac_ptr = createFactor(node_ptr);
// notify factor (state block has not been notified)
- P->notifyFactor(fac_ptr, ADD);
+ problem->notifyFactor(fac_ptr, ADD);
// check notifications
Notification notif;
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_TRUE(problem->getFactorNotification(fac_ptr, notif));
EXPECT_EQ(ADD, notif);
// update solver
solver_ptr->update(); // ADD factor should be posponed (in the notification list again)
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_TRUE(problem->getFactorNotification(fac_ptr, notif));
EXPECT_EQ(ADD, notif);
// notify factor
- P->notifyFactor(fac_ptr, REMOVE);
+ problem->notifyFactor(fac_ptr, REMOVE);
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// update solver
solver_ptr->update(); // nothing to update
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// checks
EXPECT_FALSE(solver_ptr->isStateBlockRegistered(sb_ptr));
@@ -1522,11 +1476,8 @@ TEST(SolverManager, OnlyFactor_Remove)
EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverManager, OnlyFactor_AddRemove)
+TEST_F(SolverManagerTest, OnlyFactor_AddRemove)
{
- ProblemPtr P = Problem::create("PO", 3);
- auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
-
// Create State block
auto node_ptr = createStateBlock();
auto sb_ptr = node_ptr->getStateBlock('I');
@@ -1535,27 +1486,27 @@ TEST(SolverManager, OnlyFactor_AddRemove)
auto fac_ptr = createFactor(node_ptr);
// notify factor (state block has not been notified)
- P->notifyFactor(fac_ptr, ADD);
+ problem->notifyFactor(fac_ptr, ADD);
// check notifications
Notification notif;
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_TRUE(problem->getFactorNotification(fac_ptr, notif));
EXPECT_EQ(ADD, notif);
// notify factor
- P->notifyFactor(fac_ptr, REMOVE);
+ problem->notifyFactor(fac_ptr, REMOVE);
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// update solver
solver_ptr->update(); // nothing to update
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// checks
EXPECT_FALSE(solver_ptr->isStateBlockRegistered(sb_ptr));
diff --git a/test/gtest_solver_manager_multithread.cpp b/test/gtest_solver_manager_multithread.cpp
index 0d4fe1ae84ce505020518ad5e0b0c69081b71832..bbd9ae307467d67104599488af502f5b88be3acf 100644
--- a/test/gtest_solver_manager_multithread.cpp
+++ b/test/gtest_solver_manager_multithread.cpp
@@ -17,16 +17,9 @@
//
// 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/>.
-/*
- * gtest_solver_manager.cpp
- *
- * Created on: Jun, 2018
- * Author: jvallve
- */
#include "core/utils/utils_gtest.h"
-
#include "core/problem/problem.h"
#include "core/sensor/sensor_base.h"
#include "core/state_block/state_block.h"
@@ -34,7 +27,7 @@
#include "core/factor/factor_pose_2d.h"
#include "core/factor/factor_block_absolute.h"
#include "core/state_block/local_parametrization_quaternion.h"
-#include "dummy/solver_manager_dummy.h"
+#include "dummy/solver_dummy.h"
#include <iostream>
#include <thread>
@@ -49,38 +42,47 @@ using namespace Eigen;
TEST(SolverManagerMultithread, MultiThreadingTruncatedNotifications)
{
- double Dt = 5.0;
- ProblemPtr P = Problem::create("PO", 2);
- auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
+ double Dt = 5.0;
+ ProblemPtr P = Problem::create("PO", 2);
+ YAML::Node params;
+ params["period"] = 0;
+ params["verbose"] = 0;
+ params["compute_cov"] = false;
+ auto solver_ptr = FactorySolverNode::create("SolverDummy", P, params, {});
// loop updating (without sleep)
- std::thread t([&](){
+ std::thread t([&]() {
auto start_t = std::chrono::high_resolution_clock::now();
while (true)
{
solver_ptr->update();
ASSERT_TRUE(solver_ptr->check());
- if (std::chrono::duration_cast<std::chrono::duration<double>>(std::chrono::high_resolution_clock::now() - start_t).count() > Dt)
+ if (std::chrono::duration_cast<std::chrono::duration<double>>(std::chrono::high_resolution_clock::now() -
+ start_t)
+ .count() > Dt)
break;
- }});
+ }
+ });
// loop emplacing and removing frames (window of 10 KF)
- auto start = std::chrono::high_resolution_clock::now();
+ auto start = std::chrono::high_resolution_clock::now();
TimeStamp ts(0);
while (true)
{
// Emplace Frame, Capture, feature and factor pose 2d
- FrameBasePtr F = P->emplaceFrame(ts, P->stateZero());
- auto C = CaptureBase::emplace<CaptureVoid>(F, ts, nullptr);
- auto f = FeatureBase::emplace<FeatureBase>(C, "FeaturePose2d", Vector3d::Zero(), Matrix3d::Identity());
- auto c = FactorBase::emplace<FactorPose2d>(f, f->getMeasurement(), f->getMeasurementSquareRootInformationUpper(), F, nullptr, false);
+ FrameBasePtr F = P->emplaceFrame(ts, P->stateZero());
+ auto C = CaptureBase::emplace<CaptureVoid>(F, ts, nullptr);
+ auto f = FeatureBase::emplace<FeatureBase>(C, "FeaturePose2d", Vector3d::Zero(), Matrix3d::Identity());
+ auto c = FactorBase::emplace<FactorPose2d>(
+ f, f->getMeasurement(), f->getMeasurementSquareRootInformationUpper(), F, nullptr, false);
ts += 1.0;
- if (P->getTrajectory()->size() > 10)
- P->getTrajectory()->getFirstFrame()->remove();
+ if (P->getTrajectory()->size() > 10) P->getTrajectory()->getFirstFrame()->remove();
- if (std::chrono::duration_cast<std::chrono::duration<double>>(std::chrono::high_resolution_clock::now() - start).count() > Dt)
+ if (std::chrono::duration_cast<std::chrono::duration<double>>(std::chrono::high_resolution_clock::now() -
+ start)
+ .count() > Dt)
break;
}
@@ -89,6 +91,6 @@ TEST(SolverManagerMultithread, MultiThreadingTruncatedNotifications)
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_spec_state_composite.cpp b/test/gtest_spec_state_composite.cpp
index e0ebd7ed5bbe55c3affc5f54df28ca34070679c4..a287fff326f9c8500f823ffc2d42b6f4edcde927 100644
--- a/test/gtest_spec_state_composite.cpp
+++ b/test/gtest_spec_state_composite.cpp
@@ -21,7 +21,6 @@
#include "core/utils/utils_gtest.h"
#include "core/common/wolf.h"
#include "core/composite/spec_state_composite.h"
-#include "core/common/params_base.h" // toString
using namespace wolf;
using namespace Eigen;
diff --git a/test/gtest_spec_state_sensor_composite.cpp b/test/gtest_spec_state_sensor_composite.cpp
index 90872d895a0772a617273970d54cbaf693c3069c..d79d2296e13a7e062edafefd079f3096f39aff15 100644
--- a/test/gtest_spec_state_sensor_composite.cpp
+++ b/test/gtest_spec_state_sensor_composite.cpp
@@ -21,7 +21,6 @@
#include "core/utils/utils_gtest.h"
#include "core/common/wolf.h"
#include "core/composite/spec_state_sensor_composite.h"
-#include "core/common/params_base.h" // toString
using namespace wolf;
using namespace Eigen;
diff --git a/test/gtest_state_block.cpp b/test/gtest_state_block.cpp
index ebc0b11fa375ec20f65417a3c6652ea7543a45c1..a4d30eda328b478630614bd474994a2ffa993926 100644
--- a/test/gtest_state_block.cpp
+++ b/test/gtest_state_block.cpp
@@ -17,12 +17,6 @@
//
// 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/>.
-/*
- * gtest_state_block.cpp
- *
- * Created on: Mar 31, 2020
- * Author: jsola
- */
#include "core/utils/utils_gtest.h"
diff --git a/test/gtest_state_composite.cpp b/test/gtest_state_composite.cpp
deleted file mode 100644
index 8dc4ad025c9fde0d25a3815ce1f30707c181ebb9..0000000000000000000000000000000000000000
--- a/test/gtest_state_composite.cpp
+++ /dev/null
@@ -1,263 +0,0 @@
-// WOLF - Copyright (C) 2020,2021,2022,2023
-// Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
-// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
-// Joan Vallvé Navarro (jvallve@iri.upc.edu)
-// All rights reserved.
-//
-// This file is part of WOLF: http://www.iri.upc.edu/wolf
-// WOLF is free software: you can redistribute it and/or modify
-// it under the terms of the GNU Lesser General Public License as published by
-// 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/>.
-
-#include "core/composite/state_composite.h"
-#include "core/state_block/state_block_derived.h"
-#include "core/state_block/state_quaternion.h"
-
-#include "core/utils/utils_gtest.h"
-
-using namespace wolf;
-using namespace std;
-
-class StateBlockCompositeInit : public testing::Test
-{
- public:
-
- StateBlockPtr sbp, sbv, sbx;
- StateQuaternionPtr sbq;
-
- StateBlockComposite states;
-
- void SetUp() override
- {
- sbp = states.emplace<StatePoint3d>('P', Vector3d(1,2,3));
- sbv = states.emplace<StateVector3d>('V', Vector3d(4,5,6));
- sbq = states.emplace<StateQuaternion>('Q', Vector4d(.5,.5,.5,.5));
-
- sbx = std::make_shared<StateVector3d>(Vector3d(7,8,9));
- }
-};
-
-TEST_F(StateBlockCompositeInit, stateSize)
-{
- ASSERT_EQ( states.stateSize() , 10 );
- ASSERT_EQ( states.stateSize("PQ"), 7 );
- ASSERT_EQ( states.stateSize("PV"), 6 );
-}
-
-TEST_F(StateBlockCompositeInit, stateVector)
-{
- ASSERT_EQ( states.stateVector("PQ") , (VectorXd( 7) << 1,2,3,.5,.5,.5,.5) .finished() );
- ASSERT_EQ( states.stateVector("PVQ"), (VectorXd(10) << 1,2,3,4,5,6,.5,.5,.5,.5).finished() );
- ASSERT_EQ( states.stateVector("PQV"), (VectorXd(10) << 1,2,3,.5,.5,.5,.5,4,5,6).finished() );
-}
-
-TEST_F(StateBlockCompositeInit, emplace)
-{
- // Emplaces called in SetUp()
-
- // check 3 elements
- ASSERT_EQ(states.getStateBlockMap().size(), 3);
-}
-
-TEST_F(StateBlockCompositeInit, has_key)
-{
- ASSERT_TRUE(states.has('P'));
- ASSERT_FALSE(states.has('X'));
-}
-
-TEST_F(StateBlockCompositeInit, has_sb)
-{
- ASSERT_TRUE(states.has(sbp));
- ASSERT_FALSE(states.has(sbx));
-}
-
-TEST_F(StateBlockCompositeInit, at)
-{
- ASSERT_EQ(states.at('P'), sbp);
-
- ASSERT_EQ(states.at('X'), nullptr);
-}
-
-TEST_F(StateBlockCompositeInit, set_sb)
-{
- states.set('P', sbx);
-
- ASSERT_EQ(states.at('P'), sbx);
-
- states.set('P', sbp);
-
- ASSERT_DEATH(states.set('X', sbx), "");
-
- ASSERT_EQ(states.at('X'), nullptr);
-}
-
-TEST_F(StateBlockCompositeInit, set_vec)
-{
- Vector3d p(11,22,33);
- Vector3d x(55,66,77);
-
- states.set('P', p);
-
- ASSERT_MATRIX_APPROX(states.at('P')->getState(), p, 1e-20);
-
- ASSERT_DEATH(states.set('X', x), "");
-
- ASSERT_EQ(states.at('X'), nullptr);
-}
-
-TEST_F(StateBlockCompositeInit, set_vectors)
-{
- Vector3d p(11,22,33);
- Vector4d q(11,22,33,44); q.normalize();
- Vector3d x(55,66,77);
-
- states.setVectors("PQ", {p, q});
-
- ASSERT_MATRIX_APPROX(states.at('P')->getState(), p, 1e-20);
- ASSERT_QUATERNION_VECTOR_APPROX(states.at('Q')->getState(), q, 1e-20);
-
- ASSERT_DEATH(states.setVectors("PX", {p,x}), "");
-
- ASSERT_EQ(states.at('X'), nullptr);
-}
-
-TEST_F(StateBlockCompositeInit, key_ref)
-{
- char key;
- ASSERT_TRUE(states.key(sbp, key));
- ASSERT_EQ(key, 'P');
-
- ASSERT_FALSE(states.key(sbx, key));
- ASSERT_EQ(key, '0');
-}
-
-TEST_F(StateBlockCompositeInit, key_return)
-{
- // existing key
- ASSERT_EQ(states.key(sbp), 'P');
-
- // non existing key returns zero char
- ASSERT_EQ(states.key(sbx), '0');
-}
-
-TEST_F(StateBlockCompositeInit, find)
-{
- auto it = states.find(sbp);
- ASSERT_NE(it, states.getStateBlockMap().end());
-
- it = states.find(sbx);
- ASSERT_EQ(it, states.getStateBlockMap().end());
-}
-
-TEST_F(StateBlockCompositeInit, add)
-{
- states.add('X', sbx);
-
- ASSERT_EQ(states.at('X'), sbx);
-}
-
-TEST_F(StateBlockCompositeInit, remove)
-{
- // remove existing block
- states.remove('V');
- ASSERT_EQ(states.getStateBlockMap().size(), 2);
-
- // remove non existing block -- no effect
- states.remove('X');
- ASSERT_EQ(states.getStateBlockMap().size(), 2);
-}
-
-TEST_F(StateBlockCompositeInit, perturb)
-{
- ASSERT_TRUE(states.at('P')->getState().isApprox(sbp->getState(), 1e-3));
- ASSERT_TRUE(states.at('V')->getState().isApprox(sbv->getState(), 1e-3));
- ASSERT_TRUE(states.at('Q')->getState().isApprox(sbq->getState(), 1e-3));
-
- states.perturb(0.1);
-
- // values have moved wrt original
- ASSERT_FALSE(states.at('P')->getState().isApprox(Vector3d(1,2,3), 1e-3));
- ASSERT_FALSE(states.at('V')->getState().isApprox(Vector3d(4,5,6), 1e-3));
- ASSERT_FALSE(states.at('Q')->getState().isApprox(Vector4d(.5,.5,.5,.5), 1e-3));
-}
-
-TEST_F(StateBlockCompositeInit, setIdentity)
-{
- ASSERT_TRUE(states.at('P')->getState().isApprox(sbp->getState(), 1e-3));
- ASSERT_TRUE(states.at('V')->getState().isApprox(sbv->getState(), 1e-3));
- ASSERT_TRUE(states.at('Q')->getState().isApprox(sbq->getState(), 1e-3));
-
- states.setIdentity();
-
- // values have moved wrt original
- ASSERT_TRUE(states.at('P')->getState().isApprox(Vector3d(0,0,0), 1e-10));
- ASSERT_TRUE(states.at('V')->getState().isApprox(Vector3d(0,0,0), 1e-10));
- ASSERT_TRUE(states.at('Q')->getState().isApprox(Vector4d(0,0,0,1), 1e-10));
-}
-
-TEST_F(StateBlockCompositeInit, identity)
-{
- VectorComposite v = states.identity();
-
- ASSERT_TRUE(v.at('P').isApprox(Vector3d(0,0,0), 1e-10));
- ASSERT_TRUE(v.at('V').isApprox(Vector3d(0,0,0), 1e-10));
- ASSERT_TRUE(v.at('Q').isApprox(Vector4d(0,0,0,1), 1e-10));
-}
-
-TEST_F(StateBlockCompositeInit, fix)
-{
- states.fix();
-
- ASSERT_TRUE(states.at('P')->isFixed());
- ASSERT_TRUE(states.at('V')->isFixed());
- ASSERT_TRUE(states.at('Q')->isFixed());
-}
-
-TEST_F(StateBlockCompositeInit, unfix)
-{
- states.fix();
-
- ASSERT_TRUE(states.at('P')->isFixed());
- ASSERT_TRUE(states.at('V')->isFixed());
- ASSERT_TRUE(states.at('Q')->isFixed());
-
- states.unfix();
-
- ASSERT_FALSE(states.at('P')->isFixed());
- ASSERT_FALSE(states.at('V')->isFixed());
- ASSERT_FALSE(states.at('Q')->isFixed());
-}
-
-TEST_F(StateBlockCompositeInit, isFixed)
-{
- states.fix();
-
- ASSERT_TRUE(states.isFixed());
-
- states.at('P')->unfix();
-
- ASSERT_FALSE(states.isFixed());
-}
-
-int main(int argc, char **argv)
-{
- testing::InitGoogleTest(&argc, argv);
-
- // restrict to a group of tests only
- // ::testing::GTEST_FLAG(filter) = "VectorComposite.*";
-
- // restrict to this test only
- // ::testing::GTEST_FLAG(filter) = "MatrixComposite.propagate";
-// ::testing::GTEST_FLAG(filter) = "MatrixComposite.*";
-
- return RUN_ALL_TESTS();
-}
diff --git a/test/gtest_time_stamp.cpp b/test/gtest_time_stamp.cpp
index b19c1ac201024279ac5d3f6bb5ed65eec1a16b72..ac8e2f852bdcdcce78227d73a2f4d0fef6c6a395 100644
--- a/test/gtest_time_stamp.cpp
+++ b/test/gtest_time_stamp.cpp
@@ -46,189 +46,189 @@ TEST(WolfTestTimeStamp, TimeStampInvalid)
TEST(WolfTestTimeStamp, TimeStampInitNow)
{
- wolf::TimeStamp start = wolf::TimeStamp::Now();
+ wolf::TimeStamp start = wolf::TimeStamp::Now();
- // If we don't sleep, start == time_stamp sometimes.
- // And sometimes start <= time_stamp ...
- std::this_thread::sleep_for(std::chrono::microseconds(1));
+ // If we don't sleep, start == time_stamp sometimes.
+ // And sometimes start <= time_stamp ...
+ std::this_thread::sleep_for(std::chrono::microseconds(1));
- ASSERT_NE(start.get(), 0);
+ ASSERT_NE(start.get(), 0);
- wolf::TimeStamp time_stamp = wolf::TimeStamp::Now();
+ wolf::TimeStamp time_stamp = wolf::TimeStamp::Now();
-// std::cout << std::fixed;
-// std::cout << std::setprecision(15);
-// std::cout << start.get() << " | " << time_stamp.get() << std::endl;
+ // std::cout << std::fixed;
+ // std::cout << std::setprecision(15);
+ // std::cout << start.get() << " | " << time_stamp.get() << std::endl;
- ASSERT_NE(time_stamp.get(), start.get());
+ ASSERT_NE(time_stamp.get(), start.get());
- ASSERT_LT(start.get(), time_stamp.get());
+ ASSERT_LT(start.get(), time_stamp.get());
-// PRINTF("All good at WolfTestTimeStamp::TimeStampInitNow !\n");
+ // PRINTF("All good at WolfTestTimeStamp::TimeStampInitNow !\n");
}
TEST(WolfTestTimeStamp, TimeStampInitScalar)
{
- double val(101010);
+ double val(101010);
- wolf::TimeStamp start(val);
+ wolf::TimeStamp start(val);
- ASSERT_EQ(start.get(), val);
- ASSERT_EQ(start.getSeconds(), val);
- ASSERT_EQ(start.getNanoSeconds(), (unsigned int) 0);
+ ASSERT_EQ(start.get(), val);
+ ASSERT_EQ(start.getSeconds(), val);
+ ASSERT_EQ(start.getNanoSeconds(), (unsigned int)0);
- std::stringstream ss;
- start.print(ss);
+ std::stringstream ss;
+ start.print(ss);
- ASSERT_STREQ("101010.000000000", ss.str().c_str());
+ ASSERT_STREQ("101010.000000000", ss.str().c_str());
-// PRINTF("All good at WolfTestTimeStamp::TimeStampInitScalar !\n");
+ // PRINTF("All good at WolfTestTimeStamp::TimeStampInitScalar !\n");
}
TEST(WolfTestTimeStamp, TimeStampInitScalarSecNano)
{
- double sec(101010);
- double nano(202020);
- double val(101010.000202020);
+ double sec(101010);
+ double nano(202020);
+ double val(101010.000202020);
- wolf::TimeStamp start(sec, nano);
+ wolf::TimeStamp start(sec, nano);
- // start.get -> 101010.000202020004508
+ // start.get -> 101010.000202020004508
- ASSERT_EQ(start.get(), val);
- ASSERT_EQ(start.getSeconds(), sec);
- ASSERT_EQ(start.getNanoSeconds(), nano);
+ ASSERT_EQ(start.get(), val);
+ ASSERT_EQ(start.getSeconds(), sec);
+ ASSERT_EQ(start.getNanoSeconds(), nano);
- std::stringstream ss;
- start.print(ss);
+ std::stringstream ss;
+ start.print(ss);
- ASSERT_STREQ("101010.000202020", ss.str().c_str());
+ ASSERT_STREQ("101010.000202020", ss.str().c_str());
-// PRINTF("All good at WolfTestTimeStamp::TimeStampInitScalarSecNano !\n");
+ // PRINTF("All good at WolfTestTimeStamp::TimeStampInitScalarSecNano !\n");
}
TEST(WolfTestTimeStamp, TimeStampSetNow)
{
- wolf::TimeStamp t1;
- wolf::TimeStamp t2(t1);
+ wolf::TimeStamp t1;
+ wolf::TimeStamp t2(t1);
- ASSERT_EQ(t1,t2);
+ ASSERT_EQ(t1, t2);
- // If we don't sleep, start == time_stamp sometimes.
- // And sometimes start <= time_stamp ...
- std::this_thread::sleep_for(std::chrono::microseconds(1));
+ // If we don't sleep, start == time_stamp sometimes.
+ // And sometimes start <= time_stamp ...
+ std::this_thread::sleep_for(std::chrono::microseconds(1));
- t2.setToNow();
+ t2.setToNow();
- ASSERT_LT(t1,t2);
+ ASSERT_LT(t1, t2);
}
TEST(WolfTestTimeStamp, TimeStampSetScalar)
{
- double val(101010.000202020);
+ double val(101010.000202020);
- wolf::TimeStamp start;
- start.set(val);
+ wolf::TimeStamp start;
+ start.set(val);
- ASSERT_EQ(start.get(), val);
- ASSERT_EQ(start.getSeconds(), 101010);
- ASSERT_EQ(start.getNanoSeconds(), 202020);
+ ASSERT_EQ(start.get(), val);
+ ASSERT_EQ(start.getSeconds(), 101010);
+ ASSERT_EQ(start.getNanoSeconds(), 202020);
- std::stringstream ss;
- start.print(ss);
+ std::stringstream ss;
+ start.print(ss);
- ASSERT_STREQ("101010.000202020", ss.str().c_str());
+ ASSERT_STREQ("101010.000202020", ss.str().c_str());
}
TEST(WolfTestTimeStamp, TimeStampSetSecNano)
{
- unsigned long int sec(101010);
- unsigned long int nano(202020);
+ unsigned long int sec(101010);
+ unsigned long int nano(202020);
- wolf::TimeStamp start;
- start.set(sec,nano);
+ wolf::TimeStamp start;
+ start.set(sec, nano);
- // start.get -> 101010.000202020004508
+ // start.get -> 101010.000202020004508
- ASSERT_EQ(start.getSeconds(), sec);
- ASSERT_EQ(start.getNanoSeconds(), nano);
+ ASSERT_EQ(start.getSeconds(), sec);
+ ASSERT_EQ(start.getNanoSeconds(), nano);
}
TEST(WolfTestTimeStamp, TimeStampEquality)
{
- wolf::TimeStamp start;
+ wolf::TimeStamp start;
- wolf::TimeStamp time_stamp(start);
+ wolf::TimeStamp time_stamp(start);
- ASSERT_EQ(time_stamp, start);
+ ASSERT_EQ(time_stamp, start);
- ASSERT_EQ(time_stamp.get(), start.get());
+ ASSERT_EQ(time_stamp.get(), start.get());
- std::this_thread::sleep_for(std::chrono::microseconds(1));
- time_stamp.setToNow();
+ std::this_thread::sleep_for(std::chrono::microseconds(1));
+ time_stamp.setToNow();
- ASSERT_NE(time_stamp.get(), start.get());
+ ASSERT_NE(time_stamp.get(), start.get());
- time_stamp = start;
+ time_stamp = start;
- ASSERT_EQ(time_stamp.get(), start.get());
+ ASSERT_EQ(time_stamp.get(), start.get());
-// PRINTF("All good at WolfTestTimeStamp::TimeStampEquality !\n");
+ // PRINTF("All good at WolfTestTimeStamp::TimeStampEquality !\n");
}
TEST(WolfTestTimeStamp, TimeStampInequality)
{
- wolf::TimeStamp start = wolf::TimeStamp::Now();
+ wolf::TimeStamp start = wolf::TimeStamp::Now();
- std::this_thread::sleep_for(std::chrono::microseconds(1));
+ std::this_thread::sleep_for(std::chrono::microseconds(1));
- wolf::TimeStamp time_stamp = wolf::TimeStamp::Now();
+ wolf::TimeStamp time_stamp = wolf::TimeStamp::Now();
- // error: no match for ‘operator!=’
- //ASSERT_NE(time_stamp, start);
+ // error: no match for ‘operator!=’
+ // ASSERT_NE(time_stamp, start);
- ASSERT_LT(start, time_stamp);
- ASSERT_LE(start, time_stamp);
- ASSERT_LE(start, start);
+ ASSERT_LT(start, time_stamp);
+ ASSERT_LE(start, time_stamp);
+ ASSERT_LE(start, start);
- // error: no match for ‘operator>’
- ASSERT_GT(time_stamp, start);
- ASSERT_GE(time_stamp, start);
- ASSERT_GE(start, start);
+ // error: no match for ‘operator>’
+ ASSERT_GT(time_stamp, start);
+ ASSERT_GE(time_stamp, start);
+ ASSERT_GE(start, start);
-// PRINTF("All good at WolfTestTimeStamp::TimeStampInequality !\n");
+ // PRINTF("All good at WolfTestTimeStamp::TimeStampInequality !\n");
}
TEST(WolfTestTimeStamp, TimeStampSubstraction)
{
- wolf::TimeStamp t1;
- wolf::TimeStamp t2(t1);
- double dt(1e-5);
+ wolf::TimeStamp t1;
+ wolf::TimeStamp t2(t1);
+ double dt(1e-5);
- t2+=dt;
+ t2 += dt;
- ASSERT_LT(t1, t2);
- ASSERT_EQ(t2-t1, dt);
- ASSERT_EQ(t1-t2, -dt);
+ ASSERT_LT(t1, t2);
+ ASSERT_EQ(t2 - t1, dt);
+ ASSERT_EQ(t1 - t2, -dt);
}
TEST(WolfTestTimeStamp, TimeStampAdding)
{
- wolf::TimeStamp t1,t3;
- wolf::TimeStamp t2(t1);
- double dt(1e-5);
+ wolf::TimeStamp t1, t3;
+ wolf::TimeStamp t2(t1);
+ double dt(1e-5);
- t2 +=dt;
- t3 = t1+dt;
+ t2 += dt;
+ t3 = t1 + dt;
- ASSERT_EQ(t2, t3);
+ ASSERT_EQ(t2, t3);
}
TEST(WolfTestTimeStamp, TimeStampOperatorOstream)
{
wolf::TimeStamp t(5);
- double dt = 1e-4;
- t+=dt;
+ double dt = 1e-4;
+ t += dt;
std::ostringstream ss1, ss2;
t.print(ss1);
@@ -237,24 +237,24 @@ TEST(WolfTestTimeStamp, TimeStampOperatorOstream)
ASSERT_EQ(ss1.str(), ss2.str());
-// PRINTF("All good at WolfTestTimeStamp::TimeStampOperatorOstream !\n");
+ // PRINTF("All good at WolfTestTimeStamp::TimeStampOperatorOstream !\n");
}
TEST(WolfTestTimeStamp, TimeStampSecNanoSec)
{
- unsigned long int sec = 5;
+ unsigned long int sec = 5;
unsigned long int nano = 1e5;
- wolf::TimeStamp t1(double(sec)+double(nano)*1e-9);
- wolf::TimeStamp t2(sec,nano);
+ wolf::TimeStamp t1(double(sec) + double(nano) * 1e-9);
+ wolf::TimeStamp t2(sec, nano);
- ASSERT_EQ(t1.getSeconds(),sec);
- ASSERT_EQ(t2.getSeconds(),sec);
- ASSERT_EQ(t1.getNanoSeconds(),nano);
- ASSERT_EQ(t2.getNanoSeconds(),nano);
+ ASSERT_EQ(t1.getSeconds(), sec);
+ ASSERT_EQ(t2.getSeconds(), sec);
+ ASSERT_EQ(t1.getNanoSeconds(), nano);
+ ASSERT_EQ(t2.getNanoSeconds(), nano);
}
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_track_matrix.cpp b/test/gtest_track_matrix.cpp
index e3b10aee3cd3ee9c2fdf9d414c1fd3c3db12fb54..df47a4cf9eaad7acee4f39b5a606e25fe0c7f1cd 100644
--- a/test/gtest_track_matrix.cpp
+++ b/test/gtest_track_matrix.cpp
@@ -26,56 +26,56 @@ using namespace wolf;
class TrackMatrixTest : public testing::Test
{
- public:
- TrackMatrix track_matrix;
-
- Eigen::Vector2d m;
- Eigen::Matrix2d m_cov = Eigen::Matrix2d::Identity()*0.01;
-
- FrameBasePtr F0, F1, F2, F3, F4;
- CaptureBasePtr C0, C1, C2, C3, C4;
- FeatureBasePtr f0, f1, f2, f3, f4, f5;
- ProblemPtr problem;
-
- void SetUp() override
- {
- problem = Problem::create("PO", 2);
- // unlinked captures
- // Some warnings will be thrown "linking with nullptr" for emplacing without providing frame pointer
- C0 = CaptureBase::emplace<CaptureBase>(nullptr, "CaptureBase", 0.0);
- C1 = CaptureBase::emplace<CaptureBase>(nullptr, "CaptureBase", 1.0);
- C2 = CaptureBase::emplace<CaptureBase>(nullptr, "CaptureBase", 2.0);
- C3 = CaptureBase::emplace<CaptureBase>(nullptr, "CaptureBase", 3.0);
- C4 = CaptureBase::emplace<CaptureBase>(nullptr, "CaptureBase", 4.0);
-
- // unlinked frames
- // Some warnings will be thrown "linking with nullptr" for emplacing without providing frame pointer
- F0 = std::make_shared<FrameBase>(0.0, nullptr);
- F1 = std::make_shared<FrameBase>(1.0, nullptr);
- F2 = std::make_shared<FrameBase>(2.0, nullptr);
- F3 = std::make_shared<FrameBase>(3.0, nullptr);
- F4 = std::make_shared<FrameBase>(4.0, nullptr);
-
- // unlinked features
- // Some warnings will be thrown "linking with nullptr" for emplacing without providing capture pointer
- f0 = FeatureBase::emplace<FeatureBase>(nullptr, "FeatureBase", m, m_cov);
- f1 = FeatureBase::emplace<FeatureBase>(nullptr, "FeatureBase", m, m_cov);
- f2 = FeatureBase::emplace<FeatureBase>(nullptr, "FeatureBase", m, m_cov);
- f3 = FeatureBase::emplace<FeatureBase>(nullptr, "FeatureBase", m, m_cov);
- f4 = FeatureBase::emplace<FeatureBase>(nullptr, "FeatureBase", m, m_cov);
- f5 = FeatureBase::emplace<FeatureBase>(nullptr, "FeatureBase", m, m_cov);
-
- // F0 and F4 are keyframes
- F0->link(problem);
- F4->link(problem);
-
- // link captures
- C0->link(F0);
- C1->link(F1);
- C2->link(F2);
- C3->link(F3);
- C4->link(F4);
- }
+ public:
+ TrackMatrix track_matrix;
+
+ Eigen::Vector2d m;
+ Eigen::Matrix2d m_cov = Eigen::Matrix2d::Identity() * 0.01;
+
+ FrameBasePtr F0, F1, F2, F3, F4;
+ CaptureBasePtr C0, C1, C2, C3, C4;
+ FeatureBasePtr f0, f1, f2, f3, f4, f5;
+ ProblemPtr problem;
+
+ void SetUp() override
+ {
+ problem = Problem::create("PO", 2);
+ // unlinked captures
+ // Some warnings will be thrown "linking with nullptr" for emplacing without providing frame pointer
+ C0 = CaptureBase::emplace<CaptureBase>(nullptr, "CaptureBase", 0.0);
+ C1 = CaptureBase::emplace<CaptureBase>(nullptr, "CaptureBase", 1.0);
+ C2 = CaptureBase::emplace<CaptureBase>(nullptr, "CaptureBase", 2.0);
+ C3 = CaptureBase::emplace<CaptureBase>(nullptr, "CaptureBase", 3.0);
+ C4 = CaptureBase::emplace<CaptureBase>(nullptr, "CaptureBase", 4.0);
+
+ // unlinked frames
+ // Some warnings will be thrown "linking with nullptr" for emplacing without providing frame pointer
+ F0 = std::make_shared<FrameBase>(0.0, nullptr);
+ F1 = std::make_shared<FrameBase>(1.0, nullptr);
+ F2 = std::make_shared<FrameBase>(2.0, nullptr);
+ F3 = std::make_shared<FrameBase>(3.0, nullptr);
+ F4 = std::make_shared<FrameBase>(4.0, nullptr);
+
+ // unlinked features
+ // Some warnings will be thrown "linking with nullptr" for emplacing without providing capture pointer
+ f0 = FeatureBase::emplace<FeatureBase>(nullptr, "FeatureBase", m, m_cov);
+ f1 = FeatureBase::emplace<FeatureBase>(nullptr, "FeatureBase", m, m_cov);
+ f2 = FeatureBase::emplace<FeatureBase>(nullptr, "FeatureBase", m, m_cov);
+ f3 = FeatureBase::emplace<FeatureBase>(nullptr, "FeatureBase", m, m_cov);
+ f4 = FeatureBase::emplace<FeatureBase>(nullptr, "FeatureBase", m, m_cov);
+ f5 = FeatureBase::emplace<FeatureBase>(nullptr, "FeatureBase", m, m_cov);
+
+ // F0 and F4 are keyframes
+ F0->link(problem);
+ F4->link(problem);
+
+ // link captures
+ C0->link(F0);
+ C1->link(F1);
+ C2->link(F2);
+ C3->link(F3);
+ C4->link(F4);
+ }
};
TEST_F(TrackMatrixTest, newTrack)
@@ -86,16 +86,16 @@ TEST_F(TrackMatrixTest, newTrack)
f3->link(C1);
track_matrix.newTrack(f0);
- ASSERT_EQ(track_matrix.numTracks(), (unsigned int) 1);
+ ASSERT_EQ(track_matrix.numTracks(), (unsigned int)1);
track_matrix.newTrack(f1);
- ASSERT_EQ(track_matrix.numTracks(), (unsigned int) 2);
+ ASSERT_EQ(track_matrix.numTracks(), (unsigned int)2);
track_matrix.newTrack(f2);
- ASSERT_EQ(track_matrix.numTracks(), (unsigned int) 3);
+ ASSERT_EQ(track_matrix.numTracks(), (unsigned int)3);
track_matrix.newTrack(f3);
- ASSERT_EQ(track_matrix.numTracks(), (unsigned int) 4);
+ ASSERT_EQ(track_matrix.numTracks(), (unsigned int)4);
}
TEST_F(TrackMatrixTest, add)
@@ -105,23 +105,23 @@ TEST_F(TrackMatrixTest, add)
f2->link(C2);
track_matrix.newTrack(f0);
- ASSERT_EQ(track_matrix.numTracks(), (unsigned int) 1);
+ ASSERT_EQ(track_matrix.numTracks(), (unsigned int)1);
track_matrix.add(f0->trackId(), f1);
/* KC0 C1 snapshots
*
* f0---f1 trk 0
*/
- ASSERT_EQ(track_matrix.trackSize(f1->trackId()), (unsigned int) 2);
+ ASSERT_EQ(track_matrix.trackSize(f1->trackId()), (unsigned int)2);
ASSERT_EQ(f1->trackId(), f0->trackId());
- ASSERT_EQ(track_matrix.numTracks(), (unsigned int) 1);
+ ASSERT_EQ(track_matrix.numTracks(), (unsigned int)1);
track_matrix.add(f0->trackId(), f2);
/* KC0 C1 C2 snapshots
*
* f0---f1---f2 trk 0
*/
- ASSERT_EQ(track_matrix.trackSize(f2->trackId()), (unsigned int) 3);
+ ASSERT_EQ(track_matrix.trackSize(f2->trackId()), (unsigned int)3);
ASSERT_EQ(f2->trackId(), f0->trackId());
}
@@ -139,7 +139,7 @@ TEST_F(TrackMatrixTest, add2)
*
* f0---f1 trk 0
*/
- ASSERT_EQ(track_matrix.trackSize(f1->trackId()), (unsigned int) 2);
+ ASSERT_EQ(track_matrix.trackSize(f1->trackId()), (unsigned int)2);
ASSERT_EQ(f1->trackId(), f0->trackId());
track_matrix.add(f1, f2);
@@ -147,9 +147,9 @@ TEST_F(TrackMatrixTest, add2)
*
* f0---f1---f2 trk 0
*/
- ASSERT_EQ(track_matrix.trackSize(f2->trackId()), (unsigned int) 3);
+ ASSERT_EQ(track_matrix.trackSize(f2->trackId()), (unsigned int)3);
ASSERT_EQ(f2->trackId(), f0->trackId());
- ASSERT_EQ(track_matrix.numTracks(), (unsigned int) 1);
+ ASSERT_EQ(track_matrix.numTracks(), (unsigned int)1);
track_matrix.newTrack(f3);
/* KC0 C1 C2 snapshots
@@ -157,9 +157,9 @@ TEST_F(TrackMatrixTest, add2)
* f0---f1---f2 trk 0
* f3 trk 1
*/
- ASSERT_EQ(track_matrix.trackSize(f3->trackId()), (unsigned int) 1);
+ ASSERT_EQ(track_matrix.trackSize(f3->trackId()), (unsigned int)1);
ASSERT_NE(f3->trackId(), f0->trackId());
- ASSERT_EQ(track_matrix.numTracks(), (unsigned int) 2);
+ ASSERT_EQ(track_matrix.numTracks(), (unsigned int)2);
}
TEST_F(TrackMatrixTest, numTracks)
@@ -176,11 +176,11 @@ TEST_F(TrackMatrixTest, numTracks)
* f0---f1 trk 0
*/
- ASSERT_EQ(track_matrix.numTracks(), (unsigned int) 1);
+ ASSERT_EQ(track_matrix.numTracks(), (unsigned int)1);
track_matrix.add(f0->trackId(), f2);
- ASSERT_EQ(track_matrix.numTracks(), (unsigned int) 1);
+ ASSERT_EQ(track_matrix.numTracks(), (unsigned int)1);
}
TEST_F(TrackMatrixTest, trackSize)
@@ -200,8 +200,8 @@ TEST_F(TrackMatrixTest, trackSize)
* f2 trk 1
*/
- ASSERT_EQ(track_matrix.trackSize(f0->trackId()), (unsigned int) 2);
- ASSERT_EQ(track_matrix.trackSize(f2->trackId()), (unsigned int) 1);
+ ASSERT_EQ(track_matrix.trackSize(f0->trackId()), (unsigned int)2);
+ ASSERT_EQ(track_matrix.trackSize(f2->trackId()), (unsigned int)1);
}
TEST_F(TrackMatrixTest, first_last_Feature)
@@ -222,10 +222,10 @@ TEST_F(TrackMatrixTest, first_last_Feature)
track_matrix.newTrack(f2);
ASSERT_EQ(track_matrix.firstFeature(f0->trackId()), f0);
- ASSERT_EQ(track_matrix.lastFeature (f0->trackId()), f1);
- ASSERT_EQ(track_matrix.feature (f0->trackId(), C0 ), f0);
- ASSERT_EQ(track_matrix.feature (f0->trackId(), C1 ), f1);
- ASSERT_EQ(track_matrix.feature (f2->trackId(), C1 ), f2);
+ ASSERT_EQ(track_matrix.lastFeature(f0->trackId()), f1);
+ ASSERT_EQ(track_matrix.feature(f0->trackId(), C0), f0);
+ ASSERT_EQ(track_matrix.feature(f0->trackId(), C1), f1);
+ ASSERT_EQ(track_matrix.feature(f2->trackId(), C1), f2);
}
TEST_F(TrackMatrixTest, remove_ftr)
@@ -245,7 +245,7 @@ TEST_F(TrackMatrixTest, remove_ftr)
* f2 trk 1
*/
- ASSERT_EQ(track_matrix.numTracks(), (unsigned int) 2);
+ ASSERT_EQ(track_matrix.numTracks(), (unsigned int)2);
track_matrix.remove(f0);
/* C0 C1 C2 snapshots
@@ -254,12 +254,12 @@ TEST_F(TrackMatrixTest, remove_ftr)
*
* f2 trk 1
*/
- ASSERT_EQ(track_matrix.numTracks(), (unsigned int) 2);
+ ASSERT_EQ(track_matrix.numTracks(), (unsigned int)2);
ASSERT_EQ(track_matrix.firstFeature(f0->trackId()), f1);
ASSERT_EQ(track_matrix.firstFeature(f2->trackId()), f2);
- ASSERT_EQ(track_matrix.snapshot(C0).size(), (unsigned int) 1);
+ ASSERT_EQ(track_matrix.snapshot(C0).size(), (unsigned int)1);
ASSERT_EQ(track_matrix.snapshot(C0).at(f2->trackId()), f2);
- ASSERT_EQ(track_matrix.snapshot(C1).size(), (unsigned int) 1);
+ ASSERT_EQ(track_matrix.snapshot(C1).size(), (unsigned int)1);
ASSERT_EQ(track_matrix.snapshot(C1).at(f0->trackId()), f1);
track_matrix.remove(f1);
@@ -267,14 +267,14 @@ TEST_F(TrackMatrixTest, remove_ftr)
*
* f2 trk 1
*/
- ASSERT_EQ(track_matrix.numTracks(), (unsigned int) 1);
+ ASSERT_EQ(track_matrix.numTracks(), (unsigned int)1);
ASSERT_EQ(track_matrix.firstFeature(f2->trackId()), f2);
track_matrix.remove(f2);
/* C0 C1 C2 snapshots
*
*/
- ASSERT_EQ(track_matrix.numTracks(), (unsigned int) 0);
+ ASSERT_EQ(track_matrix.numTracks(), (unsigned int)0);
}
TEST_F(TrackMatrixTest, remove_trk)
@@ -294,14 +294,14 @@ TEST_F(TrackMatrixTest, remove_trk)
* f2 trk 1
*/
- ASSERT_EQ(track_matrix.numTracks(), (unsigned int) 2);
+ ASSERT_EQ(track_matrix.numTracks(), (unsigned int)2);
track_matrix.remove(f0->trackId());
- ASSERT_EQ(track_matrix.numTracks(), (unsigned int) 1);
+ ASSERT_EQ(track_matrix.numTracks(), (unsigned int)1);
ASSERT_EQ(track_matrix.firstFeature(f2->trackId()), f2);
track_matrix.remove(f2->trackId());
- ASSERT_EQ(track_matrix.numTracks(), (unsigned int) 0);
+ ASSERT_EQ(track_matrix.numTracks(), (unsigned int)0);
}
TEST_F(TrackMatrixTest, remove_snapshot)
@@ -321,18 +321,18 @@ TEST_F(TrackMatrixTest, remove_snapshot)
* f2 trk 1
*/
- ASSERT_EQ(track_matrix.numTracks(), (unsigned int) 2);
+ ASSERT_EQ(track_matrix.numTracks(), (unsigned int)2);
track_matrix.remove(C0);
/* C1 C2 snapshots
*
* f1 trk 0
*/
- ASSERT_EQ(track_matrix.numTracks(), (unsigned int) 1);
+ ASSERT_EQ(track_matrix.numTracks(), (unsigned int)1);
ASSERT_EQ(track_matrix.firstFeature(f1->trackId()), f1);
track_matrix.remove(C1);
- ASSERT_EQ(track_matrix.numTracks(), (unsigned int) 0);
+ ASSERT_EQ(track_matrix.numTracks(), (unsigned int)0);
}
TEST_F(TrackMatrixTest, track)
@@ -355,11 +355,11 @@ TEST_F(TrackMatrixTest, track)
Track t0 = track_matrix.track(f0->trackId());
Track t2 = track_matrix.track(f2->trackId());
- ASSERT_EQ(t0.size(), (unsigned int) 2);
+ ASSERT_EQ(t0.size(), (unsigned int)2);
ASSERT_EQ(t0.at(C0->getTimeStamp()), f0);
ASSERT_EQ(t0.at(C1->getTimeStamp()), f1);
- ASSERT_EQ(t2.size(), (unsigned int) 1);
+ ASSERT_EQ(t2.size(), (unsigned int)1);
ASSERT_EQ(t2.at(C0->getTimeStamp()), f2);
}
@@ -383,11 +383,11 @@ TEST_F(TrackMatrixTest, snapshot)
Snapshot s0 = track_matrix.snapshot(C0);
Snapshot s1 = track_matrix.snapshot(C1);
- ASSERT_EQ(s0.size(), (unsigned int) 2);
+ ASSERT_EQ(s0.size(), (unsigned int)2);
ASSERT_EQ(s0.at(f0->trackId()), f0);
ASSERT_EQ(s0.at(f2->trackId()), f2);
- ASSERT_EQ(s1.size(), (unsigned int) 1);
+ ASSERT_EQ(s1.size(), (unsigned int)1);
ASSERT_EQ(s1.at(f1->trackId()), f1);
}
@@ -408,9 +408,9 @@ TEST_F(TrackMatrixTest, trackAsVector)
* f2 trk 1
*/
- std::vector<FeatureBasePtr> vt0 = track_matrix.trackAsVector(f0->trackId()); // get track 0 as vector
+ std::vector<FeatureBasePtr> vt0 = track_matrix.trackAsVector(f0->trackId()); // get track 0 as vector
- ASSERT_EQ(vt0.size(), (unsigned int) 2);
+ ASSERT_EQ(vt0.size(), (unsigned int)2);
ASSERT_EQ(vt0[0], f0);
ASSERT_EQ(vt0.at(1), f1);
}
@@ -432,11 +432,11 @@ TEST_F(TrackMatrixTest, snapshotAsList)
* f2 trk 1
*/
- std::list<FeatureBasePtr> lt0 = track_matrix.snapshotAsList(f0->getCapture()); // get track 0 as vector
+ std::list<FeatureBasePtr> lt0 = track_matrix.snapshotAsList(f0->getCapture()); // get track 0 as vector
- ASSERT_EQ(lt0.size() , (unsigned int) 2);
+ ASSERT_EQ(lt0.size(), (unsigned int)2);
ASSERT_EQ(lt0.front(), f0);
- ASSERT_EQ(lt0.back() , f2);
+ ASSERT_EQ(lt0.back(), f2);
}
TEST_F(TrackMatrixTest, matches)
@@ -462,13 +462,13 @@ TEST_F(TrackMatrixTest, matches)
TrackMatches pairs = track_matrix.matches(C0, C2);
- ASSERT_EQ(pairs.size(), (unsigned int) 1);
+ ASSERT_EQ(pairs.size(), (unsigned int)1);
ASSERT_EQ(pairs.at(f0->trackId()).first, f0);
ASSERT_EQ(pairs.at(f0->trackId()).second, f2);
pairs = track_matrix.matches(C2, C3);
- ASSERT_EQ(pairs.size(), (unsigned int) 0);
+ ASSERT_EQ(pairs.size(), (unsigned int)0);
}
TEST_F(TrackMatrixTest, trackAtKeyframes)
@@ -515,7 +515,7 @@ TEST_F(TrackMatrixTest, trackIds)
track_matrix.newTrack(f3);
track_matrix.add(f0->trackId(), f4);
track_matrix.add(f2->trackId(), f5);
-
+
/* KC0 C1 C2
*
* f0---f1---f4 trk 0
@@ -525,16 +525,14 @@ TEST_F(TrackMatrixTest, trackIds)
* f3 trk 2
*/
- ASSERT_EQ(track_matrix.trackIds().size(), 3);
+ ASSERT_EQ(track_matrix.trackIds().size(), 3);
ASSERT_EQ(track_matrix.trackIds(C0).size(), 1);
ASSERT_EQ(track_matrix.trackIds(C1).size(), 3);
ASSERT_EQ(track_matrix.trackIds(C2).size(), 2);
}
-
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}
-
diff --git a/test/gtest_trajectory.cpp b/test/gtest_trajectory.cpp
index d47b7b8991b78675aaa02d322dc891f48ca0c775..d8c4055c749754892e7b5daddb720a82d2b095f9 100644
--- a/test/gtest_trajectory.cpp
+++ b/test/gtest_trajectory.cpp
@@ -17,21 +17,14 @@
//
// 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/>.
-/*
- * gtest_trajectory.cpp
- *
- * Created on: Nov 13, 2016
- * Author: jsola
- */
#include "core/utils/utils_gtest.h"
-
#include "core/problem/problem.h"
#include "core/trajectory/trajectory_base.h"
#include "core/frame/frame_base.h"
#include "core/solver/solver_manager.h"
-#include "dummy/solver_manager_dummy.h"
+#include "dummy/solver_dummy.h"
#include <iostream>
@@ -40,10 +33,11 @@ using namespace wolf;
/// Set to true if you want debug info
bool debug = false;
+std::string wolf_dir = _WOLF_CODE_DIR;
+
TEST(TrajectoryBase, ClosestKeyFrame)
{
-
- ProblemPtr P = Problem::create("PO", 2);
+ ProblemPtr P = Problem::create("PO", 2);
TrajectoryBasePtr T = P->getTrajectory();
// Trajectory status:
@@ -51,44 +45,42 @@ TEST(TrajectoryBase, ClosestKeyFrame)
// 1 2 3 4 time stamps
// --+-----+-----+-----+---> time
- FrameBasePtr F1 = P->emplaceFrame(1, "PO", Eigen::Vector3d::Zero() );
- FrameBasePtr F2 = P->emplaceFrame(2, "PO", Eigen::Vector3d::Zero() );
- FrameBasePtr F3 = std::make_shared<FrameBase>(3,
- P->getFrameTypes(),
- VectorComposite("PO", {Eigen::Vector2d::Zero(),Eigen::Vector1d::Zero()}) );
- FrameBasePtr F4 = std::make_shared<FrameBase>(4,
- P->getFrameTypes(),
- VectorComposite("PO", {Eigen::Vector2d::Zero(),Eigen::Vector1d::Zero()}) );
+ FrameBasePtr F1 = P->emplaceFrame(1, "PO", Eigen::Vector3d::Zero());
+ FrameBasePtr F2 = P->emplaceFrame(2, "PO", Eigen::Vector3d::Zero());
+ FrameBasePtr F3 = std::make_shared<FrameBase>(
+ 3, P->getFrameTypes(), VectorComposite("PO", {Eigen::Vector2d::Zero(), Eigen::Vector1d::Zero()}));
+ FrameBasePtr F4 = std::make_shared<FrameBase>(
+ 4, P->getFrameTypes(), VectorComposite("PO", {Eigen::Vector2d::Zero(), Eigen::Vector1d::Zero()}));
- FrameBasePtr KF; // closest key-frame queried
+ FrameBasePtr KF; // closest key-frame queried
- KF = T->closestFrameToTimeStamp(-0.8); // before all keyframes --> return F1
- ASSERT_EQ(KF->id(), F1->id()); // same id!
+ KF = T->closestFrameToTimeStamp(-0.8); // before all keyframes --> return F1
+ ASSERT_EQ(KF->id(), F1->id()); // same id!
- KF = T->closestFrameToTimeStamp(1.1); // between keyframes --> return F1
- ASSERT_EQ(KF->id(), F1->id()); // same id!
+ KF = T->closestFrameToTimeStamp(1.1); // between keyframes --> return F1
+ ASSERT_EQ(KF->id(), F1->id()); // same id!
- KF = T->closestFrameToTimeStamp(1.9); // between keyframes --> return F2
- ASSERT_EQ(KF->id(), F2->id()); // same id!
+ KF = T->closestFrameToTimeStamp(1.9); // between keyframes --> return F2
+ ASSERT_EQ(KF->id(), F2->id()); // same id!
- KF = T->closestFrameToTimeStamp(2.6); // between keyframe and auxiliary frame, but closer to auxiliary frame --> return F2
- ASSERT_EQ(KF->id(), F2->id()); // same id!
+ KF = T->closestFrameToTimeStamp(
+ 2.6); // between keyframe and auxiliary frame, but closer to auxiliary frame --> return F2
+ ASSERT_EQ(KF->id(), F2->id()); // same id!
- KF = T->closestFrameToTimeStamp(3.2); // after the auxiliary frame, between closer to frame --> return F2
- ASSERT_EQ(KF->id(), F2->id()); // same id!
+ KF = T->closestFrameToTimeStamp(3.2); // after the auxiliary frame, between closer to frame --> return F2
+ ASSERT_EQ(KF->id(), F2->id()); // same id!
- KF = T->closestFrameToTimeStamp(4.2); // after the last frame --> return F2
- ASSERT_EQ(KF->id(), F2->id()); // same id!
+ KF = T->closestFrameToTimeStamp(4.2); // after the last frame --> return F2
+ ASSERT_EQ(KF->id(), F2->id()); // same id!
}
TEST(TrajectoryBase, Add_Remove_Frame)
{
using std::make_shared;
- ProblemPtr P = Problem::create("PO", 2);
+ ProblemPtr P = Problem::create("PO", 2);
TrajectoryBasePtr T = P->getTrajectory();
-
- SolverManagerDummy N(P);
+ SolverManagerPtr N = SolverDummy::create(P, wolf_dir + "/test/yaml/solver_dummy.yaml", {wolf_dir});
// Trajectory status:
// KF1 KF2 F3 frames
@@ -98,66 +90,66 @@ TEST(TrajectoryBase, Add_Remove_Frame)
std::cout << __LINE__ << std::endl;
// add F1
- FrameBasePtr F1 = P->emplaceFrame(1, "PO", Eigen::Vector3d::Zero()); // 2 non-fixed
- if (debug) P->print(2,0,0,0);
- ASSERT_EQ(T->getFrameMap(). size(), (SizeStd) 1);
- ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd) 2);
+ FrameBasePtr F1 = P->emplaceFrame(1, "PO", Eigen::Vector3d::Zero()); // 2 non-fixed
+ if (debug) P->print(2, 0, 0, 0);
+ ASSERT_EQ(T->getFrameMap().size(), (SizeStd)1);
+ ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd)2);
std::cout << __LINE__ << std::endl;
// add F2
- FrameBasePtr F2 = P->emplaceFrame(2, "PO", Eigen::Vector3d::Zero()); // 1 fixed, 1 not
- if (debug) P->print(2,0,0,0);
- ASSERT_EQ(T->getFrameMap(). size(), (SizeStd) 2);
- ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd) 4);
+ FrameBasePtr F2 = P->emplaceFrame(2, "PO", Eigen::Vector3d::Zero()); // 1 fixed, 1 not
+ if (debug) P->print(2, 0, 0, 0);
+ ASSERT_EQ(T->getFrameMap().size(), (SizeStd)2);
+ ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd)4);
std::cout << __LINE__ << std::endl;
// add F3
- FrameBasePtr F3 = std::make_shared<FrameBase>(3,
- P->getFrameTypes(),
- VectorComposite("PO", {Eigen::Vector2d::Zero(),Eigen::Vector1d::Zero()}));
- if (debug) P->print(2,0,0,0);
- ASSERT_EQ(T->getFrameMap(). size(), (SizeStd) 2);
- ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd) 4);
+ FrameBasePtr F3 = std::make_shared<FrameBase>(
+ 3, P->getFrameTypes(), VectorComposite("PO", {Eigen::Vector2d::Zero(), Eigen::Vector1d::Zero()}));
+ if (debug) P->print(2, 0, 0, 0);
+ ASSERT_EQ(T->getFrameMap().size(), (SizeStd)2);
+ ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd)4);
std::cout << __LINE__ << std::endl;
ASSERT_EQ(T->getLastFrame()->id(), F2->id());
std::cout << __LINE__ << std::endl;
- N.update();
- ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd) 0); // consumeStateBlockNotificationMap was called in update() so it should be empty
+ N->update();
+ ASSERT_EQ(P->getStateBlockNotificationMapSize(),
+ (SizeStd)0); // consumeStateBlockNotificationMap was called in update() so it should be empty
std::cout << __LINE__ << std::endl;
// remove frames and keyframes
- F2->remove(); // KF
- if (debug) P->print(2,0,0,0);
- ASSERT_EQ(T->getFrameMap(). size(), (SizeStd) 1);
- ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd) 2);
+ F2->remove(); // KF
+ if (debug) P->print(2, 0, 0, 0);
+ ASSERT_EQ(T->getFrameMap().size(), (SizeStd)1);
+ ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd)2);
std::cout << __LINE__ << std::endl;
ASSERT_EQ(T->getLastFrame()->id(), F1->id());
std::cout << __LINE__ << std::endl;
- F3->remove(); // F
- if (debug) P->print(2,0,0,0);
- ASSERT_EQ(T->getFrameMap(). size(), (SizeStd) 1);
+ F3->remove(); // F
+ if (debug) P->print(2, 0, 0, 0);
+ ASSERT_EQ(T->getFrameMap().size(), (SizeStd)1);
std::cout << __LINE__ << std::endl;
ASSERT_EQ(T->getLastFrame()->id(), F1->id());
- F1->remove(); // KF
- if (debug) P->print(2,0,0,0);
- ASSERT_EQ(T->getFrameMap(). size(), (SizeStd) 0);
+ F1->remove(); // KF
+ if (debug) P->print(2, 0, 0, 0);
+ ASSERT_EQ(T->getFrameMap().size(), (SizeStd)0);
std::cout << __LINE__ << std::endl;
- N.update();
+ N->update();
- ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd) 0); // consumeStateBlockNotificationMap was called in update() so it should be empty
+ ASSERT_EQ(P->getStateBlockNotificationMapSize(),
+ (SizeStd)0); // consumeStateBlockNotificationMap was called in update() so it should be empty
std::cout << __LINE__ << std::endl;
}
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_tree_manager.cpp b/test/gtest_tree_manager.cpp
index cfcd59307d86bdbe0a08609c7fa7c76207ce2931..f317a40fc4f22450c78ec71eb6773e7b142e295d 100644
--- a/test/gtest_tree_manager.cpp
+++ b/test/gtest_tree_manager.cpp
@@ -29,86 +29,69 @@ using namespace Eigen;
std::string wolf_dir = _WOLF_CODE_DIR;
-TEST(TreeManager, make_shared)
-{
- ProblemPtr P = Problem::create("PO", 2);
-
- auto ParamsGM = std::make_shared<ParamsTreeManagerBase>();
-
- auto GM = std::make_shared<TreeManagerDummy>(ParamsGM);
-
- P->setTreeManager(GM);
-
- ASSERT_EQ(std::static_pointer_cast<TreeManagerDummy>(P->getTreeManager())->n_KF_, 0);
- ASSERT_EQ(P->getTreeManager(), GM);
-}
-
TEST(TreeManager, createNode)
{
ProblemPtr P = Problem::create("PO", 2);
- auto yaml_server = yaml_schema_cpp::YamlServer({wolf_dir},
- wolf_dir + "/test/yaml/tree_manager_dummy.yaml");
-
+ auto yaml_server = yaml_schema_cpp::YamlServer({wolf_dir}, wolf_dir + "/test/yaml/tree_manager_dummy.yaml");
+
WOLF_INFO_COND(not yaml_server.applySchema("TreeManagerDummy"), yaml_server.getLog());
ASSERT_TRUE(yaml_server.applySchema("TreeManagerDummy"));
- auto GM = TreeManagerDummy::create(yaml_server.getNode());
+ auto TM = TreeManagerDummy::create(yaml_server.getNode());
- ASSERT_TRUE(std::dynamic_pointer_cast<TreeManagerDummy>(GM) != nullptr);
+ ASSERT_TRUE(std::dynamic_pointer_cast<TreeManagerDummy>(TM) != nullptr);
- P->setTreeManager(GM);
+ P->setTreeManager(TM);
ASSERT_EQ(std::static_pointer_cast<TreeManagerDummy>(P->getTreeManager())->n_KF_, 0);
- ASSERT_EQ(P->getTreeManager(), GM);
+ ASSERT_EQ(P->getTreeManager(), TM);
}
TEST(TreeManager, createYaml)
{
ProblemPtr P = Problem::create("PO", 2);
- auto GM = TreeManagerDummy::create(wolf_dir + "/test/yaml/tree_manager_dummy.yaml", {wolf_dir});
+ auto TM = TreeManagerDummy::create(wolf_dir + "/test/yaml/tree_manager_dummy.yaml", {wolf_dir});
- ASSERT_TRUE(std::dynamic_pointer_cast<TreeManagerDummy>(GM) != nullptr);
+ ASSERT_TRUE(std::dynamic_pointer_cast<TreeManagerDummy>(TM) != nullptr);
- P->setTreeManager(GM);
+ P->setTreeManager(TM);
ASSERT_EQ(std::static_pointer_cast<TreeManagerDummy>(P->getTreeManager())->n_KF_, 0);
- ASSERT_EQ(P->getTreeManager(), GM);
+ ASSERT_EQ(P->getTreeManager(), TM);
}
-TEST(TreeManager, FactoryParam)
+TEST(TreeManager, FactoryNode)
{
ProblemPtr P = Problem::create("PO", 2);
- auto yaml_server = yaml_schema_cpp::YamlServer({wolf_dir},
- wolf_dir + "/test/yaml/tree_manager_dummy.yaml");
+ auto yaml_server = yaml_schema_cpp::YamlServer({wolf_dir}, wolf_dir + "/test/yaml/tree_manager_dummy.yaml");
ASSERT_TRUE(yaml_server.applySchema("TreeManagerDummy"));
- auto GM = FactoryTreeManager::create("TreeManagerDummy", yaml_server.getNode());
+ auto TM = FactoryTreeManagerNode::create("TreeManagerDummy", yaml_server.getNode(), {});
- ASSERT_TRUE(std::dynamic_pointer_cast<TreeManagerDummy>(GM) != nullptr);
+ ASSERT_TRUE(std::dynamic_pointer_cast<TreeManagerDummy>(TM) != nullptr);
- P->setTreeManager(GM);
+ P->setTreeManager(TM);
ASSERT_EQ(std::static_pointer_cast<TreeManagerDummy>(P->getTreeManager())->n_KF_, 0);
- ASSERT_EQ(P->getTreeManager(), GM);
+ ASSERT_EQ(P->getTreeManager(), TM);
}
TEST(TreeManager, FactoryYaml)
{
ProblemPtr P = Problem::create("PO", 2);
- auto GM = FactoryTreeManagerYaml::create("TreeManagerDummy",
- wolf_dir + "/test/yaml/tree_manager_dummy.yaml",
- {wolf_dir});
+ auto TM = FactoryTreeManagerFile::create(
+ "TreeManagerDummy", wolf_dir + "/test/yaml/tree_manager_dummy.yaml", {wolf_dir});
- ASSERT_TRUE(std::dynamic_pointer_cast<TreeManagerDummy>(GM) != nullptr);
+ ASSERT_TRUE(std::dynamic_pointer_cast<TreeManagerDummy>(TM) != nullptr);
- P->setTreeManager(GM);
+ P->setTreeManager(TM);
ASSERT_EQ(std::static_pointer_cast<TreeManagerDummy>(P->getTreeManager())->n_KF_, 0);
- ASSERT_EQ(P->getTreeManager(), GM);
+ ASSERT_EQ(P->getTreeManager(), TM);
}
TEST(TreeManager, autoConf)
@@ -117,7 +100,7 @@ TEST(TreeManager, autoConf)
P->applyPriorOptions(0);
ASSERT_TRUE(std::dynamic_pointer_cast<TreeManagerDummy>(P->getTreeManager()) != nullptr);
- ASSERT_EQ(std::static_pointer_cast<TreeManagerDummy>(P->getTreeManager())->n_KF_, 1); // prior KF
+ ASSERT_EQ(std::static_pointer_cast<TreeManagerDummy>(P->getTreeManager())->n_KF_, 1); // prior KF
}
TEST(TreeManager, autoConfNone)
@@ -125,30 +108,30 @@ TEST(TreeManager, autoConfNone)
ProblemPtr P = Problem::autoSetup(wolf_dir + "/test/yaml/problem_tree_manager2.yaml", {wolf_dir});
P->applyPriorOptions(0);
- ASSERT_TRUE(P->getTreeManager() == nullptr); // params_tree_manager2.yaml problem/tree_manager/type: None
+ ASSERT_TRUE(P->getTreeManager() == nullptr); // params_tree_manager2.yaml problem/tree_manager/type: None
}
TEST(TreeManager, keyFrameCallback)
{
ProblemPtr P = Problem::create("PO", 3);
- auto ParamsGM = std::make_shared<ParamsTreeManagerBase>();
-
- auto GM = std::make_shared<TreeManagerDummy>(ParamsGM);
+ auto TMD = std::static_pointer_cast<TreeManagerDummy>(
+ TreeManagerDummy::create(wolf_dir + "/test/yaml/tree_manager_dummy.yaml", {wolf_dir}));
- P->setTreeManager(GM);
+ P->setTreeManager(TMD);
- ASSERT_EQ(GM->n_KF_, 0);
+ ASSERT_EQ(TMD->n_KF_, 0);
- Vector7d x; x << 1,2,3,0,0,0,1;
- auto F0 = P->emplaceFrame(0, "PO", x );
+ Vector7d x;
+ x << 1, 2, 3, 0, 0, 0, 1;
+ auto F0 = P->emplaceFrame(0, "PO", x);
P->keyFrameCallback(F0, nullptr);
- ASSERT_EQ(GM->n_KF_, 1);
+ ASSERT_EQ(TMD->n_KF_, 1);
}
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_tree_manager_sliding_window.cpp b/test/gtest_tree_manager_sliding_window.cpp
index 074f3f1c3f2a972aed266160cd49cffc203d4ea3..0abcfdf232ffc716cc88f892fc868d7b2b4c8c9f 100644
--- a/test/gtest_tree_manager_sliding_window.cpp
+++ b/test/gtest_tree_manager_sliding_window.cpp
@@ -34,19 +34,6 @@ using namespace yaml_schema_cpp;
std::string wolf_dir = _WOLF_CODE_DIR;
-TEST(TreeManagerSlidingWindow, make_shared)
-{
- ProblemPtr P = Problem::create("PO", 2);
-
- auto ParamsGM = std::make_shared<ParamsTreeManagerSlidingWindow>();
-
- auto GM = std::make_shared<TreeManagerSlidingWindow>(ParamsGM);
-
- P->setTreeManager(GM);
-
- EXPECT_EQ(P->getTreeManager(), GM);
-}
-
TEST(TreeManagerSlidingWindow, createNode)
{
ProblemPtr P = Problem::create("PO", 2);
@@ -55,32 +42,31 @@ TEST(TreeManagerSlidingWindow, createNode)
ASSERT_TRUE(server.applySchema("TreeManagerSlidingWindow"));
- auto GM = TreeManagerSlidingWindow::create(server.getNode());
+ auto TM = TreeManagerSlidingWindow::create(server.getNode());
- EXPECT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindow>(GM) != nullptr);
+ EXPECT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindow>(TM) != nullptr);
- P->setTreeManager(GM);
+ P->setTreeManager(TM);
EXPECT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindow>(P->getTreeManager()) != nullptr);
- EXPECT_EQ(P->getTreeManager(), GM);
+ EXPECT_EQ(P->getTreeManager(), TM);
}
TEST(TreeManagerSlidingWindow, createYaml)
{
ProblemPtr P = Problem::create("PO", 2);
- auto GM =
- TreeManagerSlidingWindow::create(wolf_dir + "/test/yaml/tree_manager_sliding_window1.yaml", {wolf_dir});
+ auto TM = TreeManagerSlidingWindow::create(wolf_dir + "/test/yaml/tree_manager_sliding_window1.yaml", {wolf_dir});
- EXPECT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindow>(GM) != nullptr);
+ EXPECT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindow>(TM) != nullptr);
- P->setTreeManager(GM);
+ P->setTreeManager(TM);
EXPECT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindow>(P->getTreeManager()) != nullptr);
- EXPECT_EQ(P->getTreeManager(), GM);
+ EXPECT_EQ(P->getTreeManager(), TM);
}
-TEST(TreeManager, Factory)
+TEST(TreeManager, FactoryNode)
{
ProblemPtr P = Problem::create("PO", 2);
@@ -88,29 +74,29 @@ TEST(TreeManager, Factory)
ASSERT_TRUE(server.applySchema("TreeManagerSlidingWindow"));
- auto GM = FactoryTreeManager::create("TreeManagerSlidingWindow", server.getNode());
+ auto TM = FactoryTreeManagerNode::create("TreeManagerSlidingWindow", server.getNode(), {});
- EXPECT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindow>(GM) != nullptr);
+ EXPECT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindow>(TM) != nullptr);
- P->setTreeManager(GM);
+ P->setTreeManager(TM);
EXPECT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindow>(P->getTreeManager()) != nullptr);
- ASSERT_EQ(P->getTreeManager(), GM);
+ ASSERT_EQ(P->getTreeManager(), TM);
}
TEST(TreeManager, FactoryYaml)
{
ProblemPtr P = Problem::create("PO", 2);
- auto GM = FactoryTreeManagerYaml::create(
+ auto TM = FactoryTreeManagerFile::create(
"TreeManagerSlidingWindow", wolf_dir + "/test/yaml/tree_manager_sliding_window1.yaml", {wolf_dir});
- EXPECT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindow>(GM) != nullptr);
+ EXPECT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindow>(TM) != nullptr);
- P->setTreeManager(GM);
+ P->setTreeManager(TM);
EXPECT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindow>(P->getTreeManager()) != nullptr);
- EXPECT_EQ(P->getTreeManager(), GM);
+ EXPECT_EQ(P->getTreeManager(), TM);
}
TEST(TreeManagerSlidingWindow, autoConf)
diff --git a/test/gtest_tree_manager_sliding_window_dual_rate.cpp b/test/gtest_tree_manager_sliding_window_dual_rate.cpp
index fb7a1399acf590578c16bd366337aa2b200ad548..16e98ae2475dbec13e801d02b57eb7f823e3ff8e 100644
--- a/test/gtest_tree_manager_sliding_window_dual_rate.cpp
+++ b/test/gtest_tree_manager_sliding_window_dual_rate.cpp
@@ -36,19 +36,6 @@ using namespace yaml_schema_cpp;
std::string wolf_dir = _WOLF_CODE_DIR;
-TEST(TreeManagerSlidingWindowDualRate, make_shared)
-{
- ProblemPtr P = Problem::create("PO", 2);
-
- auto ParamsGM = std::make_shared<ParamsTreeManagerSlidingWindowDualRate>();
-
- auto GM = std::make_shared<TreeManagerSlidingWindowDualRate>(ParamsGM);
-
- P->setTreeManager(GM);
-
- EXPECT_EQ(P->getTreeManager(), GM);
-}
-
TEST(TreeManagerSlidingWindowDualRate, createNode)
{
ProblemPtr P = Problem::create("PO", 2);
@@ -57,32 +44,32 @@ TEST(TreeManagerSlidingWindowDualRate, createNode)
ASSERT_TRUE(server.applySchema("TreeManagerSlidingWindowDualRate"));
- auto GM = TreeManagerSlidingWindowDualRate::create(server.getNode());
+ auto TM = TreeManagerSlidingWindowDualRate::create(server.getNode());
- EXPECT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindowDualRate>(GM) != nullptr);
+ EXPECT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindowDualRate>(TM) != nullptr);
- P->setTreeManager(GM);
+ P->setTreeManager(TM);
EXPECT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindowDualRate>(P->getTreeManager()) != nullptr);
- EXPECT_EQ(P->getTreeManager(), GM);
+ EXPECT_EQ(P->getTreeManager(), TM);
}
TEST(TreeManagerSlidingWindowDualRate, createYaml)
{
ProblemPtr P = Problem::create("PO", 2);
- auto GM = TreeManagerSlidingWindowDualRate::create(
+ auto TM = TreeManagerSlidingWindowDualRate::create(
wolf_dir + "/test/yaml/tree_manager_sliding_window_dual_rate1.yaml", {wolf_dir});
- EXPECT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindowDualRate>(GM) != nullptr);
+ EXPECT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindowDualRate>(TM) != nullptr);
- P->setTreeManager(GM);
+ P->setTreeManager(TM);
EXPECT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindowDualRate>(P->getTreeManager()) != nullptr);
- EXPECT_EQ(P->getTreeManager(), GM);
+ EXPECT_EQ(P->getTreeManager(), TM);
}
-TEST(TreeManagerSlidingWindowDualRate, Factory)
+TEST(TreeManagerSlidingWindowDualRate, FactoryNode)
{
ProblemPtr P = Problem::create("PO", 2);
@@ -90,29 +77,29 @@ TEST(TreeManagerSlidingWindowDualRate, Factory)
ASSERT_TRUE(server.applySchema("TreeManagerSlidingWindowDualRate"));
- auto GM = FactoryTreeManager::create("TreeManagerSlidingWindowDualRate", server.getNode());
- EXPECT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindowDualRate>(GM) != nullptr);
+ auto TM = FactoryTreeManagerNode::create("TreeManagerSlidingWindowDualRate", server.getNode(), {});
+ EXPECT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindowDualRate>(TM) != nullptr);
- P->setTreeManager(GM);
+ P->setTreeManager(TM);
EXPECT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindowDualRate>(P->getTreeManager()) != nullptr);
- EXPECT_EQ(P->getTreeManager(), GM);
+ EXPECT_EQ(P->getTreeManager(), TM);
}
TEST(TreeManagerSlidingWindowDualRate, FactoryYaml)
{
ProblemPtr P = Problem::create("PO", 2);
- auto GM = FactoryTreeManagerYaml::create("TreeManagerSlidingWindowDualRate",
+ auto TM = FactoryTreeManagerFile::create("TreeManagerSlidingWindowDualRate",
wolf_dir + "/test/yaml/tree_manager_sliding_window_dual_rate1.yaml",
{wolf_dir});
- EXPECT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindowDualRate>(GM) != nullptr);
+ EXPECT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindowDualRate>(TM) != nullptr);
- P->setTreeManager(GM);
+ P->setTreeManager(TM);
EXPECT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindowDualRate>(P->getTreeManager()) != nullptr);
- EXPECT_EQ(P->getTreeManager(), GM);
+ EXPECT_EQ(P->getTreeManager(), TM);
}
TEST(TreeManagerSlidingWindowDualRate, autoConf)
@@ -1149,13 +1136,13 @@ TEST(TreeManagerSlidingWindowDualRate, slidingWindowWithProcessor)
ProblemPtr problem =
Problem::autoSetup(wolf_dir + "/test/yaml/problem_sliding_window_dual_rate3.yaml", {wolf_dir});
SolverManagerPtr solver =
- FactorySolverYaml::create("SolverCeres", problem, wolf_dir + "/test/yaml/solver.yaml", {wolf_dir});
+ FactorySolverFile::create("SolverCeres", problem, wolf_dir + "/test/yaml/solver.yaml", {wolf_dir});
// BASELINE
ProblemPtr problem_bl =
Problem::autoSetup(wolf_dir + "/test/yaml/problem_sliding_window_dual_rate_baseline.yaml", {wolf_dir});
SolverManagerPtr solver_bl =
- FactorySolverYaml::create("SolverCeres", problem_bl, wolf_dir + "/test/yaml/solver.yaml", {wolf_dir});
+ FactorySolverFile::create("SolverCeres", problem_bl, wolf_dir + "/test/yaml/solver.yaml", {wolf_dir});
// aux variables
Vector7d data;
diff --git a/test/yaml/problem_sliding_window_dual_rate1.yaml b/test/yaml/problem_sliding_window_dual_rate1.yaml
index 0822e864899d7db55b6f4fe135da42ea676f1046..ea67a915f3f8c24bb13194a1e97d63be2428bb22 100644
--- a/test/yaml/problem_sliding_window_dual_rate1.yaml
+++ b/test/yaml/problem_sliding_window_dual_rate1.yaml
@@ -43,8 +43,8 @@ processors:
voting_active: true
max_time_span: 0.2 # seconds
max_buff_length: 10 # motion deltas
- dist_traveled: 10 # meters
- angle_turned: 3.1 # radians (1 rad approx 57 deg, approx 60 deg)
+ max_dist_traveled: 10 # meters
+ max_angle_turned: 3.1 # radians (1 rad approx 57 deg, approx 60 deg)
unmeasured_perturbation_std: 0.00111
diff --git a/test/yaml/problem_sliding_window_dual_rate2.yaml b/test/yaml/problem_sliding_window_dual_rate2.yaml
index cc8f5a612c61551b9642ab6c6a4bbe0debd2426c..fc2646b8b80ee8074719fd1a27233f5a96f9a2ca 100644
--- a/test/yaml/problem_sliding_window_dual_rate2.yaml
+++ b/test/yaml/problem_sliding_window_dual_rate2.yaml
@@ -49,8 +49,8 @@ processors:
voting_active: true
max_time_span: 0.2 # seconds
max_buff_length: 10 # motion deltas
- dist_traveled: 10 # meters
- angle_turned: 3.1 # radians (1 rad approx 57 deg, approx 60 deg)
+ max_dist_traveled: 10 # meters
+ max_angle_turned: 3.1 # radians (1 rad approx 57 deg, approx 60 deg)
unmeasured_perturbation_std: 0.00111
diff --git a/test/yaml/problem_sliding_window_dual_rate3.yaml b/test/yaml/problem_sliding_window_dual_rate3.yaml
index 9bea8c5c4fbed78d978368fea3e5ac1776e7c200..e3322ae2c3db032260e84de56d5821cc26572d19 100644
--- a/test/yaml/problem_sliding_window_dual_rate3.yaml
+++ b/test/yaml/problem_sliding_window_dual_rate3.yaml
@@ -51,8 +51,8 @@ processors:
voting_active: true
max_time_span: 0.2 # seconds
max_buff_length: 10 # motion deltas
- dist_traveled: 10 # meters
- angle_turned: 3.1 # radians (1 rad approx 57 deg, approx 60 deg)
+ max_dist_traveled: 10 # meters
+ max_angle_turned: 3.1 # radians (1 rad approx 57 deg, approx 60 deg)
unmeasured_perturbation_std: 0.00111
diff --git a/test/yaml/problem_sliding_window_dual_rate_baseline.yaml b/test/yaml/problem_sliding_window_dual_rate_baseline.yaml
index af3c212dfc6b7e69c4aaa5050bc5abb1fa1b413e..30246cfaef2174774bfd14d94d73ebd4fd730cd4 100644
--- a/test/yaml/problem_sliding_window_dual_rate_baseline.yaml
+++ b/test/yaml/problem_sliding_window_dual_rate_baseline.yaml
@@ -45,8 +45,8 @@ processors:
voting_active: true
max_time_span: 0.2 # seconds
max_buff_length: 10 # motion deltas
- dist_traveled: 10 # meters
- angle_turned: 3.1 # radians (1 rad approx 57 deg, approx 60 deg)
+ max_dist_traveled: 10 # meters
+ max_angle_turned: 3.1 # radians (1 rad approx 57 deg, approx 60 deg)
unmeasured_perturbation_std: 0.00111
diff --git a/test/yaml/processor_diff_drive.yaml b/test/yaml/processor_diff_drive.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..e5132056a07697eaf59843d6ca6bb55f751cc9a4
--- /dev/null
+++ b/test/yaml/processor_diff_drive.yaml
@@ -0,0 +1,19 @@
+# DO NOT MODIFY THIS FILE, USED IN gtest_factor_diff_drive & gtest_processor_diff_drive
+name: "a cool processor diff drive"
+
+time_tolerance: 0.5 # seconds
+
+keyframe_vote:
+ voting_active: true
+ max_time_span: 1.5 # seconds
+ max_buff_length: 99 # motion deltas
+ max_dist_traveled: 99 # meters
+ max_angle_turned: 99 # radians (1 rad approx 57 deg, approx 60 deg)
+ cov_det: 99
+
+unmeasured_perturbation_std: 0.001
+
+apply_loss_function: false
+
+state_provider: true
+state_provider_order: 1
\ No newline at end of file
diff --git a/test/yaml/processor_fixed_wing_model.yaml b/test/yaml/processor_fixed_wing_model.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..e3bc52f30962f74932b979e5a47f4256bdcb9aa9
--- /dev/null
+++ b/test/yaml/processor_fixed_wing_model.yaml
@@ -0,0 +1,12 @@
+name: a cool processor for a fixed wing drone
+
+time_tolerance: 0.01 # seconds
+
+keyframe_vote:
+ voting_active: false
+
+apply_loss_function: false
+
+velocity_local: [1, 0, 0]
+angle_stdev: 0.1
+min_vel_norm: 0
\ No newline at end of file
diff --git a/test/yaml/processor_landmark_external.yaml b/test/yaml/processor_landmark_external.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..a37ca4f195325ce47d40b6cbc87f63b130d11af0
--- /dev/null
+++ b/test/yaml/processor_landmark_external.yaml
@@ -0,0 +1,16 @@
+name: cool dummy processor
+keyframe_vote:
+ voting_active: true
+ min_features_for_keyframe: 1
+ time_span: 10
+ new_features_for_keyframe: 1000
+time_tolerance: 0.05
+max_new_features: -1
+voting_active: true
+apply_loss_function: false
+use_orientation: true
+match_dist_th: 0.5
+min_features_for_keyframe: 1
+filter:
+ track_length_th: 10
+ quality_th: 0.5
\ No newline at end of file
diff --git a/test/yaml/processor_loop_closure_dummy.yaml b/test/yaml/processor_loop_closure_dummy.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..42ed2006d9dde60e4dd83d764a189e57f63f2ea4
--- /dev/null
+++ b/test/yaml/processor_loop_closure_dummy.yaml
@@ -0,0 +1,10 @@
+name: a cool processor loop closure
+
+time_tolerance: 0.5 # seconds
+
+keyframe_vote:
+ voting_active: false
+
+apply_loss_function: false
+
+max_loops: -1
\ No newline at end of file
diff --git a/test/yaml/processor_motion_provider_dummy1.yaml b/test/yaml/processor_motion_provider_dummy1.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..3848617f590400ab5038adf0e2464802a4e5caa8
--- /dev/null
+++ b/test/yaml/processor_motion_provider_dummy1.yaml
@@ -0,0 +1,12 @@
+# DO NOT MODIFY THIS FILE! USED BY gtest_motion_provider
+name: "not getter processor"
+
+time_tolerance: 0.5 # seconds
+
+keyframe_vote:
+ voting_active: false
+
+apply_loss_function: false
+
+state_provider: false
+#state_provider_order: 1
\ No newline at end of file
diff --git a/test/yaml/processor_motion_provider_dummy2.yaml b/test/yaml/processor_motion_provider_dummy2.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..3fc5ecb9a4a5ab79c654a3e19e1a9a69b122e7f9
--- /dev/null
+++ b/test/yaml/processor_motion_provider_dummy2.yaml
@@ -0,0 +1,12 @@
+# DO NOT MODIFY THIS FILE! USED BY gtest_motion_provider
+name: "getter processor"
+
+time_tolerance: 0.5 # seconds
+
+keyframe_vote:
+ voting_active: false
+
+apply_loss_function: false
+
+state_provider: true
+state_provider_order: 1
\ No newline at end of file
diff --git a/test/yaml/processor_motion_provider_dummy3.yaml b/test/yaml/processor_motion_provider_dummy3.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..403035082609d755f4300d000cc8b67e0afe623e
--- /dev/null
+++ b/test/yaml/processor_motion_provider_dummy3.yaml
@@ -0,0 +1,12 @@
+# DO NOT MODIFY THIS FILE! USED BY gtest_motion_provider
+name: "getter processor lower priority"
+
+time_tolerance: 0.5 # seconds
+
+keyframe_vote:
+ voting_active: false
+
+apply_loss_function: false
+
+state_provider: true
+state_provider_order: 1 # same as proc2, will be relegated after it
\ No newline at end of file
diff --git a/test/yaml/processor_odom_2d.yaml b/test/yaml/processor_odom_2d.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..260850d8d297151c1dc27f5a8bb3b3bc5f606d49
--- /dev/null
+++ b/test/yaml/processor_odom_2d.yaml
@@ -0,0 +1,19 @@
+# DO NOT MODIFY THIS FILE, USED IN SEVERAL TESTS
+name: "a cool processor odom 2d"
+
+time_tolerance: 0.005 # seconds
+
+keyframe_vote:
+ voting_active: true
+ max_time_span: 100 # seconds
+ max_buff_length: 100 # motion deltas
+ max_dist_traveled: 100 # meters
+ max_angle_turned: 1.963495 # radians (1 rad approx 57 deg, approx 60 deg)
+ cov_det: 100 # determinant of covariance threshold
+
+unmeasured_perturbation_std: 0.001
+
+apply_loss_function: false
+
+state_provider: true
+state_provider_order: 1
\ No newline at end of file
diff --git a/test/yaml/processor_odom_2d_inactive.yaml b/test/yaml/processor_odom_2d_inactive.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..b47c761cc204bbd911f6bc4c43ae61fddb8e9852
--- /dev/null
+++ b/test/yaml/processor_odom_2d_inactive.yaml
@@ -0,0 +1,19 @@
+# DO NOT MODIFY THIS FILE, USED IN SEVERAL TESTS
+name: "a cool processor odom 2d"
+
+time_tolerance: 0.005 # seconds
+
+keyframe_vote:
+ voting_active: false
+ max_time_span: 100 #2 # seconds
+ max_buff_length: 100 #10 # motion deltas
+ max_dist_traveled: 100 #0.5 # meters
+ max_angle_turned: 0.1 # radians (1 rad approx 57 deg, approx 60 deg)
+ cov_det: 100 #1 # determinant of covariance threshold
+
+unmeasured_perturbation_std: 0.001
+
+apply_loss_function: false
+
+state_provider: true
+state_provider_order: 1
\ No newline at end of file
diff --git a/test/yaml/processor_odom_3d.yaml b/test/yaml/processor_odom_3d.yaml
index 34ab78f54be873ab3b0b2f28918aed64ff50c00a..f77c19912fa255ba701787811990a293402886e9 100644
--- a/test/yaml/processor_odom_3d.yaml
+++ b/test/yaml/processor_odom_3d.yaml
@@ -1,13 +1,14 @@
+# DO NOT MODIFY THIS FILE, USED IN SEVERAL TESTS
name: a cool processor odom 3d
time_tolerance: 0.01 # seconds
keyframe_vote:
- voting_active: false
+ voting_active: true
max_time_span: 0.2 # seconds
max_buff_length: 10 # motion deltas
- dist_traveled: 0.5 # meters
- angle_turned: 0.1 # radians (1 rad approx 57 deg, approx 60 deg)
+ max_dist_traveled: 0.5 # meters
+ max_angle_turned: 0.1 # radians (1 rad approx 57 deg, approx 60 deg)
unmeasured_perturbation_std: 0.001
diff --git a/test/yaml/processor_odom_3d_inactive.yaml b/test/yaml/processor_odom_3d_inactive.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..dce01be07a9d0956efd78730c66828362ca6e7b2
--- /dev/null
+++ b/test/yaml/processor_odom_3d_inactive.yaml
@@ -0,0 +1,18 @@
+# DO NOT MODIFY THIS FILE, USED IN SEVERAL TESTS
+name: a cool processor odom 3d
+
+time_tolerance: 0.01 # seconds
+
+keyframe_vote:
+ voting_active: false
+ max_time_span: 0.2 # seconds
+ max_buff_length: 10 # motion deltas
+ max_dist_traveled: 0.5 # meters
+ max_angle_turned: 0.1 # radians (1 rad approx 57 deg, approx 60 deg)
+
+unmeasured_perturbation_std: 0.001
+
+apply_loss_function: false
+
+state_provider: true
+state_provider_order: 1
\ No newline at end of file
diff --git a/test/yaml/processor_pose_3d.yaml b/test/yaml/processor_pose_3d.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..c554fc98cd43036f8f0182ff7a1c4e1ff62a2362
--- /dev/null
+++ b/test/yaml/processor_pose_3d.yaml
@@ -0,0 +1,9 @@
+# DO NOT MODIFY THIS FILE, USED IN gtest_processor_pose_3d
+name: a cool processor pose 3d
+
+time_tolerance: 0.01 # seconds
+
+keyframe_vote:
+ voting_active: true
+
+apply_loss_function: false
\ No newline at end of file
diff --git a/test/yaml/processor_tracker_feature_dummy.yaml b/test/yaml/processor_tracker_feature_dummy.yaml
index 1ec67d427f4584fd763a7895500bc9860d01acc9..488ea3fda90966245d5ad617457879eb53478cb6 100644
--- a/test/yaml/processor_tracker_feature_dummy.yaml
+++ b/test/yaml/processor_tracker_feature_dummy.yaml
@@ -1,8 +1,8 @@
name: cool dummy processor
-time_tolerance: 0.1
+time_tolerance: 0.25
keyframe_vote:
voting_active: true
- min_features_for_keyframe: 1
+ min_features_for_keyframe: 7
apply_loss_function: false
-max_new_features: 0
+max_new_features: 10
n_tracks_lost: 1
\ No newline at end of file
diff --git a/test/yaml/processor_tracker_landmark_dummy.yaml b/test/yaml/processor_tracker_landmark_dummy.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..4de7abe895efe1bc4165effe78a9b7ae5e500ae6
--- /dev/null
+++ b/test/yaml/processor_tracker_landmark_dummy.yaml
@@ -0,0 +1,8 @@
+name: cool dummy processor
+time_tolerance: 0.25
+keyframe_vote:
+ voting_active: true
+ min_features_for_keyframe: 7
+apply_loss_function: false
+max_new_features: 10
+n_landmarks_lost: 1
\ No newline at end of file
diff --git a/test/yaml/sensor_diff_drive.yaml b/test/yaml/sensor_diff_drive.yaml
index 9e5f3f5dfb028d5bbbc6e74d7082901a459441ab..c5dfa8631eef3d401eca2fd670abf0b82504e4f0 100644
--- a/test/yaml/sensor_diff_drive.yaml
+++ b/test/yaml/sensor_diff_drive.yaml
@@ -1,4 +1,6 @@
+# DO NOT CHANGE THE FOLLOWING VALUES they are checked in gtest_sensor_diff_drive
name: a cool sensor diff drive
+type: SensorDiffDrive
states:
P:
mode: fix
diff --git a/test/yaml/sensor_diff_drive_dynamic.yaml b/test/yaml/sensor_diff_drive_dynamic.yaml
index 013e6af1f52471d922fc8bb23491829e3bd3b625..49fb438479bca2164b76eebef75b3d6eec440f24 100644
--- a/test/yaml/sensor_diff_drive_dynamic.yaml
+++ b/test/yaml/sensor_diff_drive_dynamic.yaml
@@ -1,4 +1,5 @@
name: a_cool_sensor_diff_drive_dynamic
+type: SensorDiffDrive
states:
P:
mode: fix
diff --git a/test/yaml/sensor_diff_drive_gtest_factor_diff_drive.yaml b/test/yaml/sensor_diff_drive_gtest_factor_diff_drive.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..be19eecf41a85f47c3820002a9b13cca707f956b
--- /dev/null
+++ b/test/yaml/sensor_diff_drive_gtest_factor_diff_drive.yaml
@@ -0,0 +1,19 @@
+# DO NOT CHANGE THIS FILE, USED IN gtest_factor_diff_drive
+name: a cool sensor diff drive
+type: SensorDiffDrive
+states:
+ P:
+ mode: fix
+ state: [0, 0]
+ dynamic: false
+ O:
+ mode: fix
+ state: [0]
+ dynamic: false
+ I:
+ mode: initial_guess
+ state: [1, 1, 1]
+ dynamic: false
+
+ticks_per_wheel_revolution: 100
+ticks_std_factor: 1
diff --git a/test/yaml/sensor_diff_drive_gtest_problem.yaml b/test/yaml/sensor_diff_drive_gtest_problem.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..baedef13deb1e38ab7c9e30755ebbb158d738c3c
--- /dev/null
+++ b/test/yaml/sensor_diff_drive_gtest_problem.yaml
@@ -0,0 +1,19 @@
+# DO NOT CHANGE THE FOLLOWING VALUES they are checked in gtest_problem
+name: a cool sensor diff drive
+type: SensorDiffDrive
+states:
+ P:
+ mode: fix
+ state: [1, 2]
+ dynamic: false
+ O:
+ mode: fix
+ state: [3]
+ dynamic: false
+ I:
+ mode: initial_guess
+ state: [0.1, 0.2, 0.3]
+ dynamic: true
+
+ticks_per_wheel_revolution: 4
+ticks_std_factor: 2
diff --git a/test/yaml/sensor_odom_2d.yaml b/test/yaml/sensor_odom_2d.yaml
index 278374ccf507b409a424c5e538b0c21e1c2018ca..7772013016f548d01caa3056b3bb098205967bac 100644
--- a/test/yaml/sensor_odom_2d.yaml
+++ b/test/yaml/sensor_odom_2d.yaml
@@ -1,4 +1,6 @@
+# DO NOT MODIFY THIS FILE, USED BY gtest_sensor_odom and others
name: some_sensor_odom_2d
+type: SensorOdom2d
states:
P:
mode: fix
@@ -9,9 +11,9 @@ states:
state: [3]
dynamic: false
-k_disp_to_disp: 0.5
-k_disp_to_rot: 0.0
-k_rot_to_rot: 0.8
+k_disp_to_disp: 0.1
+k_disp_to_rot: 0.2
+k_rot_to_rot: 0.3
min_disp_var: 0.01
-min_rot_var: 0.01
+min_rot_var: 0.02
diff --git a/test/yaml/sensor_odom_3d.yaml b/test/yaml/sensor_odom_3d.yaml
index d3965dfe44e370d3a35563a244774cfb3bd5d20b..66b72f8e4d62f67ec4fa97024527e9ac69a05c6c 100644
--- a/test/yaml/sensor_odom_3d.yaml
+++ b/test/yaml/sensor_odom_3d.yaml
@@ -1,4 +1,6 @@
+# DO NOT MODIFY THIS FILE, USED BY gtest_sensor_odom and others
name: odometer 3d
+type: SensorOdom3d
states:
P:
mode: fix
@@ -9,8 +11,8 @@ states:
state: [1, 0, 0, 0]
dynamic: false
-k_disp_to_disp: 0.02 # m^2 / m
-k_disp_to_rot: 0.02 # rad^2 / m
-k_rot_to_rot: 0.01 # rad^2 / rad
+k_disp_to_disp: 0.1 # m^2 / m
+k_disp_to_rot: 0.2 # rad^2 / m
+k_rot_to_rot: 0.3 # rad^2 / rad
min_disp_var: 0.01 # m^2
-min_rot_var: 0.01 # rad^2
+min_rot_var: 0.02 # rad^2
diff --git a/test/yaml/sensor_odom_3d_gtest_factor_prior.yaml b/test/yaml/sensor_odom_3d_gtest_factor_prior.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..35611c80e426998765a52a728b40ada858408a44
--- /dev/null
+++ b/test/yaml/sensor_odom_3d_gtest_factor_prior.yaml
@@ -0,0 +1,18 @@
+# DO NOT CHANGE: consistent with hardcoded values in gtest_prior_factor
+name: odometer 3d
+type: SensorOdom3d
+states:
+ P:
+ mode: initial_guess
+ state: [1, 2, 3]
+ dynamic: false
+ O:
+ mode: initial_guess
+ state: [1, 0, 0, 0]
+ dynamic: false
+
+k_disp_to_disp: 0.02
+k_disp_to_rot: 0.02
+k_rot_to_rot: 0.01
+min_disp_var: 0.01
+min_rot_var: 0.01
diff --git a/test/yaml/sensor_pose_2d.yaml b/test/yaml/sensor_pose_2d.yaml
index 78d84857f7d3309cc12ca02eb7196d8367ddbefd..98bbf1430b6ac4e565637586097c8c22993e9402 100644
--- a/test/yaml/sensor_pose_2d.yaml
+++ b/test/yaml/sensor_pose_2d.yaml
@@ -1,4 +1,5 @@
name: a cool sensor pose 2d
+type: SensorPose2d
states:
P:
mode: fix
diff --git a/test/yaml/sensor_pose_3d.yaml b/test/yaml/sensor_pose_3d.yaml
index 128461a53abd981d9b85ee9a339e6e3fb99c79b2..06ad4ba237a1d32d9ae147cc55309b12300a4b81 100644
--- a/test/yaml/sensor_pose_3d.yaml
+++ b/test/yaml/sensor_pose_3d.yaml
@@ -1,4 +1,5 @@
name: a cool sensor pose 3d
+type: SensorPose3d
states:
P:
mode: fix
diff --git a/test/yaml/sensor_pose_3d_initial_guess.yaml b/test/yaml/sensor_pose_3d_initial_guess.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..ede75d7dde4567ffa1e62aa724b7522821429e4d
--- /dev/null
+++ b/test/yaml/sensor_pose_3d_initial_guess.yaml
@@ -0,0 +1,14 @@
+# DO NOT CHANGE THIS FILE! USED IN gtest_processor_pose_3d
+name: a cool sensor pose 3d
+type: SensorPose3d
+states:
+ P:
+ mode: initial_guess
+ state: [1, 2, 3]
+ dynamic: false
+ O:
+ mode: initial_guess
+ state: [0.5, 0.5, 0.5, 0.5]
+ dynamic: false
+
+std_noise: [1, 1, 1, 1, 1, 1]
diff --git a/test/yaml/solver_ceres.yaml b/test/yaml/solver_ceres.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..53a698323208a61dc63a35084985c13fdcebdd98
--- /dev/null
+++ b/test/yaml/solver_ceres.yaml
@@ -0,0 +1,13 @@
+period: 1
+verbose: 2
+interrupt_on_problem_change: false
+max_num_iterations: 1000
+n_threads: 2
+function_tolerance: 0.000001
+gradient_tolerance: 0.0000000001
+minimizer: "LEVENBERG_MARQUARDT"
+use_nonmonotonic_steps: false # only for LEVENBERG_MARQUARDT and DOGLEG
+max_consecutive_nonmonotonic_steps: 5 # only if use_nonmonotonic_steps = true
+compute_cov: true
+cov_period: 1 #only if compute_cov
+cov_enum: 2 #only if compute_cov
\ No newline at end of file
diff --git a/test/yaml/solver_dummy.yaml b/test/yaml/solver_dummy.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..b8ba9d35719a1447882d86e2ef8381731c825f98
--- /dev/null
+++ b/test/yaml/solver_dummy.yaml
@@ -0,0 +1,3 @@
+period: 1
+verbose: 2
+compute_cov: false
\ No newline at end of file
diff --git a/wolf_scripts/out b/wolf_scripts/out
deleted file mode 100644
index 493b8dbcb5aaec3863d3634eff897e820206f404..0000000000000000000000000000000000000000
--- a/wolf_scripts/out
+++ /dev/null
@@ -1,515 +0,0 @@
-+====================== /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_velocity.h ==================================
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_wheel_joint_position.h ==================================
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_odom_3d.h ==================================
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_image.h ==================================
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_IMU.h ==================================
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_GPS.h ==================================
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_odom_2d.h ==================================
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_motion.h ==================================
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_GPS_fix.h ==================================
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_laser_2d.h ==================================
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/capture/src/capture_image.cpp ==================================
-#include "capture_image.h"
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/capture/src/capture_GPS.cpp ==================================
-#include "capture_GPS.h"
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/capture/src/capture_odom_3d.cpp ==================================
-#include "capture_odom_3d.h"
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/capture/src/capture_velocity.cpp ==================================
-#include "capture_velocity.h"
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/capture/src/capture_motion.cpp ==================================
-#include "capture_motion.h"
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/capture/src/capture_IMU.cpp ==================================
-#include "capture_IMU.h"
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/capture/src/capture_GPS_fix.cpp ==================================
-#include "capture_GPS_fix.h"
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/capture/src/capture_odom_2d.cpp ==================================
-#include "capture_odom_2d.h"
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/capture/src/capture_wheel_joint_position.cpp ==================================
-#include "capture_wheel_joint_position.h"
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/capture/src/capture_laser_2d.cpp ==================================
-#include "capture_laser_2d.h"
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_IMU.h ==================================
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_odom_2d.h ==================================
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_quaternion_absolute.h ==================================
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_container.h ==================================
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_block_absolute.h ==================================
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_odom_2d_analytic.h ==================================
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_epipolar.h ==================================
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_GPS_2d.h ==================================
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_fix_bias.h ==================================
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_relative_2d_analytic.h ==================================
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_pose_2d.h ==================================
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_autodiff_trifocal.h ==================================
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_GPS_pseudorange_2d.h ==================================
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_corner_2d.h ==================================
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_AHP.h ==================================
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_GPS_pseudorange_3d.h ==================================
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_odom_3d.h ==================================
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_autodiff_distance_3d.h ==================================
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_diff_drive.h ==================================
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_point_to_line_2d.h ==================================
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_point_2d.h ==================================
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_pose_3d.h ==================================
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/core/include/core/frame_base.h ==================================
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/core/include/core/feature_base.h ==================================
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/core/include/core/solver_factory.h ==================================
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/core/include/core/motion_buffer.h ==================================
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/core/include/core/node_base.h ==================================
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/core/include/core/capture_pose.h ==================================
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/core/include/core/capture_base.h ==================================
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/core/include/core/processor_base.h ==================================
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/core/include/core/track_matrix.h ==================================
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/core/include/core/singleton.h ==================================
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/core/include/core/map_base.h ==================================
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/core/include/core/time_stamp.h ==================================
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/core/include/core/trajectory_base.h ==================================
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/core/include/core/params_server.hpp ==================================
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/core/include/core/logging.h ==================================
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/core/include/core/sensor_factory.h ==================================
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/core/include/core/capture_void.h ==================================
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/core/include/core/rotations.h ==================================
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/core/include/core/capture_buffer.h ==================================
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/core/include/core/problem.h ==================================
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/core/include/core/autoconf_processor_factory.h ==================================
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/core/include/core/state_homogeneous_3d.h ==================================
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/core/include/core/wolf.h ==================================
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/core/include/core/factory.h ==================================
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/core/include/core/local_parametrization_angle.h ==================================
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/core/include/core/constraint_base.h ==================================
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/core/include/core/processor_tracker_feature.h ==================================
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/core/include/core/autoconf_sensor_factory.h ==================================
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/core/include/core/constraint_analytic.h ==================================
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/core/include/core/processor_tracker.h ==================================
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/core/include/core/feature_pose.h ==================================
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/core/include/core/local_parametrization_homogeneous.h ==================================
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/core/include/core/processor_loopclosure_base.h ==================================
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/core/include/core/local_parametrization_base.h ==================================
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/core/include/core/state_angle.h ==================================
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/core/include/core/hardware_base.h ==================================
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/core/include/core/landmark_base.h ==================================
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/core/include/core/state_block.h ==================================
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/core/include/core/state_quaternion.h ==================================
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/core/include/core/sensor_base.h ==================================
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/core/include/core/feature_match.h ==================================
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/core/include/core/three_D_tools.h ==================================
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/core/include/core/processor_motion.h ==================================
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/core/include/core/converter.h ==================================
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/core/include/core/constraint_autodiff.h ==================================
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/core/include/core/processor_factory.h ==================================
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/core/include/core/local_parametrization_quaternion.h ==================================
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/core/src/feature_pose.cpp ==================================
-#include "feature_pose.h"
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/core/src/capture_void.cpp ==================================
-#include "capture_void.h"
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/core/src/local_parametrization_quaternion.cpp ==================================
-#include "local_parametrization_quaternion.h"
-#include "rotations.h"
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/core/src/time_stamp.cpp ==================================
-#include "time_stamp.h"
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/core/src/processor_loopclosure_base.cpp ==================================
-#include "processor_loopclosure_base.h"
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/core/src/state_block.cpp ==================================
-#include "state_block.h"
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/core/src/hardware_base.cpp ==================================
-#include "hardware_base.h"
-#include "sensor_base.h"
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/core/src/capture_base.cpp ==================================
-#include "capture_base.h"
-#include "sensor_base.h"
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/core/src/map_base.cpp ==================================
-#include "map_base.h"
-#include "landmark_base.h"
-#include "factory.h"
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/core/src/feature_base.cpp ==================================
-#include "feature_base.h"
-#include "constraint_base.h"
-#include "capture_base.h"
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/core/src/processor_tracker_feature.cpp ==================================
-#include "processor_tracker_feature.h"
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/core/src/processor_base.cpp ==================================
-#include "processor_base.h"
-#include "processor_motion.h"
-#include "capture_base.h"
-#include "frame_base.h"
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/core/src/processor_tracker.cpp ==================================
-#include "processor_tracker.h"
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/core/src/local_parametrization_homogeneous.cpp ==================================
-#include "local_parametrization_homogeneous.h"
-#include "rotations.h" // we use quaternion algebra here
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/core/src/capture_pose.cpp ==================================
-#include "capture_pose.h"
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/core/src/motion_buffer.cpp ==================================
-#include "motion_buffer.h"
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/core/src/node_base.cpp ==================================
-#include "node_base.h"
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/core/src/constraint_base.cpp ==================================
-#include "constraint_base.h"
-#include "frame_base.h"
-#include "landmark_base.h"
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/core/src/constraint_analytic.cpp ==================================
-#include "constraint_analytic.h"
-#include "state_block.h"
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/core/src/sensor_base.cpp ==================================
-#include "sensor_base.h"
-#include "state_block.h"
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/core/src/track_matrix.cpp ==================================
-#include "track_matrix.h"
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/core/src/problem.cpp ==================================
-#include "problem.h"
-#include "hardware_base.h"
-#include "trajectory_base.h"
-#include "map_base.h"
-#include "sensor_base.h"
-#include "factory.h"
-#include "sensor_factory.h"
-#include "processor_factory.h"
-#include "autoconf_sensor_factory.h"
-#include "autoconf_processor_factory.h"
-#include "constraint_base.h"
-#include "state_block.h"
-#include "processor_motion.h"
-#include "processor_tracker.h"
-#include "capture_pose.h"
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/core/src/processor_motion.cpp ==================================
-#include "processor_motion.h"
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/core/src/frame_base.cpp ==================================
-#include "frame_base.h"
-#include "constraint_base.h"
-#include "trajectory_base.h"
-#include "capture_base.h"
-#include "state_block.h"
-#include "state_angle.h"
-#include "state_quaternion.h"
-#include "factory.h"
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/core/src/trajectory_base.cpp ==================================
-#include "trajectory_base.h"
-#include "frame_base.h"
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/core/src/landmark_base.cpp ==================================
-#include "landmark_base.h"
-#include "constraint_base.h"
-#include "map_base.h"
-#include "state_block.h"
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/core/src/local_parametrization_base.cpp ==================================
-#include "local_parametrization_base.h"
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_odom_2d.h ==================================
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_line_2d.h ==================================
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_motion.h ==================================
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_IMU.h ==================================
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_corner_2d.h ==================================
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_GPS_pseudorange.h ==================================
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_point_image.h ==================================
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_GPS_fix.h ==================================
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_polyline_2d.h ==================================
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_diff_drive.h ==================================
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/feature/src/feature_point_image.cpp ==================================
-#include "feature_point_image.h"
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/feature/src/feature_GPS_pseudorange.cpp ==================================
-#include "feature_GPS_pseudorange.h"
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/feature/src/feature_odom_2d.cpp ==================================
-#include "feature_odom_2d.h"
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/feature/src/feature_polyline_2d.cpp ==================================
-#include "feature_polyline_2d.h"
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/feature/src/feature_diff_drive.cpp ==================================
-#include "feature_diff_drive.h"
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/feature/src/feature_IMU.cpp ==================================
-#include "feature_IMU.h"
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/feature/src/feature_motion.cpp ==================================
-#include "feature_motion.h"
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/feature/src/feature_corner_2d.cpp ==================================
-#include "feature_corner_2d.h"
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/feature/src/feature_GPS_fix.cpp ==================================
-#include "feature_GPS_fix.h"
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/feature/src/feature_line_2d.cpp ==================================
-#include "feature_corner_2d.h"
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_point_3d.h ==================================
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_container.h ==================================
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_match.h ==================================
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_AHP.h ==================================
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_polyline_2d.h ==================================
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_corner_2d.h ==================================
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_line_2d.h ==================================
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_corner_2d.cpp ==================================
-#include "landmark_corner_2d.h"
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_line_2d.cpp ==================================
-#include "landmark_line_2d.h"
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_container.cpp ==================================
-#include "landmark_container.h"
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_polyline_2d.cpp ==================================
-#include "landmark_polyline_2d.h"
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_point_3d.cpp ==================================
-#include "landmark_point_3d.h"
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_AHP.cpp ==================================
-#include "landmark_AHP.h"
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_tracker_feature_dummy.h ==================================
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_GPS.h ==================================
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_frame_nearest_neighbor_filter.h ==================================
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_odom_2d.h ==================================
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_tracker_landmark_corner.h ==================================
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_tracker_feature_corner.h ==================================
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_capture_holder.h ==================================
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_diff_drive.h ==================================
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_tracker_landmark.h ==================================
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_tracker_landmark_polyline.h ==================================
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_logging.h ==================================
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_tracker_feature_trifocal.h ==================================
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_tracker_feature_image.h ==================================
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_odom_3d.h ==================================
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_IMU.h ==================================
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_params_image.h ==================================
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_tracker_landmark_image.h ==================================
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_tracker_landmark_dummy.h ==================================
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/processor/src/processor_tracker_landmark_corner.cpp ==================================
-#include "processor_tracker_landmark_corner.h"
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/processor/src/processor_GPS.cpp ==================================
-#include "processor_GPS.h"
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/processor/src/processor_tracker_feature_dummy.cpp ==================================
-#include "processor_tracker_feature_dummy.h"
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/processor/src/processor_capture_holder.cpp ==================================
-#include "processor_capture_holder.h"
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/processor/src/processor_frame_nearest_neighbor_filter.cpp ==================================
-#include "processor_frame_nearest_neighbor_filter.h"
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/processor/src/processor_tracker_landmark.cpp ==================================
-#include "processor_tracker_landmark.h"
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/processor/src/processor_odom_3d.cpp ==================================
-#include "processor_odom_3d.h"
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/processor/src/processor_odom_2d.cpp ==================================
-#include "processor_odom_2d.h"
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/processor/src/processor_tracker_landmark_image.cpp ==================================
-#include "processor_tracker_landmark_image.h"
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/processor/src/processor_tracker_landmark_dummy.cpp ==================================
-#include "processor_tracker_landmark_dummy.h"
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/processor/src/processor_tracker_feature_trifocal.cpp ==================================
-#include "processor_tracker_feature_trifocal.h"
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/processor/src/processor_tracker_feature_image.cpp ==================================
-#include "processor_tracker_feature_image.h"
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/processor/src/processor_IMU.cpp ==================================
-#include "processor_IMU.h"
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/processor/src/processor_diff_drive.cpp ==================================
-#include "processor_diff_drive.h"
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/processor/src/processor_tracker_feature_corner.cpp ==================================
-#include "processor_tracker_feature_corner.h"
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/processor/src/processor_tracker_landmark_polyline.cpp ==================================
-#include "processor_tracker_landmark_polyline.h"
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_odom_3d.h ==================================
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_IMU.h ==================================
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_odom_2d.h ==================================
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_GPS_fix.h ==================================
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_diff_drive.h ==================================
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_camera.h ==================================
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_GPS.h ==================================
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_laser_2d.h ==================================
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_diff_drive.cpp ==================================
-#include "sensor_diff_drive.h"
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_GPS.cpp ==================================
-#include "sensor_GPS.h"
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_odom_2d.cpp ==================================
-#include "sensor_odom_2d.h"
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_IMU.cpp ==================================
-#include "sensor_IMU.h"
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_odom_3d.cpp ==================================
-#include "sensor_odom_3d.h"
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_laser_2d.cpp ==================================
-#include "sensor_laser_2d.h"
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_GPS_fix.cpp ==================================
-#include "sensor_GPS_fix.h"
--===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_camera.cpp ==================================
-#include "sensor_camera.h"
--===============================================================
diff --git a/wolf_scripts/output b/wolf_scripts/output
deleted file mode 100644
index 2652d6fd005dfc9eaad13d4b8bcf7f5e582eb84c..0000000000000000000000000000000000000000
--- a/wolf_scripts/output
+++ /dev/null
@@ -1,3883 +0,0 @@
-======================= /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_velocity.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_velocity.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_velocity.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_velocity.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_velocity.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_velocity.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_wheel_joint_position.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_wheel_joint_position.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_wheel_joint_position.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_wheel_joint_position.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_wheel_joint_position.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_wheel_joint_position.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_odom_3d.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_odom_3d.h ==================================
-#include <wolf/core/rotations.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_odom_3d.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_odom_3d.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_odom_3d.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_odom_3d.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_image.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_image.h ==================================
-#include <wolf/core/capture_base.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_image.h ==================================
-#include <wolf/feature/feature_point_image.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_image.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_image.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_image.h ==================================
-#include <wolf/sensor/sensor_camera.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_IMU.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_IMU.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_IMU.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_IMU.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_IMU.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_IMU.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_GPS.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_GPS.h ==================================
-#include <wolf/core/capture_base.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_GPS.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_GPS.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_GPS.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_GPS.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_odom_2d.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_odom_2d.h ==================================
-#include <wolf/core/rotations.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_odom_2d.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_odom_2d.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_odom_2d.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_odom_2d.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_motion.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_motion.h ==================================
-#include <wolf/core/capture_base.h>
-#include <wolf/core/motion_buffer.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_motion.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_motion.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_motion.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_motion.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_GPS_fix.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_GPS_fix.h ==================================
-#include <wolf/core/capture_base.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_GPS_fix.h ==================================
-#include <wolf/feature/feature_GPS_fix.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_GPS_fix.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_GPS_fix.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_GPS_fix.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_laser_2d.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_laser_2d.h ==================================
-#include <wolf/core/capture_base.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_laser_2d.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_laser_2d.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_laser_2d.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_laser_2d.h ==================================
-#include <wolf/sensor/sensor_laser_2d.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/src/capture_image.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/src/capture_image.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/src/capture_image.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/src/capture_image.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/src/capture_image.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/src/capture_image.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/src/capture_GPS.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/src/capture_GPS.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/src/capture_GPS.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/src/capture_GPS.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/src/capture_GPS.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/src/capture_GPS.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/src/capture_odom_3d.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/src/capture_odom_3d.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/src/capture_odom_3d.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/src/capture_odom_3d.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/src/capture_odom_3d.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/src/capture_odom_3d.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/src/capture_velocity.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/src/capture_velocity.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/src/capture_velocity.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/src/capture_velocity.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/src/capture_velocity.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/src/capture_velocity.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/src/capture_motion.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/src/capture_motion.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/src/capture_motion.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/src/capture_motion.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/src/capture_motion.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/src/capture_motion.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/src/capture_IMU.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/src/capture_IMU.cpp ==================================
-#include <wolf/core/state_quaternion.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/src/capture_IMU.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/src/capture_IMU.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/src/capture_IMU.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/src/capture_IMU.cpp ==================================
-#include <wolf/sensor/sensor_IMU.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/src/capture_GPS_fix.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/src/capture_GPS_fix.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/src/capture_GPS_fix.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/src/capture_GPS_fix.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/src/capture_GPS_fix.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/src/capture_GPS_fix.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/src/capture_odom_2d.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/src/capture_odom_2d.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/src/capture_odom_2d.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/src/capture_odom_2d.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/src/capture_odom_2d.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/src/capture_odom_2d.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/src/capture_wheel_joint_position.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/src/capture_wheel_joint_position.cpp ==================================
-#include <wolf/core/rotations.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/src/capture_wheel_joint_position.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/src/capture_wheel_joint_position.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/src/capture_wheel_joint_position.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/src/capture_wheel_joint_position.cpp ==================================
-#include <wolf/sensor/sensor_diff_drive.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/src/capture_laser_2d.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/src/capture_laser_2d.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/src/capture_laser_2d.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/src/capture_laser_2d.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/src/capture_laser_2d.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/src/capture_laser_2d.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_IMU.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_IMU.h ==================================
-#include <wolf/core/constraint_autodiff.h>
-#include <wolf/core/rotations.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_IMU.h ==================================
-#include <wolf/feature/feature_IMU.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_IMU.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_IMU.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_IMU.h ==================================
-#include <wolf/sensor/sensor_IMU.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_odom_2d.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_odom_2d.h ==================================
-#include <wolf/core/constraint_autodiff.h>
-#include <wolf/core/frame_base.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_odom_2d.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_odom_2d.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_odom_2d.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_odom_2d.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_quaternion_absolute.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_quaternion_absolute.h ==================================
-#include <wolf/core/constraint_autodiff.h>
-#include <wolf/core/local_parametrization_quaternion.h>
-#include <wolf/core/frame_base.h>
-#include <wolf/core/rotations.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_quaternion_absolute.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_quaternion_absolute.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_quaternion_absolute.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_quaternion_absolute.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_container.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_container.h ==================================
-#include <wolf/core/wolf.h>
-#include <wolf/core/constraint_autodiff.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_container.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_container.h ==================================
-#include <wolf/landmark/landmark_container.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_container.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_container.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_block_absolute.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_block_absolute.h ==================================
-#include <wolf/core/constraint_autodiff.h>
-#include <wolf/core/frame_base.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_block_absolute.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_block_absolute.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_block_absolute.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_block_absolute.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_odom_2d_analytic.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_odom_2d_analytic.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_odom_2d_analytic.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_odom_2d_analytic.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_odom_2d_analytic.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_odom_2d_analytic.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_epipolar.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_epipolar.h ==================================
-#include <wolf/core/constraint_base.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_epipolar.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_epipolar.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_epipolar.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_epipolar.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_GPS_2d.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_GPS_2d.h ==================================
-#include <wolf/core/wolf.h>
-#include <wolf/core/constraint_autodiff.h>
-#include <wolf/core/frame_base.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_GPS_2d.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_GPS_2d.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_GPS_2d.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_GPS_2d.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_fix_bias.h ==================================
-#include <wolf/capture/capture_IMU.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_fix_bias.h ==================================
-#include <wolf/core/constraint_autodiff.h>
-#include <wolf/core/frame_base.h>
-#include <wolf/core/rotations.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_fix_bias.h ==================================
-#include <wolf/feature/feature_IMU.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_fix_bias.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_fix_bias.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_fix_bias.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_relative_2d_analytic.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_relative_2d_analytic.h ==================================
-#include <wolf/core/constraint_analytic.h>
-#include <wolf/core/landmark_base.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_relative_2d_analytic.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_relative_2d_analytic.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_relative_2d_analytic.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_relative_2d_analytic.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_pose_2d.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_pose_2d.h ==================================
-#include <wolf/core/constraint_autodiff.h>
-#include <wolf/core/frame_base.h>
-#include <wolf/core/rotations.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_pose_2d.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_pose_2d.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_pose_2d.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_pose_2d.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_autodiff_trifocal.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_autodiff_trifocal.h ==================================
-//#include <wolf/core/wolf.h>
-#include <wolf/core/constraint_autodiff.h>
-#include <wolf/core/rotations.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_autodiff_trifocal.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_autodiff_trifocal.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_autodiff_trifocal.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_autodiff_trifocal.h ==================================
-#include <wolf/sensor/sensor_camera.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_GPS_pseudorange_2d.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_GPS_pseudorange_2d.h ==================================
-#include <wolf/core/constraint_autodiff.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_GPS_pseudorange_2d.h ==================================
-#include <wolf/feature/feature_GPS_pseudorange.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_GPS_pseudorange_2d.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_GPS_pseudorange_2d.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_GPS_pseudorange_2d.h ==================================
-#include <wolf/sensor/sensor_GPS.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_corner_2d.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_corner_2d.h ==================================
-#include <wolf/core/constraint_autodiff.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_corner_2d.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_corner_2d.h ==================================
-#include <wolf/landmark/landmark_corner_2d.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_corner_2d.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_corner_2d.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_AHP.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_AHP.h ==================================
-#include <wolf/core/constraint_autodiff.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_AHP.h ==================================
-//#include <wolf/feature/feature_point_image.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_AHP.h ==================================
-#include <wolf/landmark/landmark_AHP.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_AHP.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_AHP.h ==================================
-#include <wolf/sensor/sensor_camera.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_GPS_pseudorange_3d.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_GPS_pseudorange_3d.h ==================================
-#include <wolf/core/constraint_autodiff.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_GPS_pseudorange_3d.h ==================================
-#include <wolf/feature/feature_GPS_pseudorange.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_GPS_pseudorange_3d.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_GPS_pseudorange_3d.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_GPS_pseudorange_3d.h ==================================
-#include <wolf/sensor/sensor_GPS.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_odom_3d.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_odom_3d.h ==================================
-#include <wolf/core/constraint_autodiff.h>
-#include <wolf/core/rotations.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_odom_3d.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_odom_3d.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_odom_3d.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_odom_3d.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_autodiff_distance_3d.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_autodiff_distance_3d.h ==================================
-#include <wolf/core/constraint_autodiff.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_autodiff_distance_3d.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_autodiff_distance_3d.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_autodiff_distance_3d.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_autodiff_distance_3d.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_diff_drive.h ==================================
-#include <wolf/capture/capture_wheel_joint_position.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_diff_drive.h ==================================
-#include <wolf/core/constraint_autodiff.h>
-#include <wolf/core/frame_base.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_diff_drive.h ==================================
-#include <wolf/feature/feature_diff_drive.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_diff_drive.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_diff_drive.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_diff_drive.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_point_to_line_2d.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_point_to_line_2d.h ==================================
-#include <wolf/core/constraint_autodiff.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_point_to_line_2d.h ==================================
-#include <wolf/feature/feature_polyline_2d.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_point_to_line_2d.h ==================================
-#include <wolf/landmark/landmark_polyline_2d.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_point_to_line_2d.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_point_to_line_2d.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_point_2d.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_point_2d.h ==================================
-#include <wolf/core/constraint_autodiff.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_point_2d.h ==================================
-#include <wolf/feature/feature_polyline_2d.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_point_2d.h ==================================
-#include <wolf/landmark/landmark_polyline_2d.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_point_2d.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_point_2d.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_pose_3d.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_pose_3d.h ==================================
-#include <wolf/core/constraint_autodiff.h>
-#include <wolf/core/frame_base.h>
-#include <wolf/core/rotations.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_pose_3d.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_pose_3d.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_pose_3d.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_pose_3d.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/frame_base.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/frame_base.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/frame_base.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/frame_base.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/frame_base.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/frame_base.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/feature_base.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/feature_base.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/feature_base.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/feature_base.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/feature_base.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/feature_base.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/motion_buffer.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/motion_buffer.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/motion_buffer.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/motion_buffer.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/motion_buffer.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/motion_buffer.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/node_base.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/node_base.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/node_base.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/node_base.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/node_base.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/node_base.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/capture_pose.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/capture_pose.h ==================================
-#include <wolf/constraint/constraint_pose_2d.h>
-#include <wolf/constraint/constraint_pose_3d.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/capture_pose.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/capture_pose.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/capture_pose.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/capture_pose.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/capture_base.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/capture_base.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/capture_base.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/capture_base.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/capture_base.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/capture_base.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/processor_base.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/processor_base.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/processor_base.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/processor_base.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/processor_base.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/processor_base.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/track_matrix.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/track_matrix.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/track_matrix.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/track_matrix.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/track_matrix.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/track_matrix.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/singleton.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/singleton.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/singleton.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/singleton.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/singleton.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/singleton.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/map_base.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/map_base.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/map_base.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/map_base.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/map_base.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/map_base.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/time_stamp.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/time_stamp.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/time_stamp.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/time_stamp.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/time_stamp.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/time_stamp.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/trajectory_base.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/trajectory_base.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/trajectory_base.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/trajectory_base.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/trajectory_base.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/trajectory_base.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/logging.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/logging.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/logging.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/logging.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/logging.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/logging.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/sensor_factory.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/sensor_factory.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/sensor_factory.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/sensor_factory.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/sensor_factory.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/sensor_factory.h ==================================
- * #include <wolf/sensor/sensor_camera.h> // provides SensorCamera
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/capture_void.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/capture_void.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/capture_void.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/capture_void.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/capture_void.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/capture_void.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/rotations.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/rotations.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/rotations.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/rotations.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/rotations.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/rotations.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/capture_buffer.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/capture_buffer.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/capture_buffer.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/capture_buffer.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/capture_buffer.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/capture_buffer.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/problem.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/problem.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/problem.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/problem.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/problem.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/problem.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/state_homogeneous_3d.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/state_homogeneous_3d.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/state_homogeneous_3d.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/state_homogeneous_3d.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/state_homogeneous_3d.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/state_homogeneous_3d.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/wolf.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/wolf.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/wolf.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/wolf.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/wolf.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/wolf.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/factory.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/factory.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/factory.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/factory.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/factory.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/factory.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/local_parametrization_angle.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/local_parametrization_angle.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/local_parametrization_angle.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/local_parametrization_angle.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/local_parametrization_angle.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/local_parametrization_angle.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/constraint_base.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/constraint_base.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/constraint_base.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/constraint_base.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/constraint_base.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/constraint_base.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/processor_tracker_feature.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/processor_tracker_feature.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/processor_tracker_feature.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/processor_tracker_feature.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/processor_tracker_feature.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/processor_tracker_feature.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/constraint_analytic.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/constraint_analytic.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/constraint_analytic.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/constraint_analytic.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/constraint_analytic.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/constraint_analytic.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/processor_tracker.h ==================================
-#include <wolf/capture/capture_motion.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/processor_tracker.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/processor_tracker.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/processor_tracker.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/processor_tracker.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/processor_tracker.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/feature_pose.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/feature_pose.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/feature_pose.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/feature_pose.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/feature_pose.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/feature_pose.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/local_parametrization_homogeneous.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/local_parametrization_homogeneous.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/local_parametrization_homogeneous.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/local_parametrization_homogeneous.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/local_parametrization_homogeneous.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/local_parametrization_homogeneous.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/processor_loopclosure_base.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/processor_loopclosure_base.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/processor_loopclosure_base.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/processor_loopclosure_base.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/processor_loopclosure_base.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/processor_loopclosure_base.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/local_parametrization_base.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/local_parametrization_base.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/local_parametrization_base.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/local_parametrization_base.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/local_parametrization_base.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/local_parametrization_base.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/state_angle.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/state_angle.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/state_angle.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/state_angle.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/state_angle.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/state_angle.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/hardware_base.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/hardware_base.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/hardware_base.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/hardware_base.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/hardware_base.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/hardware_base.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/landmark_base.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/landmark_base.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/landmark_base.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/landmark_base.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/landmark_base.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/landmark_base.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/state_block.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/state_block.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/state_block.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/state_block.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/state_block.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/state_block.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/state_quaternion.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/state_quaternion.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/state_quaternion.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/state_quaternion.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/state_quaternion.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/state_quaternion.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/sensor_base.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/sensor_base.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/sensor_base.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/sensor_base.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/sensor_base.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/sensor_base.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/feature_match.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/feature_match.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/feature_match.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/feature_match.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/feature_match.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/feature_match.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/three_D_tools.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/three_D_tools.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/three_D_tools.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/three_D_tools.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/three_D_tools.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/three_D_tools.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/processor_motion.h ==================================
-#include <wolf/capture/capture_motion.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/processor_motion.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/processor_motion.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/processor_motion.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/processor_motion.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/processor_motion.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/constraint_autodiff.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/constraint_autodiff.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/constraint_autodiff.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/constraint_autodiff.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/constraint_autodiff.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/constraint_autodiff.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/processor_factory.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/processor_factory.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/processor_factory.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/processor_factory.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/processor_factory.h ==================================
- * #include <wolf/processor/processor_odom_2d.h> // provides ProcessorOdom2d and ProcessorFactory
- * #include <wolf/processor/processor_odom_2d.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/processor_factory.h ==================================
- * #include <wolf/sensor/sensor_odom_2d.h> // provides SensorOdom2d and SensorFactory
- * #include <wolf/sensor/sensor_odom_2d.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/local_parametrization_quaternion.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/local_parametrization_quaternion.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/local_parametrization_quaternion.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/local_parametrization_quaternion.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/local_parametrization_quaternion.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/local_parametrization_quaternion.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/feature_pose.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/feature_pose.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/feature_pose.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/feature_pose.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/feature_pose.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/feature_pose.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/capture_void.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/capture_void.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/capture_void.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/capture_void.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/capture_void.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/capture_void.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/local_parametrization_quaternion.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/local_parametrization_quaternion.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/local_parametrization_quaternion.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/local_parametrization_quaternion.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/local_parametrization_quaternion.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/local_parametrization_quaternion.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/time_stamp.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/time_stamp.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/time_stamp.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/time_stamp.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/time_stamp.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/time_stamp.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/processor_loopclosure_base.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/processor_loopclosure_base.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/processor_loopclosure_base.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/processor_loopclosure_base.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/processor_loopclosure_base.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/processor_loopclosure_base.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/hardware_base.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/hardware_base.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/hardware_base.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/hardware_base.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/hardware_base.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/hardware_base.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/capture_base.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/capture_base.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/capture_base.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/capture_base.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/capture_base.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/capture_base.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/map_base.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/map_base.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/map_base.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/map_base.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/map_base.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/map_base.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/feature_base.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/feature_base.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/feature_base.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/feature_base.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/feature_base.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/feature_base.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/processor_tracker_feature.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/processor_tracker_feature.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/processor_tracker_feature.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/processor_tracker_feature.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/processor_tracker_feature.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/processor_tracker_feature.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/processor_base.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/processor_base.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/processor_base.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/processor_base.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/processor_base.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/processor_base.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/processor_tracker.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/processor_tracker.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/processor_tracker.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/processor_tracker.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/processor_tracker.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/processor_tracker.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/local_parametrization_homogeneous.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/local_parametrization_homogeneous.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/local_parametrization_homogeneous.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/local_parametrization_homogeneous.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/local_parametrization_homogeneous.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/local_parametrization_homogeneous.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/capture_pose.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/capture_pose.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/capture_pose.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/capture_pose.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/capture_pose.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/capture_pose.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/motion_buffer.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/motion_buffer.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/motion_buffer.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/motion_buffer.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/motion_buffer.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/motion_buffer.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/node_base.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/node_base.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/node_base.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/node_base.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/node_base.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/node_base.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/constraint_base.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/constraint_base.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/constraint_base.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/constraint_base.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/constraint_base.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/constraint_base.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/constraint_analytic.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/constraint_analytic.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/constraint_analytic.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/constraint_analytic.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/constraint_analytic.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/constraint_analytic.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/sensor_base.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/sensor_base.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/sensor_base.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/sensor_base.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/sensor_base.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/sensor_base.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/track_matrix.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/track_matrix.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/track_matrix.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/track_matrix.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/track_matrix.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/track_matrix.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/problem.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/problem.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/problem.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/problem.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/problem.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/problem.cpp ==================================
-#include <wolf/sensor/sensor_GPS.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/processor_motion.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/processor_motion.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/processor_motion.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/processor_motion.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/processor_motion.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/processor_motion.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/frame_base.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/frame_base.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/frame_base.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/frame_base.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/frame_base.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/frame_base.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/trajectory_base.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/trajectory_base.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/trajectory_base.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/trajectory_base.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/trajectory_base.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/trajectory_base.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/landmark_base.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/landmark_base.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/landmark_base.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/landmark_base.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/landmark_base.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/landmark_base.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/local_parametrization_base.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/local_parametrization_base.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/local_parametrization_base.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/local_parametrization_base.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/local_parametrization_base.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/src/local_parametrization_base.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_virtual_hierarchy.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_virtual_hierarchy.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_virtual_hierarchy.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_virtual_hierarchy.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_virtual_hierarchy.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_virtual_hierarchy.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_virtual_hierarchy.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_processor_tracker_feature.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_processor_tracker_feature.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_processor_tracker_feature.cpp ==================================
-#include <wolf/core/wolf.h>
-#include <wolf/core/problem.h>
-#include <wolf/core/sensor_base.h>
-#include <wolf/core/state_block.h>
-#include <wolf/core/capture_void.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_processor_tracker_feature.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_processor_tracker_feature.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_processor_tracker_feature.cpp ==================================
-#include <wolf/processor/processor_tracker_feature_dummy.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_processor_tracker_feature.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_wolf_tree.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_wolf_tree.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_wolf_tree.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_wolf_tree.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_wolf_tree.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_wolf_tree.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_wolf_tree.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/processor_image_vision_utils.yaml ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/processor_image_vision_utils.yaml ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/processor_image_vision_utils.yaml ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/processor_image_vision_utils.yaml ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/processor_image_vision_utils.yaml ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/processor_image_vision_utils.yaml ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/processor_image_vision_utils.yaml ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_imuPlateform_Offline.cpp ==================================
-#include <wolf/capture/capture_IMU.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_imuPlateform_Offline.cpp ==================================
-#include <wolf/constraint/constraint_odom_3d.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_imuPlateform_Offline.cpp ==================================
-#include <wolf/core/wolf.h>
-#include <wolf/core/problem.h>
-#include <wolf/core/state_block.h>
-#include <wolf/core/state_quaternion.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_imuPlateform_Offline.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_imuPlateform_Offline.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_imuPlateform_Offline.cpp ==================================
-#include <wolf/processor/processor_IMU.h>
-#include <wolf/processor/processor_odom_3d.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_imuPlateform_Offline.cpp ==================================
-#include <wolf/sensor/sensor_IMU.h>
-#include <wolf/sensor/sensor_odom_3d.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_constraint_imu.cpp ==================================
-#include <wolf/capture/capture_IMU.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_constraint_imu.cpp ==================================
-#include <wolf/constraint/constraint_odom_3d.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_constraint_imu.cpp ==================================
-#include <wolf/core/capture_pose.h>
-#include <wolf/core/wolf.h>
-#include <wolf/core/problem.h>
-#include <wolf/core/state_block.h>
-#include <wolf/core/state_quaternion.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_constraint_imu.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_constraint_imu.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_constraint_imu.cpp ==================================
-#include <wolf/processor/processor_IMU.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_constraint_imu.cpp ==================================
-#include <wolf/sensor/sensor_IMU.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_image.cpp ==================================
-#include <wolf/capture/capture_image.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_image.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_image.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_image.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_image.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_image.cpp ==================================
-#include <wolf/processor/processor_tracker_feature_image.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_image.cpp ==================================
-#include <wolf/sensor/sensor_camera.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/solver/test_iQR.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/solver/test_iQR.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/solver/test_iQR.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/solver/test_iQR.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/solver/test_iQR.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/solver/test_iQR.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/solver/test_iQR.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/solver/test_ccolamd_blocks.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/solver/test_ccolamd_blocks.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/solver/test_ccolamd_blocks.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/solver/test_ccolamd_blocks.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/solver/test_ccolamd_blocks.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/solver/test_ccolamd_blocks.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/solver/test_ccolamd_blocks.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/solver/test_incremental_ccolamd_blocks.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/solver/test_incremental_ccolamd_blocks.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/solver/test_incremental_ccolamd_blocks.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/solver/test_incremental_ccolamd_blocks.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/solver/test_incremental_ccolamd_blocks.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/solver/test_incremental_ccolamd_blocks.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/solver/test_incremental_ccolamd_blocks.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/solver/test_ccolamd.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/solver/test_ccolamd.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/solver/test_ccolamd.cpp ==================================
-#include <wolf/core/wolf.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/solver/test_ccolamd.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/solver/test_ccolamd.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/solver/test_ccolamd.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/solver/test_ccolamd.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/solver/test_iQR_wolf.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/solver/test_iQR_wolf.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/solver/test_iQR_wolf.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/solver/test_iQR_wolf.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/solver/test_iQR_wolf.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/solver/test_iQR_wolf.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/solver/test_iQR_wolf.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/solver/test_iQR_wolf2.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/solver/test_iQR_wolf2.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/solver/test_iQR_wolf2.cpp ==================================
-#include <wolf/core/state_block.h>
-#include <wolf/core/constraint_base.h>
-#include <wolf/core/logging.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/solver/test_iQR_wolf2.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/solver/test_iQR_wolf2.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/solver/test_iQR_wolf2.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/solver/test_iQR_wolf2.cpp ==================================
-#include <wolf/sensor/sensor_laser_2d.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/solver/test_permutations.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/solver/test_permutations.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/solver/test_permutations.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/solver/test_permutations.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/solver/test_permutations.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/solver/test_permutations.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/solver/test_permutations.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/solver/test_SPQR.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/solver/test_SPQR.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/solver/test_SPQR.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/solver/test_SPQR.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/solver/test_SPQR.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/solver/test_SPQR.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/solver/test_SPQR.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/input_M3500b_toro.graph ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/input_M3500b_toro.graph ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/input_M3500b_toro.graph ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/input_M3500b_toro.graph ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/input_M3500b_toro.graph ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/input_M3500b_toro.graph ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/input_M3500b_toro.graph ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/.gitignore ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/.gitignore ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/.gitignore ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/.gitignore ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/.gitignore ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/.gitignore ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/.gitignore ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/camera_params_canonical.yaml ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/camera_params_canonical.yaml ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/camera_params_canonical.yaml ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/camera_params_canonical.yaml ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/camera_params_canonical.yaml ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/camera_params_canonical.yaml ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/camera_params_canonical.yaml ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/Test_ORB.png ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/Test_ORB.png ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/Test_ORB.png ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/Test_ORB.png ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/Test_ORB.png ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/Test_ORB.png ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/Test_ORB.png ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_list_remove.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_list_remove.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_list_remove.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_list_remove.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_list_remove.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_list_remove.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_list_remove.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_wolf_imported_graph.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_wolf_imported_graph.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_wolf_imported_graph.cpp ==================================
-#include <wolf/core/capture_void.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_wolf_imported_graph.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_wolf_imported_graph.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_wolf_imported_graph.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_wolf_imported_graph.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/processor_imu_t6.yaml ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/processor_imu_t6.yaml ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/processor_imu_t6.yaml ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/processor_imu_t6.yaml ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/processor_imu_t6.yaml ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/processor_imu_t6.yaml ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/processor_imu_t6.yaml ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_state_quaternion.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_state_quaternion.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_state_quaternion.cpp ==================================
-#include <wolf/core/frame_base.h>
-#include <wolf/core/state_quaternion.h>
-#include <wolf/core/time_stamp.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_state_quaternion.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_state_quaternion.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_state_quaternion.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_state_quaternion.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/camera_params_ueye_sim.yaml ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/camera_params_ueye_sim.yaml ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/camera_params_ueye_sim.yaml ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/camera_params_ueye_sim.yaml ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/camera_params_ueye_sim.yaml ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/camera_params_ueye_sim.yaml ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/camera_params_ueye_sim.yaml ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_wolf_logging.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_wolf_logging.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_wolf_logging.cpp ==================================
-#include <wolf/core/wolf.h>
-#include <wolf/core/logging.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_wolf_logging.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_wolf_logging.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_wolf_logging.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_wolf_logging.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/camera_params_ueye.yaml ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/camera_params_ueye.yaml ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/camera_params_ueye.yaml ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/camera_params_ueye.yaml ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/camera_params_ueye.yaml ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/camera_params_ueye.yaml ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/camera_params_ueye.yaml ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_diff_drive.cpp ==================================
-#include <wolf/capture/capture_wheel_joint_position.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_diff_drive.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_diff_drive.cpp ==================================
-#include <wolf/core/wolf.h>
-#include <wolf/core/problem.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_diff_drive.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_diff_drive.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_diff_drive.cpp ==================================
-#include <wolf/processor/processor_diff_drive.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_diff_drive.cpp ==================================
-#include <wolf/sensor/sensor_diff_drive.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_ceres_2_lasers_polylines.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_ceres_2_lasers_polylines.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_ceres_2_lasers_polylines.cpp ==================================
-#include <wolf/core/logging.h>
-#include <wolf/core/problem.h>
-#include <wolf/core/capture_pose.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_ceres_2_lasers_polylines.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_ceres_2_lasers_polylines.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_ceres_2_lasers_polylines.cpp ==================================
-#include <wolf/processor/processor_tracker_landmark_polyline.h>
-#include <wolf/processor/processor_odom_2d.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_ceres_2_lasers_polylines.cpp ==================================
-#include <wolf/sensor/sensor_GPS_fix.h>
-#include <wolf/sensor/sensor_laser_2d.h>
-#include <wolf/sensor/sensor_odom_2d.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/processor_imu_no_vote.yaml ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/processor_imu_no_vote.yaml ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/processor_imu_no_vote.yaml ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/processor_imu_no_vote.yaml ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/processor_imu_no_vote.yaml ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/processor_imu_no_vote.yaml ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/processor_imu_no_vote.yaml ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/sensor_imu.yaml ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/sensor_imu.yaml ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/sensor_imu.yaml ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/sensor_imu.yaml ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/sensor_imu.yaml ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/sensor_imu.yaml ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/sensor_imu.yaml ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/processor_imu_t1.yaml ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/processor_imu_t1.yaml ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/processor_imu_t1.yaml ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/processor_imu_t1.yaml ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/processor_imu_t1.yaml ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/processor_imu_t1.yaml ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/processor_imu_t1.yaml ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_capture_laser_2d.cpp ==================================
-#include <wolf/capture/capture_laser_2d.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_capture_laser_2d.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_capture_laser_2d.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_capture_laser_2d.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_capture_laser_2d.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_capture_laser_2d.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_capture_laser_2d.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/ACTIVESEARCH.yaml ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/ACTIVESEARCH.yaml ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/ACTIVESEARCH.yaml ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/ACTIVESEARCH.yaml ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/ACTIVESEARCH.yaml ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/ACTIVESEARCH.yaml ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/ACTIVESEARCH.yaml ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_map_yaml.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_map_yaml.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_map_yaml.cpp ==================================
-#include <wolf/core/wolf.h>
-#include <wolf/core/problem.h>
-#include <wolf/core/map_base.h>
-#include <wolf/core/state_block.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_map_yaml.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_map_yaml.cpp ==================================
-#include <wolf/landmark/landmark_polyline_2d.h>
-#include <wolf/landmark/landmark_AHP.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_map_yaml.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_map_yaml.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/map_polyline_example.yaml ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/map_polyline_example.yaml ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/map_polyline_example.yaml ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/map_polyline_example.yaml ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/map_polyline_example.yaml ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/map_polyline_example.yaml ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/map_polyline_example.yaml ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_yaml.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_yaml.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_yaml.cpp ==================================
-#include <wolf/core/factory.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_yaml.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_yaml.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_yaml.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_yaml.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_processor_tracker_landmark.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_processor_tracker_landmark.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_processor_tracker_landmark.cpp ==================================
-#include <wolf/core/wolf.h>
-#include <wolf/core/problem.h>
-#include <wolf/core/sensor_base.h>
-#include <wolf/core/state_block.h>
-#include <wolf/core/capture_void.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_processor_tracker_landmark.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_processor_tracker_landmark.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_processor_tracker_landmark.cpp ==================================
-#include <wolf/processor/processor_tracker_landmark_dummy.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_processor_tracker_landmark.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/processor_tracker_feature_trifocal.yaml ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/processor_tracker_feature_trifocal.yaml ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/processor_tracker_feature_trifocal.yaml ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/processor_tracker_feature_trifocal.yaml ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/processor_tracker_feature_trifocal.yaml ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/processor_tracker_feature_trifocal.yaml ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/processor_tracker_feature_trifocal.yaml ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_fcn_ptr.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_fcn_ptr.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_fcn_ptr.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_fcn_ptr.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_fcn_ptr.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_fcn_ptr.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_fcn_ptr.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_simple_AHP.cpp ==================================
-#include <wolf/capture/capture_image.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_simple_AHP.cpp ==================================
-#include <wolf/constraint/constraint_AHP.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_simple_AHP.cpp ==================================
-#include <wolf/core/wolf.h>
-#include <wolf/core/frame_base.h>
-#include <wolf/core/rotations.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_simple_AHP.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_simple_AHP.cpp ==================================
-#include <wolf/landmark/landmark_AHP.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_simple_AHP.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_simple_AHP.cpp ==================================
-#include <wolf/sensor/sensor_camera.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_sparsification.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_sparsification.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_sparsification.cpp ==================================
-#include <wolf/core/capture_void.h>
-#include <wolf/core/constraint_base.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_sparsification.cpp ==================================
-#include <wolf/feature/feature_odom_2d.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_sparsification.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_sparsification.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_sparsification.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_ceres_2_lasers.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_ceres_2_lasers.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_ceres_2_lasers.cpp ==================================
-#include <wolf/core/logging.h>
-#include <wolf/core/problem.h>
-#include <wolf/core/capture_pose.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_ceres_2_lasers.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_ceres_2_lasers.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_ceres_2_lasers.cpp ==================================
-#include <wolf/processor/processor_tracker_landmark_corner.h>
-#include <wolf/processor/processor_odom_2d.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_ceres_2_lasers.cpp ==================================
-#include <wolf/sensor/sensor_GPS_fix.h>
-#include <wolf/sensor/sensor_laser_2d.h>
-#include <wolf/sensor/sensor_odom_2d.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_eigen_quaternion.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_eigen_quaternion.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_eigen_quaternion.cpp ==================================
-#include <wolf/core/wolf.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_eigen_quaternion.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_eigen_quaternion.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_eigen_quaternion.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_eigen_quaternion.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/line_acc.dat ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/line_acc.dat ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/line_acc.dat ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/line_acc.dat ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/line_acc.dat ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/line_acc.dat ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/line_acc.dat ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_autodiff.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_autodiff.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_autodiff.cpp ==================================
-#include <wolf/core/logging.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_autodiff.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_autodiff.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_autodiff.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_autodiff.cpp ==================================
-#include <wolf/sensor/sensor_laser_2d.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_tracker_ORB.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_tracker_ORB.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_tracker_ORB.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_tracker_ORB.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_tracker_ORB.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_tracker_ORB.cpp ==================================
-#include <wolf/processor/processor_tracker_landmark_image.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_tracker_ORB.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_wolf_factories.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_wolf_factories.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_wolf_factories.cpp ==================================
-#include <wolf/core/hardware_base.h>
-#include <wolf/core/problem.h>
-#include <wolf/core/factory.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_wolf_factories.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_wolf_factories.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_wolf_factories.cpp ==================================
-#include <wolf/processor/processor_IMU.h>
-#include <wolf/processor/processor_odom_2d.h>
-#include <wolf/processor/processor_odom_3d.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_wolf_factories.cpp ==================================
-#include <wolf/sensor/sensor_GPS_fix.h>
-#include <wolf/sensor/sensor_camera.h>
-#include <wolf/sensor/sensor_odom_2d.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_sh_ptr.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_sh_ptr.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_sh_ptr.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_sh_ptr.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_sh_ptr.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_sh_ptr.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_sh_ptr.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_imuDock.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_imuDock.cpp ==================================
-#include <wolf/constraint/constraint_fix_bias.h>
-#include <wolf/constraint/constraint_pose_3d.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_imuDock.cpp ==================================
-#include <wolf/core/wolf.h>
-#include <wolf/core/problem.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_imuDock.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_imuDock.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_imuDock.cpp ==================================
-#include <wolf/processor/processor_IMU.h>
-#include <wolf/processor/processor_odom_3d.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_imuDock.cpp ==================================
-#include <wolf/sensor/sensor_IMU.h>
-#include <wolf/sensor/sensor_odom_3d.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_2_lasers_offline.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_2_lasers_offline.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_2_lasers_offline.cpp ==================================
-#include <wolf/core/logging.h>
-#include <wolf/core/problem.h>
-#include <wolf/core/capture_pose.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_2_lasers_offline.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_2_lasers_offline.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_2_lasers_offline.cpp ==================================
-#include <wolf/processor/processor_tracker_landmark_corner.h>
-#include <wolf/processor/processor_odom_2d.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_2_lasers_offline.cpp ==================================
-#include <wolf/sensor/sensor_GPS_fix.h>
-#include <wolf/sensor/sensor_laser_2d.h>
-#include <wolf/sensor/sensor_odom_2d.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_yaml_conversions.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_yaml_conversions.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_yaml_conversions.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_yaml_conversions.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_yaml_conversions.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_yaml_conversions.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_yaml_conversions.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_processor_tracker_landmark_image.cpp ==================================
-#include <wolf/capture/capture_image.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_processor_tracker_landmark_image.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_processor_tracker_landmark_image.cpp ==================================
-#include <wolf/core/wolf.h>
-#include <wolf/core/problem.h>
-#include <wolf/core/state_block.h>
-#include <wolf/core/capture_pose.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_processor_tracker_landmark_image.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_processor_tracker_landmark_image.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_processor_tracker_landmark_image.cpp ==================================
-#include <wolf/processor/processor_tracker_landmark_image.h>
-#include <wolf/processor/processor_odom_3d.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_processor_tracker_landmark_image.cpp ==================================
-#include <wolf/sensor/sensor_odom_3d.h>
-#include <wolf/sensor/sensor_camera.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_eigen_template.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_eigen_template.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_eigen_template.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_eigen_template.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_eigen_template.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_eigen_template.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_eigen_template.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/processor_odom_3d.yaml ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/processor_odom_3d.yaml ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/processor_odom_3d.yaml ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/processor_odom_3d.yaml ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/processor_odom_3d.yaml ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/processor_odom_3d.yaml ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/processor_odom_3d.yaml ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/sensor_odom_3d_HQ.yaml ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/sensor_odom_3d_HQ.yaml ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/sensor_odom_3d_HQ.yaml ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/sensor_odom_3d_HQ.yaml ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/sensor_odom_3d_HQ.yaml ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/sensor_odom_3d_HQ.yaml ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/sensor_odom_3d_HQ.yaml ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_matrix_prod.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_matrix_prod.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_matrix_prod.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_matrix_prod.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_matrix_prod.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_matrix_prod.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_matrix_prod.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/processor_imu.yaml ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/processor_imu.yaml ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/processor_imu.yaml ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/processor_imu.yaml ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/processor_imu.yaml ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/processor_imu.yaml ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/processor_imu.yaml ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_wolf_autodiffwrapper.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_wolf_autodiffwrapper.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_wolf_autodiffwrapper.cpp ==================================
-#include <wolf/core/capture_void.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_wolf_autodiffwrapper.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_wolf_autodiffwrapper.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_wolf_autodiffwrapper.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_wolf_autodiffwrapper.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_mpu.cpp ==================================
-#include <wolf/capture/capture_IMU.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_mpu.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_mpu.cpp ==================================
-#include <wolf/core/wolf.h>
-#include <wolf/core/problem.h>
-#include <wolf/core/state_block.h>
-#include <wolf/core/state_quaternion.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_mpu.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_mpu.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_mpu.cpp ==================================
-#include <wolf/processor/processor_IMU.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_mpu.cpp ==================================
-#include <wolf/sensor/sensor_IMU.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_projection_points.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_projection_points.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_projection_points.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_projection_points.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_projection_points.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_projection_points.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_projection_points.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/line_gyro.dat ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/line_gyro.dat ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/line_gyro.dat ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/line_gyro.dat ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/line_gyro.dat ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/line_gyro.dat ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/line_gyro.dat ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_wolf_prunning.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_wolf_prunning.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_wolf_prunning.cpp ==================================
-#include <wolf/core/capture_void.h>
-#include <wolf/core/constraint_base.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_wolf_prunning.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_wolf_prunning.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_wolf_prunning.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_wolf_prunning.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/camera_params.yaml ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/camera_params.yaml ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/camera_params.yaml ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/camera_params.yaml ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/camera_params.yaml ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/camera_params.yaml ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/camera_params.yaml ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_kf_callback.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_kf_callback.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_kf_callback.cpp ==================================
-#include <wolf/core/capture_void.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_kf_callback.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_kf_callback.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_kf_callback.cpp ==================================
-#include <wolf/processor/processor_odom_2d.h>
-#include <wolf/processor/processor_tracker_feature_dummy.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_kf_callback.cpp ==================================
-#include <wolf/sensor/sensor_odom_2d.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_faramotics_simulation.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_faramotics_simulation.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_faramotics_simulation.cpp ==================================
-#include <wolf/core/wolf.h>
-#include <wolf/core/feature_base.h>
-#include <wolf/core/landmark_base.h>
-#include <wolf/core/state_block.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_faramotics_simulation.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_faramotics_simulation.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_faramotics_simulation.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_faramotics_simulation.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_imuDock_autoKFs.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_imuDock_autoKFs.cpp ==================================
-#include <wolf/constraint/constraint_fix_bias.h>
-#include <wolf/constraint/constraint_pose_3d.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_imuDock_autoKFs.cpp ==================================
-#include <wolf/core/wolf.h>
-#include <wolf/core/problem.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_imuDock_autoKFs.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_imuDock_autoKFs.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_imuDock_autoKFs.cpp ==================================
-#include <wolf/processor/processor_IMU.h>
-#include <wolf/processor/processor_odom_3d.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_imuDock_autoKFs.cpp ==================================
-#include <wolf/sensor/sensor_IMU.h>
-#include <wolf/sensor/sensor_odom_3d.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_analytic_odom_constraint.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_analytic_odom_constraint.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_analytic_odom_constraint.cpp ==================================
-#include <wolf/core/capture_void.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_analytic_odom_constraint.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_analytic_odom_constraint.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_analytic_odom_constraint.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_analytic_odom_constraint.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/processor_image_feature.yaml ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/processor_image_feature.yaml ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/processor_image_feature.yaml ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/processor_image_feature.yaml ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/processor_image_feature.yaml ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/processor_image_feature.yaml ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/processor_image_feature.yaml ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_constraint_odom_3d.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_constraint_odom_3d.cpp ==================================
-#include <wolf/constraint/constraint_odom_3d.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_constraint_odom_3d.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_constraint_odom_3d.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_constraint_odom_3d.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_constraint_odom_3d.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_constraint_odom_3d.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_processor_odom_3d.cpp ==================================
-#include <wolf/capture/capture_IMU.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_processor_odom_3d.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_processor_odom_3d.cpp ==================================
-#include <wolf/core/problem.h>
-#include <wolf/core/map_base.h>
-#include <wolf/core/landmark_base.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_processor_odom_3d.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_processor_odom_3d.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_processor_odom_3d.cpp ==================================
-#include <wolf/processor/processor_odom_3d.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_processor_odom_3d.cpp ==================================
-#include <wolf/sensor/sensor_odom_2d.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_wolf_root.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_wolf_root.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_wolf_root.cpp ==================================
-#include <wolf/core/wolf.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_wolf_root.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_wolf_root.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_wolf_root.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_wolf_root.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_imu_constrained0.cpp ==================================
-#include <wolf/capture/capture_IMU.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_imu_constrained0.cpp ==================================
-#include <wolf/constraint/constraint_odom_3d.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_imu_constrained0.cpp ==================================
-#include <wolf/core/wolf.h>
-#include <wolf/core/problem.h>
-#include <wolf/core/state_block.h>
-#include <wolf/core/state_quaternion.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_imu_constrained0.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_imu_constrained0.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_imu_constrained0.cpp ==================================
-#include <wolf/processor/processor_IMU.h>
-#include <wolf/processor/processor_odom_3d.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_imu_constrained0.cpp ==================================
-#include <wolf/sensor/sensor_IMU.h>
-#include <wolf/sensor/sensor_odom_3d.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_constraint_AHP.cpp ==================================
-#include <wolf/capture/capture_image.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_constraint_AHP.cpp ==================================
-#include <wolf/constraint/constraint_AHP.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_constraint_AHP.cpp ==================================
-#include <wolf/core/state_block.h>
-#include <wolf/core/state_quaternion.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_constraint_AHP.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_constraint_AHP.cpp ==================================
-#include <wolf/landmark/landmark_AHP.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_constraint_AHP.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_constraint_AHP.cpp ==================================
-#include <wolf/sensor/sensor_camera.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/sensor_odom_3d.yaml ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/sensor_odom_3d.yaml ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/sensor_odom_3d.yaml ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/sensor_odom_3d.yaml ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/sensor_odom_3d.yaml ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/sensor_odom_3d.yaml ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/sensor_odom_3d.yaml ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_processor_imu_jacobians.cpp ==================================
-#include <wolf/capture/capture_IMU.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_processor_imu_jacobians.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_processor_imu_jacobians.cpp ==================================
-#include <wolf/core/wolf.h>
-#include <wolf/core/problem.h>
-#include <wolf/core/state_block.h>
-#include <wolf/core/state_quaternion.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_processor_imu_jacobians.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_processor_imu_jacobians.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_processor_imu_jacobians.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_processor_imu_jacobians.cpp ==================================
-#include <wolf/sensor/sensor_IMU.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/CMakeLists.txt ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/CMakeLists.txt ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/CMakeLists.txt ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/CMakeLists.txt ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/CMakeLists.txt ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/CMakeLists.txt ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/CMakeLists.txt ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/camera_params_ueye_radial_dist.yaml ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/camera_params_ueye_radial_dist.yaml ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/camera_params_ueye_radial_dist.yaml ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/camera_params_ueye_radial_dist.yaml ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/camera_params_ueye_radial_dist.yaml ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/camera_params_ueye_radial_dist.yaml ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/camera_params_ueye_radial_dist.yaml ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_sort_keyframes.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_sort_keyframes.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_sort_keyframes.cpp ==================================
-#include <wolf/core/wolf.h>
-#include <wolf/core/problem.h>
-#include <wolf/core/frame_base.h>
-#include <wolf/core/trajectory_base.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_sort_keyframes.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_sort_keyframes.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_sort_keyframes.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_sort_keyframes.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_processor_imu.cpp ==================================
-#include <wolf/capture/capture_IMU.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_processor_imu.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_processor_imu.cpp ==================================
-#include <wolf/core/wolf.h>
-#include <wolf/core/problem.h>
-#include <wolf/core/state_block.h>
-#include <wolf/core/state_quaternion.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_processor_imu.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_processor_imu.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_processor_imu.cpp ==================================
-#include <wolf/processor/processor_IMU.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_processor_imu.cpp ==================================
-#include <wolf/sensor/sensor_IMU.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/vision_utils_active_search.yaml ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/vision_utils_active_search.yaml ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/vision_utils_active_search.yaml ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/vision_utils_active_search.yaml ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/vision_utils_active_search.yaml ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/vision_utils_active_search.yaml ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/vision_utils_active_search.yaml ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_odom_2d.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_odom_2d.h ==================================
-#include <wolf/constraint/constraint_odom_2d.h>
-#include <wolf/constraint/constraint_odom_2d_analytic.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_odom_2d.h ==================================
-#include <wolf/core/feature_base.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_odom_2d.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_odom_2d.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_odom_2d.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_line_2d.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_line_2d.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_line_2d.h ==================================
-#include <wolf/core/feature_base.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_line_2d.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_line_2d.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_line_2d.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_motion.h ==================================
-#include <wolf/capture/capture_motion.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_motion.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_motion.h ==================================
-#include <wolf/core/feature_base.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_motion.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_motion.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_motion.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_IMU.h ==================================
-#include <wolf/capture/capture_IMU.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_IMU.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_IMU.h ==================================
-#include <wolf/core/feature_base.h>
-#include <wolf/core/wolf.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_IMU.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_IMU.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_IMU.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_corner_2d.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_corner_2d.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_corner_2d.h ==================================
-#include <wolf/core/feature_base.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_corner_2d.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_corner_2d.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_corner_2d.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_GPS_pseudorange.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_GPS_pseudorange.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_GPS_pseudorange.h ==================================
-#include <wolf/core/feature_base.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_GPS_pseudorange.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_GPS_pseudorange.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_GPS_pseudorange.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_point_image.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_point_image.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_point_image.h ==================================
-#include <wolf/core/feature_base.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_point_image.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_point_image.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_point_image.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_GPS_fix.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_GPS_fix.h ==================================
-#include <wolf/constraint/constraint_GPS_2d.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_GPS_fix.h ==================================
-#include <wolf/core/feature_base.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_GPS_fix.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_GPS_fix.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_GPS_fix.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_polyline_2d.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_polyline_2d.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_polyline_2d.h ==================================
-#include <wolf/core/feature_base.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_polyline_2d.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_polyline_2d.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_polyline_2d.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_diff_drive.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_diff_drive.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_diff_drive.h ==================================
-#include <wolf/core/feature_base.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_diff_drive.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_diff_drive.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_diff_drive.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_point_image.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_point_image.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_point_image.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_point_image.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_point_image.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_point_image.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_GPS_pseudorange.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_GPS_pseudorange.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_GPS_pseudorange.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_GPS_pseudorange.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_GPS_pseudorange.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_GPS_pseudorange.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_odom_2d.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_odom_2d.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_odom_2d.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_odom_2d.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_odom_2d.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_odom_2d.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_polyline_2d.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_polyline_2d.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_polyline_2d.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_polyline_2d.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_polyline_2d.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_polyline_2d.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_diff_drive.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_diff_drive.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_diff_drive.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_diff_drive.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_diff_drive.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_diff_drive.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_IMU.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_IMU.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_IMU.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_IMU.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_IMU.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_IMU.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_motion.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_motion.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_motion.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_motion.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_motion.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_motion.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_corner_2d.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_corner_2d.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_corner_2d.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_corner_2d.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_corner_2d.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_corner_2d.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_GPS_fix.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_GPS_fix.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_GPS_fix.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_GPS_fix.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_GPS_fix.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_GPS_fix.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_line_2d.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_line_2d.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_line_2d.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_line_2d.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_line_2d.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_line_2d.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_point_3d.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_point_3d.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_point_3d.h ==================================
-#include <wolf/core/landmark_base.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_point_3d.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_point_3d.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_point_3d.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_container.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_container.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_container.h ==================================
-#include <wolf/core/landmark_base.h>
-#include <wolf/core/wolf.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_container.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_container.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_container.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_match.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_match.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_match.h ==================================
-#include <wolf/core/wolf.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_match.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_match.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_match.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_AHP.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_AHP.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_AHP.h ==================================
-#include <wolf/core/landmark_base.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_AHP.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_AHP.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_AHP.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_polyline_2d.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_polyline_2d.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_polyline_2d.h ==================================
-#include <wolf/core/landmark_base.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_polyline_2d.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_polyline_2d.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_polyline_2d.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_corner_2d.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_corner_2d.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_corner_2d.h ==================================
-#include <wolf/core/landmark_base.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_corner_2d.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_corner_2d.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_corner_2d.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_line_2d.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_line_2d.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_line_2d.h ==================================
-#include <wolf/core/landmark_base.h>
-#include <wolf/core/wolf.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_line_2d.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_line_2d.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_line_2d.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_corner_2d.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_corner_2d.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_corner_2d.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_corner_2d.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_corner_2d.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_corner_2d.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_line_2d.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_line_2d.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_line_2d.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_line_2d.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_line_2d.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_line_2d.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_container.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_container.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_container.cpp ==================================
-#include <wolf/core/state_block.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_container.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_container.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_container.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_polyline_2d.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_polyline_2d.cpp ==================================
-#include <wolf/constraint/constraint_point_2d.h>
-#include <wolf/constraint/constraint_point_to_line_2d.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_polyline_2d.cpp ==================================
-#include <wolf/core/state_block.h>
-#include <wolf/core/factory.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_polyline_2d.cpp ==================================
-#include <wolf/feature/feature_polyline_2d.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_polyline_2d.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_polyline_2d.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_point_3d.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_point_3d.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_point_3d.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_point_3d.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_point_3d.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_point_3d.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_AHP.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_AHP.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_AHP.cpp ==================================
-#include <wolf/core/state_homogeneous_3d.h>
-#include <wolf/core/factory.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_AHP.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_AHP.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_AHP.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_tracker_feature_dummy.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_tracker_feature_dummy.h ==================================
-#include <wolf/constraint/constraint_epipolar.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_tracker_feature_dummy.h ==================================
-#include <wolf/core/wolf.h>
-#include <wolf/core/processor_tracker_feature.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_tracker_feature_dummy.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_tracker_feature_dummy.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_tracker_feature_dummy.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_GPS.h ==================================
-#include <wolf/capture/capture_GPS.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_GPS.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_GPS.h ==================================
-#include <wolf/core/processor_base.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_GPS.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_GPS.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_GPS.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_frame_nearest_neighbor_filter.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_frame_nearest_neighbor_filter.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_frame_nearest_neighbor_filter.h ==================================
-#include <wolf/core/processor_loopclosure_base.h>
-#include <wolf/core/state_block.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_frame_nearest_neighbor_filter.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_frame_nearest_neighbor_filter.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_frame_nearest_neighbor_filter.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_odom_2d.h ==================================
-#include <wolf/capture/capture_odom_2d.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_odom_2d.h ==================================
-#include <wolf/constraint/constraint_odom_2d.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_odom_2d.h ==================================
-#include <wolf/core/processor_motion.h>
-#include <wolf/core/rotations.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_odom_2d.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_odom_2d.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_odom_2d.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_tracker_landmark_corner.h ==================================
-#include <wolf/capture/capture_laser_2d.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_tracker_landmark_corner.h ==================================
-#include <wolf/constraint/constraint_corner_2d.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_tracker_landmark_corner.h ==================================
-#include <wolf/core/state_block.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_tracker_landmark_corner.h ==================================
-#include <wolf/feature/feature_corner_2d.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_tracker_landmark_corner.h ==================================
-#include <wolf/landmark/landmark_corner_2d.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_tracker_landmark_corner.h ==================================
-#include <wolf/sensor/sensor_laser_2d.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_tracker_feature_corner.h ==================================
-#include <wolf/capture/capture_laser_2d.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_tracker_feature_corner.h ==================================
-#include <wolf/constraint/constraint_corner_2d.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_tracker_feature_corner.h ==================================
-#include <wolf/core/state_block.h>
-#include <wolf/core/processor_tracker_feature.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_tracker_feature_corner.h ==================================
-#include <wolf/feature/feature_corner_2d.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_tracker_feature_corner.h ==================================
-#include <wolf/landmark/landmark_corner_2d.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_tracker_feature_corner.h ==================================
-#include <wolf/sensor/sensor_laser_2d.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_capture_holder.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_capture_holder.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_capture_holder.h ==================================
-#include <wolf/core/processor_base.h>
-#include <wolf/core/capture_base.h>
-#include <wolf/core/capture_buffer.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_capture_holder.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_capture_holder.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_capture_holder.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_diff_drive.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_diff_drive.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_diff_drive.h ==================================
-#include <wolf/core/processor_motion.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_diff_drive.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_diff_drive.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_diff_drive.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_tracker_landmark.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_tracker_landmark.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_tracker_landmark.h ==================================
-#include <wolf/core/processor_tracker.h>
-#include <wolf/core/capture_base.h>
-#include <wolf/core/landmark_base.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_tracker_landmark.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_tracker_landmark.h ==================================
-#include <wolf/landmark/landmark_match.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_tracker_landmark.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_tracker_landmark_polyline.h ==================================
-#include <wolf/capture/capture_laser_2d.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_tracker_landmark_polyline.h ==================================
-#include <wolf/constraint/constraint_point_2d.h>
-#include <wolf/constraint/constraint_point_to_line_2d.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_tracker_landmark_polyline.h ==================================
-#include <wolf/core/state_block.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_tracker_landmark_polyline.h ==================================
-#include <wolf/feature/feature_polyline_2d.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_tracker_landmark_polyline.h ==================================
-#include <wolf/landmark/landmark_polyline_2d.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_tracker_landmark_polyline.h ==================================
-#include <wolf/sensor/sensor_laser_2d.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_logging.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_logging.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_logging.h ==================================
-//#include <wolf/core/logging.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_logging.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_logging.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_logging.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_tracker_feature_trifocal.h ==================================
-#include <wolf/capture/capture_image.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_tracker_feature_trifocal.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_tracker_feature_trifocal.h ==================================
-#include <wolf/core/processor_tracker_feature.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_tracker_feature_trifocal.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_tracker_feature_trifocal.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_tracker_feature_trifocal.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_tracker_feature_image.h ==================================
-#include <wolf/capture/capture_image.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_tracker_feature_image.h ==================================
-#include <wolf/constraint/constraint_epipolar.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_tracker_feature_image.h ==================================
-#include <wolf/core/state_block.h>
-#include <wolf/core/state_quaternion.h>
-#include <wolf/core/processor_tracker_feature.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_tracker_feature_image.h ==================================
-#include <wolf/feature/feature_point_image.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_tracker_feature_image.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_tracker_feature_image.h ==================================
-#include <wolf/sensor/sensor_camera.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_odom_3d.h ==================================
-#include <wolf/capture/capture_odom_3d.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_odom_3d.h ==================================
-#include <wolf/constraint/constraint_odom_3d.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_odom_3d.h ==================================
-#include <wolf/core/processor_motion.h>
-#include <wolf/core/rotations.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_odom_3d.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_odom_3d.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_odom_3d.h ==================================
-#include <wolf/sensor/sensor_odom_3d.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_IMU.h ==================================
-#include <wolf/capture/capture_IMU.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_IMU.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_IMU.h ==================================
-#include <wolf/core/processor_motion.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_IMU.h ==================================
-#include <wolf/feature/feature_IMU.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_IMU.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_IMU.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_params_image.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_params_image.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_params_image.h ==================================
-#include <wolf/core/processor_tracker_feature.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_params_image.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_params_image.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_params_image.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_tracker_landmark_image.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_tracker_landmark_image.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_tracker_landmark_image.h ==================================
-#include <wolf/core/wolf.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_tracker_landmark_image.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_tracker_landmark_image.h ==================================
-#include <wolf/landmark/landmark_AHP.h>
-#include <wolf/landmark/landmark_match.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_tracker_landmark_image.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_tracker_landmark_dummy.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_tracker_landmark_dummy.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_tracker_landmark_dummy.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_tracker_landmark_dummy.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_tracker_landmark_dummy.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_tracker_landmark_dummy.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_tracker_landmark_corner.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_tracker_landmark_corner.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_tracker_landmark_corner.cpp ==================================
-#include <wolf/core/rotations.h>
-#include <wolf/core/processor_factory.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_tracker_landmark_corner.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_tracker_landmark_corner.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_tracker_landmark_corner.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_GPS.cpp ==================================
-#include <wolf/capture/capture_GPS.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_GPS.cpp ==================================
-#include <wolf/constraint/constraint_GPS_pseudorange_2d.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_GPS.cpp ==================================
-#include <wolf/core/processor_factory.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_GPS.cpp ==================================
-#include <wolf/feature/feature_GPS_pseudorange.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_GPS.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_GPS.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_tracker_feature_dummy.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_tracker_feature_dummy.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_tracker_feature_dummy.cpp ==================================
-#include <wolf/core/feature_base.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_tracker_feature_dummy.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_tracker_feature_dummy.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_tracker_feature_dummy.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_capture_holder.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_capture_holder.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_capture_holder.cpp ==================================
-#include <wolf/core/processor_factory.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_capture_holder.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_capture_holder.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_capture_holder.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_frame_nearest_neighbor_filter.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_frame_nearest_neighbor_filter.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_frame_nearest_neighbor_filter.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_frame_nearest_neighbor_filter.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_frame_nearest_neighbor_filter.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_frame_nearest_neighbor_filter.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_tracker_landmark.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_tracker_landmark.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_tracker_landmark.cpp ==================================
-#include <wolf/core/map_base.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_tracker_landmark.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_tracker_landmark.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_tracker_landmark.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_odom_3d.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_odom_3d.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_odom_3d.cpp ==================================
-#include <wolf/core/processor_factory.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_odom_3d.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_odom_3d.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_odom_3d.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_odom_2d.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_odom_2d.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_odom_2d.cpp ==================================
-#include <wolf/core/processor_factory.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_odom_2d.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_odom_2d.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_odom_2d.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_tracker_landmark_image.cpp ==================================
-#include <wolf/capture/capture_image.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_tracker_landmark_image.cpp ==================================
-#include <wolf/constraint/constraint_AHP.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_tracker_landmark_image.cpp ==================================
-#include <wolf/core/feature_base.h>
-#include <wolf/core/frame_base.h>
-#include <wolf/core/logging.h>
-#include <wolf/core/map_base.h>
-#include <wolf/core/problem.h>
-#include <wolf/core/state_block.h>
-#include <wolf/core/time_stamp.h>
-#include <wolf/core/processor_factory.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_tracker_landmark_image.cpp ==================================
-#include <wolf/feature/feature_point_image.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_tracker_landmark_image.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_tracker_landmark_image.cpp ==================================
-#include <wolf/sensor/sensor_camera.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_tracker_landmark_dummy.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_tracker_landmark_dummy.cpp ==================================
-#include <wolf/constraint/constraint_corner_2d.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_tracker_landmark_dummy.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_tracker_landmark_dummy.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_tracker_landmark_dummy.cpp ==================================
-#include <wolf/landmark/landmark_corner_2d.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_tracker_landmark_dummy.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_tracker_feature_trifocal.cpp ==================================
-#include <wolf/capture/capture_image.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_tracker_feature_trifocal.cpp ==================================
-#include <wolf/constraint/constraint_autodiff_trifocal.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_tracker_feature_trifocal.cpp ==================================
-#include <wolf/core/processor_factory.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_tracker_feature_trifocal.cpp ==================================
-#include <wolf/feature/feature_point_image.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_tracker_feature_trifocal.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_tracker_feature_trifocal.cpp ==================================
-#include <wolf/sensor/sensor_camera.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_tracker_feature_image.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_tracker_feature_image.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_tracker_feature_image.cpp ==================================
-#include <wolf/core/processor_factory.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_tracker_feature_image.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_tracker_feature_image.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_tracker_feature_image.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_IMU.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_IMU.cpp ==================================
-#include <wolf/constraint/constraint_IMU.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_IMU.cpp ==================================
-#include <wolf/core/processor_factory.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_IMU.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_IMU.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_IMU.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_diff_drive.cpp ==================================
-#include <wolf/capture/capture_wheel_joint_position.h>
-#include <wolf/capture/capture_velocity.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_diff_drive.cpp ==================================
-#include <wolf/constraint/constraint_odom_2d.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_diff_drive.cpp ==================================
-#include <wolf/core/rotations.h>
-#include <wolf/core/processor_factory.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_diff_drive.cpp ==================================
-#include <wolf/feature/feature_diff_drive.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_diff_drive.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_diff_drive.cpp ==================================
-#include <wolf/sensor/sensor_diff_drive.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_tracker_feature_corner.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_tracker_feature_corner.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_tracker_feature_corner.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_tracker_feature_corner.cpp ==================================
-#include <wolf/feature/feature_corner_2d.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_tracker_feature_corner.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_tracker_feature_corner.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_tracker_landmark_polyline.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_tracker_landmark_polyline.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_tracker_landmark_polyline.cpp ==================================
-#include <wolf/core/processor_factory.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_tracker_landmark_polyline.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_tracker_landmark_polyline.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_tracker_landmark_polyline.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_odom_3d.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_odom_3d.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_odom_3d.h ==================================
-#include <wolf/core/sensor_base.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_odom_3d.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_odom_3d.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_odom_3d.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_IMU.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_IMU.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_IMU.h ==================================
-#include <wolf/core/sensor_base.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_IMU.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_IMU.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_IMU.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_odom_2d.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_odom_2d.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_odom_2d.h ==================================
-#include <wolf/core/sensor_base.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_odom_2d.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_odom_2d.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_odom_2d.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_GPS_fix.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_GPS_fix.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_GPS_fix.h ==================================
-#include <wolf/core/sensor_base.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_GPS_fix.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_GPS_fix.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_GPS_fix.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_diff_drive.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_diff_drive.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_diff_drive.h ==================================
-#include <wolf/core/sensor_base.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_diff_drive.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_diff_drive.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_diff_drive.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_camera.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_camera.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_camera.h ==================================
-#include <wolf/core/sensor_base.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_camera.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_camera.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_camera.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_GPS.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_GPS.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_GPS.h ==================================
-#include <wolf/core/sensor_base.h>
-//#include <wolf/core/sensor_factory.h>
-//#include <wolf/core/factory.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_GPS.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_GPS.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_GPS.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_laser_2d.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_laser_2d.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_laser_2d.h ==================================
-#include <wolf/core/sensor_base.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_laser_2d.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_laser_2d.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_laser_2d.h ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_diff_drive.cpp ==================================
-#include <wolf/capture/capture_motion.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_diff_drive.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_diff_drive.cpp ==================================
-#include <wolf/core/state_block.h>
-#include <wolf/core/sensor_factory.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_diff_drive.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_diff_drive.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_diff_drive.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_GPS.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_GPS.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_GPS.cpp ==================================
-#include <wolf/core/state_block.h>
-#include <wolf/core/state_quaternion.h>
-#include <wolf/core/sensor_factory.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_GPS.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_GPS.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_GPS.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_odom_2d.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_odom_2d.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_odom_2d.cpp ==================================
-#include <wolf/core/state_block.h>
-#include <wolf/core/state_angle.h>
-#include <wolf/core/sensor_factory.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_odom_2d.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_odom_2d.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_odom_2d.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_IMU.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_IMU.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_IMU.cpp ==================================
-#include <wolf/core/state_block.h>
-#include <wolf/core/state_quaternion.h>
-#include <wolf/core/sensor_factory.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_IMU.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_IMU.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_IMU.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_odom_3d.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_odom_3d.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_odom_3d.cpp ==================================
-#include <wolf/core/state_block.h>
-#include <wolf/core/state_quaternion.h>
-#include <wolf/core/sensor_factory.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_odom_3d.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_odom_3d.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_odom_3d.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_laser_2d.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_laser_2d.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_laser_2d.cpp ==================================
-#include <wolf/core/state_block.h>
-#include <wolf/core/sensor_factory.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_laser_2d.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_laser_2d.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_laser_2d.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_GPS_fix.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_GPS_fix.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_GPS_fix.cpp ==================================
-#include <wolf/core/state_block.h>
-#include <wolf/core/state_quaternion.h>
-#include <wolf/core/sensor_factory.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_GPS_fix.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_GPS_fix.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_GPS_fix.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_camera.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_camera.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_camera.cpp ==================================
-#include <wolf/core/state_block.h>
-#include <wolf/core/state_quaternion.h>
-#include <wolf/core/sensor_factory.h>
-//#include <wolf/core/factory.h>
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_camera.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_camera.cpp ==================================
-================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_camera.cpp ==================================
-================================================================
diff --git a/wolf_scripts/templates/class_template.cpp b/wolf_scripts/templates/class_template.cpp
index 22a6cbe179c6c1b9cfd751153ab341e6566a102a..6306b63f52e70bac278fcb4d83f0b376efe0d425 100644
--- a/wolf_scripts/templates/class_template.cpp
+++ b/wolf_scripts/templates/class_template.cpp
@@ -19,20 +19,17 @@
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#include "header_file"
-
-namespace wolf {
+namespace wolf
+{
// Constructor
// TODO Modify this default API to suit your class needs
-class_name::class_name() :
- base_class()
+class_name::class_name() : base_class()
{
// TODO Add your code or remove this comment
}
// Destructor
-class_name::~class_name()
-{
-}
+class_name::~class_name() {}
-} // namespace wolf
+} // namespace wolf
diff --git a/wolf_scripts/templates/class_template.h b/wolf_scripts/templates/class_template.h
index 368f1bc347ed0347de13a67d66c99f1b70f113ab..849cbc5deea779a88b6c87b27efac77aacc9b7c5 100644
--- a/wolf_scripts/templates/class_template.h
+++ b/wolf_scripts/templates/class_template.h
@@ -20,27 +20,25 @@
#pragma once
-//Wolf includes
+// Wolf includes
#include "wolf.h"
#include "base_header_file"
namespace wolf
{
-
WOLF_PTR_TYPEDEFS(class_name);
class class_name : public base_class
{
- public:
-
- /** \brief Class constructor
- */
- // TODO Modify this default API to suit your class needs
- class_name();
-
- /** \brief Class Destructor
- */
- virtual ~class_name();
+ public:
+ /** \brief Class constructor
+ */
+ // TODO Modify this default API to suit your class needs
+ class_name();
+
+ /** \brief Class Destructor
+ */
+ virtual ~class_name();
};
-} // namespace wolf
+} // namespace wolf
diff --git a/wolf_scripts/templates/gtest_template.cpp b/wolf_scripts/templates/gtest_template.cpp
index a15aadaff4fda914c2448c6e383889b67f8b3f96..38e00df79eef976f02a18f4860e3cdd55c20c249 100644
--- a/wolf_scripts/templates/gtest_template.cpp
+++ b/wolf_scripts/templates/gtest_template.cpp
@@ -30,7 +30,7 @@ using namespace wolf;
using std::static_pointer_cast;
// Use the following in case you want to initialize tests with predefines variables or methods.
-//class class_name_class : public testing::Test{
+// class class_name_class : public testing::Test{
// public:
// virtual void SetUp()
// {
@@ -54,4 +54,3 @@ int main(int argc, char **argv)
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}
-
diff --git a/yaml_templates/NONE.yaml b/yaml_templates/NONE.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..21882391d69233abb210ce2fe6505635e0c28e73
--- /dev/null
+++ b/yaml_templates/NONE.yaml
@@ -0,0 +1 @@
+type: "NONE" # OPTIONAL - DOC Nothing to say here - TYPE string - OPTIONS [NONE]
\ No newline at end of file
diff --git a/yaml_templates/None.yaml b/yaml_templates/None.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..b33da9c353e6da477af3a54e5dce75bcf2c73c0a
--- /dev/null
+++ b/yaml_templates/None.yaml
@@ -0,0 +1 @@
+type: "None" # OPTIONAL - DOC Nothing to say here - TYPE string - OPTIONS [None]
\ No newline at end of file
diff --git a/yaml_templates/landmark/LandmarkBase.yaml b/yaml_templates/landmark/LandmarkBase.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..072ae815619b85f4781ef4055aef69cca0a0787c
--- /dev/null
+++ b/yaml_templates/landmark/LandmarkBase.yaml
@@ -0,0 +1,10 @@
+type: "whatever" # DOC Type of the landmark - TYPE string
+plugin: "core" # OPTIONAL - DOC Name of the wolf plugin where the landmark type is implemented. - TYPE string
+id: 0 # DOC Unique id of the landmark. - TYPE int
+states:
+ P:
+ type: "DerivedType" # DOC String corresponding to the name of the object class (and its schema file).
+ follow: some/path/to/derived/type/parameters.yaml
+ O:
+ type: "DerivedType" # DOC String corresponding to the name of the object class (and its schema file).
+ follow: some/path/to/derived/type/parameters.yaml
\ No newline at end of file
diff --git a/yaml_templates/map/MapBase.yaml b/yaml_templates/map/MapBase.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..c4e8e8a63b2240b8e7faba862fbd7d840fef5142
--- /dev/null
+++ b/yaml_templates/map/MapBase.yaml
@@ -0,0 +1,8 @@
+type: "MapBase" # OPTIONAL - DOC Type of the Map used in the problem. - TYPE string
+plugin: "core" # OPTIONAL - DOC Name of the wolf plugin where the map type is implemented. - TYPE string
+filename: "whatever" # OPTIONAL - DOC Optional. Absolute path of the YAML file containing the landmarks. - TYPE string
+landmarks:
+ - type: "DerivedType" # DOC String corresponding to the name of the object class (and its schema file).
+ follow: some/path/to/derived/type/parameters.yaml
+ - type: "DerivedType" # DOC String corresponding to the name of the object class (and its schema file).
+ follow: some/path/to/derived/type/parameters.yaml
\ No newline at end of file
diff --git a/yaml_templates/none.yaml b/yaml_templates/none.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..05c60e5cda02648613796dce0da2e237b6d24a05
--- /dev/null
+++ b/yaml_templates/none.yaml
@@ -0,0 +1 @@
+type: "none" # OPTIONAL - DOC Nothing to say here - TYPE string - OPTIONS [none]
\ No newline at end of file
diff --git a/yaml_templates/problem/Problem.yaml b/yaml_templates/problem/Problem.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..0816b8cd324fd06e25434cfeaaef963a476aa19e
--- /dev/null
+++ b/yaml_templates/problem/Problem.yaml
@@ -0,0 +1,18 @@
+problem:
+ tree_manager:
+ type: "DerivedType" # DOC String corresponding to the name of the object class (and its schema file).
+ follow: some/path/to/derived/type/parameters.yaml
+ dimension: 2 # DOC Dimension of the problem. "2" for 2D or "3" for 3D - TYPE int - OPTIONS [2, 3]
+map:
+ type: "DerivedType" # DOC String corresponding to the name of the object class (and its schema file).
+ follow: some/path/to/derived/type/parameters.yaml
+sensors:
+ - type: "DerivedType" # DOC String corresponding to the name of the object class (and its schema file).
+ follow: some/path/to/derived/type/parameters.yaml
+ - type: "DerivedType" # DOC String corresponding to the name of the object class (and its schema file).
+ follow: some/path/to/derived/type/parameters.yaml
+processors:
+ - type: "DerivedType" # DOC String corresponding to the name of the object class (and its schema file).
+ follow: some/path/to/derived/type/parameters.yaml
+ - type: "DerivedType" # DOC String corresponding to the name of the object class (and its schema file).
+ follow: some/path/to/derived/type/parameters.yaml
\ No newline at end of file
diff --git a/yaml_templates/problem/Problem2d.yaml b/yaml_templates/problem/Problem2d.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..3d07895c065af68a193a4657e9b473664b22fc4d
--- /dev/null
+++ b/yaml_templates/problem/Problem2d.yaml
@@ -0,0 +1,27 @@
+problem:
+ tree_manager:
+ type: "DerivedType" # DOC String corresponding to the name of the object class (and its schema file).
+ follow: some/path/to/derived/type/parameters.yaml
+ dimension: 2 # DOC Dimension of the problem: "2" for 2D - TYPE int - OPTIONS [2]
+ first_frame:
+ P:
+ state: [0.0, 0.0] # DOC A vector containing the state values - TYPE Vector2d
+ mode: "fix" # DOC The prior mode can be "factor" to add an absolute factor (requires "noise_std"), "fix" to set the values constant or "initial_guess" to just set the values - TYPE string - OPTIONS [fix, factor, initial_guess]
+ noise_std: [0.0, 0.0] # MANDATORY if $mode == "factor" - DOC A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix. - TYPE Vector2d
+ O:
+ state: [0.0] # DOC A vector containing the state values - TYPE Vector1d
+ mode: "fix" # DOC The prior mode can be "factor" to add an absolute factor (requires "noise_std"), "fix" to set the values constant or "initial_guess" to just set the values - TYPE string - OPTIONS [fix, factor, initial_guess]
+ noise_std: [0.0] # MANDATORY if $mode == "factor" - DOC A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix. - TYPE Vector1d
+map:
+ type: "DerivedType" # DOC String corresponding to the name of the object class (and its schema file).
+ follow: some/path/to/derived/type/parameters.yaml
+sensors:
+ - type: "DerivedType" # DOC String corresponding to the name of the object class (and its schema file).
+ follow: some/path/to/derived/type/parameters.yaml
+ - type: "DerivedType" # DOC String corresponding to the name of the object class (and its schema file).
+ follow: some/path/to/derived/type/parameters.yaml
+processors:
+ - type: "DerivedType" # DOC String corresponding to the name of the object class (and its schema file).
+ follow: some/path/to/derived/type/parameters.yaml
+ - type: "DerivedType" # DOC String corresponding to the name of the object class (and its schema file).
+ follow: some/path/to/derived/type/parameters.yaml
\ No newline at end of file
diff --git a/yaml_templates/problem/Problem3d.yaml b/yaml_templates/problem/Problem3d.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..ac58bb2bb262fb8bda8604738ced87e4f81df766
--- /dev/null
+++ b/yaml_templates/problem/Problem3d.yaml
@@ -0,0 +1,27 @@
+problem:
+ tree_manager:
+ type: "DerivedType" # DOC String corresponding to the name of the object class (and its schema file).
+ follow: some/path/to/derived/type/parameters.yaml
+ dimension: 3 # DOC Dimension of the problem: "3" for 3D - TYPE int - OPTIONS [3]
+ first_frame:
+ P:
+ state: [0.0, 0.0, 0.0] # DOC A vector containing the state "P" values - TYPE Vector3d
+ mode: "fix" # DOC The prior mode can be "factor" to add an absolute factor (requires "noise_std"), "fix" to set the values constant or "initial_guess" to just set the values - TYPE string - OPTIONS [fix, factor, initial_guess]
+ noise_std: [0.0, 0.0, 0.0] # MANDATORY if $mode == "factor" - DOC A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix. - TYPE Vector3d
+ O:
+ state: [0.0, 0.0, 0.0, 0.0] # DOC A vector containing the state values. It should be a quaternion (i.e. four values and normalized) - TYPE Vector4d
+ mode: "fix" # DOC The prior mode can be "factor" to add an absolute factor (requires "noise_std"), "fix" to set the values constant or "initial_guess" to just set the values - TYPE string - OPTIONS [fix, factor, initial_guess]
+ noise_std: [0.0, 0.0, 0.0] # MANDATORY if $mode == "factor" - DOC A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix. - TYPE Vector3d
+map:
+ type: "DerivedType" # DOC String corresponding to the name of the object class (and its schema file).
+ follow: some/path/to/derived/type/parameters.yaml
+sensors:
+ - type: "DerivedType" # DOC String corresponding to the name of the object class (and its schema file).
+ follow: some/path/to/derived/type/parameters.yaml
+ - type: "DerivedType" # DOC String corresponding to the name of the object class (and its schema file).
+ follow: some/path/to/derived/type/parameters.yaml
+processors:
+ - type: "DerivedType" # DOC String corresponding to the name of the object class (and its schema file).
+ follow: some/path/to/derived/type/parameters.yaml
+ - type: "DerivedType" # DOC String corresponding to the name of the object class (and its schema file).
+ follow: some/path/to/derived/type/parameters.yaml
\ No newline at end of file
diff --git a/yaml_templates/processor/MotionProvider.yaml b/yaml_templates/processor/MotionProvider.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..494d64098a1b1fa29f03b25c39b80fe3525880d4
--- /dev/null
+++ b/yaml_templates/processor/MotionProvider.yaml
@@ -0,0 +1,2 @@
+state_provider: false # DOC If the processor is used by the problem as provider of state. - TYPE bool
+state_provider_order: 0.0 # MANDATORY if $state_provider - DOC The order number of this processor when problem gets the state (only used if state_provider = true). Two processors cannot have the same priority (if so, when installing the second is increased). - TYPE double
\ No newline at end of file
diff --git a/yaml_templates/processor/ProcessorBase.yaml b/yaml_templates/processor/ProcessorBase.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..4ffa5b648aba3301a4b02b38ce8ddfaed458b682
--- /dev/null
+++ b/yaml_templates/processor/ProcessorBase.yaml
@@ -0,0 +1,5 @@
+name: "whatever" # DOC The processor"s name. It has to be unique. - TYPE string
+time_tolerance: 0.0 # DOC Maximum time difference between a Keyframe time stamp and a particular Capture of this processor to allow assigning this Capture to the Keyframe. [s]. - TYPE double
+keyframe_vote:
+ voting_active: false # DOC If the processor is allowed to decide to create a frame. - TYPE bool
+apply_loss_function: false # DOC If the factors created by the processor have loss function. - TYPE bool
\ No newline at end of file
diff --git a/yaml_templates/processor/ProcessorDiffDrive.yaml b/yaml_templates/processor/ProcessorDiffDrive.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..b4a6592a942ddcd19c0b3e0470dfeb5aec499056
--- /dev/null
+++ b/yaml_templates/processor/ProcessorDiffDrive.yaml
@@ -0,0 +1,13 @@
+name: "whatever" # DOC The processor"s name. It has to be unique. - TYPE string
+time_tolerance: 0.0 # DOC Maximum time difference between a Keyframe time stamp and a particular Capture of this processor to allow assigning this Capture to the Keyframe. [s]. - TYPE double
+keyframe_vote:
+ voting_active: false # DOC If the processor is allowed to decide to create a frame. - TYPE bool
+ max_time_span: 0.0 # MANDATORY if $voting_active - DOC Time threshold to create a new frame [s]. - TYPE double
+ max_buff_length: 0 # MANDATORY if $voting_active - DOC Maximum size of the buffer of motions. - TYPE unsigned int
+ max_dist_traveled: 0.0 # MANDATORY if $voting_active - DOC Distance traveled threshold to create a new frame [m]. - TYPE double
+ max_angle_turned: 0.0 # MANDATORY if $voting_active - DOC Angle turned threshold to create a new frame [rad]. - TYPE double
+ cov_det: 0.0 # MANDATORY if $voting_active - DOC The determinant threshold of the covariance matrix of the integrated delta, to vote for a keyframe. - TYPE double
+apply_loss_function: false # DOC If the factors created by the processor have loss function. - TYPE bool
+state_provider: false # DOC If the processor is used by the problem as provider of state. - TYPE bool
+state_provider_order: 0.0 # MANDATORY if $state_provider - DOC The order number of this processor when problem gets the state (only used if state_provider = true). Two processors cannot have the same priority (if so, when installing the second is increased). - TYPE double
+unmeasured_perturbation_std: 0.0 # DOC Noise (standard deviation) of the integrated movement in the not observed directions. - TYPE double
\ No newline at end of file
diff --git a/yaml_templates/processor/ProcessorFixedWingModel.yaml b/yaml_templates/processor/ProcessorFixedWingModel.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..d072fd837010d150cc5be6fa4cb555d4f10a726e
--- /dev/null
+++ b/yaml_templates/processor/ProcessorFixedWingModel.yaml
@@ -0,0 +1,8 @@
+name: "whatever" # DOC The processor"s name. It has to be unique. - TYPE string
+time_tolerance: 0.0 # DOC Maximum time difference between a Keyframe time stamp and a particular Capture of this processor to allow assigning this Capture to the Keyframe. [s]. - TYPE double
+keyframe_vote:
+ voting_active: false # DOC If the processor is allowed to decide to create a frame. - TYPE bool
+apply_loss_function: false # DOC If the factors created by the processor have loss function. - TYPE bool
+velocity_local: [0.0, 0.0, 0.0] # DOC The vector with the direction of the velocity in local coordinates - TYPE Vector3d
+angle_stdev: 0.0 # DOC The standard deviation noise to create the angular residuals - TYPE double
+min_vel_norm: 0.0 # DOC The velocity norm threshold to apply factors - TYPE double
\ No newline at end of file
diff --git a/yaml_templates/processor/ProcessorLandmarkExternal.yaml b/yaml_templates/processor/ProcessorLandmarkExternal.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..86fa8cf9995d9b966b6599232077693b84589849
--- /dev/null
+++ b/yaml_templates/processor/ProcessorLandmarkExternal.yaml
@@ -0,0 +1,14 @@
+name: "whatever" # DOC The processor"s name. It has to be unique. - TYPE string
+time_tolerance: 0.0 # DOC Maximum time difference between a Keyframe time stamp and a particular Capture of this processor to allow assigning this Capture to the Keyframe. [s]. - TYPE double
+keyframe_vote:
+ voting_active: false # DOC If the processor is allowed to decide to create a frame. - TYPE bool
+ min_features_for_keyframe: 0 # MANDATORY if $voting_active - DOC Minimum number of features to vote for keyframe. - TYPE unsigned int
+ time_span: 0.0 # DOC the time span from the last keyframe to vote for a new keyframe (sufficient condition) - TYPE double
+ new_features_for_keyframe: 0 # DOC the amount of new features sufficient to create a new keyframe (sufficient condition) - TYPE unsigned int
+apply_loss_function: false # DOC If the factors created by the processor have loss function. - TYPE bool
+max_new_features: 0 # DOC Maximum number of new features to be processed when adding a keyframe (-1=unlimited. 0=none.) - TYPE int
+filter:
+ quality_th: 0.0 # DOC The minimum quality to consider the detection - TYPE double
+ track_length_th: 0 # DOC The minimum track length to consider the detection - TYPE unsigned int
+use_orientation: false # DOC If the orientation measurement is considered when emplacing factors - TYPE bool
+match_dist_th: 0.0 # DOC the threshold in distance for considering a match between landmarks and detections - TYPE double
\ No newline at end of file
diff --git a/yaml_templates/processor/ProcessorLoopClosure.yaml b/yaml_templates/processor/ProcessorLoopClosure.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..f9fce055a4afe931c74d63be4d0f7fa582f31bd3
--- /dev/null
+++ b/yaml_templates/processor/ProcessorLoopClosure.yaml
@@ -0,0 +1,6 @@
+name: "whatever" # DOC The processor"s name. It has to be unique. - TYPE string
+time_tolerance: 0.0 # DOC Maximum time difference between a Keyframe time stamp and a particular Capture of this processor to allow assigning this Capture to the Keyframe. [s]. - TYPE double
+keyframe_vote:
+ voting_active: false # DOC If the processor is allowed to decide to create a frame. - TYPE bool
+apply_loss_function: false # DOC If the factors created by the processor have loss function. - TYPE bool
+max_loops: 0 # DOC The maximum amount of loops that can be closed each keyframe callback. -1 (or any negative value) is unlimited loops. - TYPE int
\ No newline at end of file
diff --git a/yaml_templates/processor/ProcessorMotion.yaml b/yaml_templates/processor/ProcessorMotion.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..eae067e48e88a1c2c3c7c4339daccdc2f820a3f5
--- /dev/null
+++ b/yaml_templates/processor/ProcessorMotion.yaml
@@ -0,0 +1,12 @@
+name: "whatever" # DOC The processor"s name. It has to be unique. - TYPE string
+time_tolerance: 0.0 # DOC Maximum time difference between a Keyframe time stamp and a particular Capture of this processor to allow assigning this Capture to the Keyframe. [s]. - TYPE double
+keyframe_vote:
+ voting_active: false # DOC If the processor is allowed to decide to create a frame. - TYPE bool
+ max_time_span: 0.0 # MANDATORY if $voting_active - DOC Time threshold to create a new frame [s]. - TYPE double
+ max_buff_length: 0 # MANDATORY if $voting_active - DOC Maximum size of the buffer of motions. - TYPE unsigned int
+ max_dist_traveled: 0.0 # MANDATORY if $voting_active - DOC Distance traveled threshold to create a new frame [m]. - TYPE double
+ max_angle_turned: 0.0 # MANDATORY if $voting_active - DOC Angle turned threshold to create a new frame [rad]. - TYPE double
+apply_loss_function: false # DOC If the factors created by the processor have loss function. - TYPE bool
+state_provider: false # DOC If the processor is used by the problem as provider of state. - TYPE bool
+state_provider_order: 0.0 # MANDATORY if $state_provider - DOC The order number of this processor when problem gets the state (only used if state_provider = true). Two processors cannot have the same priority (if so, when installing the second is increased). - TYPE double
+unmeasured_perturbation_std: 0.0 # DOC Noise (standard deviation) of the integrated movement in the not observed directions. - TYPE double
\ No newline at end of file
diff --git a/yaml_templates/processor/ProcessorOdom2d.yaml b/yaml_templates/processor/ProcessorOdom2d.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..b4a6592a942ddcd19c0b3e0470dfeb5aec499056
--- /dev/null
+++ b/yaml_templates/processor/ProcessorOdom2d.yaml
@@ -0,0 +1,13 @@
+name: "whatever" # DOC The processor"s name. It has to be unique. - TYPE string
+time_tolerance: 0.0 # DOC Maximum time difference between a Keyframe time stamp and a particular Capture of this processor to allow assigning this Capture to the Keyframe. [s]. - TYPE double
+keyframe_vote:
+ voting_active: false # DOC If the processor is allowed to decide to create a frame. - TYPE bool
+ max_time_span: 0.0 # MANDATORY if $voting_active - DOC Time threshold to create a new frame [s]. - TYPE double
+ max_buff_length: 0 # MANDATORY if $voting_active - DOC Maximum size of the buffer of motions. - TYPE unsigned int
+ max_dist_traveled: 0.0 # MANDATORY if $voting_active - DOC Distance traveled threshold to create a new frame [m]. - TYPE double
+ max_angle_turned: 0.0 # MANDATORY if $voting_active - DOC Angle turned threshold to create a new frame [rad]. - TYPE double
+ cov_det: 0.0 # MANDATORY if $voting_active - DOC The determinant threshold of the covariance matrix of the integrated delta, to vote for a keyframe. - TYPE double
+apply_loss_function: false # DOC If the factors created by the processor have loss function. - TYPE bool
+state_provider: false # DOC If the processor is used by the problem as provider of state. - TYPE bool
+state_provider_order: 0.0 # MANDATORY if $state_provider - DOC The order number of this processor when problem gets the state (only used if state_provider = true). Two processors cannot have the same priority (if so, when installing the second is increased). - TYPE double
+unmeasured_perturbation_std: 0.0 # DOC Noise (standard deviation) of the integrated movement in the not observed directions. - TYPE double
\ No newline at end of file
diff --git a/yaml_templates/processor/ProcessorOdom3d.yaml b/yaml_templates/processor/ProcessorOdom3d.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..eae067e48e88a1c2c3c7c4339daccdc2f820a3f5
--- /dev/null
+++ b/yaml_templates/processor/ProcessorOdom3d.yaml
@@ -0,0 +1,12 @@
+name: "whatever" # DOC The processor"s name. It has to be unique. - TYPE string
+time_tolerance: 0.0 # DOC Maximum time difference between a Keyframe time stamp and a particular Capture of this processor to allow assigning this Capture to the Keyframe. [s]. - TYPE double
+keyframe_vote:
+ voting_active: false # DOC If the processor is allowed to decide to create a frame. - TYPE bool
+ max_time_span: 0.0 # MANDATORY if $voting_active - DOC Time threshold to create a new frame [s]. - TYPE double
+ max_buff_length: 0 # MANDATORY if $voting_active - DOC Maximum size of the buffer of motions. - TYPE unsigned int
+ max_dist_traveled: 0.0 # MANDATORY if $voting_active - DOC Distance traveled threshold to create a new frame [m]. - TYPE double
+ max_angle_turned: 0.0 # MANDATORY if $voting_active - DOC Angle turned threshold to create a new frame [rad]. - TYPE double
+apply_loss_function: false # DOC If the factors created by the processor have loss function. - TYPE bool
+state_provider: false # DOC If the processor is used by the problem as provider of state. - TYPE bool
+state_provider_order: 0.0 # MANDATORY if $state_provider - DOC The order number of this processor when problem gets the state (only used if state_provider = true). Two processors cannot have the same priority (if so, when installing the second is increased). - TYPE double
+unmeasured_perturbation_std: 0.0 # DOC Noise (standard deviation) of the integrated movement in the not observed directions. - TYPE double
\ No newline at end of file
diff --git a/yaml_templates/processor/ProcessorPose2d.yaml b/yaml_templates/processor/ProcessorPose2d.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..4ffa5b648aba3301a4b02b38ce8ddfaed458b682
--- /dev/null
+++ b/yaml_templates/processor/ProcessorPose2d.yaml
@@ -0,0 +1,5 @@
+name: "whatever" # DOC The processor"s name. It has to be unique. - TYPE string
+time_tolerance: 0.0 # DOC Maximum time difference between a Keyframe time stamp and a particular Capture of this processor to allow assigning this Capture to the Keyframe. [s]. - TYPE double
+keyframe_vote:
+ voting_active: false # DOC If the processor is allowed to decide to create a frame. - TYPE bool
+apply_loss_function: false # DOC If the factors created by the processor have loss function. - TYPE bool
\ No newline at end of file
diff --git a/yaml_templates/processor/ProcessorPose3d.yaml b/yaml_templates/processor/ProcessorPose3d.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..4ffa5b648aba3301a4b02b38ce8ddfaed458b682
--- /dev/null
+++ b/yaml_templates/processor/ProcessorPose3d.yaml
@@ -0,0 +1,5 @@
+name: "whatever" # DOC The processor"s name. It has to be unique. - TYPE string
+time_tolerance: 0.0 # DOC Maximum time difference between a Keyframe time stamp and a particular Capture of this processor to allow assigning this Capture to the Keyframe. [s]. - TYPE double
+keyframe_vote:
+ voting_active: false # DOC If the processor is allowed to decide to create a frame. - TYPE bool
+apply_loss_function: false # DOC If the factors created by the processor have loss function. - TYPE bool
\ No newline at end of file
diff --git a/yaml_templates/processor/ProcessorSequence.yaml b/yaml_templates/processor/ProcessorSequence.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..dde44f4747cfe9bab3982faa855d6eb8221eadfb
--- /dev/null
+++ b/yaml_templates/processor/ProcessorSequence.yaml
@@ -0,0 +1,7 @@
+name: "whatever" # DOC The processor"s name. It has to be unique. - TYPE string
+time_tolerance: 0.0 # DOC Maximum time difference between a Keyframe time stamp and a particular Capture of this processor to allow assigning this Capture to the Keyframe. [s]. - TYPE double
+keyframe_vote:
+ voting_active: false # DOC If the processor is allowed to decide to create a frame. - TYPE bool
+apply_loss_function: false # DOC If the factors created by the processor have loss function. - TYPE bool
+type: "whatever" # DOC The processor"s class name. - TYPE string
+plugin: "whatever" # DOC The name of the wolf plugin where the processor is implemented. - TYPE string
\ No newline at end of file
diff --git a/yaml_templates/processor/ProcessorTracker.yaml b/yaml_templates/processor/ProcessorTracker.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..0789aea55c4be275382ada137f8d08136b7e8406
--- /dev/null
+++ b/yaml_templates/processor/ProcessorTracker.yaml
@@ -0,0 +1,7 @@
+name: "whatever" # DOC The processor"s name. It has to be unique. - TYPE string
+time_tolerance: 0.0 # DOC Maximum time difference between a Keyframe time stamp and a particular Capture of this processor to allow assigning this Capture to the Keyframe. [s]. - TYPE double
+keyframe_vote:
+ voting_active: false # DOC If the processor is allowed to decide to create a frame. - TYPE bool
+ min_features_for_keyframe: 0 # MANDATORY if $voting_active - DOC Minimum number of features to vote for keyframe. - TYPE unsigned int
+apply_loss_function: false # DOC If the factors created by the processor have loss function. - TYPE bool
+max_new_features: 0 # DOC Maximum number of new features to be processed when adding a keyframe (-1=unlimited. 0=none.) - TYPE int
\ No newline at end of file
diff --git a/yaml_templates/processor/ProcessorTrackerFeature.yaml b/yaml_templates/processor/ProcessorTrackerFeature.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..0789aea55c4be275382ada137f8d08136b7e8406
--- /dev/null
+++ b/yaml_templates/processor/ProcessorTrackerFeature.yaml
@@ -0,0 +1,7 @@
+name: "whatever" # DOC The processor"s name. It has to be unique. - TYPE string
+time_tolerance: 0.0 # DOC Maximum time difference between a Keyframe time stamp and a particular Capture of this processor to allow assigning this Capture to the Keyframe. [s]. - TYPE double
+keyframe_vote:
+ voting_active: false # DOC If the processor is allowed to decide to create a frame. - TYPE bool
+ min_features_for_keyframe: 0 # MANDATORY if $voting_active - DOC Minimum number of features to vote for keyframe. - TYPE unsigned int
+apply_loss_function: false # DOC If the factors created by the processor have loss function. - TYPE bool
+max_new_features: 0 # DOC Maximum number of new features to be processed when adding a keyframe (-1=unlimited. 0=none.) - TYPE int
\ No newline at end of file
diff --git a/yaml_templates/processor/ProcessorTrackerLandmark.yaml b/yaml_templates/processor/ProcessorTrackerLandmark.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..0789aea55c4be275382ada137f8d08136b7e8406
--- /dev/null
+++ b/yaml_templates/processor/ProcessorTrackerLandmark.yaml
@@ -0,0 +1,7 @@
+name: "whatever" # DOC The processor"s name. It has to be unique. - TYPE string
+time_tolerance: 0.0 # DOC Maximum time difference between a Keyframe time stamp and a particular Capture of this processor to allow assigning this Capture to the Keyframe. [s]. - TYPE double
+keyframe_vote:
+ voting_active: false # DOC If the processor is allowed to decide to create a frame. - TYPE bool
+ min_features_for_keyframe: 0 # MANDATORY if $voting_active - DOC Minimum number of features to vote for keyframe. - TYPE unsigned int
+apply_loss_function: false # DOC If the factors created by the processor have loss function. - TYPE bool
+max_new_features: 0 # DOC Maximum number of new features to be processed when adding a keyframe (-1=unlimited. 0=none.) - TYPE int
\ No newline at end of file
diff --git a/yaml_templates/sensor/SensorBase.yaml b/yaml_templates/sensor/SensorBase.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..c24db30d6f9579400fd6ce2323d3176408a4c5b5
--- /dev/null
+++ b/yaml_templates/sensor/SensorBase.yaml
@@ -0,0 +1,3 @@
+name: "whatever" # DOC The sensor"s name. It has to be unique. - TYPE string
+states:
+ keys: "whatever" # DOC The keys corresponding to the states of the sensor, to be filled with _value in the derived schemas. - TYPE string
\ No newline at end of file
diff --git a/yaml_templates/sensor/SensorDiffDrive.yaml b/yaml_templates/sensor/SensorDiffDrive.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..517a0a696711a381069080694f5eb72a74188aba
--- /dev/null
+++ b/yaml_templates/sensor/SensorDiffDrive.yaml
@@ -0,0 +1,22 @@
+name: "whatever" # DOC The sensor"s name. It has to be unique. - TYPE string
+states:
+ P:
+ state: [0.0, 0.0] # DOC A vector containing the state values - TYPE Vector2d
+ mode: "fix" # DOC The prior mode can be "factor" to add an absolute factor (requires "noise_std"), "fix" to set the values constant or "initial_guess" to just set the values - TYPE string - OPTIONS [fix, factor, initial_guess]
+ noise_std: [0.0, 0.0] # MANDATORY if $mode == "factor" - DOC A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix. - TYPE Vector2d
+ dynamic: false # DOC If the state is dynamic, i.e. it changes along time. - TYPE bool
+ drift_std: [0.0, 0.0] # OPTIONAL - DOC A vector containing the stdev values of the noise of the drift factor (only if dynamic==true), i.e. the sqrt of the diagonal elements of the covariance matrix. - TYPE Vector2d
+ O:
+ state: [0.0] # DOC A vector containing the state values - TYPE Vector1d
+ mode: "fix" # DOC The prior mode can be "factor" to add an absolute factor (requires "noise_std"), "fix" to set the values constant or "initial_guess" to just set the values - TYPE string - OPTIONS [fix, factor, initial_guess]
+ noise_std: [0.0] # MANDATORY if $mode == "factor" - DOC A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix. - TYPE Vector1d
+ dynamic: false # DOC If the state is dynamic, i.e. it changes along time. - TYPE bool
+ drift_std: [0.0] # OPTIONAL - DOC A vector containing the stdev values of the noise of the drift factor (only if dynamic==true), i.e. the sqrt of the diagonal elements of the covariance matrix. - TYPE Vector1d
+ I:
+ state: [0.0, 0.0, 0.0] # DOC A vector containing the intrinsic state values. 0=left wheel radius (m), 1=right wheel radius (m), 2=wheel separation (m) - TYPE Vector3d
+ mode: "fix" # DOC The prior mode can be "factor" to add an absolute factor (requires "noise_std"), "fix" to set the values constant or "initial_guess" to just set the values - TYPE string - OPTIONS [fix, factor, initial_guess]
+ noise_std: [0.0, 0.0, 0.0] # MANDATORY if $mode == "factor" - DOC A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix. - TYPE Vector3d
+ dynamic: false # DOC If the state is dynamic, i.e. it changes along time. - TYPE bool
+ drift_std: [0.0, 0.0, 0.0] # OPTIONAL - DOC A vector containing the stdev values of the noise of the drift factor (only if dynamic==true), i.e. the sqrt of the diagonal elements of the covariance matrix. - TYPE Vector3d
+ticks_per_wheel_revolution: 0.0 # DOC ratio of displacement variance to displacement, for odometry noise calculation. - TYPE double
+ticks_std_factor: 0.0 # DOC ratio of displacement variance to rotation, for odometry noise calculation. - TYPE double
\ No newline at end of file
diff --git a/yaml_templates/sensor/SensorMotionModel.yaml b/yaml_templates/sensor/SensorMotionModel.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..302faa16bc06b96df9c1b6102b1b939bd36208f0
--- /dev/null
+++ b/yaml_templates/sensor/SensorMotionModel.yaml
@@ -0,0 +1,3 @@
+name: "whatever" # DOC The sensor"s name. It has to be unique. - TYPE string
+states:
+ {}
\ No newline at end of file
diff --git a/yaml_templates/sensor/SensorOdom2d.yaml b/yaml_templates/sensor/SensorOdom2d.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..502df10cc84a114adc789a4d1fe9628cf3ee9f75
--- /dev/null
+++ b/yaml_templates/sensor/SensorOdom2d.yaml
@@ -0,0 +1,19 @@
+name: "whatever" # DOC The sensor"s name. It has to be unique. - TYPE string
+states:
+ P:
+ state: [0.0, 0.0] # DOC A vector containing the state values - TYPE Vector2d
+ mode: "fix" # DOC The prior mode can be "factor" to add an absolute factor (requires "noise_std"), "fix" to set the values constant or "initial_guess" to just set the values - TYPE string - OPTIONS [fix, factor, initial_guess]
+ noise_std: [0.0, 0.0] # MANDATORY if $mode == "factor" - DOC A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix. - TYPE Vector2d
+ dynamic: false # DOC If the state is dynamic, i.e. it changes along time. - TYPE bool
+ drift_std: [0.0, 0.0] # OPTIONAL - DOC A vector containing the stdev values of the noise of the drift factor (only if dynamic==true), i.e. the sqrt of the diagonal elements of the covariance matrix. - TYPE Vector2d
+ O:
+ state: [0.0] # DOC A vector containing the state values - TYPE Vector1d
+ mode: "fix" # DOC The prior mode can be "factor" to add an absolute factor (requires "noise_std"), "fix" to set the values constant or "initial_guess" to just set the values - TYPE string - OPTIONS [fix, factor, initial_guess]
+ noise_std: [0.0] # MANDATORY if $mode == "factor" - DOC A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix. - TYPE Vector1d
+ dynamic: false # DOC If the state is dynamic, i.e. it changes along time. - TYPE bool
+ drift_std: [0.0] # OPTIONAL - DOC A vector containing the stdev values of the noise of the drift factor (only if dynamic==true), i.e. the sqrt of the diagonal elements of the covariance matrix. - TYPE Vector1d
+k_disp_to_disp: 0.0 # DOC ratio of displacement variance to displacement, for odometry noise calculation. - TYPE double
+k_disp_to_rot: 0.0 # DOC ratio of displacement variance to rotation, for odometry noise calculation. - TYPE double
+k_rot_to_rot: 0.0 # DOC ratio of rotation variance to rotation, for odometry noise calculation. - TYPE double
+min_disp_var: 0.0 # DOC minimum displacement variance, for odometry noise calculation. - TYPE double
+min_rot_var: 0.0 # DOC minimum rotation variance, for odometry noise calculation. - TYPE double
\ No newline at end of file
diff --git a/yaml_templates/sensor/SensorOdom3d.yaml b/yaml_templates/sensor/SensorOdom3d.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..09723e6eb5bb1795f7c842c4cab9f2336321ccb1
--- /dev/null
+++ b/yaml_templates/sensor/SensorOdom3d.yaml
@@ -0,0 +1,19 @@
+name: "whatever" # DOC The sensor"s name. It has to be unique. - TYPE string
+states:
+ P:
+ state: [0.0, 0.0, 0.0] # DOC A vector containing the state "P" values - TYPE Vector3d
+ mode: "fix" # DOC The prior mode can be "factor" to add an absolute factor (requires "noise_std"), "fix" to set the values constant or "initial_guess" to just set the values - TYPE string - OPTIONS [fix, factor, initial_guess]
+ noise_std: [0.0, 0.0, 0.0] # MANDATORY if $mode == "factor" - DOC A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix. - TYPE Vector3d
+ dynamic: false # DOC If the state is dynamic, i.e. it changes along time. - TYPE bool
+ drift_std: [0.0, 0.0, 0.0] # OPTIONAL - DOC A vector containing the stdev values of the noise of the drift factor (only if dynamic==true), i.e. the sqrt of the diagonal elements of the covariance matrix. - TYPE Vector3d
+ O:
+ state: [0.0, 0.0, 0.0, 0.0] # DOC A vector containing the state values. It should be a quaternion (i.e. four values and normalized) - TYPE Vector4d
+ mode: "fix" # DOC The prior mode can be "factor" to add an absolute factor (requires "noise_std"), "fix" to set the values constant or "initial_guess" to just set the values - TYPE string - OPTIONS [fix, factor, initial_guess]
+ noise_std: [0.0, 0.0, 0.0] # MANDATORY if $mode == "factor" - DOC A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix. - TYPE Vector3d
+ dynamic: false # DOC If the state is dynamic, i.e. it changes along time. - TYPE bool
+ drift_std: [0.0, 0.0, 0.0] # OPTIONAL - DOC A vector containing the stdev values of the noise of the drift factor (only if dynamic==true), i.e. the sqrt of the diagonal elements of the covariance matrix. - TYPE Vector3d
+k_disp_to_disp: 0.0 # DOC ratio of displacement variance to displacement, for odometry noise calculation. - TYPE double
+k_disp_to_rot: 0.0 # DOC ratio of displacement variance to rotation, for odometry noise calculation. - TYPE double
+k_rot_to_rot: 0.0 # DOC ratio of rotation variance to rotation, for odometry noise calculation. - TYPE double
+min_disp_var: 0.0 # DOC minimum displacement variance, for odometry noise calculation. - TYPE double
+min_rot_var: 0.0 # DOC minimum rotation variance, for odometry noise calculation. - TYPE double
\ No newline at end of file
diff --git a/yaml_templates/sensor/SensorPose2d.yaml b/yaml_templates/sensor/SensorPose2d.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..1ff3c2e019cac8892800df8aa6f2bd63fa546301
--- /dev/null
+++ b/yaml_templates/sensor/SensorPose2d.yaml
@@ -0,0 +1,15 @@
+name: "whatever" # DOC The sensor"s name. It has to be unique. - TYPE string
+states:
+ P:
+ state: [0.0, 0.0] # DOC A vector containing the state values - TYPE Vector2d
+ mode: "fix" # DOC The prior mode can be "factor" to add an absolute factor (requires "noise_std"), "fix" to set the values constant or "initial_guess" to just set the values - TYPE string - OPTIONS [fix, factor, initial_guess]
+ noise_std: [0.0, 0.0] # MANDATORY if $mode == "factor" - DOC A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix. - TYPE Vector2d
+ dynamic: false # DOC If the state is dynamic, i.e. it changes along time. - TYPE bool
+ drift_std: [0.0, 0.0] # OPTIONAL - DOC A vector containing the stdev values of the noise of the drift factor (only if dynamic==true), i.e. the sqrt of the diagonal elements of the covariance matrix. - TYPE Vector2d
+ O:
+ state: [0.0] # DOC A vector containing the state values - TYPE Vector1d
+ mode: "fix" # DOC The prior mode can be "factor" to add an absolute factor (requires "noise_std"), "fix" to set the values constant or "initial_guess" to just set the values - TYPE string - OPTIONS [fix, factor, initial_guess]
+ noise_std: [0.0] # MANDATORY if $mode == "factor" - DOC A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix. - TYPE Vector1d
+ dynamic: false # DOC If the state is dynamic, i.e. it changes along time. - TYPE bool
+ drift_std: [0.0] # OPTIONAL - DOC A vector containing the stdev values of the noise of the drift factor (only if dynamic==true), i.e. the sqrt of the diagonal elements of the covariance matrix. - TYPE Vector1d
+std_noise: [0.0, 0.0, 0.0] # DOC Vector containing the standard deviation of the position and orientation measurements (square root of the covariance matrix diagonal). - TYPE Eigen::Vector3d
\ No newline at end of file
diff --git a/yaml_templates/sensor/SensorPose3d.yaml b/yaml_templates/sensor/SensorPose3d.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..eed8825743a601df649cae7bdb555419a12c9d21
--- /dev/null
+++ b/yaml_templates/sensor/SensorPose3d.yaml
@@ -0,0 +1,15 @@
+name: "whatever" # DOC The sensor"s name. It has to be unique. - TYPE string
+states:
+ P:
+ state: [0.0, 0.0, 0.0] # DOC A vector containing the state "P" values - TYPE Vector3d
+ mode: "fix" # DOC The prior mode can be "factor" to add an absolute factor (requires "noise_std"), "fix" to set the values constant or "initial_guess" to just set the values - TYPE string - OPTIONS [fix, factor, initial_guess]
+ noise_std: [0.0, 0.0, 0.0] # MANDATORY if $mode == "factor" - DOC A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix. - TYPE Vector3d
+ dynamic: false # DOC If the state is dynamic, i.e. it changes along time. - TYPE bool
+ drift_std: [0.0, 0.0, 0.0] # OPTIONAL - DOC A vector containing the stdev values of the noise of the drift factor (only if dynamic==true), i.e. the sqrt of the diagonal elements of the covariance matrix. - TYPE Vector3d
+ O:
+ state: [0.0, 0.0, 0.0, 0.0] # DOC A vector containing the state values. It should be a quaternion (i.e. four values and normalized) - TYPE Vector4d
+ mode: "fix" # DOC The prior mode can be "factor" to add an absolute factor (requires "noise_std"), "fix" to set the values constant or "initial_guess" to just set the values - TYPE string - OPTIONS [fix, factor, initial_guess]
+ noise_std: [0.0, 0.0, 0.0] # MANDATORY if $mode == "factor" - DOC A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix. - TYPE Vector3d
+ dynamic: false # DOC If the state is dynamic, i.e. it changes along time. - TYPE bool
+ drift_std: [0.0, 0.0, 0.0] # OPTIONAL - DOC A vector containing the stdev values of the noise of the drift factor (only if dynamic==true), i.e. the sqrt of the diagonal elements of the covariance matrix. - TYPE Vector3d
+std_noise: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] # DOC Vector containing the standard deviation of the position and orientation measurements (square root of the covariance matrix diagonal). - TYPE Eigen::Vector6d
\ No newline at end of file
diff --git a/yaml_templates/sensor/SensorSequence.yaml b/yaml_templates/sensor/SensorSequence.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..e41d961fa3da96dd208510bcdbf9fc8cdf000cbb
--- /dev/null
+++ b/yaml_templates/sensor/SensorSequence.yaml
@@ -0,0 +1,5 @@
+name: "whatever" # DOC The sensor"s name. It has to be unique. - TYPE string
+states:
+ keys: "whatever" # DOC The keys corresponding to the states of the sensor, to be filled with _value in the derived schemas. - TYPE string
+type: "whatever" # DOC The sensor"s class name. - TYPE string
+plugin: "whatever" # DOC The name of the wolf plugin where the sensor is implemented. - TYPE string
\ No newline at end of file
diff --git a/yaml_templates/sensor/SpecStateSensor.yaml b/yaml_templates/sensor/SpecStateSensor.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..a836a91f3f76bf4437deab175d58f1f7f09645ac
--- /dev/null
+++ b/yaml_templates/sensor/SpecStateSensor.yaml
@@ -0,0 +1,6 @@
+type: "whatever" # DOC The derived type of the StateBlock - TYPE string
+state: [0.0, 0.0, 0.0] # DOC A vector containing the state values - TYPE VectorXd
+mode: "fix" # DOC The prior mode can be "factor" to add an absolute factor (requires "noise_std"), "fix" to set the values constant or "initial_guess" to just set the values - TYPE string - OPTIONS [fix, factor, initial_guess]
+noise_std: [0.0, 0.0, 0.0] # MANDATORY if $mode == "factor" - DOC A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix. - TYPE VectorXd
+dynamic: false # DOC If the state is dynamic, i.e. it changes along time. - TYPE bool
+drift_std: [0.0, 0.0, 0.0] # OPTIONAL - DOC A vector containing the stdev values of the noise of the drift factor (only if dynamic==true), i.e. the sqrt of the diagonal elements of the covariance matrix. - TYPE VectorXd
\ No newline at end of file
diff --git a/yaml_templates/sensor/SpecStateSensorO2d.yaml b/yaml_templates/sensor/SpecStateSensorO2d.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..44aab0d675c7eaa4ef9ec77e00de1cb03b6c74e7
--- /dev/null
+++ b/yaml_templates/sensor/SpecStateSensorO2d.yaml
@@ -0,0 +1,5 @@
+state: [0.0] # DOC A vector containing the state values - TYPE Vector1d
+mode: "fix" # DOC The prior mode can be "factor" to add an absolute factor (requires "noise_std"), "fix" to set the values constant or "initial_guess" to just set the values - TYPE string - OPTIONS [fix, factor, initial_guess]
+noise_std: [0.0] # MANDATORY if $mode == "factor" - DOC A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix. - TYPE Vector1d
+dynamic: false # DOC If the state is dynamic, i.e. it changes along time. - TYPE bool
+drift_std: [0.0] # OPTIONAL - DOC A vector containing the stdev values of the noise of the drift factor (only if dynamic==true), i.e. the sqrt of the diagonal elements of the covariance matrix. - TYPE Vector1d
\ No newline at end of file
diff --git a/yaml_templates/sensor/SpecStateSensorO3d.yaml b/yaml_templates/sensor/SpecStateSensorO3d.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..9e55c6cf169588f6bbea29b8378ca713e4346174
--- /dev/null
+++ b/yaml_templates/sensor/SpecStateSensorO3d.yaml
@@ -0,0 +1,5 @@
+state: [0.0, 0.0, 0.0, 0.0] # DOC A vector containing the state values. It should be a quaternion (i.e. four values and normalized) - TYPE Vector4d
+mode: "fix" # DOC The prior mode can be "factor" to add an absolute factor (requires "noise_std"), "fix" to set the values constant or "initial_guess" to just set the values - TYPE string - OPTIONS [fix, factor, initial_guess]
+noise_std: [0.0, 0.0, 0.0] # MANDATORY if $mode == "factor" - DOC A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix. - TYPE Vector3d
+dynamic: false # DOC If the state is dynamic, i.e. it changes along time. - TYPE bool
+drift_std: [0.0, 0.0, 0.0] # OPTIONAL - DOC A vector containing the stdev values of the noise of the drift factor (only if dynamic==true), i.e. the sqrt of the diagonal elements of the covariance matrix. - TYPE Vector3d
\ No newline at end of file
diff --git a/yaml_templates/sensor/SpecStateSensorP2d.yaml b/yaml_templates/sensor/SpecStateSensorP2d.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..125c7e9ba70aba065189f067b39dd71dc32d8be6
--- /dev/null
+++ b/yaml_templates/sensor/SpecStateSensorP2d.yaml
@@ -0,0 +1,5 @@
+state: [0.0, 0.0] # DOC A vector containing the state values - TYPE Vector2d
+mode: "fix" # DOC The prior mode can be "factor" to add an absolute factor (requires "noise_std"), "fix" to set the values constant or "initial_guess" to just set the values - TYPE string - OPTIONS [fix, factor, initial_guess]
+noise_std: [0.0, 0.0] # MANDATORY if $mode == "factor" - DOC A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix. - TYPE Vector2d
+dynamic: false # DOC If the state is dynamic, i.e. it changes along time. - TYPE bool
+drift_std: [0.0, 0.0] # OPTIONAL - DOC A vector containing the stdev values of the noise of the drift factor (only if dynamic==true), i.e. the sqrt of the diagonal elements of the covariance matrix. - TYPE Vector2d
\ No newline at end of file
diff --git a/yaml_templates/sensor/SpecStateSensorP3d.yaml b/yaml_templates/sensor/SpecStateSensorP3d.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..cb22ff89a36c67414d7f185dda91bb8f5d695b7b
--- /dev/null
+++ b/yaml_templates/sensor/SpecStateSensorP3d.yaml
@@ -0,0 +1,5 @@
+state: [0.0, 0.0, 0.0] # DOC A vector containing the state "P" values - TYPE Vector3d
+mode: "fix" # DOC The prior mode can be "factor" to add an absolute factor (requires "noise_std"), "fix" to set the values constant or "initial_guess" to just set the values - TYPE string - OPTIONS [fix, factor, initial_guess]
+noise_std: [0.0, 0.0, 0.0] # MANDATORY if $mode == "factor" - DOC A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix. - TYPE Vector3d
+dynamic: false # DOC If the state is dynamic, i.e. it changes along time. - TYPE bool
+drift_std: [0.0, 0.0, 0.0] # OPTIONAL - DOC A vector containing the stdev values of the noise of the drift factor (only if dynamic==true), i.e. the sqrt of the diagonal elements of the covariance matrix. - TYPE Vector3d
\ No newline at end of file
diff --git a/yaml_templates/solver/SolverCeres.yaml b/yaml_templates/solver/SolverCeres.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..765c8ac1a4acf944e0f32d519455888f3566e5e1
--- /dev/null
+++ b/yaml_templates/solver/SolverCeres.yaml
@@ -0,0 +1,14 @@
+period: 0.0 # DOC Period of the solver thread. - TYPE double
+verbose: 0 # DOC Verbosity of the solver. 0: Nothing, 1: Brief report, 2: Full report. - TYPE int - OPTIONS [0, 1, 2]
+compute_cov: false # DOC If the solver has to compute any covariance matrix block. - TYPE bool
+cov_enum: 0 # MANDATORY if $compute_cov - DOC Which covariance matrix blocks have to be computed. 0: All blocks and all cross-covariances. 1: All marginals. 2: Marginals of landmarks and current robot pose plus cross covariances of current robot and all landmarks. 3: Last frame P and V. 4: Last frame P, O, V and T. 5: Last frame P and T. - TYPE int - OPTIONS [0, 1, 2, 3, 4, 5]
+cov_period: 0.0 # MANDATORY if $compute_cov - DOC Period of the covariance computation. - TYPE double
+minimizer: "LEVENBERG_MARQUARDT" # DOC Type of minimizer. - TYPE string - OPTIONS [LEVENBERG_MARQUARDT, levenberg_marquardt, DOGLEG, dogleg, LBFGS, lbfgs, BFGS, bfgs]
+interrupt_on_problem_change: false # DOC If the solver has to interrupted each time the problem changes to rebuild the problem. - TYPE bool
+min_num_iterations: 0 # MANDATORY if $interrupt_on_problem_change - DOC Amount of solver iterations during which the solver cannot be interrupted (used in interrupt_on_problem_change == true). - TYPE unsigned int
+max_num_iterations: 0 # DOC Maximum amount of solver iterations. If the solver didn"t converge after this amount of iterations, it stops anyway. - TYPE unsigned int
+function_tolerance: 0.0 # DOC Function tolerance. Convergence criterion. Typical value: 1e-8 - TYPE double
+gradient_tolerance: 0.0 # DOC Gradient tolerance. Convergence criterion. Typical value: 1e-8 - TYPE double
+n_threads: 1 # DOC Amount of threads used by ceres. - TYPE unsigned int - OPTIONS [1, 2, 3, 4]
+use_nonmonotonic_steps: false # OPTIONAL - DOC If the solver is allowed to update the solution with non-monotonic steps. Only used in LEVENBERG_MARQUARDT and DOGLEG minimizers. - TYPE bool
+max_consecutive_nonmonotonic_steps: 2 # OPTIONAL - DOC Amount of consecutive non-monotonic steps allowed. Only used in LEVENBERG_MARQUARDT and DOGLEG minimizers. - TYPE unsigned int
\ No newline at end of file
diff --git a/yaml_templates/solver/SolverManager.yaml b/yaml_templates/solver/SolverManager.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..15968cb34ff891261c37b0c849f3e4fda7d56c68
--- /dev/null
+++ b/yaml_templates/solver/SolverManager.yaml
@@ -0,0 +1,5 @@
+period: 0.0 # DOC Period of the solver thread. - TYPE double
+verbose: 0 # DOC Verbosity of the solver. 0: Nothing, 1: Brief report, 2: Full report. - TYPE int - OPTIONS [0, 1, 2]
+compute_cov: false # DOC If the solver has to compute any covariance matrix block. - TYPE bool
+cov_enum: 0 # MANDATORY if $compute_cov - DOC Which covariance matrix blocks have to be computed. 0: All blocks and all cross-covariances. 1: All marginals. 2: Marginals of landmarks and current robot pose plus cross covariances of current robot and all landmarks. 3: Last frame P and V. 4: Last frame P, O, V and T. 5: Last frame P and T. - TYPE int - OPTIONS [0, 1, 2, 3, 4, 5]
+cov_period: 0.0 # MANDATORY if $compute_cov - DOC Period of the covariance computation. - TYPE double
\ No newline at end of file
diff --git a/yaml_templates/state/SpecState.yaml b/yaml_templates/state/SpecState.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..d5544cc069c545497a6decce58cb8055d6623c13
--- /dev/null
+++ b/yaml_templates/state/SpecState.yaml
@@ -0,0 +1,4 @@
+type: "whatever" # DOC The derived type of the StateBlock - TYPE string
+state: [0.0, 0.0, 0.0] # DOC A vector containing the state values - TYPE VectorXd
+mode: "fix" # DOC The prior mode can be "factor" to add an absolute factor (requires "noise_std"), "fix" to set the values constant or "initial_guess" to just set the values - TYPE string - OPTIONS [fix, factor, initial_guess]
+noise_std: [0.0, 0.0, 0.0] # MANDATORY if $mode == "factor" - DOC A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix. - TYPE VectorXd
\ No newline at end of file
diff --git a/yaml_templates/state/SpecStateO2d.yaml b/yaml_templates/state/SpecStateO2d.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..8716ad1064682fad5487c0ef61bcc296a4dd28ba
--- /dev/null
+++ b/yaml_templates/state/SpecStateO2d.yaml
@@ -0,0 +1,3 @@
+state: [0.0] # DOC A vector containing the state values - TYPE Vector1d
+mode: "fix" # DOC The prior mode can be "factor" to add an absolute factor (requires "noise_std"), "fix" to set the values constant or "initial_guess" to just set the values - TYPE string - OPTIONS [fix, factor, initial_guess]
+noise_std: [0.0] # MANDATORY if $mode == "factor" - DOC A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix. - TYPE Vector1d
\ No newline at end of file
diff --git a/yaml_templates/state/SpecStateO3d.yaml b/yaml_templates/state/SpecStateO3d.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..d759a324b167ec0465936ae2c3ae82cd8235eaad
--- /dev/null
+++ b/yaml_templates/state/SpecStateO3d.yaml
@@ -0,0 +1,3 @@
+state: [0.0, 0.0, 0.0, 0.0] # DOC A vector containing the state values. It should be a quaternion (i.e. four values and normalized) - TYPE Vector4d
+mode: "fix" # DOC The prior mode can be "factor" to add an absolute factor (requires "noise_std"), "fix" to set the values constant or "initial_guess" to just set the values - TYPE string - OPTIONS [fix, factor, initial_guess]
+noise_std: [0.0, 0.0, 0.0] # MANDATORY if $mode == "factor" - DOC A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix. - TYPE Vector3d
\ No newline at end of file
diff --git a/yaml_templates/state/SpecStateP2d.yaml b/yaml_templates/state/SpecStateP2d.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..c27c76bd94a4f1e1fe2bda3a9163f27f7a467668
--- /dev/null
+++ b/yaml_templates/state/SpecStateP2d.yaml
@@ -0,0 +1,3 @@
+state: [0.0, 0.0] # DOC A vector containing the state values - TYPE Vector2d
+mode: "fix" # DOC The prior mode can be "factor" to add an absolute factor (requires "noise_std"), "fix" to set the values constant or "initial_guess" to just set the values - TYPE string - OPTIONS [fix, factor, initial_guess]
+noise_std: [0.0, 0.0] # MANDATORY if $mode == "factor" - DOC A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix. - TYPE Vector2d
\ No newline at end of file
diff --git a/yaml_templates/state/SpecStateP3d.yaml b/yaml_templates/state/SpecStateP3d.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..e4780f145fb67f9668f0771941998828b6652633
--- /dev/null
+++ b/yaml_templates/state/SpecStateP3d.yaml
@@ -0,0 +1,3 @@
+state: [0.0, 0.0, 0.0] # DOC A vector containing the state "P" values - TYPE Vector3d
+mode: "fix" # DOC The prior mode can be "factor" to add an absolute factor (requires "noise_std"), "fix" to set the values constant or "initial_guess" to just set the values - TYPE string - OPTIONS [fix, factor, initial_guess]
+noise_std: [0.0, 0.0, 0.0] # MANDATORY if $mode == "factor" - DOC A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix. - TYPE Vector3d
\ No newline at end of file
diff --git a/yaml_templates/state/SpecStateV2d.yaml b/yaml_templates/state/SpecStateV2d.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..c27c76bd94a4f1e1fe2bda3a9163f27f7a467668
--- /dev/null
+++ b/yaml_templates/state/SpecStateV2d.yaml
@@ -0,0 +1,3 @@
+state: [0.0, 0.0] # DOC A vector containing the state values - TYPE Vector2d
+mode: "fix" # DOC The prior mode can be "factor" to add an absolute factor (requires "noise_std"), "fix" to set the values constant or "initial_guess" to just set the values - TYPE string - OPTIONS [fix, factor, initial_guess]
+noise_std: [0.0, 0.0] # MANDATORY if $mode == "factor" - DOC A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix. - TYPE Vector2d
\ No newline at end of file
diff --git a/yaml_templates/state/SpecStateV3d.yaml b/yaml_templates/state/SpecStateV3d.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..616cce89b4f042861a186a648f7b29f24cfc7783
--- /dev/null
+++ b/yaml_templates/state/SpecStateV3d.yaml
@@ -0,0 +1,3 @@
+state: [0.0, 0.0, 0.0] # DOC A vector containing the state "V" values - TYPE Vector3d
+mode: "fix" # DOC The prior mode can be "factor" to add an absolute factor (requires "noise_std"), "fix" to set the values constant or "initial_guess" to just set the values - TYPE string - OPTIONS [fix, factor, initial_guess]
+noise_std: [0.0, 0.0, 0.0] # MANDATORY if $mode == "factor" - DOC A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix. - TYPE Vector3d
\ No newline at end of file
diff --git a/yaml_templates/tree_manager/TreeManagerBase.yaml b/yaml_templates/tree_manager/TreeManagerBase.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..2aca9fcb4712036db19382bf2a91f66d6266545c
--- /dev/null
+++ b/yaml_templates/tree_manager/TreeManagerBase.yaml
@@ -0,0 +1 @@
+type: "whatever" # DOC Type of the TreeManager. To keep all frames, use \none\. - TYPE string
\ No newline at end of file
diff --git a/yaml_templates/tree_manager/TreeManagerSlidingWindow.yaml b/yaml_templates/tree_manager/TreeManagerSlidingWindow.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..956dc1d54073a90446b6087b979fef775fde7617
--- /dev/null
+++ b/yaml_templates/tree_manager/TreeManagerSlidingWindow.yaml
@@ -0,0 +1,4 @@
+type: "whatever" # DOC Type of the TreeManager. To keep all frames, use \none\. - TYPE string
+n_frames: 0 # DOC Total number of frames of the sliding window. - TYPE unsigned int
+n_fix_first_frames: 0 # DOC Amount of frames fixed at the begining of the sliding window. - TYPE unsigned int
+viral_remove_parent_without_children: false # DOC If the other wolf nodes (landmarks) have to be removed when removing frames. Otherwise, they will remain alive but unconstrained. - TYPE bool
\ No newline at end of file
diff --git a/yaml_templates/tree_manager/TreeManagerSlidingWindowDualRate.yaml b/yaml_templates/tree_manager/TreeManagerSlidingWindowDualRate.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..96b3355c55a041e5c8378fe15796010368e13823
--- /dev/null
+++ b/yaml_templates/tree_manager/TreeManagerSlidingWindowDualRate.yaml
@@ -0,0 +1,6 @@
+type: "whatever" # DOC Type of the TreeManager. To keep all frames, use \none\. - TYPE string
+n_frames: 0 # DOC Total number of frames of the sliding window. - TYPE unsigned int
+n_fix_first_frames: 0 # DOC Amount of frames fixed at the begining of the sliding window. - TYPE unsigned int
+viral_remove_parent_without_children: false # DOC If the other wolf nodes (landmarks) have to be removed when removing frames. Otherwise, they will remain alive but unconstrained. - TYPE bool
+n_frames_recent: 0 # DOC Amount of frames kept in the recent part of the sliding window. - TYPE unsigned int
+rate_old_frames: 0 # DOC Rate of keyframes that are kept from the recent part to the sparse part of the sliding window. One of each "rate_old_frames" will be kept. - TYPE unsigned int
\ No newline at end of file