From 6efadee13b93a3d64be39de7fdc02633a9c0c004 Mon Sep 17 00:00:00 2001
From: cont-integration <CI@iri.upc.edu>
Date: Tue, 4 Jul 2023 14:21:53 +0200
Subject: [PATCH] [skip ci] applied clang format
---
demos/demo_analytic_autodiff_factor.cpp | 243 +++--
demos/demo_wolf_imported_graph.cpp | 409 ++++----
demos/hello_wolf/capture_range_bearing.cpp | 12 +-
demos/hello_wolf/capture_range_bearing.h | 43 +-
demos/hello_wolf/factor_bearing.h | 117 ++-
demos/hello_wolf/feature_range_bearing.cpp | 8 +-
demos/hello_wolf/feature_range_bearing.h | 9 +-
demos/hello_wolf/hello_wolf_autoconf.cpp | 147 ++-
demos/hello_wolf/landmark_point_2d.cpp | 5 +-
demos/hello_wolf/landmark_point_2d.h | 7 +-
demos/hello_wolf/processor_range_bearing.cpp | 56 +-
demos/hello_wolf/processor_range_bearing.h | 95 +-
demos/hello_wolf/sensor_range_bearing.cpp | 1 -
demos/hello_wolf/sensor_range_bearing.h | 1 -
demos/solver/test_SPQR.cpp | 64 +-
demos/solver/test_ccolamd.cpp | 41 +-
demos/solver/test_ccolamd_blocks.cpp | 126 +--
demos/solver/test_iQR.cpp | 255 ++---
demos/solver/test_iQR_wolf.cpp | 773 +++++++-------
demos/solver/test_iQR_wolf2.cpp | 296 +++---
.../test_incremental_ccolamd_blocks.cpp | 213 ++--
demos/solver/test_permutations.cpp | 44 +-
include/core/capture/capture_diff_drive.h | 38 +-
.../core/capture/capture_landmarks_external.h | 45 +-
include/core/capture/capture_motion.h | 148 +--
include/core/capture/capture_odom_2d.h | 28 +-
include/core/capture/capture_odom_3d.h | 28 +-
include/core/capture/capture_pose.h | 48 +-
include/core/capture/capture_void.h | 20 +-
.../ceres_wrapper/cost_function_wrapper.h | 35 +-
.../create_numeric_diff_cost_function.h | 69 +-
.../ceres_wrapper/iteration_update_callback.h | 70 +-
.../local_parametrization_wrapper.h | 42 +-
include/core/ceres_wrapper/qr_manager.h | 53 +-
include/core/ceres_wrapper/solver_ceres.h | 184 ++--
include/core/ceres_wrapper/sparse_utils.h | 67 +-
include/core/ceres_wrapper/wolf_jet.h | 10 +-
include/core/common/factory.h | 4 +-
include/core/common/node_base.h | 81 +-
include/core/common/node_state_blocks.h | 11 +-
include/core/common/params_base.h | 23 +-
include/core/common/time_stamp.h | 388 +++----
include/core/common/wolf.h | 228 ++--
include/core/composite/matrix_composite.h | 349 +++----
include/core/composite/spec_state_composite.h | 2 +-
.../composite/spec_state_sensor_composite.h | 2 +-
include/core/composite/type_composite.h | 13 +-
include/core/composite/vector_composite.h | 83 +-
include/core/factor/factor_autodiff.h | 119 ++-
include/core/factor/factor_base.h | 2 +-
include/core/factor/factor_block_absolute.h | 4 +-
include/core/factor/factor_block_difference.h | 1 -
include/core/factor/factor_diff_drive.h | 2 -
include/core/factor/factor_distance_3d.h | 1 -
include/core/factor/factor_pose_2d.h | 1 -
.../factor/factor_pose_2d_with_extrinsics.h | 20 +-
include/core/factor/factor_pose_3d.h | 1 -
.../factor/factor_pose_3d_with_extrinsics.h | 20 +-
.../core/factor/factor_quaternion_absolute.h | 1 -
include/core/factor/factor_relative_pose_3d.h | 1 -
include/core/feature/feature_base.h | 10 +-
include/core/feature/feature_diff_drive.h | 20 +-
include/core/feature/feature_match.h | 18 +-
include/core/feature/feature_motion.h | 27 +-
include/core/feature/feature_odom_2d.h | 35 +-
include/core/feature/feature_pose.h | 31 +-
include/core/frame/frame_base.h | 8 +-
include/core/hardware/hardware_base.h | 10 +-
include/core/landmark/factory_landmark.h | 32 +-
include/core/landmark/landmark_base.h | 2 +-
include/core/landmark/landmark_match.h | 23 +-
include/core/map/factory_map.h | 13 +-
include/core/map/map_base.h | 9 +-
include/core/math/SE2.h | 238 +++--
include/core/math/SE3.h | 481 ++++-----
include/core/math/covariance.h | 70 +-
include/core/math/rotations.h | 354 +++----
include/core/problem/problem.h | 2 +-
include/core/processor/factory_processor.h | 53 +-
include/core/processor/motion_buffer.h | 85 +-
include/core/processor/motion_provider.h | 82 +-
include/core/processor/processor_base.h | 3 +-
include/core/processor/processor_diff_drive.h | 1 -
.../processor/processor_fixed_wing_model.h | 101 +-
.../processor/processor_landmark_external.h | 1 -
.../core/processor/processor_loop_closure.h | 123 ++-
include/core/processor/processor_motion.h | 840 +++++++--------
include/core/processor/processor_odom_2d.h | 3 +-
include/core/processor/processor_odom_3d.h | 99 +-
include/core/processor/processor_pose.h | 5 +-
include/core/processor/processor_tracker.h | 3 +-
.../processor/processor_tracker_feature.h | 229 +++--
.../processor/processor_tracker_landmark.h | 1 -
include/core/processor/track_matrix.h | 116 ++-
include/core/sensor/factory_sensor.h | 5 +-
include/core/sensor/sensor_base.h | 10 +-
include/core/sensor/sensor_diff_drive.h | 3 +-
include/core/sensor/sensor_motion_model.h | 1 -
include/core/sensor/sensor_odom.h | 5 +-
include/core/sensor/sensor_pose.h | 5 +-
include/core/solver/factory_solver.h | 37 +-
include/core/solver/solver_manager.h | 416 ++++----
.../solver_suitesparse/ccolamd_ordering.h | 99 +-
.../solver_suitesparse/cost_function_base.h | 147 +--
.../solver_suitesparse/cost_function_sparse.h | 840 ++++++++-------
.../cost_function_sparse_base.h | 440 ++++----
include/core/solver_suitesparse/qr_solver.h | 970 +++++++++---------
include/core/solver_suitesparse/solver_QR.h | 24 +-
.../core/solver_suitesparse/solver_manager.h | 29 +-
.../core/solver_suitesparse/sparse_utils.h | 17 +-
.../core/state_block/factory_state_block.h | 29 +-
.../state_block/local_parametrization_angle.h | 41 +-
.../state_block/local_parametrization_base.h | 46 +-
.../local_parametrization_homogeneous.h | 32 +-
.../local_parametrization_quaternion.h | 74 +-
include/core/state_block/state_angle.h | 30 +-
include/core/state_block/state_block.h | 409 ++++----
.../core/state_block/state_block_derived.h | 24 +-
.../core/state_block/state_homogeneous_3d.h | 40 +-
include/core/state_block/state_quaternion.h | 65 +-
include/core/trajectory/trajectory_base.h | 26 +-
.../core/tree_manager/factory_tree_manager.h | 29 +-
include/core/tree_manager/tree_manager_base.h | 74 +-
.../tree_manager_sliding_window.h | 59 +-
.../tree_manager_sliding_window_dual_rate.h | 52 +-
include/core/utils/eigen_assert.h | 66 +-
include/core/utils/graph_search.h | 36 +-
include/core/utils/loader.h | 1 -
include/core/utils/logging.h | 379 +++----
include/core/utils/singleton.h | 43 +-
include/core/utils/utils_gtest.h | 212 ++--
src/capture/capture_base.cpp | 214 ++--
src/capture/capture_diff_drive.cpp | 36 +-
src/capture/capture_landmarks_external.cpp | 37 +-
src/capture/capture_motion.cpp | 105 +-
src/capture/capture_odom_2d.cpp | 17 +-
src/capture/capture_odom_3d.cpp | 18 +-
src/capture/capture_pose.cpp | 1 -
src/capture/capture_void.cpp | 12 +-
.../local_parametrization_wrapper.cpp | 13 +-
src/ceres_wrapper/qr_manager.cpp | 101 +-
src/ceres_wrapper/solver_ceres.cpp | 383 +++----
src/common/node_base.cpp | 8 +-
src/common/node_state_blocks.cpp | 26 +-
src/common/time_stamp.cpp | 55 +-
src/composite/matrix_composite.cpp | 263 +++--
src/composite/spec_state_sensor_composite.cpp | 22 +-
src/composite/vector_composite.cpp | 1 -
src/factor/factor_analytic.cpp | 2 +-
src/feature/feature_base.cpp | 9 +-
src/feature/feature_diff_drive.cpp | 13 +-
src/feature/feature_motion.cpp | 15 +-
src/feature/feature_odom_2d.cpp | 13 +-
src/feature/feature_pose.cpp | 10 +-
src/frame/frame_base.cpp | 1 -
src/hardware/hardware_base.cpp | 1 -
src/landmark/landmark_base.cpp | 15 +-
src/map/map_base.cpp | 1 -
src/problem/problem.cpp | 2 +-
src/processor/motion_buffer.cpp | 110 +-
src/processor/motion_provider.cpp | 10 +-
src/processor/processor_base.cpp | 4 +-
src/processor/processor_diff_drive.cpp | 3 +-
src/processor/processor_fixed_wing_model.cpp | 43 +-
src/processor/processor_landmark_external.cpp | 1 -
src/processor/processor_loop_closure.cpp | 1 -
src/processor/processor_motion.cpp | 3 +-
src/processor/processor_odom_2d.cpp | 1 -
src/processor/processor_odom_3d.cpp | 1 -
src/processor/processor_tracker.cpp | 7 +-
src/processor/processor_tracker_feature.cpp | 1 -
src/processor/processor_tracker_landmark.cpp | 1 -
src/processor/track_matrix.cpp | 112 +-
src/sensor/sensor_diff_drive.cpp | 1 -
src/sensor/sensor_motion_model.cpp | 1 -
src/solver/solver_manager.cpp | 294 +++---
src/solver_suitesparse/solver_manager.cpp | 381 ++++---
.../local_parametrization_base.cpp | 18 +-
.../local_parametrization_homogeneous.cpp | 27 +-
.../local_parametrization_quaternion.cpp | 38 +-
src/state_block/state_block.cpp | 18 +-
src/state_block/state_block_derived.cpp | 1 -
src/trajectory/trajectory_base.cpp | 110 +-
.../tree_manager_sliding_window.cpp | 21 +-
.../tree_manager_sliding_window_dual_rate.cpp | 29 +-
src/utils/graph_search.cpp | 42 +-
src/utils/loader.cpp | 1 -
test/dummy/dummy_object.h | 47 +-
test/dummy/dummy_object_derived.cpp | 8 +-
test/dummy/dummy_object_derived.h | 18 +-
test/dummy/dummy_object_derived_derived.cpp | 7 +-
test/dummy/dummy_object_derived_derived.h | 18 +-
test/dummy/factor_dummy_zero_1.h | 59 +-
test/dummy/factor_dummy_zero_15.h | 1 -
test/dummy/factor_feature_dummy.h | 1 -
test/dummy/factor_landmark_dummy.h | 3 +-
test/dummy/factor_relative_pose_2d_autodiff.h | 16 +-
test/dummy/factory_dummy_object.h | 18 +-
test/dummy/landmark_dummy.h | 64 +-
test/dummy/processor_diff_drive_mock.h | 3 +-
test/dummy/processor_loop_closure_dummy.h | 6 +-
test/dummy/processor_motion_provider_dummy.h | 75 +-
.../processor_motion_provider_dummy_pov.h | 75 +-
.../dummy/processor_tracker_feature_dummy.cpp | 8 +-
test/dummy/processor_tracker_feature_dummy.h | 1 -
.../processor_tracker_landmark_dummy.cpp | 1 -
test/dummy/processor_tracker_landmark_dummy.h | 1 -
test/dummy/sensor_dummy.h | 1 -
test/dummy/sensor_dummy_poia.h | 1 -
test/dummy/solver_manager_dummy.h | 203 ++--
test/dummy/tree_manager_dummy.h | 31 +-
test/gtest_SE2.cpp | 63 +-
test/gtest_SE3.cpp | 205 ++--
test/gtest_buffer_frame.cpp | 203 ++--
test/gtest_capture_base.cpp | 60 +-
test/gtest_emplace.cpp | 12 +-
test/gtest_example.cpp | 18 +-
test/gtest_factor_autodiff.cpp | 25 +-
test/gtest_factor_base.cpp | 28 +-
test/gtest_factor_block_difference.cpp | 2 -
test/gtest_factor_pose_2d_with_extrinsics.cpp | 2 +-
test/gtest_factor_pose_3d_with_extrinsics.cpp | 2 +-
...gtest_factor_relative_pose_2d_autodiff.cpp | 21 +-
...actor_relative_pose_2d_with_extrinsics.cpp | 2 +-
test/gtest_factor_relative_pose_3d.cpp | 4 +-
...actor_relative_pose_3d_with_extrinsics.cpp | 2 +-
...r_relative_position_2d_with_extrinsics.cpp | 12 +-
...r_relative_position_3d_with_extrinsics.cpp | 53 +-
...est_factor_velocity_local_direction_3d.cpp | 337 +++---
test/gtest_factory_state_block.cpp | 62 +-
test/gtest_feature_base.cpp | 45 +-
test/gtest_frame_base.cpp | 7 +-
test/gtest_local_param.cpp | 112 +-
test/gtest_logging.cpp | 6 +-
test/gtest_make_posdef.cpp | 8 +-
test/gtest_map_yaml.cpp | 32 +-
test/gtest_motion_buffer.cpp | 96 +-
test/gtest_motion_provider.cpp | 4 +-
test/gtest_node_state_blocks.cpp | 9 +-
test/gtest_problem.cpp | 11 +-
test/gtest_processor_base.cpp | 5 +-
test/gtest_processor_fixed_wing_model.cpp | 51 +-
test/gtest_processor_landmark_external.cpp | 2 +-
test/gtest_processor_odom_2d.cpp | 4 +-
.../gtest_processor_tracker_feature_dummy.cpp | 10 +-
...gtest_processor_tracker_landmark_dummy.cpp | 6 +-
test/gtest_rotation.cpp | 508 ++++-----
test/gtest_sensor_diff_drive.cpp | 2 +-
test/gtest_sensor_odom.cpp | 28 +-
test/gtest_sensor_pose.cpp | 6 +-
test/gtest_shared_from_this.cpp | 74 +-
test/gtest_solver_ceres.cpp | 479 +++++----
test/gtest_solver_manager_multithread.cpp | 38 +-
test/gtest_time_stamp.cpp | 210 ++--
test/gtest_track_matrix.cpp | 194 ++--
test/gtest_trajectory.cpp | 105 +-
test/gtest_tree_manager.cpp | 26 +-
test/gtest_tree_manager_sliding_window.cpp | 3 +-
wolf_scripts/templates/class_template.cpp | 13 +-
wolf_scripts/templates/class_template.h | 24 +-
wolf_scripts/templates/gtest_template.cpp | 3 +-
261 files changed, 10013 insertions(+), 9711 deletions(-)
diff --git a/demos/demo_analytic_autodiff_factor.cpp b/demos/demo_analytic_autodiff_factor.cpp
index 8e43ff8fc..31c294458 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,219 @@ 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 +307,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 +335,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 82e3dc3e3..ae87c1af8 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 ca2f7c10a..fb2aaa1e4 100644
--- a/demos/hello_wolf/capture_range_bearing.cpp
+++ b/demos/hello_wolf/capture_range_bearing.cpp
@@ -22,12 +22,12 @@
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 5600dd0fe..e24ec94c4 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 2e4ed1d62..53caf18af 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 a99fa854e..e5a806203 100644
--- a/demos/hello_wolf/feature_range_bearing.cpp
+++ b/demos/hello_wolf/feature_range_bearing.cpp
@@ -22,9 +22,8 @@
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)
{
//
}
@@ -34,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 db4e11fab..e729b350f 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_autoconf.cpp b/demos/hello_wolf/hello_wolf_autoconf.cpp
index 441601784..c5ee1d7d2 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,38 @@ 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"]);
// 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 +166,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 +254,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 ==================
@@ -278,34 +283,21 @@ int main()
* 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.
+ 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)
@@ -363,7 +355,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 e5dc4bafa..481ca4eea 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 d86c3e151..753370bf7 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 be6851b6f..5475edd89 100644
--- a/demos/hello_wolf/processor_range_bearing.cpp
+++ b/demos/hello_wolf/processor_range_bearing.cpp
@@ -26,9 +26,8 @@
namespace wolf
{
-
-ProcessorRangeBearing::ProcessorRangeBearing(const YAML::Node& _params) :
- ProcessorBase("ProcessorRangeBearing", 2, _params)
+ProcessorRangeBearing::ProcessorRangeBearing(const YAML::Node& _params)
+ : ProcessorBase("ProcessorRangeBearing", 2, _params)
{
//
}
@@ -43,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(), getTimeTolerance() );
+ 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()) < getTimeTolerance()) && "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
@@ -68,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);
@@ -92,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));
@@ -123,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;
}
@@ -142,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;
}
@@ -162,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 */
@@ -175,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 e4b893c9a..64e98aaf4 100644
--- a/demos/hello_wolf/processor_range_bearing.h
+++ b/demos/hello_wolf/processor_range_bearing.h
@@ -28,59 +28,72 @@
namespace wolf
{
-
using namespace Eigen;
WOLF_PTR_TYPEDEFS(ProcessorRangeBearing);
class ProcessorRangeBearing : public ProcessorBase
{
- public:
- typedef Eigen::Transform<double, 2, Eigen::Isometry> Trf;
+ public:
+ typedef Eigen::Transform<double, 2, Eigen::Isometry> Trf;
+
+ ProcessorRangeBearing(const YAML::Node& _params);
+ ~ProcessorRangeBearing() override
+ { /* empty */
+ }
+ void configure(SensorBasePtr _sensor) override;
- ProcessorRangeBearing(const YAML::Node& _params);
- ~ProcessorRangeBearing() override {/* empty */}
- void configure(SensorBasePtr _sensor) override;
+ // Factory method for high level API
+ WOLF_PROCESSOR_CREATE(ProcessorRangeBearing);
- // 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;
+ }
- 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 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;
- /** \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
- 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;
- 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;
+ 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/sensor_range_bearing.cpp b/demos/hello_wolf/sensor_range_bearing.cpp
index b16764688..2aea3a8c4 100644
--- a/demos/hello_wolf/sensor_range_bearing.cpp
+++ b/demos/hello_wolf/sensor_range_bearing.cpp
@@ -22,7 +22,6 @@
namespace wolf
{
-
WOLF_PTR_TYPEDEFS(SensorRangeBearing);
SensorRangeBearing::SensorRangeBearing(const YAML::Node& _params)
diff --git a/demos/hello_wolf/sensor_range_bearing.h b/demos/hello_wolf/sensor_range_bearing.h
index 1ba1a2b0c..aca391c4a 100644
--- a/demos/hello_wolf/sensor_range_bearing.h
+++ b/demos/hello_wolf/sensor_range_bearing.h
@@ -25,7 +25,6 @@
namespace wolf
{
-
WOLF_PTR_TYPEDEFS(SensorRangeBearing)
class SensorRangeBearing : public SensorBase
diff --git a/demos/solver/test_SPQR.cpp b/demos/solver/test_SPQR.cpp
index 66a17bdeb..010ecb423 100644
--- a/demos/solver/test_SPQR.cpp
+++ b/demos/solver/test_SPQR.cpp
@@ -25,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;
@@ -60,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 5a9bef001..5140f30cc 100644
--- a/demos/solver/test_ccolamd.cpp
+++ b/demos/solver/test_ccolamd.cpp
@@ -21,7 +21,7 @@
// Wolf includes
#include "core/common/wolf.h"
-//std includes
+// std includes
#include <cstdlib>
#include <iostream>
#include <fstream>
@@ -42,7 +42,7 @@
using namespace Eigen;
using namespace wolf;
-//main
+// main
int main(int argc, char *argv[])
{
if (argc != 2 || atoi(argv[1]) < 1)
@@ -54,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;
@@ -89,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();
@@ -121,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;
@@ -135,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 3351beb34..b3011a4c8 100644
--- a/demos/solver/test_ccolamd_blocks.cpp
+++ b/demos/solver/test_ccolamd_blocks.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/>.
-//std includes
+// std includes
#include <cstdlib>
#include <iostream>
#include <fstream>
@@ -38,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;
@@ -76,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
@@ -123,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();
@@ -151,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);
@@ -180,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 4ab060512..4b09adc75 100644
--- a/demos/solver/test_iQR.cpp
+++ b/demos/solver/test_iQR.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/>.
-//std includes
+// std includes
#include <cstdlib>
#include <iostream>
#include <fstream>
@@ -40,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;
@@ -114,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;
@@ -148,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);
@@ -178,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;
@@ -201,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
@@ -234,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
@@ -263,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;
@@ -297,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
@@ -317,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();
@@ -333,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 041057ba1..ec7ecc751 100644
--- a/demos/solver/test_iQR_wolf.cpp
+++ b/demos/solver/test_iQR_wolf.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/>.
-//std includes
+// std includes
#include <cstdlib>
#include <string>
#include <iostream>
@@ -41,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;
@@ -500,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");
@@ -511,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;
@@ -520,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
@@ -536,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
@@ -564,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 2371a3dd9..d87c9455b 100644
--- a/demos/solver/test_iQR_wolf2.cpp
+++ b/demos/solver/test_iQR_wolf2.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/>.
-//std includes
+// std includes
#include <cstdlib>
#include <string>
#include <iostream>
@@ -29,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"
@@ -38,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;
@@ -117,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());
@@ -204,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));
@@ -216,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));
@@ -236,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
@@ -249,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 ============================================================================================
@@ -350,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;
@@ -359,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();
@@ -375,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;
@@ -398,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
@@ -436,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 07f9b7f6a..1e099edfe 100644
--- a/demos/solver/test_incremental_ccolamd_blocks.cpp
+++ b/demos/solver/test_incremental_ccolamd_blocks.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/>.
-//std includes
+// std includes
#include <cstdlib>
#include <iostream>
#include <fstream>
@@ -38,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;
@@ -78,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;
@@ -123,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
@@ -168,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);
@@ -205,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);
@@ -230,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);
@@ -251,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 94dcad3d9..887522e03 100644
--- a/demos/solver/test_permutations.cpp
+++ b/demos/solver/test_permutations.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/>.
-//std includes
+// std includes
#include <cstdlib>
#include <iostream>
#include <fstream>
@@ -33,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);
@@ -41,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();
@@ -66,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;
@@ -104,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;
@@ -122,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 2310e147c..63c7ed69e 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 f186ae1e9..3dffe5bf7 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 51de061db..b00f186bd 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 b111f605c..6b52e5c35 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 13463ab08..e93dc3c05 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 64b958100..7ff804a01 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 8360c6216..1b5653cba 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 0fa474d37..d60fe7937 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 4447ff577..cbde48524 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 dd01fb5bb..4a88d7e7c 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 bce52b1fa..03cd030e8 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 7b961de2b..6087e4bf2 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 7d3047dd8..7d3d1350f 100644
--- a/include/core/ceres_wrapper/solver_ceres.h
+++ b/include/core/ceres_wrapper/solver_ceres.h
@@ -20,99 +20,100 @@
#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;
+ 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()
+ ParamsCeres() : ParamsSolver()
{
loadHardcodedValues();
}
- ParamsCeres(const YAML::Node& _input_node) :
- ParamsSolver(_input_node)
+ 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>();
+ 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;
+ 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>();
+ 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.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.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.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.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)");
+ 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>();
+ 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>();
+ 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;
+ 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;
@@ -123,91 +124,87 @@ struct ParamsCeres : public ParamsSolver
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_;
+ protected:
+ std::map<FactorBasePtr, ceres::ResidualBlockId> fac_2_residual_idx_;
+ std::map<FactorBasePtr, ceres::CostFunctionPtr> fac_2_costfunction_;
- ParamsCeresPtr params_ceres_;
+ // this map is only for handling automatic destruction of localParametrizationWrapper objects
+ std::map<StateBlockPtr, LocalParametrizationWrapperPtr> state_blocks_local_param_;
- // profiling
- unsigned int n_iter_acc_, n_iter_max_;
- unsigned int n_convergence_, n_interrupted_, n_no_convergence_;
+ ceres::Solver::Summary summary_;
+ std::unique_ptr<ceres::Problem> ceres_problem_;
+ std::unique_ptr<ceres::Covariance> covariance_;
- public:
+ ParamsCeresPtr params_ceres_;
- 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);
- WOLF_SOLVER_CREATE(SolverCeres, ParamsCeres);
+ SolverCeres(const ProblemPtr& _wolf_problem, const ParamsCeresPtr& _params);
- ~SolverCeres() override;
+ WOLF_SOLVER_CREATE(SolverCeres, ParamsCeres);
- ceres::Solver::Summary getSummary();
+ ~SolverCeres() override;
- std::unique_ptr<ceres::Problem>& getCeresProblem();
+ ceres::Solver::Summary getSummary();
- bool computeCovariancesDerived(CovarianceBlocksToBeComputed _blocks
- = CovarianceBlocksToBeComputed::ROBOT_LANDMARKS) override;
+ std::unique_ptr<ceres::Problem>& getCeresProblem();
- bool computeCovariancesDerived(const std::vector<StateBlockPtr>& st_list) override;
+ bool computeCovariancesDerived(
+ CovarianceBlocksToBeComputed _blocks = CovarianceBlocksToBeComputed::ROBOT_LANDMARKS) override;
- bool hasConverged() override;
- bool wasStopped() override;
- unsigned int iterations() override;
- double initialCost() override;
- double finalCost() override;
- double totalTime() override;
+ bool computeCovariancesDerived(const std::vector<StateBlockPtr>& st_list) override;
- ceres::Solver::Options& getSolverOptions();
+ bool hasConverged() override;
+ bool wasStopped() override;
+ unsigned int iterations() override;
+ double initialCost() override;
+ double finalCost() override;
+ double totalTime() override;
- const Eigen::SparseMatrixd computeHessian() const;
+ ceres::Solver::Options& getSolverOptions();
- virtual int numStateBlocksDerived() const override;
+ const Eigen::SparseMatrixd computeHessian() const;
- virtual int numFactorsDerived() const override;
+ virtual int numStateBlocksDerived() const override;
- protected:
+ virtual int numFactorsDerived() const override;
- bool checkDerived(std::string prefix="") const override;
+ protected:
+ 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;
};
inline ceres::Solver::Summary SolverCeres::getSummary()
@@ -227,21 +224,18 @@ inline ceres::Solver::Options& SolverCeres::getSolverOptions()
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 +254,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 aa8d3ec2b..173c2b889 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 a4a5888dd..962be7f6e 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 d7a874faf..32f0ed481 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.
@@ -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 487341744..d5636ed9a 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 8f0215435..ea5d6ed8d 100644
--- a/include/core/common/node_state_blocks.h
+++ b/include/core/common/node_state_blocks.h
@@ -101,9 +101,9 @@ class NodeStateBlocks : public NodeBase, public std::enable_shared_from_this<Nod
*
**/
void emplaceFactorStateBlock(const char& _key,
- const Eigen::VectorXd& _x,
- const Eigen::MatrixXd& _cov,
- unsigned int _start_idx = 0);
+ const Eigen::VectorXd& _x,
+ const Eigen::MatrixXd& _cov,
+ unsigned int _start_idx = 0);
void emplacePriors(const SpecStateComposite& _specs);
@@ -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/params_base.h b/include/core/common/params_base.h
index 8ae98bcab..05a198bb3 100644
--- a/include/core/common/params_base.h
+++ b/include/core/common/params_base.h
@@ -22,22 +22,21 @@
#include "yaml-cpp/yaml.h"
-namespace wolf
+namespace wolf
{
-
struct ParamsBase
{
- ParamsBase() = default;
- ParamsBase(const YAML::Node& _n)
- {
- //
- }
-
- virtual ~ParamsBase() = default;
- virtual std::string print() const = 0;
+ ParamsBase() = default;
+ ParamsBase(const YAML::Node& _n)
+ {
+ //
+ }
+
+ virtual ~ParamsBase() = default;
+ virtual std::string print() const = 0;
};
-template<typename Derived>
+template <typename Derived>
std::string toString(const Eigen::DenseBase<Derived>& mat)
{
std::stringstream ss;
@@ -108,4 +107,4 @@ inline std::string toString(long double _arg)
{
return std::to_string(_arg);
}
-}
+} // namespace wolf
diff --git a/include/core/common/time_stamp.h b/include/core/common/time_stamp.h
index 36aa22711..f0f91e051 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 f8fee3de0..9be1c0426 100644
--- a/include/core/common/wolf.h
+++ b/include/core/common/wolf.h
@@ -27,17 +27,17 @@
// 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
@@ -47,8 +47,8 @@
#include "yaml-cpp/yaml.h"
#include "yaml-schema-cpp/yaml_conversion.hpp"
-namespace wolf {
-
+namespace wolf
+{
/**
* \brief Vector and Matrices size type for the Wolf project
*
@@ -68,15 +68,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
@@ -93,30 +93,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
*
@@ -125,7 +125,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
@@ -133,7 +134,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){
@@ -150,43 +152,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());
+ }
};
//
@@ -196,58 +198,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);
@@ -291,7 +292,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
@@ -332,15 +333,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/matrix_composite.h b/include/core/composite/matrix_composite.h
index 3ae0fec99..92ddbca72 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 3f0e2139c..6ba838167 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 1821e7273..351d29990 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/type_composite.h b/include/core/composite/type_composite.h
index 848ca52e1..f46734ad8 100644
--- a/include/core/composite/type_composite.h
+++ b/include/core/composite/type_composite.h
@@ -25,10 +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);
+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)
@@ -51,7 +50,7 @@ bool isTypeCompositeValid(const TypeComposite& _types, SizeEigen _dim = 0);
// for (auto&& pair : *this)
// {
// YAML::Node n_type;
-// n_type["type"] = pair.second;
+// n_type["type"] = pair.second;
// n[pair.first] = n_type;
// }
@@ -64,7 +63,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 +119,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 d632d5bcc..84ae27858 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 68fcf9f17..45412f5cc 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 11ba8567b..af51ee8db 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 aa3af2b3d..afc9299a9 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 809c99612..b5087cb17 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 4ab23b0a0..b7546fdec 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 67816af68..7ed72d0f6 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 2510db856..3df6484ec 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 69901d3c9..6b59bab3f 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 ca967a87e..a3a18e589 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 1bcae3a42..aa3e57ebe 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 3f0fdcb1f..116e6eb8e 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 df4e20cfa..43bc6db56 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 67f0794c5..8c85bca40 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 3e8a84ca0..d7314441d 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 034cc3ac5..5cc32a60a 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 96da75694..8675f4c6a 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 075e82bc7..f647522a2 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 8479a8e14..bd9c547ac 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 3472c2be4..ae9691f84 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 cc9e5516c..3bb567ca8 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 b9e14bf69..cc3c60090 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 03abdad51..b11a2ebcf 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->emplacePriors(SpecStateComposite(_node["states"])); \
+ lmk->emplacePriors(SpecStateComposite(_node["states"])); \
return lmk; \
}
diff --git a/include/core/landmark/landmark_match.h b/include/core/landmark/landmark_match.h
index 072494273..5132e25bf 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 0b4a347c2..f2a803269 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.
@@ -108,14 +107,16 @@ namespace wolf
*/
typedef Factory<MapBasePtr, const YAML::Node&> FactoryMap;
-template<>
+template <>
inline std::string FactoryMap::getClass() const
{
- return "FactoryMap";
+ return "FactoryMap";
}
-#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 = FactoryMap::registerCreator(#MapType, MapType::create); \
+ }
} /* namespace wolf */
diff --git a/include/core/map/map_base.h b/include/core/map/map_base.h
index 9d9fd4ed2..aeaa5550c 100644
--- a/include/core/map/map_base.h
+++ b/include/core/map/map_base.h
@@ -38,7 +38,6 @@ class LandmarkBase;
namespace wolf
{
-
/*
* Macro for defining Autoconf map creator.
*
@@ -110,15 +109,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 5e14eeaea..79b6cf27e 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 fe456a910..be94f1b80 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 e083cb4c7..729549c27 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 651bd5900..2823f74df 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 6d0262992..07391d09f 100644
--- a/include/core/problem/problem.h
+++ b/include/core/problem/problem.h
@@ -180,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);
diff --git a/include/core/processor/factory_processor.h b/include/core/processor/factory_processor.h
index 3422c8361..985152c4a 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);
* }
@@ -105,35 +105,38 @@ namespace wolf
* 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&,
- const std::vector<std::string>&> FactoryProcessorNode;
-template<>
+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##NodeRegistered = \
- wolf::FactoryProcessorNode::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 e192871d1..c2109c9b6 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 71c064d00..49d1651b3 100644
--- a/include/core/processor/motion_provider.h
+++ b/include/core/processor/motion_provider.h
@@ -33,54 +33,58 @@ WOLF_PTR_TYPEDEFS(MotionProvider);
class 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;
-
- VectorComposite getOdometry ( ) const;
- void setOdometry(const VectorComposite&);
-
- 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_;
- YAML::Node params_motion_provider_;
-
- private:
- VectorComposite odometry_;
- mutable std::mutex mut_odometry_;
-
+ 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;
+
+ VectorComposite getOdometry() const;
+ void setOdometry(const VectorComposite&);
+
+ 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_;
+ YAML::Node params_motion_provider_;
+
+ private:
+ VectorComposite odometry_;
+ mutable std::mutex mut_odometry_;
};
-inline MotionProvider::MotionProvider(const TypeComposite& _state_types, const YAML::Node& _params) :
- state_types_(_state_types),
- params_motion_provider_(Clone(_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
{
diff --git a/include/core/processor/processor_base.h b/include/core/processor/processor_base.h
index 1ac7c1cd1..20b548aa4 100644
--- a/include/core/processor/processor_base.h
+++ b/include/core/processor/processor_base.h
@@ -46,7 +46,6 @@ class SensorBase;
namespace wolf
{
-
/*
* Macro for defining Autoconf processor creator for WOLF's high level API.
*
@@ -114,7 +113,7 @@ namespace wolf
} \
return create(server.getNode(), {}); \
}
-
+
/** \brief Buffer for arbitrary type objects
*
* Object and functions to manage a buffer of objects.
diff --git a/include/core/processor/processor_diff_drive.h b/include/core/processor/processor_diff_drive.h
index bf6d0e402..e7fd6fd01 100644
--- a/include/core/processor/processor_diff_drive.h
+++ b/include/core/processor/processor_diff_drive.h
@@ -24,7 +24,6 @@
namespace wolf
{
-
WOLF_PTR_TYPEDEFS(ProcessorDiffDrive);
class ProcessorDiffDrive : public ProcessorOdom2d
diff --git a/include/core/processor/processor_fixed_wing_model.h b/include/core/processor/processor_fixed_wing_model.h
index 27a1c8f0c..2fa506a34 100644
--- a/include/core/processor/processor_fixed_wing_model.h
+++ b/include/core/processor/processor_fixed_wing_model.h
@@ -24,54 +24,67 @@
namespace wolf
{
-
WOLF_PTR_TYPEDEFS(ProcessorFixedWingModel);
class ProcessorFixedWingModel : public ProcessorBase
{
- 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_;
+ 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 1dd8cdb1c..1702ec0a3 100644
--- a/include/core/processor/processor_landmark_external.h
+++ b/include/core/processor/processor_landmark_external.h
@@ -26,7 +26,6 @@
namespace wolf
{
-
WOLF_PTR_TYPEDEFS(ProcessorLandmarkExternal);
// Class
diff --git a/include/core/processor/processor_loop_closure.h b/include/core/processor/processor_loop_closure.h
index 7cfaa92da..335161845 100644
--- a/include/core/processor/processor_loop_closure.h
+++ b/include/core/processor/processor_loop_closure.h
@@ -23,8 +23,8 @@
// Wolf related headers
#include "core/processor/processor_base.h"
-namespace wolf{
-
+namespace wolf
+{
WOLF_STRUCT_PTR_TYPEDEFS(MatchLoopClosure);
/** \brief Match between a capture and a capture
@@ -34,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
@@ -53,57 +53,69 @@ struct MatchLoopClosure
*/
class ProcessorLoopClosure : public ProcessorBase
{
- public:
-
- ProcessorLoopClosure(const std::string& _type, int _dim, const YAML::Node& _params);
-
- ~ProcessorLoopClosure() override = default;
- void configure(SensorBasePtr _sensor) override { };
-
- int getMaxLoops() const;
- void setMaxLoops(int _max_loops);
-
- 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);
-
- /** \brief Returns if findLoopClosures() has to be called for the given capture
- */
- virtual bool voteFindLoopClosures(CaptureBasePtr cap) = 0;
-
- /** \brief detects and emplaces all features of the given capture
- */
- virtual void emplaceFeatures(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 validates a loop closure
- */
- virtual bool validateLoopClosure(MatchLoopClosurePtr) = 0;
-
- /** \brief emplaces the factor(s) corresponding to a Loop Closure between two captures
- */
- virtual void emplaceFactors(MatchLoopClosurePtr) = 0;
-
- void processCapture(CaptureBasePtr) override;
- void processKeyFrame(FrameBasePtr _frm) override;
-
- bool triggerInCapture(CaptureBasePtr _cap) const override { return true;};
- bool triggerInKeyFrame(FrameBasePtr _frm) const override { return true;};
-
- bool storeKeyFrame(FrameBasePtr _frm) override { return false;};
- bool storeCapture(CaptureBasePtr _cap) override { return false;};
-
- bool voteForKeyFrame() const override { return false;};
+ public:
+ ProcessorLoopClosure(const std::string& _type, int _dim, const YAML::Node& _params);
+
+ ~ProcessorLoopClosure() override = default;
+ void configure(SensorBasePtr _sensor) override{};
+
+ int getMaxLoops() const;
+ void setMaxLoops(int _max_loops);
+
+ 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);
+
+ /** \brief Returns if findLoopClosures() has to be called for the given capture
+ */
+ virtual bool voteFindLoopClosures(CaptureBasePtr cap) = 0;
+
+ /** \brief detects and emplaces all features of the given capture
+ */
+ virtual void emplaceFeatures(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 validates a loop closure
+ */
+ virtual bool validateLoopClosure(MatchLoopClosurePtr) = 0;
+
+ /** \brief emplaces the factor(s) corresponding to a Loop Closure between two captures
+ */
+ virtual void emplaceFactors(MatchLoopClosurePtr) = 0;
+
+ void processCapture(CaptureBasePtr) override;
+ void processKeyFrame(FrameBasePtr _frm) override;
+
+ bool triggerInCapture(CaptureBasePtr _cap) const override
+ {
+ return true;
+ };
+ bool triggerInKeyFrame(FrameBasePtr _frm) const override
+ {
+ return true;
+ };
+
+ bool storeKeyFrame(FrameBasePtr _frm) override
+ {
+ return false;
+ };
+ bool storeCapture(CaptureBasePtr _cap) override
+ {
+ return false;
+ };
+
+ bool voteForKeyFrame() const override
+ {
+ return false;
+ };
};
-
inline int ProcessorLoopClosure::getMaxLoops() const
{
return params_["max_loops"].as<int>();
@@ -114,5 +126,4 @@ 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 7ff8ea303..503a9357a 100644
--- a/include/core/processor/processor_motion.h
+++ b/include/core/processor/processor_motion.h
@@ -31,7 +31,6 @@
namespace wolf
{
-
/** \brief class for Motion processors
*
* This processor integrates motion data into vehicle states.
@@ -59,7 +58,7 @@ namespace wolf
*
* 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.
*
@@ -111,415 +110,427 @@ namespace wolf
*/
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:
- 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
+ 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
@@ -568,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
);
}
@@ -655,5 +666,4 @@ inline void ProcessorMotion::setMaxAngleTurned(const double& _max_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 ee573516a..0c3268f44 100644
--- a/include/core/processor/processor_odom_2d.h
+++ b/include/core/processor/processor_odom_2d.h
@@ -27,7 +27,6 @@
namespace wolf
{
-
WOLF_PTR_TYPEDEFS(ProcessorOdom2d);
class ProcessorOdom2d : public ProcessorMotion
@@ -41,7 +40,7 @@ class ProcessorOdom2d : public ProcessorMotion
WOLF_PROCESSOR_CREATE(ProcessorOdom2d);
double getCovDet() const;
- bool voteForKeyFrame() const override;
+ bool voteForKeyFrame() const override;
protected:
void computeCurrentDelta(const Eigen::VectorXd& _data,
diff --git a/include/core/processor/processor_odom_3d.h b/include/core/processor/processor_odom_3d.h
index f6d97b527..ea9855622 100644
--- a/include/core/processor/processor_odom_3d.h
+++ b/include/core/processor/processor_odom_3d.h
@@ -26,8 +26,8 @@
#include "core/factor/factor_relative_pose_3d.h"
#include <cmath>
-namespace wolf {
-
+namespace wolf
+{
WOLF_PTR_TYPEDEFS(ProcessorOdom3d);
/** \brief Processor for 3d odometry integration.
@@ -37,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)
@@ -54,60 +55,60 @@ WOLF_PTR_TYPEDEFS(ProcessorOdom3d);
*/
class ProcessorOdom3d : public ProcessorMotion
{
- public:
- ProcessorOdom3d(const YAML::Node& _params);
- ~ProcessorOdom3d() override;
- void configure(SensorBasePtr _sensor) override;
+ public:
+ ProcessorOdom3d(const YAML::Node& _params);
+ ~ProcessorOdom3d() override;
+ void configure(SensorBasePtr _sensor) override;
- // Factory method for high level API
- WOLF_PROCESSOR_CREATE(ProcessorOdom3d);
+ // Factory method for high level API
+ WOLF_PROCESSOR_CREATE(ProcessorOdom3d);
- 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:
+ // 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;
- bool voteForKeyFrame() const override;
+ bool voteForKeyFrame() 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;
+ 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;
+ 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 7edc72618..03aae6627 100644
--- a/include/core/processor/processor_pose.h
+++ b/include/core/processor/processor_pose.h
@@ -25,7 +25,6 @@
namespace wolf
{
-
// class
template <unsigned int DIM>
class ProcessorPose : public ProcessorBase
@@ -80,10 +79,8 @@ WOLF_DECLARED_PTR_TYPEDEFS(ProcessorPose3d);
namespace wolf
{
-
template <unsigned int DIM>
-inline ProcessorPose<DIM>::ProcessorPose(const YAML::Node& _params)
- : ProcessorBase("ProcessorPose", DIM, _params)
+inline ProcessorPose<DIM>::ProcessorPose(const YAML::Node& _params) : ProcessorBase("ProcessorPose", DIM, _params)
{
static_assert(DIM == 2 or DIM == 3);
}
diff --git a/include/core/processor/processor_tracker.h b/include/core/processor/processor_tracker.h
index c6e99e5c0..9653707c7 100644
--- a/include/core/processor/processor_tracker.h
+++ b/include/core/processor/processor_tracker.h
@@ -25,7 +25,6 @@
namespace wolf
{
-
WOLF_PTR_TYPEDEFS(ProcessorTracker);
/** \brief General tracker processor
@@ -98,7 +97,7 @@ class ProcessorTracker : public ProcessorBase
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
+ 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
diff --git a/include/core/processor/processor_tracker_feature.h b/include/core/processor/processor_tracker_feature.h
index 2d682e6ac..522d60f02 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,7 +29,6 @@
namespace wolf
{
-
WOLF_PTR_TYPEDEFS(ProcessorTrackerFeature);
/** \brief Feature tracker processor
@@ -52,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
@@ -85,117 +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,
- 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,
+ 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 062e05712..d024f4132 100644
--- a/include/core/processor/processor_tracker_landmark.h
+++ b/include/core/processor/processor_tracker_landmark.h
@@ -27,7 +27,6 @@
namespace wolf
{
-
WOLF_PTR_TYPEDEFS(ProcessorTrackerLandmark);
/** \brief Landmark tracker processor
diff --git a/include/core/processor/track_matrix.h b/include/core/processor/track_matrix.h
index 6daa3880d..3e13668e0 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 daf4cdf25..073e6c68a 100644
--- a/include/core/sensor/factory_sensor.h
+++ b/include/core/sensor/factory_sensor.h
@@ -39,7 +39,6 @@ class Composite;
namespace wolf
{
-
/** \brief Sensor factory class
*
* This factory can create objects of classes deriving from SensorBase.
@@ -88,13 +87,13 @@ namespace wolf
* 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)
diff --git a/include/core/sensor/sensor_base.h b/include/core/sensor/sensor_base.h
index c305e75f7..ad7ce431e 100644
--- a/include/core/sensor/sensor_base.h
+++ b/include/core/sensor/sensor_base.h
@@ -119,20 +119,20 @@ class SensorBase : public NodeStateBlocks
HardwareBaseWPtr hardware_ptr_;
ProcessorBasePtrList processor_list_;
- static unsigned int sensor_id_count_; ///< Object counter (acts as simple ID factory)
+ static unsigned int sensor_id_count_; ///< Object counter (acts as simple ID factory)
- unsigned int sensor_id_; // sensor ID
+ unsigned int sensor_id_; // sensor ID
std::map<char, bool> state_block_dynamic_; // mark dynamic state blocks
- CaptureBasePtr last_capture_; // last capture of the sensor (in the WOLF tree)
+ CaptureBasePtr last_capture_; // last capture of the sensor (in the WOLF tree)
protected:
std::map<char, Eigen::MatrixXd> drift_covs_; // covariance of the drift of dynamic state blocks [unit²/s]
- YAML::Node params_; ///< Params yaml node
+ YAML::Node params_; ///< Params yaml node
- int dim_compatible_; ///< Dimension compatibility of the sensor: 2: 2D, 3: 3D, -1: both
+ int dim_compatible_; ///< Dimension compatibility of the sensor: 2: 2D, 3: 3D, -1: both
void setProblem(ProblemPtr _problem) override final;
diff --git a/include/core/sensor/sensor_diff_drive.h b/include/core/sensor/sensor_diff_drive.h
index 3ac570830..087ea3b82 100644
--- a/include/core/sensor/sensor_diff_drive.h
+++ b/include/core/sensor/sensor_diff_drive.h
@@ -24,13 +24,12 @@
namespace wolf
{
-
WOLF_PTR_TYPEDEFS(SensorDiffDrive);
class SensorDiffDrive : public SensorBase
{
SensorDiffDrive(const YAML::Node& _params);
-
+
public:
WOLF_SENSOR_CREATE(SensorDiffDrive);
diff --git a/include/core/sensor/sensor_motion_model.h b/include/core/sensor/sensor_motion_model.h
index 792f32714..c4ed862cc 100644
--- a/include/core/sensor/sensor_motion_model.h
+++ b/include/core/sensor/sensor_motion_model.h
@@ -25,7 +25,6 @@
namespace wolf
{
-
WOLF_PTR_TYPEDEFS(SensorMotionModel);
class SensorMotionModel : public SensorBase
diff --git a/include/core/sensor/sensor_odom.h b/include/core/sensor/sensor_odom.h
index fcab5b0a9..89eb373a1 100644
--- a/include/core/sensor/sensor_odom.h
+++ b/include/core/sensor/sensor_odom.h
@@ -26,14 +26,11 @@
namespace wolf
{
-
template <unsigned int DIM>
class SensorOdom : public SensorBase
{
protected:
-
- SensorOdom(const YAML::Node& _params)
- : SensorBase("SensorOdom" + toString(DIM) + "d", DIM, _params)
+ SensorOdom(const YAML::Node& _params) : SensorBase("SensorOdom" + toString(DIM) + "d", DIM, _params)
{
static_assert(DIM == 2 or DIM == 3);
}
diff --git a/include/core/sensor/sensor_pose.h b/include/core/sensor/sensor_pose.h
index cdd591e4d..57fd3c571 100644
--- a/include/core/sensor/sensor_pose.h
+++ b/include/core/sensor/sensor_pose.h
@@ -26,14 +26,11 @@
namespace wolf
{
-
template <unsigned int DIM>
class SensorPose : public SensorBase
{
protected:
-
- SensorPose(const YAML::Node& _params)
- : SensorBase("SensorPose" + toString(DIM) + "d", DIM, _params)
+ SensorPose(const YAML::Node& _params) : SensorBase("SensorPose" + toString(DIM) + "d", DIM, _params)
{
static_assert(DIM == 2 or DIM == 3);
}
diff --git a/include/core/solver/factory_solver.h b/include/core/solver/factory_solver.h
index f31fa0273..e320d8437 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,37 @@ 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&> FactorySolver;
-template<>
+template <>
inline std::string FactorySolver::getClass() const
{
- return "FactorySolver";
+ return "FactorySolver";
}
-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>&>
+ FactorySolverYaml;
-template<>
+template <>
inline std::string FactorySolverYaml::getClass() const
{
- return "FactorySolverYaml";
+ return "FactorySolverYaml";
}
-#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::FactorySolver::registerCreator(#SolverType, SolverType::create); \
+ } \
+ namespace \
+ { \
+ const bool WOLF_UNUSED SolverType##RegisteredYaml = \
+ wolf::FactorySolverYaml::registerCreator(#SolverType, SolverType::create); \
+ }
} /* namespace wolf */
diff --git a/include/core/solver/solver_manager.h b/include/core/solver/solver_manager.h
index c4e02ede2..6e190280a 100644
--- a/include/core/solver/solver_manager.h
+++ b/include/core/solver/solver_manager.h
@@ -20,15 +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)
@@ -44,26 +44,25 @@ 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); \
-} \
+#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;
@@ -72,206 +71,199 @@ struct ParamsSolver;
*/
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_;
+ 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();
+
+ virtual ParamsSolverPtr 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;
-
+ 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
+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
+ 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)
{
- 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";
+ cov_enum = (SolverManager::CovarianceBlocksToBeComputed)_input_node["cov_enum"].as<int>();
+ cov_period = _input_node["cov_period"].as<double>();
}
-
- ~ParamsSolver() override = default;
+ }
+ 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;
};
-} // namespace wolf
+} // namespace wolf
diff --git a/include/core/solver_suitesparse/ccolamd_ordering.h b/include/core/solver_suitesparse/ccolamd_ordering.h
index 12bb7e698..7272b3a1b 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 3668a5bf7..75e62aba9 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 52d94c4c1..1c1c77f95 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 26e9dc23e..4983d82c1 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 40bae47e7..a1ca79663 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 6ddcda8a0..a65e87bcb 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 a7660386e..95c1d31fd 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 4c746537b..cbab6408b 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 94bf908ef..cd35e83a2 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 affd72fde..36e5542fd 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 d1c3010e8..6fa3dec2d 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 d7e066e9a..f3be4e47e 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 777930c7d..f037dba0e 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 67c5feb37..cbec8d964 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 835b6d9ec..cc5f509fe 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 508a40535..d105cf2da 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 608de913b..2da7eba41 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 00d3c00bd..30dbf4b9d 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 60627e9d9..5f16217e2 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 7fcbe3ec6..d77b9dd26 100644
--- a/include/core/tree_manager/factory_tree_manager.h
+++ b/include/core/tree_manager/factory_tree_manager.h
@@ -28,28 +28,31 @@
namespace wolf
{
-
-typedef Factory<std::shared_ptr<TreeManagerBase>,
- const YAML::Node&> FactoryTreeManager;
-template<>
+typedef Factory<std::shared_ptr<TreeManagerBase>, const YAML::Node&> FactoryTreeManager;
+template <>
inline std::string FactoryTreeManager::getClass() const
{
return "FactoryTreeManager";
}
-typedef Factory<std::shared_ptr<TreeManagerBase>,
- const std::string&,
- const std::vector<std::string>&> FactoryTreeManagerYaml;
-template<>
+typedef Factory<std::shared_ptr<TreeManagerBase>, const std::string&, const std::vector<std::string>&>
+ FactoryTreeManagerYaml;
+template <>
inline std::string FactoryTreeManagerYaml::getClass() const
{
return "FactoryTreeManagerYaml";
}
-#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::FactoryTreeManager::registerCreator(#TreeManagerType, TreeManagerType::create); \
+ } \
+ namespace \
+ { \
+ const bool WOLF_UNUSED TreeManagerType##YamlRegistered = \
+ wolf::FactoryTreeManagerYaml::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 3e43ccfab..af62e3e78 100644
--- a/include/core/tree_manager/tree_manager_base.h
+++ b/include/core/tree_manager/tree_manager_base.h
@@ -39,39 +39,36 @@ namespace wolf
* must have a constructor available with the API:
*
* TreeManagerClass(const ParamsTreeManagerPtr _params);
- *
- * Also, there should be the schema file 'TreeManagerClass.schema' containing the specifications
+ *
+ * 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); \
-} \
+#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)
- {
- }
+ ParamsTreeManagerBase(const YAML::Node& _input_node) : ParamsBase(_input_node) {}
~ParamsTreeManagerBase() override = default;
@@ -83,20 +80,23 @@ struct ParamsTreeManagerBase : public ParamsBase
class TreeManagerBase : public NodeBase
{
- public:
- TreeManagerBase(const std::string& _type, ParamsTreeManagerBasePtr _params) :
- NodeBase("TREE_MANAGER", _type),
- params_(_params)
- {}
+ public:
+ TreeManagerBase(const std::string& _type, ParamsTreeManagerBasePtr _params)
+ : NodeBase("TREE_MANAGER", _type), params_(_params)
+ {
+ }
- virtual ~TreeManagerBase() {}
+ virtual ~TreeManagerBase() {}
- virtual void keyFrameCallback(FrameBasePtr _key_frame) = 0;
+ virtual void keyFrameCallback(FrameBasePtr _key_frame) = 0;
- bool hasChildren() const override{return true;};
+ bool hasChildren() const override
+ {
+ return true;
+ };
- protected:
- ParamsTreeManagerBasePtr params_;
+ protected:
+ ParamsTreeManagerBasePtr params_;
};
} /* 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 9f0c0e8c0..7e86ed378 100644
--- a/include/core/tree_manager/tree_manager_sliding_window.h
+++ b/include/core/tree_manager/tree_manager_sliding_window.h
@@ -24,50 +24,47 @@
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";
- }
+ 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;
+ 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(ParamsTreeManagerSlidingWindowPtr _params)
+ : TreeManagerBase("TreeManagerSlidingWindow", _params), params_sw_(_params){};
+ WOLF_TREE_MANAGER_CREATE(TreeManagerSlidingWindow, ParamsTreeManagerSlidingWindow)
- ~TreeManagerSlidingWindow() override{}
+ ~TreeManagerSlidingWindow() override {}
- void keyFrameCallback(FrameBasePtr _frame) override;
+ void keyFrameCallback(FrameBasePtr _frame) override;
- protected:
- ParamsTreeManagerSlidingWindowPtr params_sw_;
+ protected:
+ ParamsTreeManagerSlidingWindowPtr params_sw_;
};
} /* 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 33dd30966..aa1fa0123 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,46 @@
namespace wolf
{
-
WOLF_STRUCT_PTR_TYPEDEFS(ParamsTreeManagerSlidingWindowDualRate)
WOLF_PTR_TYPEDEFS(TreeManagerSlidingWindowDualRate)
struct ParamsTreeManagerSlidingWindowDualRate : public ParamsTreeManagerSlidingWindow
{
- 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
+ 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)
{
- return ParamsTreeManagerBase::print() + "\n"
- + "n_frames_recent: " + toString(n_frames_recent) + "\n"
- + "rate_old_frames: " + toString(rate_old_frames) + "\n";
+ 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";
+ }
- unsigned int n_frames_recent, rate_old_frames;
+ unsigned int n_frames_recent, rate_old_frames;
};
class TreeManagerSlidingWindowDualRate : public TreeManagerSlidingWindow
{
- public:
- TreeManagerSlidingWindowDualRate(ParamsTreeManagerSlidingWindowDualRatePtr _params);
-
- WOLF_TREE_MANAGER_CREATE(TreeManagerSlidingWindowDualRate, ParamsTreeManagerSlidingWindowDualRate)
+ 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:
- ParamsTreeManagerSlidingWindowDualRatePtr params_swdr_;
- unsigned int count_frames_;
- //TrajectoryIter first_recent_frame_it_;
+ protected:
+ ParamsTreeManagerSlidingWindowDualRatePtr params_swdr_;
+ unsigned int count_frames_;
+ // TrajectoryIter first_recent_frame_it_;
};
} /* namespace wolf */
diff --git a/include/core/utils/eigen_assert.h b/include/core/utils/eigen_assert.h
index a6035cc8b..c478277fc 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 142f40d06..b5e4abcd6 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 23a54c750..8255d1ec4 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 09af0b8d1..86b02e32f 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 c92f18c75..408073eb1 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/utils/utils_gtest.h b/include/core/utils/utils_gtest.h
index 97f082db2..5f85f806e 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/src/capture/capture_base.cpp b/src/capture/capture_base.cpp
index a6e566b54..42af536ad 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,21 +284,22 @@ 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();
}
@@ -286,85 +311,88 @@ void CaptureBase::printHeader(int _depth, bool _factored_by, bool _metric, bool
{
_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 b6c628d33..30dd73d05 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 a2394cf9f..7ef379124 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 e6a35dbe3..9ba0776f3 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,8 +102,7 @@ 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" : "");
@@ -111,29 +110,29 @@ void CaptureMotion::printHeader(int _depth, bool _factored_by, bool _metric, boo
{
_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 c9d3b6550..de755b1b5 100644
--- a/src/capture/capture_odom_2d.cpp
+++ b/src/capture/capture_odom_2d.cpp
@@ -22,22 +22,21 @@
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 f359b7e96..e68ed9837 100644
--- a/src/capture/capture_odom_3d.cpp
+++ b/src/capture/capture_odom_3d.cpp
@@ -22,22 +22,21 @@
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)
{
//
}
@@ -48,4 +47,3 @@ CaptureOdom3d::~CaptureOdom3d()
}
} /* namespace wolf */
-
diff --git a/src/capture/capture_pose.cpp b/src/capture/capture_pose.cpp
index 5910874bb..ce8a4d543 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 2c960af33..a18c8db42 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 9143b059f..eb1631397 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 984af4d6e..186208106 100644
--- a/src/ceres_wrapper/qr_manager.cpp
+++ b/src/ceres_wrapper/qr_manager.cpp
@@ -20,16 +20,16 @@
#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)
{
//
}
@@ -46,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_);
@@ -55,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");
@@ -72,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()));
}
}
@@ -126,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;
@@ -153,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());
@@ -169,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();
@@ -179,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());
@@ -190,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());
@@ -204,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 c242d4911..3caa98e8f 100644
--- a/src/ceres_wrapper/solver_ceres.cpp
+++ b/src/ceres_wrapper/solver_ceres.cpp
@@ -29,35 +29,32 @@
namespace wolf
{
-
-SolverCeres::SolverCeres(const ProblemPtr& _wolf_problem) :
- SolverCeres(_wolf_problem, std::make_shared<ParamsCeres>())
+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 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)
{
- covariance_ = std::make_unique<ceres::Covariance>(params_ceres_->covariance_options);
+ 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));
+ getSolverOptions().callbacks.push_back(
+ new IterationUpdateCallback(wolf_problem_,
+ params_ceres_->min_num_iterations,
+ params_ceres_->verbose != SolverManager::ReportVerbosity::QUIET));
}
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())
{
@@ -79,7 +76,7 @@ std::string SolverCeres::solveDerived(const ReportVerbosity report_level)
std::string report;
- //return report
+ // return report
if (report_level == ReportVerbosity::BRIEF)
report = summary_.BriefReport();
else if (report_level == ReportVerbosity::FULL)
@@ -102,7 +99,7 @@ std::string SolverCeres::solveDerived(const ReportVerbosity report_level)
}
bool SolverCeres::computeCovariancesDerived(const CovarianceBlocksToBeComputed _blocks)
-{
+{
// update problem
update();
@@ -112,12 +109,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 +121,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 +158,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 +176,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 +201,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 +223,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 +294,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 +360,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 +410,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 +425,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 +437,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 +467,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 +484,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 +492,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 +535,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 +593,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 +602,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 +617,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 +649,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 +661,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 +701,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 +738,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 +763,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 +782,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,39 +812,44 @@ 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
@@ -821,16 +857,13 @@ void SolverCeres::printProfilingDerived(std::ostream& _stream) const
_stream << "Iterations:"
<< "\n\tUser-defined limit: " << params_ceres_->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 5d9393c61..b680bc94b 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 2b0ff729c..9ee3c37cf 100644
--- a/src/common/node_state_blocks.cpp
+++ b/src/common/node_state_blocks.cpp
@@ -144,11 +144,12 @@ StateBlockPtr NodeStateBlocks::addStateBlock(const char& _sb_key, const StateBlo
}
void NodeStateBlocks::emplaceFactorStateBlock(const char& _key,
- const Eigen::VectorXd& _x,
- const Eigen::MatrixXd& _cov,
- unsigned int _start_idx)
+ const Eigen::VectorXd& _x,
+ const Eigen::MatrixXd& _cov,
+ unsigned int _start_idx)
{
- assert(isCovariance(_cov) and "NodeStateBlocks::emplaceFactorStateBlock: provided _cov is not a covariance matrix");
+ assert(isCovariance(_cov) and
+ "NodeStateBlocks::emplaceFactorStateBlock: provided _cov is not a covariance matrix");
StateBlockPtr _sb = getStateBlock(_key);
@@ -162,7 +163,8 @@ void NodeStateBlocks::emplaceFactorStateBlock(const char& _key,
// not segment of state not allowed
if (_start_idx != 0)
throw std::runtime_error(
- "NodeStateBlocks::emplaceFactorStateBlock: 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
@@ -182,7 +184,8 @@ void NodeStateBlocks::emplaceFactorStateBlock(const char& _key,
// state size
if (_x.size() + _start_idx > _sb->getSize())
throw std::runtime_error(
- "NodeStateBlocks::emplaceFactorStateBlock: 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::emplaceFactorStateBlock(const char& _key,
// existing prior
if (factor_prior_map_.count(_key))
throw std::runtime_error(
- std::string("NodeStateBlocks::emplaceFactorStateBlock: 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())
@@ -262,7 +265,8 @@ void NodeStateBlocks::emplacePriors(const SpecStateComposite& _specs)
if (prior.isFixed()) state_block_composite_.at(key)->fix();
// emplace factor prior (if specified)
- if (prior.isFactor()) emplaceFactorStateBlock(key, prior.getState(), prior.getNoiseStd().cwiseAbs2().asDiagonal());
+ if (prior.isFactor())
+ emplaceFactorStateBlock(key, prior.getState(), prior.getNoiseStd().cwiseAbs2().asDiagonal());
// set state (already set inside emplaceFactorStateBlock)
else
@@ -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 627e39310..7756ac8a2 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 fcd838c51..adced7708 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_sensor_composite.cpp b/src/composite/spec_state_sensor_composite.cpp
index 3f6176af3..5608f4c72 100644
--- a/src/composite/spec_state_sensor_composite.cpp
+++ b/src/composite/spec_state_sensor_composite.cpp
@@ -20,26 +20,22 @@
#include "core/composite/spec_state_sensor_composite.h"
#include "core/state_block/factory_state_block.h"
-#include "core/common/params_base.h" // toString()
+#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 +51,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 +61,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 +74,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/vector_composite.cpp b/src/composite/vector_composite.cpp
index 715685670..68c13b490 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 83031d196..f879376e8 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 baa1665cc..e227b4e68 100644
--- a/src/feature/feature_base.cpp
+++ b/src/feature/feature_base.cpp
@@ -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 4c97a4d33..5703bb078 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 c80cc7c93..c677ad335 100644
--- a/src/feature/feature_motion.cpp
+++ b/src/feature/feature_motion.cpp
@@ -22,15 +22,14 @@
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 f07bb4e6f..e2f68e89c 100644
--- a/src/feature/feature_odom_2d.cpp
+++ b/src/feature/feature_odom_2d.cpp
@@ -20,12 +20,13 @@
#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;
+ // std::cout << "New FeatureOdom2d: measurement " << _measurement.transpose() << std::endl << "covariance" <<
+ // std::endl << _meas_covariance << std::endl;
}
FeatureOdom2d::~FeatureOdom2d()
@@ -33,4 +34,4 @@ FeatureOdom2d::~FeatureOdom2d()
//
}
-} // namespace wolf
+} // namespace wolf
diff --git a/src/feature/feature_pose.cpp b/src/feature/feature_pose.cpp
index b53ed99b8..443e08780 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 c60c5c9b6..75eed4795 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)
diff --git a/src/hardware/hardware_base.cpp b/src/hardware/hardware_base.cpp
index f41685c7d..d4a8a45ed 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;
diff --git a/src/landmark/landmark_base.cpp b/src/landmark/landmark_base.cpp
index 037fd2c82..db392e445 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 fe02ce398..7b757f95c 100644
--- a/src/map/map_base.cpp
+++ b/src/map/map_base.cpp
@@ -35,7 +35,6 @@
namespace wolf
{
-
MapBase::MapBase() : NodeBase("MAP", "Base")
{
// std::cout << "constructed M"<< std::endl;
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index 964aafcb0..d06e06e27 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -376,7 +376,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)
{
diff --git a/src/processor/motion_buffer.cpp b/src/processor/motion_buffer.cpp
index 30f395823..f5dde074f 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 3726ad2c0..0ad0109bc 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 1d86e6f81..44f3cc0f1 100644
--- a/src/processor/processor_base.cpp
+++ b/src/processor/processor_base.cpp
@@ -197,8 +197,8 @@ 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);
diff --git a/src/processor/processor_diff_drive.cpp b/src/processor/processor_diff_drive.cpp
index 8b95f0be0..b9d091017 100644
--- a/src/processor/processor_diff_drive.cpp
+++ b/src/processor/processor_diff_drive.cpp
@@ -28,10 +28,9 @@
namespace wolf
{
-
ProcessorDiffDrive::ProcessorDiffDrive(const YAML::Node& _params)
: ProcessorOdom2d(_params),
- radians_per_tick_(0.0) // This param needs further initialization. See this->configure(sensor).
+ 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
diff --git a/src/processor/processor_fixed_wing_model.cpp b/src/processor/processor_fixed_wing_model.cpp
index 5334f5ecb..582b6e73f 100644
--- a/src/processor/processor_fixed_wing_model.cpp
+++ b/src/processor/processor_fixed_wing_model.cpp
@@ -26,42 +26,38 @@
namespace wolf
{
-
-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>())
+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");
+ 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",
- velocity_local_,
- Eigen::Matrix1d(angle_stdev_ * 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(),
@@ -75,5 +71,4 @@ void ProcessorFixedWingModel::processKeyFrame(FrameBasePtr _keyframe_ptr)
// 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 84850d4ac..53021d8cc 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();
diff --git a/src/processor/processor_loop_closure.cpp b/src/processor/processor_loop_closure.cpp
index 85d190a13..90aaaa897 100644
--- a/src/processor/processor_loop_closure.cpp
+++ b/src/processor/processor_loop_closure.cpp
@@ -22,7 +22,6 @@
namespace wolf
{
-
ProcessorLoopClosure::ProcessorLoopClosure(const std::string& _type, int _dim, const YAML::Node& _params)
: ProcessorBase(_type, _dim, _params)
{
diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp
index 746279a02..2da24c114 100644
--- a/src/processor/processor_motion.cpp
+++ b/src/processor/processor_motion.cpp
@@ -23,7 +23,6 @@
namespace wolf
{
-
ProcessorMotion::ProcessorMotion(const std::string& _type,
TypeComposite _state_types,
int _dim,
@@ -345,7 +344,7 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
// 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);
}
diff --git a/src/processor/processor_odom_2d.cpp b/src/processor/processor_odom_2d.cpp
index 8963cda0f..0a8117a34 100644
--- a/src/processor/processor_odom_2d.cpp
+++ b/src/processor/processor_odom_2d.cpp
@@ -26,7 +26,6 @@
namespace wolf
{
-
ProcessorOdom2d::ProcessorOdom2d(const YAML::Node& _params)
: ProcessorMotion("ProcessorOdom2d", {{'P', "StatePoint2d"}, {'O', "StateAngle"}}, 2, 3, 3, 3, 2, 0, _params)
{
diff --git a/src/processor/processor_odom_3d.cpp b/src/processor/processor_odom_3d.cpp
index 42795eb4c..28900f34f 100644
--- a/src/processor/processor_odom_3d.cpp
+++ b/src/processor/processor_odom_3d.cpp
@@ -24,7 +24,6 @@
namespace wolf
{
-
ProcessorOdom3d::ProcessorOdom3d(const YAML::Node& _params)
: ProcessorMotion("ProcessorOdom3d", {{'P', "StatePoint3d"}, {'O', "StateQuaternion"}}, 3, 7, 7, 6, 6, 0, _params)
{
diff --git a/src/processor/processor_tracker.cpp b/src/processor/processor_tracker.cpp
index 46d93f01e..bc093a4e3 100644
--- a/src/processor/processor_tracker.cpp
+++ b/src/processor/processor_tracker.cpp
@@ -26,7 +26,6 @@
namespace wolf
{
-
ProcessorTracker::ProcessorTracker(const std::string& _type,
const StateKeys& _state_keys,
int _dim,
@@ -124,8 +123,7 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
}
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());
+ 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
@@ -174,8 +172,7 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
break;
}
case RUNNING_WITH_KEYFRAME: {
- FrameBasePtr keyframe_from_callback =
- buffer_frame_.select(last_ptr_->getTimeStamp(), getTimeTolerance());
+ FrameBasePtr keyframe_from_callback = buffer_frame_.select(last_ptr_->getTimeStamp(), getTimeTolerance());
buffer_frame_.removeUpTo(keyframe_from_callback->getTimeStamp());
WOLF_DEBUG("PT ",
diff --git a/src/processor/processor_tracker_feature.cpp b/src/processor/processor_tracker_feature.cpp
index bd5dc55b4..5abd3844f 100644
--- a/src/processor/processor_tracker_feature.cpp
+++ b/src/processor/processor_tracker_feature.cpp
@@ -22,7 +22,6 @@
namespace wolf
{
-
ProcessorTrackerFeature::ProcessorTrackerFeature(const std::string& _type,
const StateKeys& _structure,
int _dim,
diff --git a/src/processor/processor_tracker_landmark.cpp b/src/processor/processor_tracker_landmark.cpp
index c27f201c7..e6703dec0 100644
--- a/src/processor/processor_tracker_landmark.cpp
+++ b/src/processor/processor_tracker_landmark.cpp
@@ -25,7 +25,6 @@
namespace wolf
{
-
ProcessorTrackerLandmark::ProcessorTrackerLandmark(const std::string& _type,
const StateKeys& _structure,
const YAML::Node& _params)
diff --git a/src/processor/track_matrix.cpp b/src/processor/track_matrix.cpp
index e57e0adfa..37d32b7dc 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_diff_drive.cpp b/src/sensor/sensor_diff_drive.cpp
index 19ff50d07..6956f9818 100644
--- a/src/sensor/sensor_diff_drive.cpp
+++ b/src/sensor/sensor_diff_drive.cpp
@@ -24,7 +24,6 @@
namespace wolf
{
-
SensorDiffDrive::SensorDiffDrive(const YAML::Node& _params)
: SensorBase("SensorDiffDrive", 2, _params),
ticks_per_wheel_revolution_(_params["ticks_per_wheel_revolution"].as<double>()),
diff --git a/src/sensor/sensor_motion_model.cpp b/src/sensor/sensor_motion_model.cpp
index b4239f9dd..6cd04a151 100644
--- a/src/sensor/sensor_motion_model.cpp
+++ b/src/sensor/sensor_motion_model.cpp
@@ -22,7 +22,6 @@
namespace wolf
{
-
SensorMotionModel::SensorMotionModel(const YAML::Node& _params) : SensorBase("SensorMotionModel", -1, _params) {}
SensorMotionModel::~SensorMotionModel() {}
diff --git a/src/solver/solver_manager.cpp b/src/solver/solver_manager.cpp
index d0b295cc6..cff673c7b 100644
--- a/src/solver/solver_manager.cpp
+++ b/src/solver/solver_manager.cpp
@@ -23,54 +23,47 @@
#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) : 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)
{
assert(_problem != nullptr && "Passed a nullptr ProblemPtr.");
}
-SolverManager::~SolverManager()
-{
-}
+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 +76,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 +92,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 +111,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 +145,8 @@ void SolverManager::update()
wolf_problem_->resetTransformed();
#ifdef _WOLF_DEBUG
- assert(check("after update()"));
- #endif
+ assert(check("after update()"));
+#endif
}
wolf::ProblemPtr SolverManager::getProblem()
@@ -174,16 +167,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,13 +196,15 @@ 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;
}
@@ -224,9 +221,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 +236,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 +266,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 +294,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 +322,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 +345,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 +355,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 +389,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 +424,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();
}
@@ -470,7 +486,8 @@ SolverManager::ReportVerbosity SolverManager::getVerbosity() const
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 +502,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);
};
@@ -553,7 +565,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 +577,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 +602,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 +616,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 +635,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 +654,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 +665,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 +699,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 4b2a57342..56c3bc896 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 9c5b8b92c..ac00ac494 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 30b9eb046..cc501ff09 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,25 @@ 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 +73,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 ae2a948f6..4cf32c40b 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,20 @@ 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 +60,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 +77,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 +87,20 @@ 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 +108,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 +120,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 9765c619e..c6642a392 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 e2c3ff10e..99e8f61cd 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 5b9d96728..4a1be6f26 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) ? (" -- " + std::to_string(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 1f64403e9..cf3d61af8 100644
--- a/src/tree_manager/tree_manager_sliding_window.cpp
+++ b/src/tree_manager/tree_manager_sliding_window.cpp
@@ -22,25 +22,21 @@
namespace wolf
{
-
void TreeManagerSlidingWindow::keyFrameCallback(FrameBasePtr _frame)
{
- int n_f = getProblem()->getTrajectory()->size();
+ 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_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));
- 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());
@@ -61,5 +57,4 @@ void TreeManagerSlidingWindow::keyFrameCallback(FrameBasePtr _frame)
// 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 eb8567761..3a7cc6491 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,26 @@
namespace wolf
{
-
-TreeManagerSlidingWindowDualRate::TreeManagerSlidingWindowDualRate(ParamsTreeManagerSlidingWindowDualRatePtr _params) :
- TreeManagerSlidingWindow(_params),
- params_swdr_(_params),
- count_frames_(0)
+TreeManagerSlidingWindowDualRate::TreeManagerSlidingWindowDualRate(ParamsTreeManagerSlidingWindowDualRatePtr _params)
+ : TreeManagerSlidingWindow(_params), params_swdr_(_params), count_frames_(0)
{
NodeBase::setType("TreeManagerSlidingWindowDualRate");
}
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 <= params_swdr_->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(params_swdr_->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 +58,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)
@@ -82,12 +81,10 @@ void TreeManagerSlidingWindowDualRate::keyFrameCallback(FrameBasePtr _frame)
// iterate counter
count_frames_++;
- if (count_frames_ == params_swdr_->rate_old_frames)
- count_frames_ = 0;
+ if (count_frames_ == params_swdr_->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 cf48d302a..05fa8e86e 100644
--- a/src/utils/graph_search.cpp
+++ b/src/utils/graph_search.cpp
@@ -22,35 +22,28 @@
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)
@@ -61,11 +54,11 @@ FactorBasePtrList GraphSearch::computeShortestPath(NodeStateBlocksPtr node1,
// 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 56f251465..44dfffcd3 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/dummy/dummy_object.h b/test/dummy/dummy_object.h
index 7dbb1d6b6..07a2983dc 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 02c690931..f5b3e10c0 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 3be8a35d9..90640c4ec 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 004176952..516bf6b60 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 ba58696fc..8ba7a5a7f 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 4bd1853b0..033d694dd 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 0b029e9d0..14cb3bc08 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 f63523187..44c5a0007 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 45220b0a1..4909085cf 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_relative_pose_2d_autodiff.h b/test/dummy/factor_relative_pose_2d_autodiff.h
index fb20d1348..f8f9ec07f 100644
--- a/test/dummy/factor_relative_pose_2d_autodiff.h
+++ b/test/dummy/factor_relative_pose_2d_autodiff.h
@@ -36,10 +36,10 @@ class FactorRelativePose2dAutodiff : public FactorAutodiff<FactorRelativePose2dA
{
public:
FactorRelativePose2dAutodiff(const FeatureBasePtr& _ftr_ptr,
- const FrameBasePtr& _frame_other_ptr,
- const ProcessorBasePtr& _processor_ptr,
- bool _apply_loss_function,
- FactorStatus _status = FAC_ACTIVE)
+ const FrameBasePtr& _frame_other_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status = FAC_ACTIVE)
: FactorAutodiff("FactorOdom2dAutodiff",
TOP_OTHER,
_ftr_ptr->getMeasurement(),
@@ -65,10 +65,10 @@ class FactorRelativePose2dAutodiff : public FactorAutodiff<FactorRelativePose2dA
template <typename T>
inline bool FactorRelativePose2dAutodiff::operator()(const T* const _p1,
- const T* const _o1,
- const T* const _p2,
- const T* const _o2,
- T* _residuals) const
+ 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 eaad26a94..12d2b3e40 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 e7d8228e4..f17f4585c 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
index 0cc61417a..d85674d25 100644
--- a/test/dummy/processor_diff_drive_mock.h
+++ b/test/dummy/processor_diff_drive_mock.h
@@ -27,7 +27,6 @@
namespace wolf
{
-
WOLF_PTR_TYPEDEFS(ProcessorDiffDriveMock);
class ProcessorDiffDriveMock : public ProcessorDiffDrive
@@ -84,7 +83,7 @@ class ProcessorDiffDriveMock : public ProcessorDiffDrive
{
return Base::deltaZero();
}
-
+
VectorXd getCalibration(const CaptureBaseConstPtr _capture) const override
{
return Base::getCalibration(_capture);
diff --git a/test/dummy/processor_loop_closure_dummy.h b/test/dummy/processor_loop_closure_dummy.h
index ebb677ae0..e1829caf2 100644
--- a/test/dummy/processor_loop_closure_dummy.h
+++ b/test/dummy/processor_loop_closure_dummy.h
@@ -24,7 +24,6 @@
namespace wolf
{
-
WOLF_PTR_TYPEDEFS(ProcessorLoopClosureDummy);
// dummy class:
@@ -119,6 +118,7 @@ class ProcessorLoopClosureDummy : public ProcessorLoopClosure
// Register in the FactoryProcessor
#include "core/processor/factory_processor.h"
-namespace wolf {
+namespace wolf
+{
WOLF_REGISTER_PROCESSOR(ProcessorLoopClosureDummy);
-} // namespace wolf
+} // namespace wolf
diff --git a/test/dummy/processor_motion_provider_dummy.h b/test/dummy/processor_motion_provider_dummy.h
index c619e86eb..5eff68400 100644
--- a/test/dummy/processor_motion_provider_dummy.h
+++ b/test/dummy/processor_motion_provider_dummy.h
@@ -25,46 +25,65 @@
namespace wolf
{
-
WOLF_PTR_TYPEDEFS(MotionProviderDummy);
class ProcessorMotionProviderDummy : public ProcessorBase, public MotionProvider
{
- public:
- ProcessorMotionProviderDummy(const YAML::Node& _params) :
- ProcessorBase("ProcessorMotionProviderDummy", 2, _params),
- MotionProvider({{'P',"StatePoint2d"},{'O',"StateAngle"}}, _params)
- {}
- ~ProcessorMotionProviderDummy(){};
+ 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(ProcessorMotionProviderDummy);
+ // 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 {
+namespace wolf
+{
WOLF_REGISTER_PROCESSOR(ProcessorMotionProviderDummy);
-} // namespace wolf
+} // namespace wolf
diff --git a/test/dummy/processor_motion_provider_dummy_pov.h b/test/dummy/processor_motion_provider_dummy_pov.h
index a54ecacaa..0ce065488 100644
--- a/test/dummy/processor_motion_provider_dummy_pov.h
+++ b/test/dummy/processor_motion_provider_dummy_pov.h
@@ -25,46 +25,65 @@
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(){};
+ 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);
+ // 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);};
+ 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 {
+namespace wolf
+{
WOLF_REGISTER_PROCESSOR(ProcessorMotionProviderDummyPOV);
-} // namespace wolf
+} // namespace wolf
diff --git a/test/dummy/processor_tracker_feature_dummy.cpp b/test/dummy/processor_tracker_feature_dummy.cpp
index 6cd82f5cc..4d763733b 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,
@@ -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(),
- applyLossFunction());
+ 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 ae9c80984..ed35b621f 100644
--- a/test/dummy/processor_tracker_feature_dummy.h
+++ b/test/dummy/processor_tracker_feature_dummy.h
@@ -26,7 +26,6 @@
namespace wolf
{
-
WOLF_PTR_TYPEDEFS(ProcessorTrackerFeatureDummy);
// Class
diff --git a/test/dummy/processor_tracker_landmark_dummy.cpp b/test/dummy/processor_tracker_landmark_dummy.cpp
index ec0d45406..e27308395 100644
--- a/test/dummy/processor_tracker_landmark_dummy.cpp
+++ b/test/dummy/processor_tracker_landmark_dummy.cpp
@@ -26,7 +26,6 @@
namespace wolf
{
-
ProcessorTrackerLandmarkDummy::ProcessorTrackerLandmarkDummy(const YAML::Node& _params)
: ProcessorTrackerLandmark("ProcessorTrackerLandmarkDummy", "PO", _params),
n_landmarks_lost_(_params["n_landmarks_lost"].as<unsigned int>())
diff --git a/test/dummy/processor_tracker_landmark_dummy.h b/test/dummy/processor_tracker_landmark_dummy.h
index 3a62646ca..ebe439379 100644
--- a/test/dummy/processor_tracker_landmark_dummy.h
+++ b/test/dummy/processor_tracker_landmark_dummy.h
@@ -24,7 +24,6 @@
namespace wolf
{
-
WOLF_PTR_TYPEDEFS(ProcessorTrackerLandmarkDummy);
class ProcessorTrackerLandmarkDummy : public ProcessorTrackerLandmark
diff --git a/test/dummy/sensor_dummy.h b/test/dummy/sensor_dummy.h
index c2c640fd7..c8b677de8 100644
--- a/test/dummy/sensor_dummy.h
+++ b/test/dummy/sensor_dummy.h
@@ -27,7 +27,6 @@
namespace wolf
{
-
template <unsigned int DIM>
class SensorDummy : public SensorBase
{
diff --git a/test/dummy/sensor_dummy_poia.h b/test/dummy/sensor_dummy_poia.h
index 6d8ae584e..3801ba02b 100644
--- a/test/dummy/sensor_dummy_poia.h
+++ b/test/dummy/sensor_dummy_poia.h
@@ -27,7 +27,6 @@
namespace wolf
{
-
template <unsigned int DIM>
class SensorDummyPoia : public SensorBase
{
diff --git a/test/dummy/solver_manager_dummy.h b/test/dummy/solver_manager_dummy.h
index 0b368f64c..33f83a029 100644
--- a/test/dummy/solver_manager_dummy.h
+++ b/test/dummy/solver_manager_dummy.h
@@ -24,97 +24,122 @@
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;
- };
+ 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;
+ };
};
-}
+} // namespace wolf
diff --git a/test/dummy/tree_manager_dummy.h b/test/dummy/tree_manager_dummy.h
index c1c939022..90aacad7e 100644
--- a/test/dummy/tree_manager_dummy.h
+++ b/test/dummy/tree_manager_dummy.h
@@ -24,13 +24,11 @@
namespace wolf
{
-
WOLF_STRUCT_PTR_TYPEDEFS(ParamsTreeManagerDummy)
struct ParamsTreeManagerDummy : public ParamsTreeManagerBase
{
ParamsTreeManagerDummy() = default;
- ParamsTreeManagerDummy(const YAML::Node& _n):
- ParamsTreeManagerBase(_n)
+ ParamsTreeManagerDummy(const YAML::Node& _n) : ParamsTreeManagerBase(_n)
{
toy_param = _n["toy_param"].as<double>();
}
@@ -41,8 +39,7 @@ struct ParamsTreeManagerDummy : public ParamsTreeManagerBase
std::string print() const override
{
- return ParamsTreeManagerBase::print() + "\n"
- + "toy_param: " + toString(toy_param) + "\n";
+ return ParamsTreeManagerBase::print() + "\n" + "toy_param: " + toString(toy_param) + "\n";
}
};
@@ -50,27 +47,25 @@ 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(ParamsTreeManagerBasePtr _params) : TreeManagerBase("TreeManagerDummy", _params), n_KF_(0){};
+ WOLF_TREE_MANAGER_CREATE(TreeManagerDummy, ParamsTreeManagerBase)
- ~TreeManagerDummy() override{}
+ ~TreeManagerDummy() override {}
- void keyFrameCallback(FrameBasePtr _KF) override
- {
- n_KF_++;
- };
+ void keyFrameCallback(FrameBasePtr _KF) override
+ {
+ n_KF_++;
+ };
- int n_KF_;
+ int n_KF_;
};
} /* namespace wolf */
// Register in the FactoryTreeManager
#include "core/tree_manager/factory_tree_manager.h"
-namespace wolf {
+namespace wolf
+{
WOLF_REGISTER_TREE_MANAGER(TreeManagerDummy)
} /* namespace wolf */
diff --git a/test/gtest_SE2.cpp b/test/gtest_SE2.cpp
index 84733273d..87164bebf 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 700f6787f..ca50261cc 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 a37315513..54e12a7f3 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 ed25b7d02..4bfbe7b5a 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_emplace.cpp b/test/gtest_emplace.cpp
index c0d16d1b6..3a6cb174c 100644
--- a/test/gtest_emplace.cpp
+++ b/test/gtest_emplace.cpp
@@ -89,18 +89,20 @@ TEST(Emplace, Processor)
ASSERT_EQ(P->getHardware(), S->getHardware());
ASSERT_EQ(P->getHardware()->getSensorList().front(), S);
- auto prc =
- ProcessorBase::emplace<ProcessorOdom2d>(S, YAML::LoadFile(wolf_dir + "/test/yaml/processor_odom_2d.yaml"), {wolf_dir});
+ 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<SensorDummy2d>(
- P->getHardware(), YAML::LoadFile(wolf_dir + "/test/yaml/sensor_tests/sensor_PO_2D_initial_guess.yaml"), {wolf_dir});
+ P->getHardware(),
+ YAML::LoadFile(wolf_dir + "/test/yaml/sensor_tests/sensor_PO_2D_initial_guess.yaml"),
+ {wolf_dir});
- ProcessorOdom2dPtr prc2 =
- ProcessorBase::emplace<ProcessorOdom2d>(sen2, YAML::LoadFile(wolf_dir + "/test/yaml/processor_odom_2d.yaml"), {wolf_dir});
+ 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());
diff --git a/test/gtest_example.cpp b/test/gtest_example.cpp
index e81269f39..3bcd8720c 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_autodiff.cpp b/test/gtest_factor_autodiff.cpp
index e2e5b94d6..4d29a8cbb 100644
--- a/test/gtest_factor_autodiff.cpp
+++ b/test/gtest_factor_autodiff.cpp
@@ -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<FactorRelativePose2dAutodiff>(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<FactorRelativePose2dAutodiff>(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<FactorRelativePose2dAutodiff>(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);
diff --git a/test/gtest_factor_base.cpp b/test/gtest_factor_base.cpp
index 3a6a15df2..f7ea48c4c 100644
--- a/test/gtest_factor_base.cpp
+++ b/test/gtest_factor_base.cpp
@@ -36,24 +36,24 @@ class FactorBaseTest : public testing::Test
void SetUp() override
{
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["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"]["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;
+ rand_params["noise_p_std"] = 0.1;
+ rand_params["noise_o_std"] = 0.1;
- S0 = SensorDummy2d::create(rand_params, {});
+ 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);
+ 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(SpecStateComposite(rand_params["states"]));
diff --git a/test/gtest_factor_block_difference.cpp b/test/gtest_factor_block_difference.cpp
index b9c736e9d..f974f971c 100644
--- a/test/gtest_factor_block_difference.cpp
+++ b/test/gtest_factor_block_difference.cpp
@@ -230,7 +230,6 @@ TEST_F(FixtureFactorBlockDifference, DiffPxy)
nullptr,
false);
-
// perturbate
KF0_->getP()->setState(Vector3d::Random());
KF1_->getP()->setState(Vector3d::Random());
@@ -261,7 +260,6 @@ TEST_F(FixtureFactorBlockDifference, DiffPyz)
nullptr,
false);
-
// perturbate
KF0_->getP()->setState(Vector3d::Random());
KF1_->getP()->setState(Vector3d::Random());
diff --git a/test/gtest_factor_pose_2d_with_extrinsics.cpp b/test/gtest_factor_pose_2d_with_extrinsics.cpp
index 813db2bbe..053870538 100644
--- a/test/gtest_factor_pose_2d_with_extrinsics.cpp
+++ b/test/gtest_factor_pose_2d_with_extrinsics.cpp
@@ -45,7 +45,7 @@ SolverCeres solver(problem_ptr);
// Sensor
auto sensor_pose2d =
- problem_ptr->installSensor("SensorPose2d", wolf_dir + "/test/yaml/sensor_pose_2d.yaml", {wolf_dir});
+ 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());
diff --git a/test/gtest_factor_pose_3d_with_extrinsics.cpp b/test/gtest_factor_pose_3d_with_extrinsics.cpp
index 0b417126a..536f22a52 100644
--- a/test/gtest_factor_pose_3d_with_extrinsics.cpp
+++ b/test/gtest_factor_pose_3d_with_extrinsics.cpp
@@ -45,7 +45,7 @@ SolverCeres solver(problem_ptr);
// Sensor
auto sensor_pose3d =
- problem_ptr->installSensor("SensorPose3d", wolf_dir + "/test/yaml/sensor_pose_3d.yaml", {wolf_dir});
+ 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());
diff --git a/test/gtest_factor_relative_pose_2d_autodiff.cpp b/test/gtest_factor_relative_pose_2d_autodiff.cpp
index 022da481d..6154a736d 100644
--- a/test/gtest_factor_relative_pose_2d_autodiff.cpp
+++ b/test/gtest_factor_relative_pose_2d_autodiff.cpp
@@ -25,17 +25,16 @@
#include "core/capture/capture_odom_2d.h"
#include "core/math/rotations.h"
-
using namespace Eigen;
using namespace wolf;
using std::cout;
using std::endl;
// 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);
+ProblemPtr problem_ptr = Problem::create("PO", 2);
SolverCeres solver(problem_ptr);
// Two frames
@@ -65,11 +64,11 @@ TEST(FactorRelativePose2dAutodiff, 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
@@ -106,11 +105,11 @@ TEST(FactorRelativePose2dAutodiff, 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
@@ -134,6 +133,6 @@ TEST(FactorRelativePose2dAutodiff, 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 9fb33e2ca..94d4d0374 100644
--- a/test/gtest_factor_relative_pose_2d_with_extrinsics.cpp
+++ b/test/gtest_factor_relative_pose_2d_with_extrinsics.cpp
@@ -49,7 +49,7 @@ ProblemPtr problem_ptr = Problem::create("PO", 2);
SolverCeres solver(problem_ptr);
// 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});
// Two frames
FrameBasePtr frm0 = problem_ptr->emplaceFrame(0, "PO", Vector3d::Zero());
diff --git a/test/gtest_factor_relative_pose_3d.cpp b/test/gtest_factor_relative_pose_3d.cpp
index 859e93ed9..7ad85c709 100644
--- a/test/gtest_factor_relative_pose_3d.cpp
+++ b/test/gtest_factor_relative_pose_3d.cpp
@@ -40,7 +40,7 @@ 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);
@@ -217,7 +217,7 @@ TEST(FactorRelativePose3d, frame_solve_frame0)
// solve for frm0
std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
- //WOLF_INFO(report);
+ // WOLF_INFO(report);
checksFrameFrame();
}
diff --git a/test/gtest_factor_relative_pose_3d_with_extrinsics.cpp b/test/gtest_factor_relative_pose_3d_with_extrinsics.cpp
index 0368ea83f..7afafe8d9 100644
--- a/test/gtest_factor_relative_pose_3d_with_extrinsics.cpp
+++ b/test/gtest_factor_relative_pose_3d_with_extrinsics.cpp
@@ -47,7 +47,7 @@ ProblemPtr problem_ptr = Problem::create("PO", 3);
SolverCeres solver(problem_ptr);
// 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());
diff --git a/test/gtest_factor_relative_position_2d_with_extrinsics.cpp b/test/gtest_factor_relative_position_2d_with_extrinsics.cpp
index 5e63eeea2..086376fae 100644
--- a/test/gtest_factor_relative_position_2d_with_extrinsics.cpp
+++ b/test/gtest_factor_relative_position_2d_with_extrinsics.cpp
@@ -49,7 +49,7 @@ ProblemPtr problem_ptr = Problem::create("PO", 2);
SolverCeres solver(problem_ptr);
// 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());
@@ -97,14 +97,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()
diff --git a/test/gtest_factor_relative_position_3d_with_extrinsics.cpp b/test/gtest_factor_relative_position_3d_with_extrinsics.cpp
index 61c3a89b7..b63ebe189 100644
--- a/test/gtest_factor_relative_position_3d_with_extrinsics.cpp
+++ b/test/gtest_factor_relative_position_3d_with_extrinsics.cpp
@@ -50,7 +50,7 @@ ProblemPtr problem_ptr = Problem::create("PO", 3);
SolverCeres solver(problem_ptr);
// 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());
@@ -123,30 +123,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()
diff --git a/test/gtest_factor_velocity_local_direction_3d.cpp b/test/gtest_factor_velocity_local_direction_3d.cpp
index f07b2ccbe..977149e6f 100644
--- a/test/gtest_factor_velocity_local_direction_3d.cpp
+++ b/test/gtest_factor_velocity_local_direction_3d.cpp
@@ -25,7 +25,6 @@
#include "core/capture/capture_odom_2d.h"
#include "core/math/rotations.h"
-
using namespace Eigen;
using namespace wolf;
@@ -33,161 +32,147 @@ int N_TESTS = 100;
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
+ 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);
+ }
+
+ 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 +186,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 +217,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 +266,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 83f04481e..ff2c68f94 100644
--- a/test/gtest_factory_state_block.cpp
+++ b/test/gtest_factory_state_block.cpp
@@ -25,7 +25,6 @@
#include "core/sensor/factory_sensor.h"
#include "core/state_block/state_block_derived.h"
-
using namespace wolf;
using namespace Eigen;
@@ -40,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)
@@ -139,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);
}
////////////////////////////////////////////////////////
@@ -160,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());
@@ -171,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());
@@ -182,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());
@@ -193,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());
@@ -212,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());
@@ -220,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());
@@ -272,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());
@@ -283,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);
@@ -345,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");
@@ -376,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 2cf2ec040..7db36fade 100644
--- a/test/gtest_feature_base.cpp
+++ b/test/gtest_feature_base.cpp
@@ -27,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 69aec4336..051006402 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,8 +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);
- auto p = P->installProcessor("ProcessorOdom2d", S, wolf_dir + "/test/yaml/processor_odom_2d.yaml", {wolf_dir});
+ 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 73080589f..f6c0df1ad 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 5b6add59a..0f843330a 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 40bf05882..1fc54de9f 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 d811cacc9..51a8aa9eb 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 7a9f35d46..c9ee303de 100644
--- a/test/gtest_motion_buffer.cpp
+++ b/test/gtest_motion_buffer.cpp
@@ -33,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)
{
@@ -58,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)
@@ -110,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);
// }
@@ -181,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 aad3b80be..51152122d 100644
--- a/test/gtest_motion_provider.cpp
+++ b/test/gtest_motion_provider.cpp
@@ -60,13 +60,13 @@ class MotionProviderTest : public testing::Test
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);
-
+
prc3 = problem->installProcessor("ProcessorMotionProviderDummyPOV",
sen,
wolf_dir + "/test/yaml/processor_motion_provider_dummy3.yaml",
diff --git a/test/gtest_node_state_blocks.cpp b/test/gtest_node_state_blocks.cpp
index 002776eb1..6ee7208a7 100644
--- a/test/gtest_node_state_blocks.cpp
+++ b/test/gtest_node_state_blocks.cpp
@@ -122,7 +122,7 @@ TEST(NodeStateBlocksTest, constructor_specs)
ASSERT_FALSE(N->isFrame());
ASSERT_FALSE(N->isLandmark());
ASSERT_FALSE(N->isSensor());
-
+
// apply priors
N->emplacePriors(specs);
@@ -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->emplacePriors(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));
diff --git a/test/gtest_problem.cpp b/test/gtest_problem.cpp
index 9d7351249..c353ccadd 100644
--- a/test/gtest_problem.cpp
+++ b/test/gtest_problem.cpp
@@ -324,7 +324,7 @@ TEST(Problem, StateBlocks)
// consume notifications
SolverManagerDummyPtr SM = std::make_shared<SolverManagerDummy>(P);
ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd)(2 + 2));
- SM->update(); // calls P->consumeNotifications();
+ SM->update(); // calls P->consumeNotifications();
ASSERT_EQ(P->getStateBlockNotificationMapSize(),
(SizeStd)(0)); // consumeNotifications empties the notification map
@@ -383,7 +383,8 @@ TEST(Problem, perturb)
auto problem = Problem::create("PO", 2);
// make a sensor first
- auto sensor = problem->installSensor("SensorDiffDrive", wolf_dir + "/test/yaml/sensor_diff_drive_gtest_problem.yaml", {wolf_dir});
+ auto sensor = problem->installSensor(
+ "SensorDiffDrive", wolf_dir + "/test/yaml/sensor_diff_drive_gtest_problem.yaml", {wolf_dir});
Vector3d pose;
pose << 0, 0, 0;
@@ -462,9 +463,11 @@ TEST(Problem, check)
auto problem = Problem::create("PO", 2);
// make sensors first
- auto sensor = problem->installSensor("SensorDiffDrive", wolf_dir + "/test/yaml/sensor_diff_drive_gtest_problem.yaml", {wolf_dir});
+ 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});
+ 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++)
diff --git a/test/gtest_processor_base.cpp b/test/gtest_processor_base.cpp
index 96c3ba38a..a32fc4689 100644
--- a/test/gtest_processor_base.cpp
+++ b/test/gtest_processor_base.cpp
@@ -124,8 +124,9 @@ TEST(ProcessorBase, KeyFrameCallback)
// 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->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";
diff --git a/test/gtest_processor_fixed_wing_model.cpp b/test/gtest_processor_fixed_wing_model.cpp
index f8027176c..a6fa39feb 100644
--- a/test/gtest_processor_fixed_wing_model.cpp
+++ b/test/gtest_processor_fixed_wing_model.cpp
@@ -37,26 +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
- processor = problem->installProcessor("ProcessorFixedWingModel", sensor, wolf_dir + "/test/yaml/processor_fixed_wing_model.yaml", {wolf_dir});
- }
+ 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
+ processor = problem->installProcessor(
+ "ProcessorFixedWingModel", sensor, wolf_dir + "/test/yaml/processor_fixed_wing_model.yaml", {wolf_dir});
+ }
};
TEST_F(ProcessorFixedWingModelTest, setup)
@@ -67,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);
@@ -91,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);
@@ -118,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);
@@ -141,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 13a5d9c66..c43e75fd1 100644
--- a/test/gtest_processor_landmark_external.cpp
+++ b/test/gtest_processor_landmark_external.cpp
@@ -101,7 +101,7 @@ void ProcessorLandmarkExternalTest::initProblem(int _dim,
// Sensors
sensor = problem->installSensor("SensorOdom" + toString(dim) + "d",
wolf_dir + "/test/yaml/sensor_odom_" + toString(dim) + "d.yaml",
- {wolf_dir});
+ {wolf_dir});
sensor_odom = problem->installSensor("SensorOdom" + toString(dim) + "d",
wolf_dir + "/test/yaml/sensor_odom_" + toString(dim) + "d.yaml",
{wolf_dir});
diff --git a/test/gtest_processor_odom_2d.cpp b/test/gtest_processor_odom_2d.cpp
index 5d058ea76..65b356bd7 100644
--- a/test/gtest_processor_odom_2d.cpp
+++ b/test/gtest_processor_odom_2d.cpp
@@ -92,7 +92,7 @@ TEST(ProcessorOdom2d, VoteForKfAndSolve)
// 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;
- int N = 7; // number of process() steps
+ int N = 7; // number of process() steps
// Create Wolf tree nodes
ProblemPtr problem = Problem::create("PO", 2);
@@ -221,7 +221,7 @@ TEST(ProcessorOdom2d, KF_callback)
double dt = .01;
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
+ int N = 8; // number of process() steps
// NOTE: We integrate and create KFs as follows:
// n = 0 1 2 3 4 5 6 7 8
diff --git a/test/gtest_processor_tracker_feature_dummy.cpp b/test/gtest_processor_tracker_feature_dummy.cpp
index 4fbe17490..41da87625 100644
--- a/test/gtest_processor_tracker_feature_dummy.cpp
+++ b/test/gtest_processor_tracker_feature_dummy.cpp
@@ -122,7 +122,7 @@ TEST_F(ProcessorTrackerFeatureDummyTest, processNew)
CaptureBasePtr inc_cap = std::make_shared<CaptureVoid>(1, sensor);
processor->setInc(inc_cap);
- auto n_new_feat = processor->callProcessNew(10); // detect 10 features
+ auto n_new_feat = processor->callProcessNew(10); // detect 10 features
ASSERT_EQ(n_new_feat, 10); // detected 10 features
ASSERT_EQ(processor->getLast()->getFeatureList().size(), 10); // detected 10 features
@@ -143,13 +143,13 @@ TEST_F(ProcessorTrackerFeatureDummyTest, processKnown)
CaptureBasePtr inc_cap = std::make_shared<CaptureVoid>(1, sensor);
processor->setInc(inc_cap);
- processor->callProcessNew(15); // detect 15 features, 1 track lost per tracking
+ processor->callProcessNew(15); // detect 15 features, 1 track lost per tracking
ASSERT_EQ(processor->getLast()->getFeatureList().size(), 15); // detected 15 features
ASSERT_EQ(processor->getIncoming()->getFeatureList().size(), 14); // 1 track lost
ASSERT_EQ(processor->getMatchesLastFromIncoming().size(), 14); // 1 track lost
- processor->callReset(); // now incoming is last and last is origin
+ processor->callReset(); // now incoming is last and last is origin
// Put a capture on last_ptr_
CaptureBasePtr new_cap = std::make_shared<CaptureVoid>(0, sensor);
@@ -206,13 +206,13 @@ TEST_F(ProcessorTrackerFeatureDummyTest, establishFactors)
CaptureBasePtr inc_cap = CaptureBase::emplace<CaptureVoid>(inc_frm, 1, sensor);
processor->setInc(inc_cap);
- processor->callProcessNew(15); // detect 15 features, 1 track lost per tracking
+ processor->callProcessNew(15); // detect 15 features, 1 track lost per tracking
ASSERT_EQ(processor->getLast()->getFeatureList().size(), 15); // detected 15 features
ASSERT_EQ(processor->getIncoming()->getFeatureList().size(), 14); // 1 track lost
ASSERT_EQ(processor->getMatchesLastFromIncoming().size(), 14); // 1 track lost
- processor->callReset(); // now incoming is last and last is origin
+ processor->callReset(); // now incoming is last and last is origin
// test establishFactors()
processor->callEstablishFactors();
diff --git a/test/gtest_processor_tracker_landmark_dummy.cpp b/test/gtest_processor_tracker_landmark_dummy.cpp
index a8194b169..3f8dae8a4 100644
--- a/test/gtest_processor_tracker_landmark_dummy.cpp
+++ b/test/gtest_processor_tracker_landmark_dummy.cpp
@@ -170,7 +170,7 @@ TEST_F(ProcessorTrackerLandmarkDummyTest, processNew)
auto n_new_feat =
processor->callProcessNew(10); // detect 10 features in last, create landmarks, find landmarks in incoming
- ASSERT_EQ(n_new_feat, 10); // detected 10 features
+ ASSERT_EQ(n_new_feat, 10); // detected 10 features
ASSERT_EQ(processor->getLast()->getFeatureList().size(), 10); // detected 10 features
ASSERT_EQ(problem->getMap()->getLandmarkList().size(), 10); // created 10 landmarks
ASSERT_EQ(processor->getIncoming()->getFeatureList().size(), 9); // 1 of each 10 landmarks is not found
@@ -245,9 +245,9 @@ TEST_F(ProcessorTrackerLandmarkDummyTest, establishFactors)
CaptureBasePtr inc_cap = CaptureBase::emplace<CaptureVoid>(inc_frm, 1, sensor);
processor->setInc(inc_cap);
- processor->callProcessNew(15); // detect 15 features, 1 of each 10 tracks is lost
+ processor->callProcessNew(15); // detect 15 features, 1 of each 10 tracks is lost
- ASSERT_EQ(processor->getLast()->getFeatureList().size(), 15); // detected 15 features
+ ASSERT_EQ(processor->getLast()->getFeatureList().size(), 15); // detected 15 features
ASSERT_EQ(processor->getIncoming()->getFeatureList().size(), 14); // 1 of each 10 tracks is lost
ASSERT_EQ(processor->getMatchesLandmarkFromLast().size(), 15); // all landmarks
ASSERT_EQ(processor->getMatchesLandmarkFromIncoming().size(), 14); // 1 of each 10 tracks is lost
diff --git a/test/gtest_rotation.cpp b/test/gtest_rotation.cpp
index 0cba6a4f5..565a2003e 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.
- 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 :
+ /* 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 :
- 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. 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
- /* 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 :
+ 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.
- 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 :
+ /* 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 :
- 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_sensor_diff_drive.cpp b/test/gtest_sensor_diff_drive.cpp
index 6b7c4bf1b..9ad36f391 100644
--- a/test/gtest_sensor_diff_drive.cpp
+++ b/test/gtest_sensor_diff_drive.cpp
@@ -50,7 +50,7 @@ TEST(SensorDiffDrive, create)
param["states"]["I"]["mode"] = "fix";
param["states"]["I"]["dynamic"] = false;
- param["name"] = "just a sensor";
+ param["name"] = "just a sensor";
param["ticks_per_wheel_revolution"] = 4;
param["ticks_std_factor"] = 2;
diff --git a/test/gtest_sensor_odom.cpp b/test/gtest_sensor_odom.cpp
index 5dec8d6b5..94aec75ef 100644
--- a/test/gtest_sensor_odom.cpp
+++ b/test/gtest_sensor_odom.cpp
@@ -28,20 +28,20 @@ using namespace yaml_schema_cpp;
std::string wolf_dir = _WOLF_CODE_DIR;
-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;
+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,
diff --git a/test/gtest_sensor_pose.cpp b/test/gtest_sensor_pose.cpp
index 6ad469b78..7a642d31f 100644
--- a/test/gtest_sensor_pose.cpp
+++ b/test/gtest_sensor_pose.cpp
@@ -109,8 +109,9 @@ TEST(SensorPose, creators_and_factories)
// 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);
+ 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>(
@@ -141,7 +142,6 @@ TEST(SensorPose, creators_and_factories)
}
}
-
int main(int argc, char** argv)
{
testing::InitGoogleTest(&argc, argv);
diff --git a/test/gtest_shared_from_this.cpp b/test/gtest_shared_from_this.cpp
index 82dda2203..6859e3625 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 a01a0f0ee..ac48b773d 100644
--- a/test/gtest_solver_ceres.cpp
+++ b/test/gtest_solver_ceres.cpp
@@ -67,8 +67,8 @@ FactorBasePtr createFactor(NodeStateBlocksPtr node_ptr)
}
TEST(SolverCeres, Create)
{
- ProblemPtr P = Problem::create("PO", 3);
- auto solver_ptr = std::make_shared<SolverCeres>(P);
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverCeres>(P);
// check pointers
EXPECT_EQ(P, solver_ptr->getProblem());
@@ -81,14 +81,15 @@ TEST(SolverCeres, Create)
// FLOATING STATE BLOCKS
TEST(SolverCeres, FloatingStateBlock_Add)
{
- ProblemPtr P = Problem::create("PO", 3);
- auto solver_ptr = std::make_shared<SolverCeres>(P);
+ 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);
+ P->notifyStateBlock(sb_ptr, ADD);
// check notifications
Notification notif;
@@ -111,14 +112,15 @@ TEST(SolverCeres, FloatingStateBlock_Add)
TEST(SolverCeres, FloatingStateBlock_DoubleAdd)
{
- ProblemPtr P = Problem::create("PO", 3);
- auto solver_ptr = std::make_shared<SolverCeres>(P);
+ 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);
+ P->notifyStateBlock(sb_ptr, ADD);
// check notifications
Notification notif;
@@ -134,7 +136,7 @@ TEST(SolverCeres, FloatingStateBlock_DoubleAdd)
EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
// notify stateblock again
- P->notifyStateBlock(sb_ptr,ADD);
+ P->notifyStateBlock(sb_ptr, ADD);
// check notifications
EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
@@ -156,14 +158,15 @@ TEST(SolverCeres, FloatingStateBlock_DoubleAdd)
TEST(SolverCeres, FloatingStateBlock_AddFix)
{
- ProblemPtr P = Problem::create("PO", 3);
- auto solver_ptr = std::make_shared<SolverCeres>(P);
+ 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);
+ P->notifyStateBlock(sb_ptr, ADD);
// check notifications
Notification notif;
@@ -225,14 +228,15 @@ TEST(SolverCeres, FloatingStateBlock_AddFix)
TEST(SolverCeres, FloatingStateBlock_AddFixed)
{
- ProblemPtr P = Problem::create("PO", 3);
- auto solver_ptr = std::make_shared<SolverCeres>(P);
+ 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);
+ P->notifyStateBlock(sb_ptr, ADD);
// check notifications
Notification notif;
@@ -274,14 +278,15 @@ TEST(SolverCeres, FloatingStateBlock_AddFixed)
TEST(SolverCeres, FloatingStateBlock_AddUpdateLocalParam)
{
- ProblemPtr P = Problem::create("PO", 3);
- auto solver_ptr = std::make_shared<SolverCeres>(P);
+ 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);
+ P->notifyStateBlock(sb_ptr, ADD);
// check notifications
Notification notif;
@@ -326,7 +331,7 @@ 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());
@@ -334,11 +339,12 @@ TEST(SolverCeres, FloatingStateBlock_AddUpdateLocalParam)
TEST(SolverCeres, FloatingStateBlock_AddLocalParamRemoveLocalParam)
{
- ProblemPtr P = Problem::create("PO", 3);
- auto solver_ptr = std::make_shared<SolverCeres>(P);
+ 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,7 +356,7 @@ TEST(SolverCeres, FloatingStateBlock_AddLocalParamRemoveLocalParam)
EXPECT_TRUE(sb_ptr->localParamUpdated());
// notify stateblock
- P->notifyStateBlock(sb_ptr,ADD);
+ P->notifyStateBlock(sb_ptr, ADD);
// check notifications
Notification notif;
@@ -366,7 +372,7 @@ TEST(SolverCeres, FloatingStateBlock_AddLocalParamRemoveLocalParam)
EXPECT_EQ(P->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());
@@ -397,14 +403,15 @@ TEST(SolverCeres, FloatingStateBlock_AddLocalParamRemoveLocalParam)
TEST(SolverCeres, FloatingStateBlock_Remove)
{
- ProblemPtr P = Problem::create("PO", 3);
- auto solver_ptr = std::make_shared<SolverCeres>(P);
+ 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);
+ P->notifyStateBlock(sb_ptr, ADD);
// check notifications
Notification notif;
@@ -420,7 +427,7 @@ TEST(SolverCeres, FloatingStateBlock_Remove)
EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
// remove state_block
- P->notifyStateBlock(sb_ptr,REMOVE);
+ P->notifyStateBlock(sb_ptr, REMOVE);
// check notifications
EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
@@ -441,14 +448,15 @@ TEST(SolverCeres, FloatingStateBlock_Remove)
TEST(SolverCeres, FloatingStateBlock_AddRemove)
{
- ProblemPtr P = Problem::create("PO", 3);
- auto solver_ptr = std::make_shared<SolverCeres>(P);
+ 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);
+ P->notifyStateBlock(sb_ptr, ADD);
// check notifications
Notification notif;
@@ -457,7 +465,7 @@ TEST(SolverCeres, FloatingStateBlock_AddRemove)
EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
// remove state_block
- P->notifyStateBlock(sb_ptr,REMOVE); // should cancell out the ADD
+ P->notifyStateBlock(sb_ptr, REMOVE); // should cancell out the ADD
// check notifications
EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
@@ -477,14 +485,15 @@ TEST(SolverCeres, FloatingStateBlock_AddRemove)
TEST(SolverCeres, FloatingStateBlock_AddRemoveAdd)
{
- ProblemPtr P = Problem::create("PO", 3);
- auto solver_ptr = std::make_shared<SolverCeres>(P);
+ 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);
+ P->notifyStateBlock(sb_ptr, ADD);
// check notifications
Notification notif;
@@ -493,14 +502,14 @@ TEST(SolverCeres, FloatingStateBlock_AddRemoveAdd)
EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
// remove state_block
- P->notifyStateBlock(sb_ptr,REMOVE); // should cancell out the ADD
+ P->notifyStateBlock(sb_ptr, REMOVE); // should cancell out the ADD
// check notifications
EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
// notify stateblock
- P->notifyStateBlock(sb_ptr,ADD); // again ADD in notification list
+ P->notifyStateBlock(sb_ptr, ADD); // again ADD in notification list
// check notifications
EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
@@ -521,26 +530,27 @@ TEST(SolverCeres, FloatingStateBlock_AddRemoveAdd)
TEST(SolverCeres, FloatingStateBlock_DoubleRemove)
{
- ProblemPtr P = Problem::create("PO", 3);
- auto solver_ptr = std::make_shared<SolverCeres>(P);
+ 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);
+ P->notifyStateBlock(sb_ptr, ADD);
// update solver
solver_ptr->update();
// remove state_block
- P->notifyStateBlock(sb_ptr,REMOVE);
+ P->notifyStateBlock(sb_ptr, REMOVE);
// update solver
solver_ptr->update();
// remove state_block
- P->notifyStateBlock(sb_ptr,REMOVE);
+ P->notifyStateBlock(sb_ptr, REMOVE);
// check notifications
Notification notif;
@@ -561,17 +571,18 @@ TEST(SolverCeres, FloatingStateBlock_DoubleRemove)
TEST(SolverCeres, FloatingStateBlock_AddUpdated)
{
- ProblemPtr P = Problem::create("PO", 3);
- auto solver_ptr = std::make_shared<SolverCeres>(P);
+ 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 +591,30 @@ TEST(SolverCeres, FloatingStateBlock_AddUpdated)
// == When an ADD is notified: a ADD notification should be stored ==
// notify state block
- P->notifyStateBlock(sb_ptr,ADD);
+ P->notifyStateBlock(sb_ptr, ADD);
// check notifications
// check notifications
Notification notif;
- EXPECT_EQ(P->getStateBlockNotificationMapSize(),1);
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 1);
EXPECT_TRUE(P->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(P->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 P->consumeStateBlockNotificationMap();
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
// Check flags have been reset
EXPECT_FALSE(sb_ptr->fixUpdated());
EXPECT_FALSE(sb_ptr->stateUpdated());
@@ -611,49 +622,51 @@ TEST(SolverCeres, FloatingStateBlock_AddUpdated)
// == When an REMOVE is notified: a REMOVE notification should be stored ==
// remove state_block
- P->notifyStateBlock(sb_ptr,REMOVE);
+ P->notifyStateBlock(sb_ptr, REMOVE);
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(),1);
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 1);
EXPECT_TRUE(P->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);
+ P->notifyStateBlock(sb_ptr, ADD);
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
// == UPDATES should clear the list of notifications ==
// notify state block
- P->notifyStateBlock(sb_ptr,ADD);
+ P->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->getStateBlockNotificationMapSize(),
+ 0); // After solver_manager->update, notifications should be empty
EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
// == ADD + REMOVE: notification map should be empty ==
- P->notifyStateBlock(sb_ptr,ADD);
- P->notifyStateBlock(sb_ptr,REMOVE);
- EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ P->notifyStateBlock(sb_ptr, ADD);
+ P->notifyStateBlock(sb_ptr, REMOVE);
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
}
////////////////////////////////////////////////////////
// STATE BLOCKS AND FACTOR
TEST(SolverCeres, StateBlockAndFactor_Add)
{
- ProblemPtr P = Problem::create("PO", 3);
- auto solver_ptr = std::make_shared<SolverCeres>(P);
+ 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);
+ P->notifyStateBlock(sb_ptr, ADD);
// check notifications
Notification notif;
@@ -669,7 +682,7 @@ TEST(SolverCeres, StateBlockAndFactor_Add)
EXPECT_TRUE(solver_ptr->isStateBlockFloating(sb_ptr));
// notify factor (state block now not floating)
- P->notifyFactor(fac_ptr,ADD);
+ P->notifyFactor(fac_ptr, ADD);
// check notifications
EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
@@ -680,7 +693,7 @@ TEST(SolverCeres, StateBlockAndFactor_Add)
solver_ptr->update();
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
// checks
@@ -692,20 +705,21 @@ TEST(SolverCeres, StateBlockAndFactor_Add)
TEST(SolverCeres, StateBlockAndFactor_DoubleAdd)
{
- ProblemPtr P = Problem::create("PO", 3);
- auto solver_ptr = std::make_shared<SolverCeres>(P);
+ 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);
+ P->notifyStateBlock(sb_ptr, ADD);
// notify factor
- P->notifyFactor(fac_ptr,ADD);
+ P->notifyFactor(fac_ptr, ADD);
// check notifications
Notification notif;
@@ -719,14 +733,14 @@ TEST(SolverCeres, StateBlockAndFactor_DoubleAdd)
solver_ptr->update();
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
// notify stateblock again
- P->notifyStateBlock(sb_ptr,ADD);
+ P->notifyStateBlock(sb_ptr, ADD);
// notify factor
- P->notifyFactor(fac_ptr,ADD);
+ P->notifyFactor(fac_ptr, ADD);
// check notifications
EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
@@ -738,7 +752,7 @@ TEST(SolverCeres, StateBlockAndFactor_DoubleAdd)
solver_ptr->update();
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
// check stateblock
@@ -750,20 +764,21 @@ TEST(SolverCeres, StateBlockAndFactor_DoubleAdd)
TEST(SolverCeres, StateBlockAndFactor_Fix)
{
- ProblemPtr P = Problem::create("PO", 3);
- auto solver_ptr = std::make_shared<SolverCeres>(P);
+ 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);
+ P->notifyStateBlock(sb_ptr, ADD);
// notify factor
- P->notifyFactor(fac_ptr,ADD);
+ P->notifyFactor(fac_ptr, ADD);
// check notifications
Notification notif;
@@ -781,7 +796,7 @@ TEST(SolverCeres, StateBlockAndFactor_Fix)
solver_ptr->update();
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
// check flags
@@ -802,14 +817,14 @@ TEST(SolverCeres, StateBlockAndFactor_Fix)
EXPECT_FALSE(sb_ptr->localParamUpdated());
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
// update solver manager
solver_ptr->update();
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
// check flags
@@ -824,20 +839,21 @@ TEST(SolverCeres, StateBlockAndFactor_Fix)
TEST(SolverCeres, StateBlockAndFactor_AddFixed)
{
- ProblemPtr P = Problem::create("PO", 3);
- auto solver_ptr = std::make_shared<SolverCeres>(P);
+ 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);
+ P->notifyStateBlock(sb_ptr, ADD);
// notify factor
- P->notifyFactor(fac_ptr,ADD);
+ P->notifyFactor(fac_ptr, ADD);
// check notifications
Notification notif;
@@ -863,7 +879,7 @@ TEST(SolverCeres, StateBlockAndFactor_AddFixed)
solver_ptr->update();
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
// check flags
@@ -879,20 +895,21 @@ TEST(SolverCeres, StateBlockAndFactor_AddFixed)
TEST(SolverCeres, StateBlockAndFactor_UpdateLocalParam)
{
- ProblemPtr P = Problem::create("PO", 3);
- auto solver_ptr = std::make_shared<SolverCeres>(P);
+ 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);
+ P->notifyStateBlock(sb_ptr, ADD);
// notify factor
- P->notifyFactor(fac_ptr,ADD);
+ P->notifyFactor(fac_ptr, ADD);
// check notifications
Notification notif;
@@ -922,7 +939,7 @@ TEST(SolverCeres, StateBlockAndFactor_UpdateLocalParam)
solver_ptr->update();
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
// check flags
@@ -933,17 +950,18 @@ 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)
{
- ProblemPtr P = Problem::create("PO", 3);
- auto solver_ptr = std::make_shared<SolverCeres>(P);
+ 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,10 +976,10 @@ TEST(SolverCeres, StateBlockAndFactor_AddLocalParamRemoveLocalParam)
EXPECT_TRUE(sb_ptr->localParamUpdated());
// notify stateblock
- P->notifyStateBlock(sb_ptr,ADD);
+ P->notifyStateBlock(sb_ptr, ADD);
// notify factor
- P->notifyFactor(fac_ptr,ADD);
+ P->notifyFactor(fac_ptr, ADD);
// check notifications
Notification notif;
@@ -974,13 +992,13 @@ TEST(SolverCeres, StateBlockAndFactor_AddLocalParamRemoveLocalParam)
solver_ptr->update();
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
EXPECT_EQ(P->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
@@ -1007,20 +1025,21 @@ TEST(SolverCeres, StateBlockAndFactor_AddLocalParamRemoveLocalParam)
TEST(SolverCeres, StateBlockAndFactor_RemoveStateBlock)
{
- ProblemPtr P = Problem::create("PO", 3);
- auto solver_ptr = std::make_shared<SolverCeres>(P);
+ 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);
+ P->notifyStateBlock(sb_ptr, ADD);
// notify factor
- P->notifyFactor(fac_ptr,ADD);
+ P->notifyFactor(fac_ptr, ADD);
// check notifications
Notification notif;
@@ -1033,11 +1052,11 @@ TEST(SolverCeres, StateBlockAndFactor_RemoveStateBlock)
solver_ptr->update();
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
// remove state_block
- P->notifyStateBlock(sb_ptr,REMOVE);
+ P->notifyStateBlock(sb_ptr, REMOVE);
// check notifications
EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
@@ -1045,7 +1064,7 @@ TEST(SolverCeres, StateBlockAndFactor_RemoveStateBlock)
EXPECT_EQ(P->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));
@@ -1060,20 +1079,21 @@ TEST(SolverCeres, StateBlockAndFactor_RemoveStateBlock)
TEST(SolverCeres, StateBlockAndFactor_RemoveFactor)
{
- ProblemPtr P = Problem::create("PO", 3);
- auto solver_ptr = std::make_shared<SolverCeres>(P);
+ 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);
+ P->notifyStateBlock(sb_ptr, ADD);
// notify factor
- P->notifyFactor(fac_ptr,ADD);
+ P->notifyFactor(fac_ptr, ADD);
// check notifications
Notification notif;
@@ -1086,11 +1106,11 @@ TEST(SolverCeres, StateBlockAndFactor_RemoveFactor)
solver_ptr->update();
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
// remove state_block
- P->notifyFactor(fac_ptr,REMOVE); // state block should be automatically stored as floating
+ P->notifyFactor(fac_ptr, REMOVE); // state block should be automatically stored as floating
// check notifications
EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
@@ -1101,7 +1121,7 @@ TEST(SolverCeres, StateBlockAndFactor_RemoveFactor)
solver_ptr->update();
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
// checks
@@ -1113,20 +1133,21 @@ TEST(SolverCeres, StateBlockAndFactor_RemoveFactor)
TEST(SolverCeres, StateBlockAndFactor_AddRemoveStateBlock)
{
- ProblemPtr P = Problem::create("PO", 3);
- auto solver_ptr = std::make_shared<SolverCeres>(P);
+ 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);
+ P->notifyStateBlock(sb_ptr, ADD);
// notify factor
- P->notifyFactor(fac_ptr,ADD);
+ P->notifyFactor(fac_ptr, ADD);
// check notifications
Notification notif;
@@ -1136,18 +1157,19 @@ TEST(SolverCeres, StateBlockAndFactor_AddRemoveStateBlock)
EXPECT_EQ(ADD, notif);
// remove state_block
- P->notifyStateBlock(sb_ptr,REMOVE); // cancells out ADD notification
+ P->notifyStateBlock(sb_ptr, REMOVE); // cancells out ADD notification
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
EXPECT_TRUE(P->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_EQ(P->getStateBlockNotificationMapSize(), 0);
EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
EXPECT_EQ(ADD, notif);
@@ -1159,20 +1181,21 @@ TEST(SolverCeres, StateBlockAndFactor_AddRemoveStateBlock)
TEST(SolverCeres, StateBlockAndFactor_AddRemoveFactor)
{
- ProblemPtr P = Problem::create("PO", 3);
- auto solver_ptr = std::make_shared<SolverCeres>(P);
+ 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);
+ P->notifyStateBlock(sb_ptr, ADD);
// notify factor
- P->notifyFactor(fac_ptr,ADD);
+ P->notifyFactor(fac_ptr, ADD);
// check notifications
Notification notif;
@@ -1182,18 +1205,18 @@ TEST(SolverCeres, StateBlockAndFactor_AddRemoveFactor)
EXPECT_EQ(ADD, notif);
// remove state_block
- P->notifyFactor(fac_ptr,REMOVE); // cancells out ADD notification
+ P->notifyFactor(fac_ptr, REMOVE); // cancells out ADD notification
// check notifications
- EXPECT_EQ(P->getFactorNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
EXPECT_TRUE(P->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->getStateBlockNotificationMapSize(), 0);
EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
// checks
@@ -1205,20 +1228,21 @@ TEST(SolverCeres, StateBlockAndFactor_AddRemoveFactor)
TEST(SolverCeres, StateBlockAndFactor_DoubleRemoveStateBlock)
{
- ProblemPtr P = Problem::create("PO", 3);
- auto solver_ptr = std::make_shared<SolverCeres>(P);
+ 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);
+ P->notifyStateBlock(sb_ptr, ADD);
// notify factor
- P->notifyFactor(fac_ptr,ADD);
+ P->notifyFactor(fac_ptr, ADD);
// check notifications
Notification notif;
@@ -1228,23 +1252,23 @@ TEST(SolverCeres, StateBlockAndFactor_DoubleRemoveStateBlock)
EXPECT_EQ(ADD, notif);
// remove state_block
- P->notifyStateBlock(sb_ptr,REMOVE); // cancells out the ADD
+ P->notifyStateBlock(sb_ptr, REMOVE); // cancells out the ADD
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
EXPECT_TRUE(P->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_EQ(P->getStateBlockNotificationMapSize(), 0);
EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
EXPECT_EQ(ADD, notif);
// remove state_block
- P->notifyStateBlock(sb_ptr,REMOVE);
+ P->notifyStateBlock(sb_ptr, REMOVE);
// check notifications
EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
@@ -1253,10 +1277,10 @@ TEST(SolverCeres, StateBlockAndFactor_DoubleRemoveStateBlock)
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_EQ(P->getStateBlockNotificationMapSize(), 0);
EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
EXPECT_EQ(ADD, notif);
@@ -1268,20 +1292,21 @@ TEST(SolverCeres, StateBlockAndFactor_DoubleRemoveStateBlock)
TEST(SolverCeres, StateBlockAndFactor_DoubleRemoveFactor)
{
- ProblemPtr P = Problem::create("PO", 3);
- auto solver_ptr = std::make_shared<SolverCeres>(P);
+ 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);
+ P->notifyStateBlock(sb_ptr, ADD);
// notify factor
- P->notifyFactor(fac_ptr,ADD);
+ P->notifyFactor(fac_ptr, ADD);
// check notifications
Notification notif;
@@ -1291,30 +1316,30 @@ TEST(SolverCeres, StateBlockAndFactor_DoubleRemoveFactor)
EXPECT_EQ(ADD, notif);
// remove state_block
- P->notifyFactor(fac_ptr,REMOVE); // cancells out ADD
+ P->notifyFactor(fac_ptr, REMOVE); // cancells out ADD
// check notifications
- EXPECT_EQ(P->getFactorNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
EXPECT_TRUE(P->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(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
// remove state_block
- P->notifyFactor(fac_ptr,REMOVE);
+ P->notifyFactor(fac_ptr, REMOVE);
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
EXPECT_TRUE(P->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));
@@ -1325,11 +1350,12 @@ TEST(SolverCeres, StateBlockAndFactor_DoubleRemoveFactor)
TEST(SolverCeres, StateBlockAndFactor_AddUpdatedStateBlock)
{
- ProblemPtr P = Problem::create("PO", 3);
- auto solver_ptr = std::make_shared<SolverCeres>(P);
+ 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 +1364,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,10 +1373,10 @@ TEST(SolverCeres, StateBlockAndFactor_AddUpdatedStateBlock)
// == When an ADD is notified: a ADD notification should be stored ==
// notify state block
- P->notifyStateBlock(sb_ptr,ADD);
+ P->notifyStateBlock(sb_ptr, ADD);
// notify factor
- P->notifyFactor(fac_ptr,ADD);
+ P->notifyFactor(fac_ptr, ADD);
// check notifications
Notification notif;
@@ -1362,18 +1388,18 @@ TEST(SolverCeres, StateBlockAndFactor_AddUpdatedStateBlock)
// == 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(P->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 P->consumeStateBlockNotificationMap();
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
// Check flags have been reset
EXPECT_FALSE(sb_ptr->fixUpdated());
EXPECT_FALSE(sb_ptr->stateUpdated());
@@ -1381,58 +1407,60 @@ TEST(SolverCeres, StateBlockAndFactor_AddUpdatedStateBlock)
// == When an REMOVE is notified: a REMOVE notification should be stored ==
// remove state_block
- P->notifyStateBlock(sb_ptr,REMOVE);
+ P->notifyStateBlock(sb_ptr, REMOVE);
- EXPECT_EQ(P->getStateBlockNotificationMapSize(),1);
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 1);
EXPECT_TRUE(P->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);
+ P->notifyStateBlock(sb_ptr, ADD);
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
// == UPDATES should clear the list of notifications ==
// notify state block
- P->notifyStateBlock(sb_ptr,ADD);
+ P->notifyStateBlock(sb_ptr, ADD);
// update solver
solver_ptr->update();
- EXPECT_EQ(P->getStateBlockNotificationMapSize(),0); // After solver_manager->update, notifications should be empty
+ EXPECT_EQ(P->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);
+ P->notifyStateBlock(sb_ptr, ADD);
+ P->notifyStateBlock(sb_ptr, REMOVE);
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
}
////////////////////////////////////////////////////////
// ONLY FACTOR
TEST(SolverCeres, OnlyFactor_Add)
{
- ProblemPtr P = Problem::create("PO", 3);
- auto solver_ptr = std::make_shared<SolverCeres>(P);
+ 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);
+ P->notifyFactor(fac_ptr, ADD);
// check notifications
Notification notif;
- EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
EXPECT_TRUE(P->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_EQ(P->getStateBlockNotificationMapSize(), 0);
EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
EXPECT_EQ(ADD, notif);
@@ -1444,45 +1472,46 @@ TEST(SolverCeres, OnlyFactor_Add)
TEST(SolverCeres, OnlyFactor_Remove)
{
- ProblemPtr P = Problem::create("PO", 3);
- auto solver_ptr = std::make_shared<SolverCeres>(P);
+ 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);
+ P->notifyFactor(fac_ptr, ADD);
// check notifications
Notification notif;
- EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
EXPECT_TRUE(P->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_EQ(P->getStateBlockNotificationMapSize(), 0);
EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
EXPECT_EQ(ADD, notif);
// notify factor
- P->notifyFactor(fac_ptr,REMOVE);
+ P->notifyFactor(fac_ptr, REMOVE);
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
- EXPECT_EQ(P->getFactorNotificationMapSize(),0);
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->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(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
// checks
EXPECT_FALSE(solver_ptr->isStateBlockRegistered(sb_ptr));
@@ -1492,37 +1521,38 @@ TEST(SolverCeres, OnlyFactor_Remove)
TEST(SolverCeres, OnlyFactor_AddRemove)
{
- ProblemPtr P = Problem::create("PO", 3);
- auto solver_ptr = std::make_shared<SolverCeres>(P);
+ 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);
+ P->notifyFactor(fac_ptr, ADD);
// check notifications
Notification notif;
- EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
EXPECT_EQ(ADD, notif);
// notify factor
- P->notifyFactor(fac_ptr,REMOVE);
+ P->notifyFactor(fac_ptr, REMOVE);
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
- EXPECT_EQ(P->getFactorNotificationMapSize(),0);
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->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(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
// checks
EXPECT_FALSE(solver_ptr->isStateBlockRegistered(sb_ptr));
@@ -1532,7 +1562,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_manager_multithread.cpp b/test/gtest_solver_manager_multithread.cpp
index 0f3933f66..aa8a2380d 100644
--- a/test/gtest_solver_manager_multithread.cpp
+++ b/test/gtest_solver_manager_multithread.cpp
@@ -20,7 +20,6 @@
#include "core/utils/utils_gtest.h"
-
#include "core/problem/problem.h"
#include "core/sensor/sensor_base.h"
#include "core/state_block/state_block.h"
@@ -43,38 +42,43 @@ 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);
+ auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
// 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;
}
@@ -83,6 +87,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_time_stamp.cpp b/test/gtest_time_stamp.cpp
index b19c1ac20..ac8e2f852 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 e3b10aee3..df47a4cf9 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 885b2a0fd..d57825455 100644
--- a/test/gtest_trajectory.cpp
+++ b/test/gtest_trajectory.cpp
@@ -20,7 +20,6 @@
#include "core/utils/utils_gtest.h"
-
#include "core/problem/problem.h"
#include "core/trajectory/trajectory_base.h"
#include "core/frame/frame_base.h"
@@ -36,8 +35,7 @@ bool debug = false;
TEST(TrajectoryBase, ClosestKeyFrame)
{
-
- ProblemPtr P = Problem::create("PO", 2);
+ ProblemPtr P = Problem::create("PO", 2);
TrajectoryBasePtr T = P->getTrajectory();
// Trajectory status:
@@ -45,41 +43,40 @@ 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);
@@ -92,66 +89,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
+ 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();
- 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 cfcd59307..666fae18b 100644
--- a/test/gtest_tree_manager.cpp
+++ b/test/gtest_tree_manager.cpp
@@ -47,9 +47,8 @@ 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"));
@@ -81,8 +80,7 @@ TEST(TreeManager, FactoryParam)
{
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());
@@ -99,9 +97,8 @@ 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 GM = FactoryTreeManagerYaml::create(
+ "TreeManagerDummy", wolf_dir + "/test/yaml/tree_manager_dummy.yaml", {wolf_dir});
ASSERT_TRUE(std::dynamic_pointer_cast<TreeManagerDummy>(GM) != nullptr);
@@ -117,7 +114,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,7 +122,7 @@ 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)
@@ -140,8 +137,9 @@ TEST(TreeManager, keyFrameCallback)
ASSERT_EQ(GM->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);
@@ -149,6 +147,6 @@ TEST(TreeManager, keyFrameCallback)
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 074f3f1c3..e92f21858 100644
--- a/test/gtest_tree_manager_sliding_window.cpp
+++ b/test/gtest_tree_manager_sliding_window.cpp
@@ -69,8 +69,7 @@ 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 GM = TreeManagerSlidingWindow::create(wolf_dir + "/test/yaml/tree_manager_sliding_window1.yaml", {wolf_dir});
EXPECT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindow>(GM) != nullptr);
diff --git a/wolf_scripts/templates/class_template.cpp b/wolf_scripts/templates/class_template.cpp
index 22a6cbe17..6306b63f5 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 368f1bc34..849cbc5de 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 a15aadaff..38e00df79 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();
}
-
--
GitLab