diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index db03ba20fa09c0cfc1b946c9d76eaab4015a230c..2367b92de25c00e59e30cb44db384a8af12ae3bc 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -163,8 +163,8 @@ ENDIF(YAMLCPP_FOUND)
SET(HDRS_BASE
capture_base.h
constraint_analytic.h
+ constraint_autodiff.h
constraint_base.h
- constraint_sparse.h
factory.h
feature_base.h
feature_match.h
@@ -207,6 +207,7 @@ SET(HDRS
capture_void.h
constraint_container.h
constraint_corner_2D.h
+ constraint_AHP.h
constraint_epipolar.h
constraint_imu.h
constraint_fix.h
@@ -326,20 +327,18 @@ SET(SRCS_DTASSC
#optional HDRS and SRCS
IF (Ceres_FOUND)
SET(HDRS_WRAPPER
- ceres_wrapper/auto_diff_cost_function_wrapper.h
+ ceres_wrapper/sparse_utils.h
+ ceres_wrapper/solver_manager.h
ceres_wrapper/ceres_manager.h
+ #ceres_wrapper/qr_manager.h
ceres_wrapper/cost_function_wrapper.h
- ceres_wrapper/create_auto_diff_cost_function.h
- ceres_wrapper/create_auto_diff_cost_function_ceres.h
- ceres_wrapper/create_auto_diff_cost_function_wrapper.h
ceres_wrapper/create_numeric_diff_cost_function.h
- ceres_wrapper/create_numeric_diff_cost_function_ceres.h
ceres_wrapper/local_parametrization_wrapper.h
)
SET(SRCS_WRAPPER
+ ceres_wrapper/solver_manager.cpp
ceres_wrapper/ceres_manager.cpp
- ceres_wrapper/create_auto_diff_cost_function.cpp
- ceres_wrapper/create_numeric_diff_cost_function.cpp
+ #ceres_wrapper/qr_manager.cpp
ceres_wrapper/local_parametrization_wrapper.cpp
)
ELSE(Ceres_FOUND)
@@ -424,7 +423,7 @@ IF(YAMLCPP_FOUND)
ENDIF(laser_scan_utils_FOUND)
IF(OpenCV_FOUND)
SET(SRCS ${SRCS}
- yaml/processor_image_yaml.cpp
+ yaml/processor_image_yaml.cpp
)
ENDIF(OpenCV_FOUND)
ENDIF(YAMLCPP_FOUND)
diff --git a/src/ceres_wrapper/auto_diff_cost_function_wrapper.h b/src/ceres_wrapper/auto_diff_cost_function_wrapper.h
deleted file mode 100644
index ba5362ded5b1e7eb39bec9603dfa0db08e8c9fe9..0000000000000000000000000000000000000000
--- a/src/ceres_wrapper/auto_diff_cost_function_wrapper.h
+++ /dev/null
@@ -1,1176 +0,0 @@
-#ifndef TRUNK_SRC_AUTODIFF_COST_FUNCTION_WRAPPER_H_
-#define TRUNK_SRC_AUTODIFF_COST_FUNCTION_WRAPPER_H_
-
-// WOLF
-#include "../wolf.h"
-
-// CERES
-#include "ceres/jet.h"
-#include "ceres/sized_cost_function.h"
-
-// GENERAL
-#include <array>
-
-namespace wolf {
-
-template <class ConstraintType, 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>
-class AutoDiffCostFunctionWrapperBase : public ceres::SizedCostFunction<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>
-{
- typedef ceres::Jet<Scalar, 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:
- ConstraintType* constraint_ptr_;
- unsigned int n_blocks_;
- std::vector<unsigned int> block_sizes_, jacobian_locations_;
- std::array<WolfJet, BLOCK_0_SIZE>* jets_0_;
- std::array<WolfJet, BLOCK_1_SIZE>* jets_1_;
- std::array<WolfJet, BLOCK_2_SIZE>* jets_2_;
- std::array<WolfJet, BLOCK_3_SIZE>* jets_3_;
- std::array<WolfJet, BLOCK_4_SIZE>* jets_4_;
- std::array<WolfJet, BLOCK_5_SIZE>* jets_5_;
- std::array<WolfJet, BLOCK_6_SIZE>* jets_6_;
- std::array<WolfJet, BLOCK_7_SIZE>* jets_7_;
- std::array<WolfJet, BLOCK_8_SIZE>* jets_8_;
- std::array<WolfJet, BLOCK_9_SIZE>* jets_9_;
- std::array<WolfJet, MEASUREMENT_SIZE>* residuals_jets_;
-
- public:
-
- AutoDiffCostFunctionWrapperBase(ConstraintType* _constraint_ptr) :
- ceres::SizedCostFunction<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>(),
- constraint_ptr_(_constraint_ptr),
- n_blocks_(10),
- 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}),
- jacobian_locations_({0,
- BLOCK_0_SIZE,
- BLOCK_0_SIZE+BLOCK_1_SIZE,
- BLOCK_0_SIZE+BLOCK_1_SIZE+BLOCK_2_SIZE,
- BLOCK_0_SIZE+BLOCK_1_SIZE+BLOCK_2_SIZE+BLOCK_3_SIZE,
- BLOCK_0_SIZE+BLOCK_1_SIZE+BLOCK_2_SIZE+BLOCK_3_SIZE+BLOCK_4_SIZE,
- BLOCK_0_SIZE+BLOCK_1_SIZE+BLOCK_2_SIZE+BLOCK_3_SIZE+BLOCK_4_SIZE+BLOCK_5_SIZE,
- BLOCK_0_SIZE+BLOCK_1_SIZE+BLOCK_2_SIZE+BLOCK_3_SIZE+BLOCK_4_SIZE+BLOCK_5_SIZE+BLOCK_6_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_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}),
- jets_0_(new std::array<WolfJet, BLOCK_0_SIZE>),
- jets_1_(new std::array<WolfJet, BLOCK_1_SIZE>),
- jets_2_(new std::array<WolfJet, BLOCK_2_SIZE>),
- jets_3_(new std::array<WolfJet, BLOCK_3_SIZE>),
- jets_4_(new std::array<WolfJet, BLOCK_4_SIZE>),
- jets_5_(new std::array<WolfJet, BLOCK_5_SIZE>),
- jets_6_(new std::array<WolfJet, BLOCK_6_SIZE>),
- jets_7_(new std::array<WolfJet, BLOCK_7_SIZE>),
- jets_8_(new std::array<WolfJet, BLOCK_8_SIZE>),
- jets_9_(new std::array<WolfJet, BLOCK_9_SIZE>),
- residuals_jets_(new std::array<WolfJet, MEASUREMENT_SIZE>)
- {
- // initialize jets
- unsigned int i, last_jet_idx = 0;
- for (i = 0; i < BLOCK_0_SIZE; i++)
- (*jets_0_)[i] = WolfJet(0, last_jet_idx++);
- for (i = 0; i < BLOCK_1_SIZE; i++)
- (*jets_1_)[i] = WolfJet(0, last_jet_idx++);
- for (i = 0; i < BLOCK_2_SIZE; i++)
- (*jets_2_)[i] = WolfJet(0, last_jet_idx++);
- for (i = 0; i < BLOCK_3_SIZE; i++)
- (*jets_3_)[i] = WolfJet(0, last_jet_idx++);
- for (i = 0; i < BLOCK_4_SIZE; i++)
- (*jets_4_)[i] = WolfJet(0, last_jet_idx++);
- for (i = 0; i < BLOCK_5_SIZE; i++)
- (*jets_5_)[i] = WolfJet(0, last_jet_idx++);
- for (i = 0; i < BLOCK_6_SIZE; i++)
- (*jets_6_)[i] = WolfJet(0, last_jet_idx++);
- for (i = 0; i < BLOCK_7_SIZE; i++)
- (*jets_7_)[i] = WolfJet(0, last_jet_idx++);
- for (i = 0; i < BLOCK_8_SIZE; i++)
- (*jets_8_)[i] = WolfJet(0, last_jet_idx++);
- for (i = 0; i < BLOCK_9_SIZE; i++)
- (*jets_9_)[i] = WolfJet(0, last_jet_idx++);
-
- };
-
- virtual ~AutoDiffCostFunctionWrapperBase()
- {
- delete jets_0_;
- delete jets_1_;
- delete jets_2_;
- delete jets_3_;
- delete jets_4_;
- delete jets_5_;
- delete jets_6_;
- delete jets_7_;
- delete jets_8_;
- delete jets_9_;
- delete residuals_jets_;
- };
-
- virtual bool Evaluate(double const* const* parameters, double* residuals, double** jacobians) const
- {
- // only residuals
- if (jacobians == nullptr)
- {
- (*this->constraint_ptr_)(parameters[0], parameters[1], parameters[2], parameters[3], parameters[4],
- parameters[5], parameters[6], parameters[7], parameters[8], parameters[9], residuals);
- }
- // also compute jacobians
- else
- {
- // update jets real part
- unsigned int i;
- for (i = 0; i < BLOCK_0_SIZE; i++)
- (*jets_0_)[i].a = parameters[0][i];
- for (i = 0; i < BLOCK_1_SIZE; i++)
- (*jets_1_)[i].a = parameters[1][i];
- for (i = 0; i < BLOCK_2_SIZE; i++)
- (*jets_2_)[i].a = parameters[2][i];
- for (i = 0; i < BLOCK_3_SIZE; i++)
- (*jets_3_)[i].a = parameters[3][i];
- for (i = 0; i < BLOCK_4_SIZE; i++)
- (*jets_4_)[i].a = parameters[4][i];
- for (i = 0; i < BLOCK_5_SIZE; i++)
- (*jets_5_)[i].a = parameters[5][i];
- for (i = 0; i < BLOCK_6_SIZE; i++)
- (*jets_6_)[i].a = parameters[6][i];
- for (i = 0; i < BLOCK_7_SIZE; i++)
- (*jets_7_)[i].a = parameters[7][i];
- for (i = 0; i < BLOCK_8_SIZE; i++)
- (*jets_8_)[i].a = parameters[8][i];
- for (i = 0; i < BLOCK_9_SIZE; i++)
- (*jets_9_)[i].a = parameters[9][i];
-
- // call functor
- (*this->constraint_ptr_)(jets_0_->data(), jets_1_->data(), jets_2_->data(), jets_3_->data(), jets_4_->data(),
- jets_5_->data(), jets_6_->data(), jets_7_->data(), jets_8_->data(), jets_9_->data(),
- residuals_jets_->data());
-
- // fill the residual array
- for (i = 0; i < MEASUREMENT_SIZE; i++)
- residuals[i] = (*residuals_jets_)[i].a;
-
- // fill the jacobian matrices
- for (i = 0; i<this->n_blocks_; i++)
- if (jacobians[i] != nullptr)
- for (unsigned int row = 0; row < MEASUREMENT_SIZE; row++)
- std::copy((*residuals_jets_)[row].v.data() + jacobian_locations_.at(i),
- (*residuals_jets_)[row].v.data() + jacobian_locations_.at(i) + block_sizes_.at(i),
- jacobians[i] + row * block_sizes_.at(i));
- }
- return true;
- }
-};
-
-// SPECIALIZATION 9 BLOCKS
-template <class ConstraintType, 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>
-class AutoDiffCostFunctionWrapperBase<ConstraintType, 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>
- : public ceres::SizedCostFunction<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>
-{
- typedef ceres::Jet<Scalar, 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> WolfJet;
-
- protected:
- ConstraintType* constraint_ptr_;
- unsigned int n_blocks_;
- std::vector<unsigned int> block_sizes_, jacobian_locations_;
- std::array<WolfJet, BLOCK_0_SIZE>* jets_0_;
- std::array<WolfJet, BLOCK_1_SIZE>* jets_1_;
- std::array<WolfJet, BLOCK_2_SIZE>* jets_2_;
- std::array<WolfJet, BLOCK_3_SIZE>* jets_3_;
- std::array<WolfJet, BLOCK_4_SIZE>* jets_4_;
- std::array<WolfJet, BLOCK_5_SIZE>* jets_5_;
- std::array<WolfJet, BLOCK_6_SIZE>* jets_6_;
- std::array<WolfJet, BLOCK_7_SIZE>* jets_7_;
- std::array<WolfJet, BLOCK_8_SIZE>* jets_8_;
- std::array<WolfJet, MEASUREMENT_SIZE>* residuals_jets_;
-
- public:
-
- AutoDiffCostFunctionWrapperBase(ConstraintType* _constraint_ptr) :
- ceres::SizedCostFunction<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>(),
- constraint_ptr_(_constraint_ptr),
- n_blocks_(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}),
- jacobian_locations_({0,
- BLOCK_0_SIZE,
- BLOCK_0_SIZE+BLOCK_1_SIZE,
- BLOCK_0_SIZE+BLOCK_1_SIZE+BLOCK_2_SIZE,
- BLOCK_0_SIZE+BLOCK_1_SIZE+BLOCK_2_SIZE+BLOCK_3_SIZE,
- BLOCK_0_SIZE+BLOCK_1_SIZE+BLOCK_2_SIZE+BLOCK_3_SIZE+BLOCK_4_SIZE,
- BLOCK_0_SIZE+BLOCK_1_SIZE+BLOCK_2_SIZE+BLOCK_3_SIZE+BLOCK_4_SIZE+BLOCK_5_SIZE,
- BLOCK_0_SIZE+BLOCK_1_SIZE+BLOCK_2_SIZE+BLOCK_3_SIZE+BLOCK_4_SIZE+BLOCK_5_SIZE+BLOCK_6_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}),
- jets_0_(new std::array<WolfJet, BLOCK_0_SIZE>),
- jets_1_(new std::array<WolfJet, BLOCK_1_SIZE>),
- jets_2_(new std::array<WolfJet, BLOCK_2_SIZE>),
- jets_3_(new std::array<WolfJet, BLOCK_3_SIZE>),
- jets_4_(new std::array<WolfJet, BLOCK_4_SIZE>),
- jets_5_(new std::array<WolfJet, BLOCK_5_SIZE>),
- jets_6_(new std::array<WolfJet, BLOCK_6_SIZE>),
- jets_7_(new std::array<WolfJet, BLOCK_7_SIZE>),
- jets_8_(new std::array<WolfJet, BLOCK_8_SIZE>),
- residuals_jets_(new std::array<WolfJet, MEASUREMENT_SIZE>)
- {
- // initialize jets
- unsigned int i, last_jet_idx = 0;
- for (i = 0; i < BLOCK_0_SIZE; i++)
- (*jets_0_)[i] = WolfJet(0, last_jet_idx++);
- for (i = 0; i < BLOCK_1_SIZE; i++)
- (*jets_1_)[i] = WolfJet(0, last_jet_idx++);
- for (i = 0; i < BLOCK_2_SIZE; i++)
- (*jets_2_)[i] = WolfJet(0, last_jet_idx++);
- for (i = 0; i < BLOCK_3_SIZE; i++)
- (*jets_3_)[i] = WolfJet(0, last_jet_idx++);
- for (i = 0; i < BLOCK_4_SIZE; i++)
- (*jets_4_)[i] = WolfJet(0, last_jet_idx++);
- for (i = 0; i < BLOCK_5_SIZE; i++)
- (*jets_5_)[i] = WolfJet(0, last_jet_idx++);
- for (i = 0; i < BLOCK_6_SIZE; i++)
- (*jets_6_)[i] = WolfJet(0, last_jet_idx++);
- for (i = 0; i < BLOCK_7_SIZE; i++)
- (*jets_7_)[i] = WolfJet(0, last_jet_idx++);
- for (i = 0; i < BLOCK_8_SIZE; i++)
- (*jets_8_)[i] = WolfJet(0, last_jet_idx++);
- };
-
- virtual ~AutoDiffCostFunctionWrapperBase()
- {
- delete jets_0_;
- delete jets_1_;
- delete jets_2_;
- delete jets_3_;
- delete jets_4_;
- delete jets_5_;
- delete jets_6_;
- delete jets_7_;
- delete jets_8_;
- delete residuals_jets_;
- };
-
- virtual bool Evaluate(double const* const* parameters, double* residuals, double** jacobians) const
- {
- // only residuals
- if (jacobians == nullptr)
- (*this->constraint_ptr_)(parameters[0], parameters[1], parameters[2], parameters[3], parameters[4],
- parameters[5], parameters[6], parameters[7], parameters[8], residuals);
-
- // also compute jacobians
- else
- {
- // update jets real part
- unsigned int i;
- for (i = 0; i < BLOCK_0_SIZE; i++)
- (*jets_0_)[i].a = parameters[0][i];
- for (i = 0; i < BLOCK_1_SIZE; i++)
- (*jets_1_)[i].a = parameters[1][i];
- for (i = 0; i < BLOCK_2_SIZE; i++)
- (*jets_2_)[i].a = parameters[2][i];
- for (i = 0; i < BLOCK_3_SIZE; i++)
- (*jets_3_)[i].a = parameters[3][i];
- for (i = 0; i < BLOCK_4_SIZE; i++)
- (*jets_4_)[i].a = parameters[4][i];
- for (i = 0; i < BLOCK_5_SIZE; i++)
- (*jets_5_)[i].a = parameters[5][i];
- for (i = 0; i < BLOCK_6_SIZE; i++)
- (*jets_6_)[i].a = parameters[6][i];
- for (i = 0; i < BLOCK_7_SIZE; i++)
- (*jets_7_)[i].a = parameters[7][i];
- for (i = 0; i < BLOCK_8_SIZE; i++)
- (*jets_8_)[i].a = parameters[8][i];
-
- // call functor
- (*this->constraint_ptr_)(jets_0_->data(), jets_1_->data(), jets_2_->data(), jets_3_->data(), jets_4_->data(),
- jets_5_->data(), jets_6_->data(), jets_7_->data(), jets_8_->data(),
- residuals_jets_->data());
-
- // fill the residual array
- for (i = 0; i < MEASUREMENT_SIZE; i++)
- residuals[i] = (*residuals_jets_)[i].a;
-
- // fill the jacobian matrices
- for (i = 0; i<this->n_blocks_; i++)
- if (jacobians[i] != nullptr)
- for (unsigned int row = 0; row < MEASUREMENT_SIZE; row++)
- std::copy((*residuals_jets_)[row].v.data() + jacobian_locations_.at(i),
- (*residuals_jets_)[row].v.data() + jacobian_locations_.at(i) + block_sizes_.at(i),
- jacobians[i] + row * block_sizes_.at(i));
- }
- return true;
- }
-};
-
-// SPECIALIZATION 8 BLOCKS
-template <class ConstraintType, 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>
-class AutoDiffCostFunctionWrapperBase<ConstraintType, 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 ceres::SizedCostFunction<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>
-{
- typedef ceres::Jet<Scalar, BLOCK_0_SIZE + BLOCK_1_SIZE + BLOCK_2_SIZE + BLOCK_3_SIZE + BLOCK_4_SIZE +
- BLOCK_5_SIZE + BLOCK_6_SIZE + BLOCK_7_SIZE> WolfJet;
-
- protected:
- ConstraintType* constraint_ptr_;
- unsigned int n_blocks_;
- std::vector<unsigned int> block_sizes_, jacobian_locations_;
- std::array<WolfJet, BLOCK_0_SIZE>* jets_0_;
- std::array<WolfJet, BLOCK_1_SIZE>* jets_1_;
- std::array<WolfJet, BLOCK_2_SIZE>* jets_2_;
- std::array<WolfJet, BLOCK_3_SIZE>* jets_3_;
- std::array<WolfJet, BLOCK_4_SIZE>* jets_4_;
- std::array<WolfJet, BLOCK_5_SIZE>* jets_5_;
- std::array<WolfJet, BLOCK_6_SIZE>* jets_6_;
- std::array<WolfJet, BLOCK_7_SIZE>* jets_7_;
- std::array<WolfJet, MEASUREMENT_SIZE>* residuals_jets_;
-
- public:
-
- AutoDiffCostFunctionWrapperBase(ConstraintType* _constraint_ptr) :
- ceres::SizedCostFunction<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>(),
- constraint_ptr_(_constraint_ptr),
- n_blocks_(8),
- 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}),
- jacobian_locations_({0,
- BLOCK_0_SIZE,
- BLOCK_0_SIZE+BLOCK_1_SIZE,
- BLOCK_0_SIZE+BLOCK_1_SIZE+BLOCK_2_SIZE,
- BLOCK_0_SIZE+BLOCK_1_SIZE+BLOCK_2_SIZE+BLOCK_3_SIZE,
- BLOCK_0_SIZE+BLOCK_1_SIZE+BLOCK_2_SIZE+BLOCK_3_SIZE+BLOCK_4_SIZE,
- BLOCK_0_SIZE+BLOCK_1_SIZE+BLOCK_2_SIZE+BLOCK_3_SIZE+BLOCK_4_SIZE+BLOCK_5_SIZE,
- BLOCK_0_SIZE+BLOCK_1_SIZE+BLOCK_2_SIZE+BLOCK_3_SIZE+BLOCK_4_SIZE+BLOCK_5_SIZE+BLOCK_6_SIZE}),
- jets_0_(new std::array<WolfJet, BLOCK_0_SIZE>),
- jets_1_(new std::array<WolfJet, BLOCK_1_SIZE>),
- jets_2_(new std::array<WolfJet, BLOCK_2_SIZE>),
- jets_3_(new std::array<WolfJet, BLOCK_3_SIZE>),
- jets_4_(new std::array<WolfJet, BLOCK_4_SIZE>),
- jets_5_(new std::array<WolfJet, BLOCK_5_SIZE>),
- jets_6_(new std::array<WolfJet, BLOCK_6_SIZE>),
- jets_7_(new std::array<WolfJet, BLOCK_7_SIZE>),
- residuals_jets_(new std::array<WolfJet, MEASUREMENT_SIZE>)
- {
- // initialize jets
- unsigned int i, last_jet_idx = 0;
- for (i = 0; i < BLOCK_0_SIZE; i++)
- (*jets_0_)[i] = WolfJet(0, last_jet_idx++);
- for (i = 0; i < BLOCK_1_SIZE; i++)
- (*jets_1_)[i] = WolfJet(0, last_jet_idx++);
- for (i = 0; i < BLOCK_2_SIZE; i++)
- (*jets_2_)[i] = WolfJet(0, last_jet_idx++);
- for (i = 0; i < BLOCK_3_SIZE; i++)
- (*jets_3_)[i] = WolfJet(0, last_jet_idx++);
- for (i = 0; i < BLOCK_4_SIZE; i++)
- (*jets_4_)[i] = WolfJet(0, last_jet_idx++);
- for (i = 0; i < BLOCK_5_SIZE; i++)
- (*jets_5_)[i] = WolfJet(0, last_jet_idx++);
- for (i = 0; i < BLOCK_6_SIZE; i++)
- (*jets_6_)[i] = WolfJet(0, last_jet_idx++);
- for (i = 0; i < BLOCK_7_SIZE; i++)
- (*jets_7_)[i] = WolfJet(0, last_jet_idx++);
- };
-
- virtual ~AutoDiffCostFunctionWrapperBase()
- {
- delete jets_0_;
- delete jets_1_;
- delete jets_2_;
- delete jets_3_;
- delete jets_4_;
- delete jets_5_;
- delete jets_6_;
- delete jets_7_;
- delete residuals_jets_;
- };
-
- virtual bool Evaluate(double const* const* parameters, double* residuals, double** jacobians) const
- {
- // only residuals
- if (jacobians == nullptr)
- (*this->constraint_ptr_)(parameters[0], parameters[1], parameters[2], parameters[3], parameters[4],
- parameters[5], parameters[6], parameters[7], residuals);
-
- // also compute jacobians
- else
- {
- // update jets real part
- unsigned int i;
- for (i = 0; i < BLOCK_0_SIZE; i++)
- (*jets_0_)[i].a = parameters[0][i];
- for (i = 0; i < BLOCK_1_SIZE; i++)
- (*jets_1_)[i].a = parameters[1][i];
- for (i = 0; i < BLOCK_2_SIZE; i++)
- (*jets_2_)[i].a = parameters[2][i];
- for (i = 0; i < BLOCK_3_SIZE; i++)
- (*jets_3_)[i].a = parameters[3][i];
- for (i = 0; i < BLOCK_4_SIZE; i++)
- (*jets_4_)[i].a = parameters[4][i];
- for (i = 0; i < BLOCK_5_SIZE; i++)
- (*jets_5_)[i].a = parameters[5][i];
- for (i = 0; i < BLOCK_6_SIZE; i++)
- (*jets_6_)[i].a = parameters[6][i];
- for (i = 0; i < BLOCK_7_SIZE; i++)
- (*jets_7_)[i].a = parameters[7][i];
-
- // call functor
- (*this->constraint_ptr_)(jets_0_->data(), jets_1_->data(), jets_2_->data(), jets_3_->data(), jets_4_->data(),
- jets_5_->data(), jets_6_->data(), jets_7_->data(),
- residuals_jets_->data());
-
- // fill the residual array
- for (i = 0; i < MEASUREMENT_SIZE; i++)
- residuals[i] = (*residuals_jets_)[i].a;
-
- // fill the jacobian matrices
- for (i = 0; i<n_blocks_; i++)
- if (jacobians[i] != nullptr)
- for (unsigned int row = 0; row < MEASUREMENT_SIZE; row++)
- std::copy((*residuals_jets_)[row].v.data() + jacobian_locations_.at(i),
- (*residuals_jets_)[row].v.data() + jacobian_locations_.at(i) + block_sizes_.at(i),
- jacobians[i] + row * block_sizes_.at(i));
- }
- return true;
- }
-};
-
-// SPECIALIZATION 7 BLOCKS
-template <class ConstraintType, 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>
-class AutoDiffCostFunctionWrapperBase<ConstraintType, 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, 0>
- : public ceres::SizedCostFunction<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,0>
-{
- typedef ceres::Jet<Scalar, BLOCK_0_SIZE + BLOCK_1_SIZE + BLOCK_2_SIZE + BLOCK_3_SIZE + BLOCK_4_SIZE +
- BLOCK_5_SIZE + BLOCK_6_SIZE> WolfJet;
-
- protected:
- ConstraintType* constraint_ptr_;
- unsigned int n_blocks_;
- std::vector<unsigned int> block_sizes_, jacobian_locations_;
- std::array<WolfJet, BLOCK_0_SIZE>* jets_0_;
- std::array<WolfJet, BLOCK_1_SIZE>* jets_1_;
- std::array<WolfJet, BLOCK_2_SIZE>* jets_2_;
- std::array<WolfJet, BLOCK_3_SIZE>* jets_3_;
- std::array<WolfJet, BLOCK_4_SIZE>* jets_4_;
- std::array<WolfJet, BLOCK_5_SIZE>* jets_5_;
- std::array<WolfJet, BLOCK_6_SIZE>* jets_6_;
- std::array<WolfJet, MEASUREMENT_SIZE>* residuals_jets_;
-
- public:
-
- AutoDiffCostFunctionWrapperBase(ConstraintType* _constraint_ptr) :
- ceres::SizedCostFunction<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,0>(),
- constraint_ptr_(_constraint_ptr),
- n_blocks_(7),
- block_sizes_({BLOCK_0_SIZE, BLOCK_1_SIZE, BLOCK_2_SIZE, BLOCK_3_SIZE, BLOCK_4_SIZE,
- BLOCK_5_SIZE, BLOCK_6_SIZE}),
- jacobian_locations_({0,
- BLOCK_0_SIZE,
- BLOCK_0_SIZE+BLOCK_1_SIZE,
- BLOCK_0_SIZE+BLOCK_1_SIZE+BLOCK_2_SIZE,
- BLOCK_0_SIZE+BLOCK_1_SIZE+BLOCK_2_SIZE+BLOCK_3_SIZE,
- BLOCK_0_SIZE+BLOCK_1_SIZE+BLOCK_2_SIZE+BLOCK_3_SIZE+BLOCK_4_SIZE,
- BLOCK_0_SIZE+BLOCK_1_SIZE+BLOCK_2_SIZE+BLOCK_3_SIZE+BLOCK_4_SIZE+BLOCK_5_SIZE}),
- jets_0_(new std::array<WolfJet, BLOCK_0_SIZE>),
- jets_1_(new std::array<WolfJet, BLOCK_1_SIZE>),
- jets_2_(new std::array<WolfJet, BLOCK_2_SIZE>),
- jets_3_(new std::array<WolfJet, BLOCK_3_SIZE>),
- jets_4_(new std::array<WolfJet, BLOCK_4_SIZE>),
- jets_5_(new std::array<WolfJet, BLOCK_5_SIZE>),
- jets_6_(new std::array<WolfJet, BLOCK_6_SIZE>),
- residuals_jets_(new std::array<WolfJet, MEASUREMENT_SIZE>)
- {
- // initialize jets
- unsigned int i, last_jet_idx = 0;
- for (i = 0; i < BLOCK_0_SIZE; i++)
- (*jets_0_)[i] = WolfJet(0, last_jet_idx++);
- for (i = 0; i < BLOCK_1_SIZE; i++)
- (*jets_1_)[i] = WolfJet(0, last_jet_idx++);
- for (i = 0; i < BLOCK_2_SIZE; i++)
- (*jets_2_)[i] = WolfJet(0, last_jet_idx++);
- for (i = 0; i < BLOCK_3_SIZE; i++)
- (*jets_3_)[i] = WolfJet(0, last_jet_idx++);
- for (i = 0; i < BLOCK_4_SIZE; i++)
- (*jets_4_)[i] = WolfJet(0, last_jet_idx++);
- for (i = 0; i < BLOCK_5_SIZE; i++)
- (*jets_5_)[i] = WolfJet(0, last_jet_idx++);
- for (i = 0; i < BLOCK_6_SIZE; i++)
- (*jets_6_)[i] = WolfJet(0, last_jet_idx++);
- };
-
- virtual ~AutoDiffCostFunctionWrapperBase()
- {
- delete jets_0_;
- delete jets_1_;
- delete jets_2_;
- delete jets_3_;
- delete jets_4_;
- delete jets_5_;
- delete jets_6_;
- delete residuals_jets_;
- };
-
- virtual bool Evaluate(double const* const* parameters, double* residuals, double** jacobians) const
- {
- // only residuals
- if (jacobians == nullptr)
- (*this->constraint_ptr_)(parameters[0], parameters[1], parameters[2], parameters[3], parameters[4],
- parameters[5], parameters[6], residuals);
-
- // also compute jacobians
- else
- {
- // update jets real part
- unsigned int i;
- for (i = 0; i < BLOCK_0_SIZE; i++)
- (*jets_0_)[i].a = parameters[0][i];
- for (i = 0; i < BLOCK_1_SIZE; i++)
- (*jets_1_)[i].a = parameters[1][i];
- for (i = 0; i < BLOCK_2_SIZE; i++)
- (*jets_2_)[i].a = parameters[2][i];
- for (i = 0; i < BLOCK_3_SIZE; i++)
- (*jets_3_)[i].a = parameters[3][i];
- for (i = 0; i < BLOCK_4_SIZE; i++)
- (*jets_4_)[i].a = parameters[4][i];
- for (i = 0; i < BLOCK_5_SIZE; i++)
- (*jets_5_)[i].a = parameters[5][i];
- for (i = 0; i < BLOCK_6_SIZE; i++)
- (*jets_6_)[i].a = parameters[6][i];
-
- // call functor
- (*this->constraint_ptr_)(jets_0_->data(), jets_1_->data(), jets_2_->data(), jets_3_->data(), jets_4_->data(),
- jets_5_->data(), jets_6_->data(),
- residuals_jets_->data());
-
- // fill the residual array
- for (i = 0; i < MEASUREMENT_SIZE; i++)
- residuals[i] = (*residuals_jets_)[i].a;
-
- // fill the jacobian matrices
- for (i = 0; i<n_blocks_; i++)
- if (jacobians[i] != nullptr)
- for (unsigned int row = 0; row < MEASUREMENT_SIZE; row++)
- std::copy((*residuals_jets_)[row].v.data() + jacobian_locations_.at(i),
- (*residuals_jets_)[row].v.data() + jacobian_locations_.at(i) + block_sizes_.at(i),
- jacobians[i] + row * block_sizes_.at(i));
- }
- return true;
- }
-};
-
-// SPECIALIZATION 6 BLOCKS
-template <class ConstraintType, 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>
-class AutoDiffCostFunctionWrapperBase<ConstraintType, MEASUREMENT_SIZE,
- BLOCK_0_SIZE, BLOCK_1_SIZE, BLOCK_2_SIZE, BLOCK_3_SIZE, BLOCK_4_SIZE,
- BLOCK_5_SIZE, 0, 0, 0, 0>
- : public ceres::SizedCostFunction<MEASUREMENT_SIZE,
- BLOCK_0_SIZE,BLOCK_1_SIZE,BLOCK_2_SIZE,BLOCK_3_SIZE,BLOCK_4_SIZE,
- BLOCK_5_SIZE,0,0,0,0>
-{
- typedef ceres::Jet<Scalar, BLOCK_0_SIZE + BLOCK_1_SIZE + BLOCK_2_SIZE + BLOCK_3_SIZE + BLOCK_4_SIZE +
- BLOCK_5_SIZE> WolfJet;
-
- protected:
- ConstraintType* constraint_ptr_;
- unsigned int n_blocks_;
- std::vector<unsigned int> block_sizes_, jacobian_locations_;
- std::array<WolfJet, BLOCK_0_SIZE>* jets_0_;
- std::array<WolfJet, BLOCK_1_SIZE>* jets_1_;
- std::array<WolfJet, BLOCK_2_SIZE>* jets_2_;
- std::array<WolfJet, BLOCK_3_SIZE>* jets_3_;
- std::array<WolfJet, BLOCK_4_SIZE>* jets_4_;
- std::array<WolfJet, BLOCK_5_SIZE>* jets_5_;
- std::array<WolfJet, MEASUREMENT_SIZE>* residuals_jets_;
-
- public:
-
- AutoDiffCostFunctionWrapperBase(ConstraintType* _constraint_ptr) :
- ceres::SizedCostFunction<MEASUREMENT_SIZE,
- BLOCK_0_SIZE,BLOCK_1_SIZE,BLOCK_2_SIZE,BLOCK_3_SIZE,BLOCK_4_SIZE,
- BLOCK_5_SIZE,0,0,0,0>(),
- constraint_ptr_(_constraint_ptr),
- n_blocks_(6),
- block_sizes_({BLOCK_0_SIZE, BLOCK_1_SIZE, BLOCK_2_SIZE, BLOCK_3_SIZE, BLOCK_4_SIZE,
- BLOCK_5_SIZE}),
- jacobian_locations_({0,
- BLOCK_0_SIZE,
- BLOCK_0_SIZE+BLOCK_1_SIZE,
- BLOCK_0_SIZE+BLOCK_1_SIZE+BLOCK_2_SIZE,
- BLOCK_0_SIZE+BLOCK_1_SIZE+BLOCK_2_SIZE+BLOCK_3_SIZE,
- BLOCK_0_SIZE+BLOCK_1_SIZE+BLOCK_2_SIZE+BLOCK_3_SIZE+BLOCK_4_SIZE}),
- jets_0_(new std::array<WolfJet, BLOCK_0_SIZE>),
- jets_1_(new std::array<WolfJet, BLOCK_1_SIZE>),
- jets_2_(new std::array<WolfJet, BLOCK_2_SIZE>),
- jets_3_(new std::array<WolfJet, BLOCK_3_SIZE>),
- jets_4_(new std::array<WolfJet, BLOCK_4_SIZE>),
- jets_5_(new std::array<WolfJet, BLOCK_5_SIZE>),
- residuals_jets_(new std::array<WolfJet, MEASUREMENT_SIZE>)
- {
- // initialize jets
- unsigned int i, last_jet_idx = 0;
- for (i = 0; i < BLOCK_0_SIZE; i++)
- (*jets_0_)[i] = WolfJet(0, last_jet_idx++);
- for (i = 0; i < BLOCK_1_SIZE; i++)
- (*jets_1_)[i] = WolfJet(0, last_jet_idx++);
- for (i = 0; i < BLOCK_2_SIZE; i++)
- (*jets_2_)[i] = WolfJet(0, last_jet_idx++);
- for (i = 0; i < BLOCK_3_SIZE; i++)
- (*jets_3_)[i] = WolfJet(0, last_jet_idx++);
- for (i = 0; i < BLOCK_4_SIZE; i++)
- (*jets_4_)[i] = WolfJet(0, last_jet_idx++);
- for (i = 0; i < BLOCK_5_SIZE; i++)
- (*jets_5_)[i] = WolfJet(0, last_jet_idx++);
- };
-
- virtual ~AutoDiffCostFunctionWrapperBase()
- {
- delete jets_0_;
- delete jets_1_;
- delete jets_2_;
- delete jets_3_;
- delete jets_4_;
- delete jets_5_;
- delete residuals_jets_;
- };
-
- virtual bool Evaluate(double const* const* parameters, double* residuals, double** jacobians) const
- {
- // only residuals
- if (jacobians == nullptr)
- (*this->constraint_ptr_)(parameters[0], parameters[1], parameters[2], parameters[3], parameters[4],
- parameters[5], residuals);
-
- // also compute jacobians
- else
- {
- // update jets real part
- unsigned int i;
- for (i = 0; i < BLOCK_0_SIZE; i++)
- (*jets_0_)[i].a = parameters[0][i];
- for (i = 0; i < BLOCK_1_SIZE; i++)
- (*jets_1_)[i].a = parameters[1][i];
- for (i = 0; i < BLOCK_2_SIZE; i++)
- (*jets_2_)[i].a = parameters[2][i];
- for (i = 0; i < BLOCK_3_SIZE; i++)
- (*jets_3_)[i].a = parameters[3][i];
- for (i = 0; i < BLOCK_4_SIZE; i++)
- (*jets_4_)[i].a = parameters[4][i];
- for (i = 0; i < BLOCK_5_SIZE; i++)
- (*jets_5_)[i].a = parameters[5][i];
-
- // call functor
- (*this->constraint_ptr_)(jets_0_->data(), jets_1_->data(), jets_2_->data(), jets_3_->data(), jets_4_->data(),
- jets_5_->data(), residuals_jets_->data());
-
- // fill the residual array
- for (i = 0; i < MEASUREMENT_SIZE; i++)
- residuals[i] = (*residuals_jets_)[i].a;
-
- // fill the jacobian matrices
- for (i = 0; i<n_blocks_; i++)
- if (jacobians[i] != nullptr)
- for (unsigned int row = 0; row < MEASUREMENT_SIZE; row++)
- std::copy((*residuals_jets_)[row].v.data() + jacobian_locations_.at(i),
- (*residuals_jets_)[row].v.data() + jacobian_locations_.at(i) + block_sizes_.at(i),
- jacobians[i] + row * block_sizes_.at(i));
- }
- return true;
- }
-};
-
-// SPECIALIZATION 5 BLOCKS
-template <class ConstraintType, 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>
-class AutoDiffCostFunctionWrapperBase<ConstraintType, MEASUREMENT_SIZE,
- BLOCK_0_SIZE, BLOCK_1_SIZE, BLOCK_2_SIZE, BLOCK_3_SIZE, BLOCK_4_SIZE,
- 0, 0, 0, 0, 0>
- : public ceres::SizedCostFunction<MEASUREMENT_SIZE,
- BLOCK_0_SIZE,BLOCK_1_SIZE,BLOCK_2_SIZE,BLOCK_3_SIZE,BLOCK_4_SIZE,
- 0,0,0,0,0>
-{
- typedef ceres::Jet<Scalar, BLOCK_0_SIZE + BLOCK_1_SIZE + BLOCK_2_SIZE + BLOCK_3_SIZE + BLOCK_4_SIZE> WolfJet;
-
- protected:
- ConstraintType* constraint_ptr_;
- unsigned int n_blocks_;
- std::vector<unsigned int> block_sizes_, jacobian_locations_;
- std::array<WolfJet, BLOCK_0_SIZE>* jets_0_;
- std::array<WolfJet, BLOCK_1_SIZE>* jets_1_;
- std::array<WolfJet, BLOCK_2_SIZE>* jets_2_;
- std::array<WolfJet, BLOCK_3_SIZE>* jets_3_;
- std::array<WolfJet, BLOCK_4_SIZE>* jets_4_;
- std::array<WolfJet, MEASUREMENT_SIZE>* residuals_jets_;
-
- public:
-
- AutoDiffCostFunctionWrapperBase(ConstraintType* _constraint_ptr) :
- ceres::SizedCostFunction<MEASUREMENT_SIZE,
- BLOCK_0_SIZE,BLOCK_1_SIZE,BLOCK_2_SIZE,BLOCK_3_SIZE,BLOCK_4_SIZE,
- 0,0,0,0,0>(),
- constraint_ptr_(_constraint_ptr),
- n_blocks_(5),
- block_sizes_({BLOCK_0_SIZE, BLOCK_1_SIZE, BLOCK_2_SIZE, BLOCK_3_SIZE, BLOCK_4_SIZE}),
- jacobian_locations_({0,
- BLOCK_0_SIZE,
- BLOCK_0_SIZE+BLOCK_1_SIZE,
- BLOCK_0_SIZE+BLOCK_1_SIZE+BLOCK_2_SIZE,
- BLOCK_0_SIZE+BLOCK_1_SIZE+BLOCK_2_SIZE+BLOCK_3_SIZE}),
- jets_0_(new std::array<WolfJet, BLOCK_0_SIZE>),
- jets_1_(new std::array<WolfJet, BLOCK_1_SIZE>),
- jets_2_(new std::array<WolfJet, BLOCK_2_SIZE>),
- jets_3_(new std::array<WolfJet, BLOCK_3_SIZE>),
- jets_4_(new std::array<WolfJet, BLOCK_4_SIZE>),
- residuals_jets_(new std::array<WolfJet, MEASUREMENT_SIZE>)
- {
- // initialize jets
- unsigned int i, last_jet_idx = 0;
- for (i = 0; i < BLOCK_0_SIZE; i++)
- (*jets_0_)[i] = WolfJet(0, last_jet_idx++);
- for (i = 0; i < BLOCK_1_SIZE; i++)
- (*jets_1_)[i] = WolfJet(0, last_jet_idx++);
- for (i = 0; i < BLOCK_2_SIZE; i++)
- (*jets_2_)[i] = WolfJet(0, last_jet_idx++);
- for (i = 0; i < BLOCK_3_SIZE; i++)
- (*jets_3_)[i] = WolfJet(0, last_jet_idx++);
- for (i = 0; i < BLOCK_4_SIZE; i++)
- (*jets_4_)[i] = WolfJet(0, last_jet_idx++);
- };
-
- virtual ~AutoDiffCostFunctionWrapperBase()
- {
- delete jets_0_;
- delete jets_1_;
- delete jets_2_;
- delete jets_3_;
- delete jets_4_;
- delete residuals_jets_;
- };
-
- virtual bool Evaluate(double const* const* parameters, double* residuals, double** jacobians) const
- {
- // only residuals
- if (jacobians == nullptr)
- (*this->constraint_ptr_)(parameters[0], parameters[1], parameters[2], parameters[3], parameters[4],
- residuals);
-
- // also compute jacobians
- else
- {
- // update jets real part
- unsigned int i;
- for (i = 0; i < BLOCK_0_SIZE; i++)
- (*jets_0_)[i].a = parameters[0][i];
- for (i = 0; i < BLOCK_1_SIZE; i++)
- (*jets_1_)[i].a = parameters[1][i];
- for (i = 0; i < BLOCK_2_SIZE; i++)
- (*jets_2_)[i].a = parameters[2][i];
- for (i = 0; i < BLOCK_3_SIZE; i++)
- (*jets_3_)[i].a = parameters[3][i];
- for (i = 0; i < BLOCK_4_SIZE; i++)
- (*jets_4_)[i].a = parameters[4][i];
-
- // call functor
- (*this->constraint_ptr_)(jets_0_->data(), jets_1_->data(), jets_2_->data(), jets_3_->data(), jets_4_->data(),
- residuals_jets_->data());
-
- // fill the residual array
- for (i = 0; i < MEASUREMENT_SIZE; i++)
- residuals[i] = (*residuals_jets_)[i].a;
-
- // fill the jacobian matrices
- for (i = 0; i<n_blocks_; i++)
- if (jacobians[i] != nullptr)
- for (unsigned int row = 0; row < MEASUREMENT_SIZE; row++)
- std::copy((*residuals_jets_)[row].v.data() + jacobian_locations_.at(i),
- (*residuals_jets_)[row].v.data() + jacobian_locations_.at(i) + block_sizes_.at(i),
- jacobians[i] + row * block_sizes_.at(i));
- }
- return true;
- }
-};
-
-// SPECIALIZATION 4 BLOCKS
-template <class ConstraintType, 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 AutoDiffCostFunctionWrapperBase<ConstraintType, MEASUREMENT_SIZE,
- BLOCK_0_SIZE, BLOCK_1_SIZE, BLOCK_2_SIZE, BLOCK_3_SIZE, 0,
- 0, 0, 0, 0, 0>
- : public ceres::SizedCostFunction<MEASUREMENT_SIZE,
- BLOCK_0_SIZE,BLOCK_1_SIZE,BLOCK_2_SIZE,BLOCK_3_SIZE,0,
- 0,0,0,0,0>
-{
- typedef ceres::Jet<Scalar, BLOCK_0_SIZE + BLOCK_1_SIZE + BLOCK_2_SIZE + BLOCK_3_SIZE> WolfJet;
-
- protected:
- ConstraintType* constraint_ptr_;
- unsigned int n_blocks_;
- std::vector<unsigned int> block_sizes_, jacobian_locations_;
- std::array<WolfJet, BLOCK_0_SIZE>* jets_0_;
- std::array<WolfJet, BLOCK_1_SIZE>* jets_1_;
- std::array<WolfJet, BLOCK_2_SIZE>* jets_2_;
- std::array<WolfJet, BLOCK_3_SIZE>* jets_3_;
- std::array<WolfJet, MEASUREMENT_SIZE>* residuals_jets_;
-
- public:
-
- AutoDiffCostFunctionWrapperBase(ConstraintType* _constraint_ptr) :
- ceres::SizedCostFunction<MEASUREMENT_SIZE,
- BLOCK_0_SIZE,BLOCK_1_SIZE,BLOCK_2_SIZE,BLOCK_3_SIZE,0,
- 0,0,0,0,0>(),
- constraint_ptr_(_constraint_ptr),
- n_blocks_(4),
- block_sizes_({BLOCK_0_SIZE, BLOCK_1_SIZE, BLOCK_2_SIZE, BLOCK_3_SIZE}),
- jacobian_locations_({0,
- BLOCK_0_SIZE,
- BLOCK_0_SIZE+BLOCK_1_SIZE,
- BLOCK_0_SIZE+BLOCK_1_SIZE+BLOCK_2_SIZE}),
- jets_0_(new std::array<WolfJet, BLOCK_0_SIZE>),
- jets_1_(new std::array<WolfJet, BLOCK_1_SIZE>),
- jets_2_(new std::array<WolfJet, BLOCK_2_SIZE>),
- jets_3_(new std::array<WolfJet, BLOCK_3_SIZE>),
- residuals_jets_(new std::array<WolfJet, MEASUREMENT_SIZE>)
- {
- // initialize jets
- unsigned int i, last_jet_idx = 0;
- for (i = 0; i < BLOCK_0_SIZE; i++)
- (*jets_0_)[i] = WolfJet(0, last_jet_idx++);
- for (i = 0; i < BLOCK_1_SIZE; i++)
- (*jets_1_)[i] = WolfJet(0, last_jet_idx++);
- for (i = 0; i < BLOCK_2_SIZE; i++)
- (*jets_2_)[i] = WolfJet(0, last_jet_idx++);
- for (i = 0; i < BLOCK_3_SIZE; i++)
- (*jets_3_)[i] = WolfJet(0, last_jet_idx++);
- };
-
- virtual ~AutoDiffCostFunctionWrapperBase()
- {
- delete jets_0_;
- delete jets_1_;
- delete jets_2_;
- delete jets_3_;
- delete residuals_jets_;
- };
-
- virtual bool Evaluate(double const* const* parameters, double* residuals, double** jacobians) const
- {
- // only residuals
- if (jacobians == nullptr)
- (*this->constraint_ptr_)(parameters[0], parameters[1], parameters[2], parameters[3], residuals);
- // also compute jacobians
- else
- {
- // update jets real part
- unsigned int i;
- for (i = 0; i < BLOCK_0_SIZE; i++)
- (*jets_0_)[i].a = parameters[0][i];
- for (i = 0; i < BLOCK_1_SIZE; i++)
- (*jets_1_)[i].a = parameters[1][i];
- for (i = 0; i < BLOCK_2_SIZE; i++)
- (*jets_2_)[i].a = parameters[2][i];
- for (i = 0; i < BLOCK_3_SIZE; i++)
- (*jets_3_)[i].a = parameters[3][i];
-
- // call functor
- (*this->constraint_ptr_)(jets_0_->data(), jets_1_->data(), jets_2_->data(), jets_3_->data(), residuals_jets_->data());
-
- // fill the residual array
- for (i = 0; i < MEASUREMENT_SIZE; i++)
- residuals[i] = (*residuals_jets_)[i].a;
-
- // fill the jacobian matrices
- for (i = 0; i<n_blocks_; i++)
- if (jacobians[i] != nullptr)
- for (unsigned int row = 0; row < MEASUREMENT_SIZE; row++)
- std::copy((*residuals_jets_)[row].v.data() + jacobian_locations_.at(i),
- (*residuals_jets_)[row].v.data() + jacobian_locations_.at(i) + block_sizes_.at(i),
- jacobians[i] + row * block_sizes_.at(i));
- }
- return true;
- }
-};
-
-// SPECIALIZATION 3 BLOCKS
-template <class ConstraintType, const unsigned int MEASUREMENT_SIZE,
- unsigned int BLOCK_0_SIZE, unsigned int BLOCK_1_SIZE, unsigned int BLOCK_2_SIZE>
-class AutoDiffCostFunctionWrapperBase<ConstraintType, MEASUREMENT_SIZE,
- BLOCK_0_SIZE, BLOCK_1_SIZE, BLOCK_2_SIZE, 0, 0,
- 0, 0, 0, 0, 0>
- : public ceres::SizedCostFunction<MEASUREMENT_SIZE,
- BLOCK_0_SIZE,BLOCK_1_SIZE,BLOCK_2_SIZE,0,0,
- 0,0,0,0,0>
-{
- typedef ceres::Jet<Scalar, BLOCK_0_SIZE + BLOCK_1_SIZE + BLOCK_2_SIZE> WolfJet;
-
- protected:
- ConstraintType* constraint_ptr_;
- unsigned int n_blocks_;
- std::vector<unsigned int> block_sizes_, jacobian_locations_;
- std::array<WolfJet, BLOCK_0_SIZE>* jets_0_;
- std::array<WolfJet, BLOCK_1_SIZE>* jets_1_;
- std::array<WolfJet, BLOCK_2_SIZE>* jets_2_;
- std::array<WolfJet, MEASUREMENT_SIZE>* residuals_jets_;
-
- public:
-
- AutoDiffCostFunctionWrapperBase(ConstraintType* _constraint_ptr) :
- ceres::SizedCostFunction<MEASUREMENT_SIZE,
- BLOCK_0_SIZE,BLOCK_1_SIZE,BLOCK_2_SIZE,0,0,
- 0,0,0,0,0>(),
- constraint_ptr_(_constraint_ptr),
- n_blocks_(3),
- block_sizes_({BLOCK_0_SIZE, BLOCK_1_SIZE, BLOCK_2_SIZE}),
- jacobian_locations_({0,
- BLOCK_0_SIZE,
- BLOCK_0_SIZE+BLOCK_1_SIZE}),
- jets_0_(new std::array<WolfJet, BLOCK_0_SIZE>),
- jets_1_(new std::array<WolfJet, BLOCK_1_SIZE>),
- jets_2_(new std::array<WolfJet, BLOCK_2_SIZE>),
- residuals_jets_(new std::array<WolfJet, MEASUREMENT_SIZE>)
- {
- // initialize jets
- unsigned int i, last_jet_idx = 0;
- for (i = 0; i < BLOCK_0_SIZE; i++)
- (*jets_0_)[i] = WolfJet(0, last_jet_idx++);
- for (i = 0; i < BLOCK_1_SIZE; i++)
- (*jets_1_)[i] = WolfJet(0, last_jet_idx++);
- for (i = 0; i < BLOCK_2_SIZE; i++)
- (*jets_2_)[i] = WolfJet(0, last_jet_idx++);
-
- };
-
- virtual ~AutoDiffCostFunctionWrapperBase()
- {
- delete jets_0_;
- delete jets_1_;
- delete jets_2_;
- delete residuals_jets_;
- };
-
- virtual bool Evaluate(double const* const* parameters, double* residuals, double** jacobians) const
- {
- // only residuals
- if (jacobians == nullptr)
- (*this->constraint_ptr_)(parameters[0], parameters[1], parameters[2], residuals);
-
- // also compute jacobians
- else
- {
- // update jets real part
- unsigned int i;
- for (i = 0; i < BLOCK_0_SIZE; i++)
- (*jets_0_)[i].a = parameters[0][i];
- for (i = 0; i < BLOCK_1_SIZE; i++)
- (*jets_1_)[i].a = parameters[1][i];
- for (i = 0; i < BLOCK_2_SIZE; i++)
- (*jets_2_)[i].a = parameters[2][i];
-
- // call functor
- (*this->constraint_ptr_)(jets_0_->data(), jets_1_->data(), jets_2_->data(), residuals_jets_->data());
-
- // fill the residual array
- for (i = 0; i < MEASUREMENT_SIZE; i++)
- residuals[i] = (*residuals_jets_)[i].a;
-
- // fill the jacobian matrices
- for (i = 0; i<n_blocks_; i++)
- if (jacobians[i] != nullptr)
- for (unsigned int row = 0; row < MEASUREMENT_SIZE; row++)
- std::copy((*residuals_jets_)[row].v.data() + jacobian_locations_.at(i),
- (*residuals_jets_)[row].v.data() + jacobian_locations_.at(i) + block_sizes_.at(i),
- jacobians[i] + row * block_sizes_.at(i));
- }
- return true;
- }
-};
-
-// SPECIALIZATION 2 BLOCKS
-template <class ConstraintType, const unsigned int MEASUREMENT_SIZE,
- unsigned int BLOCK_0_SIZE, unsigned int BLOCK_1_SIZE>
-class AutoDiffCostFunctionWrapperBase<ConstraintType, MEASUREMENT_SIZE,
- BLOCK_0_SIZE, BLOCK_1_SIZE, 0, 0, 0,
- 0, 0, 0, 0, 0>
- : public ceres::SizedCostFunction<MEASUREMENT_SIZE,
- BLOCK_0_SIZE,BLOCK_1_SIZE,0,0,0,
- 0,0,0,0,0>
-{
- typedef ceres::Jet<Scalar, BLOCK_0_SIZE + BLOCK_1_SIZE> WolfJet;
-
- protected:
- ConstraintType* constraint_ptr_;
- unsigned int n_blocks_;
- std::vector<unsigned int> block_sizes_, jacobian_locations_;
- std::array<WolfJet, BLOCK_0_SIZE>* jets_0_;
- std::array<WolfJet, BLOCK_1_SIZE>* jets_1_;
- std::array<WolfJet, MEASUREMENT_SIZE>* residuals_jets_;
-
- public:
-
- AutoDiffCostFunctionWrapperBase(ConstraintType* _constraint_ptr) :
- ceres::SizedCostFunction<MEASUREMENT_SIZE,
- BLOCK_0_SIZE,BLOCK_1_SIZE,0,0,0,
- 0,0,0,0,0>(),
- constraint_ptr_(_constraint_ptr),
- n_blocks_(2),
- block_sizes_({BLOCK_0_SIZE, BLOCK_1_SIZE}),
- jacobian_locations_({0,
- BLOCK_0_SIZE}),
- jets_0_(new std::array<WolfJet, BLOCK_0_SIZE>),
- jets_1_(new std::array<WolfJet, BLOCK_1_SIZE>),
- residuals_jets_(new std::array<WolfJet, MEASUREMENT_SIZE>)
- {
- // initialize jets
- unsigned int i, last_jet_idx = 0;
- for (i = 0; i < BLOCK_0_SIZE; i++)
- (*jets_0_)[i] = WolfJet(0, last_jet_idx++);
- for (i = 0; i < BLOCK_1_SIZE; i++)
- (*jets_1_)[i] = WolfJet(0, last_jet_idx++);
- };
-
- virtual ~AutoDiffCostFunctionWrapperBase()
- {
- delete jets_0_;
- delete jets_1_;
- delete residuals_jets_;
- };
-
- virtual bool Evaluate(double const* const* parameters, double* residuals, double** jacobians) const
- {
- // only residuals
- if (jacobians == nullptr)
- (*this->constraint_ptr_)(parameters[0], parameters[1], residuals);
-
- // also compute jacobians
- else
- {
- // update jets real part
- unsigned int i;
- for (i = 0; i < BLOCK_0_SIZE; i++)
- (*jets_0_)[i].a = parameters[0][i];
- for (i = 0; i < BLOCK_1_SIZE; i++)
- (*jets_1_)[i].a = parameters[1][i];
-
- // call functor
- (*this->constraint_ptr_)(jets_0_->data(), jets_1_->data(), residuals_jets_->data());
-
- // fill the residual array
- for (i = 0; i < MEASUREMENT_SIZE; i++)
- residuals[i] = (*residuals_jets_)[i].a;
-
- // fill the jacobian matrices
- for (i = 0; i<n_blocks_; i++)
- if (jacobians[i] != nullptr)
- for (unsigned int row = 0; row < MEASUREMENT_SIZE; row++)
- std::copy((*residuals_jets_)[row].v.data() + jacobian_locations_.at(i),
- (*residuals_jets_)[row].v.data() + jacobian_locations_.at(i) + block_sizes_.at(i),
- jacobians[i] + row * block_sizes_.at(i));
- }
- return true;
- }
-};
-
-// SPECIALIZATION 1 BLOCK
-template <class ConstraintType, const unsigned int MEASUREMENT_SIZE,
- unsigned int BLOCK_0_SIZE>
-class AutoDiffCostFunctionWrapperBase<ConstraintType, MEASUREMENT_SIZE,
- BLOCK_0_SIZE, 0, 0, 0, 0,
- 0, 0, 0, 0, 0>
- : public ceres::SizedCostFunction<MEASUREMENT_SIZE,
- BLOCK_0_SIZE,0,0,0,0,
- 0,0,0,0,0>
-{
- typedef ceres::Jet<Scalar, BLOCK_0_SIZE> WolfJet;
-
- protected:
- ConstraintType* constraint_ptr_;
- unsigned int n_blocks_;
- std::vector<unsigned int> block_sizes_, jacobian_locations_;
- std::array<WolfJet, BLOCK_0_SIZE>* jets_0_;
- std::array<WolfJet, MEASUREMENT_SIZE>* residuals_jets_;
-
- public:
-
- AutoDiffCostFunctionWrapperBase(ConstraintType* _constraint_ptr) :
- ceres::SizedCostFunction<MEASUREMENT_SIZE,
- BLOCK_0_SIZE,0,0,0,0,
- 0,0,0,0,0>(),
- constraint_ptr_(_constraint_ptr),
- n_blocks_(1),
- block_sizes_({BLOCK_0_SIZE}),
- jacobian_locations_({0}),
- jets_0_(new std::array<WolfJet, BLOCK_0_SIZE>),
- residuals_jets_(new std::array<WolfJet, MEASUREMENT_SIZE>)
- {
- // initialize jets
- unsigned int i, last_jet_idx = 0;
- for (i = 0; i < BLOCK_0_SIZE; i++)
- (*jets_0_)[i] = WolfJet(0, last_jet_idx++);
- };
-
- virtual ~AutoDiffCostFunctionWrapperBase()
- {
- delete jets_0_;
- delete residuals_jets_;
- };
-
- virtual bool Evaluate(double const* const* parameters, double* residuals, double** jacobians) const
- {
- // only residuals
- if (jacobians == nullptr)
- (*this->constraint_ptr_)(parameters[0], residuals);
- // also compute jacobians
- else
- {
- // update jets real part
- unsigned int i;
- for (i = 0; i < BLOCK_0_SIZE; i++)
- (*jets_0_)[i].a = parameters[0][i];
-
- // call functor
- (*this->constraint_ptr_)(jets_0_->data(), residuals_jets_->data());
-
- // fill the residual array
- for (i = 0; i < MEASUREMENT_SIZE; i++)
- residuals[i] = (*residuals_jets_)[i].a;
-
- // fill the jacobian matrices
- for (i = 0; i<n_blocks_; i++)
- if (jacobians[i] != nullptr)
- for (unsigned int row = 0; row < MEASUREMENT_SIZE; row++)
- std::copy((*residuals_jets_)[row].v.data() + jacobian_locations_.at(i),
- (*residuals_jets_)[row].v.data() + jacobian_locations_.at(i) + block_sizes_.at(i),
- jacobians[i] + row * block_sizes_.at(i));
- }
- return true;
- }
-};
-
-} // namespace wolf
-
-#endif /* TRUNK_SRC_AUTODIFF_COST_FUNCTION_WRAPPER_BASE_H_ */
diff --git a/src/ceres_wrapper/ceres_manager.cpp b/src/ceres_wrapper/ceres_manager.cpp
index a66b289b7a4777401c608818447feaf404d9e2d0..acf4585d32b2310294be3da40ea4c236503c4d78 100644
--- a/src/ceres_wrapper/ceres_manager.cpp
+++ b/src/ceres_wrapper/ceres_manager.cpp
@@ -1,14 +1,14 @@
#include "ceres_manager.h"
+#include "create_numeric_diff_cost_function.h"
#include "../trajectory_base.h"
#include "../map_base.h"
#include "../landmark_base.h"
namespace wolf {
-CeresManager::CeresManager(ProblemPtr _wolf_problem, const ceres::Solver::Options& _ceres_options, const bool _use_wolf_auto_diff) :
- ceres_options_(_ceres_options),
- wolf_problem_(_wolf_problem),
- use_wolf_auto_diff_(_use_wolf_auto_diff)
+CeresManager::CeresManager(ProblemPtr _wolf_problem, const ceres::Solver::Options& _ceres_options) :
+ SolverManager(_wolf_problem),
+ ceres_options_(_ceres_options)
{
ceres::Covariance::Options covariance_options;
#if CERES_VERSION_MINOR >= 13
@@ -25,7 +25,6 @@ CeresManager::CeresManager(ProblemPtr _wolf_problem, const ceres::Solver::Option
covariance_ = new ceres::Covariance(covariance_options);
ceres::Problem::Options problem_options;
-
problem_options.cost_function_ownership = ceres::DO_NOT_TAKE_OWNERSHIP;
problem_options.loss_function_ownership = ceres::TAKE_OWNERSHIP;
problem_options.local_parameterization_ownership = ceres::DO_NOT_TAKE_OWNERSHIP;
@@ -36,11 +35,9 @@ CeresManager::~CeresManager()
{
// std::cout << "ceres residual blocks: " << ceres_problem_->NumResidualBlocks() << std::endl;
// std::cout << "ceres parameter blocks: " << ceres_problem_->NumParameterBlocks() << std::endl;
- while (!id_2_residual_idx_.empty())
- removeConstraint(id_2_residual_idx_.begin()->first);
+ while (!ctr_2_residual_idx_.empty())
+ removeConstraint(ctr_2_residual_idx_.begin()->first);
// std::cout << "all residuals removed! \n";
- removeAllStateBlocks();
-// std::cout << "all parameter blocks removed! \n";
delete covariance_;
//std::cout << "covariance deleted! \n";
@@ -48,23 +45,28 @@ CeresManager::~CeresManager()
//std::cout << "ceres problem deleted! \n";
}
-ceres::Solver::Summary CeresManager::solve()
+std::string CeresManager::solve(const unsigned int& _report_level)
{
- //std::cout << "Residual blocks: " << ceres_problem_->NumResidualBlocks() << " Parameter blocks: " << ceres_problem_->NumParameterBlocks() << std::endl;
+ //std::cout << "Residual blocks: " << ceres_problem_->NumResidualBlocks() << " Parameter blocks: " << ceres_problem_->NumParameterBlocks() << std::endl;
// update problem
- update();
+ update();
//std::cout << "After Update: Residual blocks: " << ceres_problem_->NumResidualBlocks() << " Parameter blocks: " << ceres_problem_->NumParameterBlocks() << std::endl;
- // create summary
- ceres::Solver::Summary ceres_summary_;
-
- // run Ceres Solver
- ceres::Solve(ceres_options_, ceres_problem_, &ceres_summary_);
- //std::cout << "solved" << std::endl;
- //return results
- return ceres_summary_;
+ // run Ceres Solver
+ ceres::Solve(ceres_options_, ceres_problem_, &summary_);
+ //std::cout << "solved" << std::endl;
+
+ //return report
+ if (_report_level == 0)
+ return std::string();
+ else if (_report_level == 1)
+ return summary_.BriefReport();
+ else if (_report_level == 2)
+ return summary_.FullReport();
+ else
+ throw std::invalid_argument( "Report level should be 0, 1 or 2!" );
}
void CeresManager::computeCovariances(CovarianceBlocksToBeComputed _blocks)
@@ -102,64 +104,37 @@ void CeresManager::computeCovariances(CovarianceBlocksToBeComputed _blocks)
}
// double loop all against all (without repetitions)
for (unsigned int i = 0; i < all_state_blocks.size(); i++)
- {
for (unsigned int j = i; j < all_state_blocks.size(); j++)
{
state_block_pairs.emplace_back(all_state_blocks[i],all_state_blocks[j]);
double_pairs.emplace_back(all_state_blocks[i]->getPtr(),all_state_blocks[j]->getPtr());
}
- }
break;
}
case ALL_MARGINALS:
{
// first create a vector containing all state blocks
-// std::vector<StateBlockPtr> all_state_blocks, landmark_state_blocks;
- //frame state blocks
for(auto fr_ptr : wolf_problem_->getTrajectoryPtr()->getFrameList())
- {
if (fr_ptr->isKey())
for (auto sb : fr_ptr->getStateBlockVec())
if (sb)
- {
-// std::cout << "CeresManager::computeCovariances(): State block @ " << sb.get() << std::endl;
-// all_state_blocks.push_back(sb);
for(auto sb2 : fr_ptr->getStateBlockVec())
- {
if (sb)
{
state_block_pairs.emplace_back(sb, sb2);
double_pairs.emplace_back(sb->getPtr(), sb2->getPtr());
if (sb == sb2) break;
}
- }
- }
- }
// landmark state blocks
for(auto l_ptr : wolf_problem_->getMapPtr()->getLandmarkList())
- {
-// landmark_state_blocks = l_ptr->getUsedStateBlockVec();
-// all_state_blocks.insert(all_state_blocks.end(), landmark_state_blocks.begin(), landmark_state_blocks.end());
- //all_state_blocks.push_back(l_ptr->getPPtr());
- //all_state_blocks.push_back(l_ptr->getOPtr());
-
-
for (auto sb : l_ptr->getUsedStateBlockVec())
- {
-// std::cout << "CeresManager::computeCovariances(): State block @ " << sb.get() << std::endl;
-// all_state_blocks.push_back(sb);
for(auto sb2 : l_ptr->getUsedStateBlockVec())
{
state_block_pairs.emplace_back(sb, sb2);
double_pairs.emplace_back(sb->getPtr(), sb2->getPtr());
if (sb == sb2) break;
}
- }
-
-
-
- }
// // loop all marginals (PO marginals)
// for (unsigned int i = 0; 2*i+1 < all_state_blocks.size(); i++)
// {
@@ -230,108 +205,70 @@ void CeresManager::computeCovariances(CovarianceBlocksToBeComputed _blocks)
std::cout << "WARNING: Couldn't compute covariances!" << std::endl;
}
-void CeresManager::update()
+void CeresManager::computeCovariances(const StateBlockList& st_list)
{
- //std::cout << "CeresManager: updating... " << std::endl;
- //std::cout << wolf_problem_->getStateBlockNotificationList().size() << " state block notifications" << std::endl;
- //std::cout << wolf_problem_->getConstraintNotificationList().size() << " constraint notifications" << std::endl;
-
- // REMOVE CONSTRAINTS
- auto ctr_notification_it = wolf_problem_->getConstraintNotificationList().begin();
- while ( ctr_notification_it != wolf_problem_->getConstraintNotificationList().end() )
- {
- if (ctr_notification_it->notification_ == REMOVE)
- {
- removeConstraint(ctr_notification_it->id_);
- ctr_notification_it = wolf_problem_->getConstraintNotificationList().erase(ctr_notification_it);
- }
- else
- ctr_notification_it++;
- }
-
- // REMOVE STATE BLOCKS
- auto state_notification_it = wolf_problem_->getStateBlockNotificationList().begin();
- while ( state_notification_it != wolf_problem_->getStateBlockNotificationList().end() )
- {
- if (state_notification_it->notification_ == REMOVE)
- {
- removeStateBlock((double *)(state_notification_it->scalar_ptr_));
- state_notification_it = wolf_problem_->getStateBlockNotificationList().erase(state_notification_it);
- }
- else
- state_notification_it++;
- }
-
- // ADD/UPDATE STATE BLOCKS
- while (!wolf_problem_->getStateBlockNotificationList().empty())
- {
- switch (wolf_problem_->getStateBlockNotificationList().front().notification_)
+ //std::cout << "CeresManager: computing covariances..." << std::endl;
+
+ // update problem
+ update();
+
+ // CLEAR STORED COVARIANCE BLOCKS IN WOLF PROBLEM
+ wolf_problem_->clearCovariance();
+
+ // CREATE DESIRED COVARIANCES LIST
+ std::vector<std::pair<StateBlockPtr, StateBlockPtr>> state_block_pairs;
+ std::vector<std::pair<const double*, const double*>> double_pairs;
+
+ // double loop all against all (without repetitions)
+ for (auto st_it1 = st_list.begin(); st_it1 != st_list.end(); st_it1++)
+ for (auto st_it2 = st_it1; st_it2 != st_list.end(); st_it2++)
{
- case ADD:
- {
- addStateBlock(wolf_problem_->getStateBlockNotificationList().front().state_block_ptr_);
- break;
- }
- case UPDATE:
- {
- updateStateBlockStatus(wolf_problem_->getStateBlockNotificationList().front().state_block_ptr_);
- break;
- }
- default:
- throw std::runtime_error("CeresManager::update: State Block notification must be ADD, UPATE or REMOVE.");
+ state_block_pairs.push_back(std::pair<StateBlockPtr, StateBlockPtr>(*st_it1,*st_it2));
+ double_pairs.push_back(std::pair<const double*, const double*>((*st_it1)->getPtr(),(*st_it2)->getPtr()));
}
- wolf_problem_->getStateBlockNotificationList().pop_front();
- }
- // ADD CONSTRAINTS
- while (!wolf_problem_->getConstraintNotificationList().empty())
- {
- switch (wolf_problem_->getConstraintNotificationList().front().notification_)
+
+ //std::cout << "pairs... " << double_pairs.size() << std::endl;
+
+ // COMPUTE DESIRED COVARIANCES
+ if (covariance_->Compute(double_pairs, ceres_problem_))
+ // STORE DESIRED COVARIANCES
+ for (unsigned int i = 0; i < double_pairs.size(); i++)
{
- case ADD:
- {
-// std::cout << "adding constraint" << std::endl;
- addConstraint(wolf_problem_->getConstraintNotificationList().front().constraint_ptr_,wolf_problem_->getConstraintNotificationList().front().id_);
- //std::cout << "added" << std::endl;
- break;
- }
- default:
- throw std::runtime_error("CeresManager::update: Constraint notification must be ADD or REMOVE.");
+ Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> cov(state_block_pairs[i].first->getSize(),state_block_pairs[i].second->getSize());
+ covariance_->GetCovarianceBlock(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 << "getted covariance " << std::endl << cov << std::endl;
}
- wolf_problem_->getConstraintNotificationList().pop_front();
- }
-// std::cout << "all constraints added" << std::endl;
-// std::cout << "ceres residual blocks: " << ceres_problem_->NumResidualBlocks() << std::endl;
-// std::cout << "wrapper residual blocks: " << id_2_residual_idx_.size() << std::endl;
-// std::cout << "parameter blocks: " << ceres_problem_->NumParameterBlocks() << std::endl;
-
- assert(ceres_problem_->NumResidualBlocks() == id_2_residual_idx_.size() && "ceres residuals different from wrapper residuals");
- assert(wolf_problem_->getConstraintNotificationList().empty() && "wolf problem's constraints notification list not empty after update");
- assert(wolf_problem_->getStateBlockNotificationList().empty() && "wolf problem's state_blocks notification list not empty after update");
+ else
+ std::cout << "WARNING: Couldn't compute covariances!" << std::endl;
}
-void CeresManager::addConstraint(ConstraintBasePtr _ctr_ptr, unsigned int _id)
+void CeresManager::addConstraint(ConstraintBasePtr _ctr_ptr)
{
- id_2_costfunction_[_id] = createCostFunction(_ctr_ptr);
+ ctr_2_costfunction_[_ctr_ptr] = createCostFunction(_ctr_ptr);
-// std::cout << "adding residual " << _ctr_ptr->id() << std::endl;
-// std::cout << "residual pointer " << _ctr_ptr << std::endl;
+// std::cout << "adding constraint " << _ctr_ptr->id() << std::endl;
+// std::cout << "constraint pointer " << _ctr_ptr << std::endl;
if (_ctr_ptr->getApplyLossFunction())
- id_2_residual_idx_[_id] = ceres_problem_->AddResidualBlock(id_2_costfunction_[_id], new ceres::CauchyLoss(0.5), _ctr_ptr->getStateScalarPtrVector());
+ ctr_2_residual_idx_[_ctr_ptr] = ceres_problem_->AddResidualBlock(ctr_2_costfunction_[_ctr_ptr].get(), new ceres::CauchyLoss(0.5), _ctr_ptr->getStateScalarPtrVector());
else
- id_2_residual_idx_[_id] = ceres_problem_->AddResidualBlock(id_2_costfunction_[_id], NULL, _ctr_ptr->getStateScalarPtrVector());
+ ctr_2_residual_idx_[_ctr_ptr] = ceres_problem_->AddResidualBlock(ctr_2_costfunction_[_ctr_ptr].get(), NULL, _ctr_ptr->getStateScalarPtrVector());
+
+ assert(ceres_problem_->NumResidualBlocks() == ctr_2_residual_idx_.size() && "ceres residuals different from wrapper residuals");
}
-void CeresManager::removeConstraint(const unsigned int& _corr_id)
+void CeresManager::removeConstraint(ConstraintBasePtr _ctr_ptr)
{
-// std::cout << "removing constraint " << _corr_id << std::endl;
+// std::cout << "removing constraint " << _ctr_ptr->id() << std::endl;
- assert(id_2_residual_idx_.find(_corr_id) != id_2_residual_idx_.end());
- ceres_problem_->RemoveResidualBlock(id_2_residual_idx_[_corr_id]);
- id_2_residual_idx_.erase(_corr_id);
+ assert(ctr_2_residual_idx_.find(_ctr_ptr) != ctr_2_residual_idx_.end());
+ ceres_problem_->RemoveResidualBlock(ctr_2_residual_idx_[_ctr_ptr]);
+ ctr_2_residual_idx_.erase(_ctr_ptr);
+ ctr_2_costfunction_.erase(_ctr_ptr);
// std::cout << "removingremoved!" << std::endl;
- // The cost functions will be deleted by ceres_problem destructor (IT MUST HAVE THE OWNERSHIP)
+ assert(ceres_problem_->NumResidualBlocks() == ctr_2_residual_idx_.size() && "ceres residuals different from wrapper residuals");
}
void CeresManager::addStateBlock(StateBlockPtr _st_ptr)
@@ -350,25 +287,13 @@ void CeresManager::addStateBlock(StateBlockPtr _st_ptr)
// std::cout << "No Local Parametrization to be added" << std::endl;
ceres_problem_->AddParameterBlock(_st_ptr->getPtr(), _st_ptr->getSize(), nullptr);
}
- if (_st_ptr->isFixed())
- updateStateBlockStatus(_st_ptr);
}
-void CeresManager::removeStateBlock(double* _st_ptr)
+void CeresManager::removeStateBlock(StateBlockPtr _st_ptr)
{
//std::cout << "Removing State Block " << _st_ptr << std::endl;
- assert(_st_ptr != nullptr);
- ceres_problem_->RemoveParameterBlock(_st_ptr);
-}
-
-void CeresManager::removeAllStateBlocks()
-{
- std::vector<double*> parameter_blocks;
-
- ceres_problem_->GetParameterBlocks(¶meter_blocks);
-
- for (unsigned int i = 0; i< parameter_blocks.size(); i++)
- ceres_problem_->RemoveParameterBlock(parameter_blocks[i]);
+ assert(_st_ptr);
+ ceres_problem_->RemoveParameterBlock(_st_ptr->getPtr());
}
void CeresManager::updateStateBlockStatus(StateBlockPtr _st_ptr)
@@ -380,22 +305,18 @@ void CeresManager::updateStateBlockStatus(StateBlockPtr _st_ptr)
ceres_problem_->SetParameterBlockVariable(_st_ptr->getPtr());
}
-ceres::CostFunction* CeresManager::createCostFunction(ConstraintBasePtr _corrPtr)
+ceres::CostFunctionPtr CeresManager::createCostFunction(ConstraintBasePtr _ctr_ptr)
{
- assert(_corrPtr != nullptr);
- //std::cout << "creating cost function for constraint " << _corrPtr->id() << std::endl;
-
- // analitic jacobian
- if (_corrPtr->getJacobianMethod() == JAC_ANALYTIC)
- return new CostFunctionWrapper((ConstraintAnalytic*)(_corrPtr.get())); // TODO revise pointer types
+ assert(_ctr_ptr != nullptr);
+ //std::cout << "creating cost function for constraint " << _ctr_ptr->id() << std::endl;
- // auto jacobian
- else if (_corrPtr->getJacobianMethod() == JAC_AUTO)
- return createAutoDiffCostFunction(_corrPtr, use_wolf_auto_diff_);
+ // analitic & autodiff jacobian
+ if (_ctr_ptr->getJacobianMethod() == JAC_ANALYTIC || _ctr_ptr->getJacobianMethod() == JAC_AUTO)
+ return std::make_shared<CostFunctionWrapper>(_ctr_ptr);
// numeric jacobian
- else if (_corrPtr->getJacobianMethod() == JAC_NUMERIC)
- return createNumericDiffCostFunction(_corrPtr, use_wolf_auto_diff_);
+ else if (_ctr_ptr->getJacobianMethod() == JAC_NUMERIC)
+ return createNumericDiffCostFunction(_ctr_ptr);
else
throw std::invalid_argument( "Bad Jacobian Method!" );
diff --git a/src/ceres_wrapper/ceres_manager.h b/src/ceres_wrapper/ceres_manager.h
index 1f41da1e2bb6226f646a6d8a19b72928bb04c46b..c36144c4ed60cb8f63e840475f3e90be138efd1e 100644
--- a/src/ceres_wrapper/ceres_manager.h
+++ b/src/ceres_wrapper/ceres_manager.h
@@ -7,84 +7,71 @@
#include "glog/logging.h"
//wolf includes
+#include "solver_manager.h"
#include "cost_function_wrapper.h"
#include "local_parametrization_wrapper.h"
-#include "../wolf.h"
-#include "../state_block.h"
-#include "create_auto_diff_cost_function.h"
#include "create_numeric_diff_cost_function.h"
-namespace wolf {
+namespace ceres {
+ typedef std::shared_ptr<CostFunction> CostFunctionPtr;
+}
+namespace wolf {
-
-/** \brief Enumeration of covariance blocks to be computed
- *
- * Enumeration of covariance blocks to be computed
- *
- */
-typedef enum
-{
- ALL, ///< All blocks and all cross-covariances
- ALL_MARGINALS, ///< All marginals
- ROBOT_LANDMARKS ///< marginals of landmarks and current robot pose plus cross covariances of current robot and all landmarks
-} CovarianceBlocksToBeComputed;
+WOLF_PTR_TYPEDEFS(CeresManager);
/** \brief Ceres manager for WOLF
*
*/
-class CeresManager
+class CeresManager : public SolverManager
{
protected:
- std::map<unsigned int, ceres::ResidualBlockId> id_2_residual_idx_;
- std::map<unsigned int, ceres::CostFunction*> id_2_costfunction_;
+ std::map<ConstraintBasePtr, ceres::ResidualBlockId> ctr_2_residual_idx_;
+ std::map<ConstraintBasePtr, ceres::CostFunctionPtr> ctr_2_costfunction_;
ceres::Problem* ceres_problem_;
ceres::Solver::Options ceres_options_;
ceres::Covariance* covariance_;
- ProblemPtr wolf_problem_;
- bool use_wolf_auto_diff_;
+ ceres::Solver::Summary summary_;
public:
- CeresManager(ProblemPtr _wolf_problem, const ceres::Solver::Options& _ceres_options = ceres::Solver::Options(), const bool _use_wolf_auto_diff = true);
+ CeresManager(ProblemPtr _wolf_problem, const ceres::Solver::Options& _ceres_options = ceres::Solver::Options());
~CeresManager();
- ceres::Solver::Summary solve();
+ virtual std::string solve(const unsigned int& _report_level);
- void computeCovariances(CovarianceBlocksToBeComputed _blocks = ROBOT_LANDMARKS);
+ ceres::Solver::Summary getSummary();
- ceres::Solver::Options& getSolverOptions();
+ virtual void computeCovariances(CovarianceBlocksToBeComputed _blocks = ROBOT_LANDMARKS);
- void setUseWolfAutoDiff(bool _use_wolf_auto_diff);
+ virtual void computeCovariances(const StateBlockList& st_list);
- private:
-
- void update();
+ ceres::Solver::Options& getSolverOptions();
- void addConstraint(ConstraintBasePtr _corr_ptr, unsigned int _id);
+ private:
- void removeConstraint(const unsigned int& _corr_idx);
+ virtual void addConstraint(ConstraintBasePtr _ctr_ptr);
- void addStateBlock(StateBlockPtr _st_ptr);
+ virtual void removeConstraint(ConstraintBasePtr _ctr_ptr);
- void removeStateBlock(double* _st_ptr);
+ virtual void addStateBlock(StateBlockPtr _st_ptr);
- void removeAllStateBlocks();
+ virtual void removeStateBlock(StateBlockPtr _st_ptr);
- void updateStateBlockStatus(StateBlockPtr _st_ptr);
+ virtual void updateStateBlockStatus(StateBlockPtr _st_ptr);
- ceres::CostFunction* createCostFunction(ConstraintBasePtr _corrPtr);
+ ceres::CostFunctionPtr createCostFunction(ConstraintBasePtr _ctr_ptr);
};
-inline ceres::Solver::Options& CeresManager::getSolverOptions()
+inline ceres::Solver::Summary CeresManager::getSummary()
{
- return ceres_options_;
+ return summary_;
}
-inline void CeresManager::setUseWolfAutoDiff(bool _use_wolf_auto_diff)
+inline ceres::Solver::Options& CeresManager::getSolverOptions()
{
- use_wolf_auto_diff_ = _use_wolf_auto_diff;
+ return ceres_options_;
}
} // namespace wolf
diff --git a/src/ceres_wrapper/cost_function_wrapper.h b/src/ceres_wrapper/cost_function_wrapper.h
index 2a0a1f9d5b4ee03348b476ea3521e83303f62830..256b81703040f692323e0c647106e833b200ddd2 100644
--- a/src/ceres_wrapper/cost_function_wrapper.h
+++ b/src/ceres_wrapper/cost_function_wrapper.h
@@ -13,60 +13,32 @@
namespace wolf {
+WOLF_PTR_TYPEDEFS(CostFunctionWrapper);
+
class CostFunctionWrapper : public ceres::CostFunction
{
protected:
- ConstraintAnalytic* constraint_ptr_;
- std::vector<unsigned int> state_blocks_sizes_;
+ ConstraintBasePtr constraint_ptr_;
public:
- CostFunctionWrapper(ConstraintAnalytic* _constraint_ptr) :
+ CostFunctionWrapper(ConstraintBasePtr _constraint_ptr) :
ceres::CostFunction(),
- constraint_ptr_(_constraint_ptr),
- state_blocks_sizes_(constraint_ptr_->getStateSizes())
+ constraint_ptr_(_constraint_ptr)
{
- for (unsigned int i = 0; i < constraint_ptr_->getStateBlockPtrVector().size(); i++)
- mutable_parameter_block_sizes()->push_back(state_blocks_sizes_[i]);
+ for (auto st_block_size : constraint_ptr_->getStateSizes())
+ mutable_parameter_block_sizes()->push_back(st_block_size);
set_num_residuals(constraint_ptr_->getSize());
};
virtual ~CostFunctionWrapper()
{
-
};
virtual bool Evaluate(double const* const* parameters, double* residuals, double** jacobians) const
{
- // load parameters evaluation value
- std::vector<Eigen::Map<const Eigen::VectorXs>> state_blocks_map_;
- for (unsigned int i = 0; i < state_blocks_sizes_.size(); i++)
- state_blocks_map_.push_back(Eigen::Map<const Eigen::VectorXs>((Scalar*)parameters[i], state_blocks_sizes_[i]));
-
- // residuals
- Eigen::Map<Eigen::VectorXs> residuals_map((Scalar*)residuals, constraint_ptr_->getSize());
- residuals_map = constraint_ptr_->evaluateResiduals(state_blocks_map_);
-
- // also compute jacobians
- if (jacobians != nullptr)
- {
- std::vector<Eigen::Map<Eigen::MatrixXs>> jacobians_map_;
- std::vector<bool> compute_jacobians_(state_blocks_sizes_.size());
-
- for (unsigned int i = 0; i < state_blocks_sizes_.size(); i++)
- {
- compute_jacobians_[i] = (jacobians[i] != nullptr);
- if (jacobians[i] != nullptr)
- jacobians_map_.push_back(Eigen::Map<Eigen::MatrixXs>((Scalar*)jacobians[i], constraint_ptr_->getSize(), state_blocks_sizes_[i]));
- else
- jacobians_map_.push_back(Eigen::Map<Eigen::MatrixXs>(nullptr, 0, 0)); //TODO: check if it can be done
- }
-
- // evaluate jacobians
- constraint_ptr_->evaluateJacobians(state_blocks_map_, jacobians_map_, compute_jacobians_);
- }
- return true;
+ return constraint_ptr_->evaluate(parameters, residuals, jacobians);
}
};
diff --git a/src/ceres_wrapper/create_auto_diff_cost_function.cpp b/src/ceres_wrapper/create_auto_diff_cost_function.cpp
deleted file mode 100644
index c47c144c3fd4109363a78598ffce879e1384f397..0000000000000000000000000000000000000000
--- a/src/ceres_wrapper/create_auto_diff_cost_function.cpp
+++ /dev/null
@@ -1,144 +0,0 @@
-/*
- * create_auto_diff_cost_function.cpp
- *
- * Created on: May 18, 2016
- * Author: jvallve
- */
-
-#include "create_auto_diff_cost_function.h"
-
-// Constraints
-#include "../constraint_sparse.h"
-#include "../constraint_fix.h"
-#include "../constraint_fix_3D.h"
-#include "../constraint_fix_bias.h"
-#include "../constraint_gps_2D.h"
-#include "../constraint_gps_pseudorange_3D.h"
-#include "../constraint_gps_pseudorange_2D.h"
-#include "../constraint_odom_2D.h"
-#include "../constraint_odom_3D.h"
-#include "../constraint_corner_2D.h"
-#include "../constraint_point_2D.h"
-#include "../constraint_point_to_line_2D.h"
-#include "../constraint_container.h"
-
-#ifdef CV_VERSION
- #include "../constraint_AHP.h"
-#endif
-
-#include "../constraint_imu.h"
-
-// Wolf and ceres auto_diff creators
-#include "create_auto_diff_cost_function_wrapper.h"
-#include "create_auto_diff_cost_function_ceres.h"
-
-namespace wolf {
-
-ceres::CostFunction* createAutoDiffCostFunction(ConstraintBasePtr _ctr_ptr, bool _use_wolf_autodiff)
-{
- switch (_ctr_ptr->getTypeId())
- {
- case CTR_GPS_FIX_2D:
- if (_use_wolf_autodiff)
- return createAutoDiffCostFunctionWrapper<ConstraintGPS2D>(_ctr_ptr);
- else
- return createAutoDiffCostFunctionCeres<ConstraintGPS2D>(_ctr_ptr);
-
- case CTR_FIX:
- if (_use_wolf_autodiff)
- return createAutoDiffCostFunctionWrapper<ConstraintFix>(_ctr_ptr);
- else
- return createAutoDiffCostFunctionCeres<ConstraintFix>(_ctr_ptr);
-
- case CTR_FIX_3D:
- if (_use_wolf_autodiff)
- return createAutoDiffCostFunctionWrapper<ConstraintFix3D>(_ctr_ptr);
- else
- return createAutoDiffCostFunctionCeres<ConstraintFix3D>(_ctr_ptr);
-
- case CTR_FIX_BIAS:
- if(_use_wolf_autodiff)
- return createAutoDiffCostFunctionWrapper<ConstraintFixBias>(_ctr_ptr);
- else
- return createAutoDiffCostFunctionCeres<ConstraintFixBias>(_ctr_ptr);
-
- case CTR_ODOM_2D:
- if (_use_wolf_autodiff)
- return createAutoDiffCostFunctionWrapper<ConstraintOdom2D>(_ctr_ptr);
- else
- return createAutoDiffCostFunctionCeres<ConstraintOdom2D>(_ctr_ptr);
-
- case CTR_ODOM_3D:
- if (_use_wolf_autodiff)
- return createAutoDiffCostFunctionWrapper<ConstraintOdom3D>(_ctr_ptr);
- else
- return createAutoDiffCostFunctionCeres<ConstraintOdom3D>(_ctr_ptr);
-
- case CTR_CORNER_2D:
- if (_use_wolf_autodiff)
- return createAutoDiffCostFunctionWrapper<ConstraintCorner2D>(_ctr_ptr);
- else
- return createAutoDiffCostFunctionCeres<ConstraintCorner2D>(_ctr_ptr);
-
- case CTR_CONTAINER:
- if (_use_wolf_autodiff)
- return createAutoDiffCostFunctionWrapper<ConstraintContainer>(_ctr_ptr);
- else
- return createAutoDiffCostFunctionCeres<ConstraintContainer>(_ctr_ptr);
-
- case CTR_GPS_PR_3D:
- if (_use_wolf_autodiff)
- return createAutoDiffCostFunctionWrapper<ConstraintGPSPseudorange3D>(_ctr_ptr);
- else
- return createAutoDiffCostFunctionCeres<ConstraintGPSPseudorange3D>(_ctr_ptr);
-
- case CTR_GPS_PR_2D:
- if (_use_wolf_autodiff)
- return createAutoDiffCostFunctionWrapper<ConstraintGPSPseudorange2D>(_ctr_ptr);
- else
- return createAutoDiffCostFunctionCeres<ConstraintGPSPseudorange2D>(_ctr_ptr);
-
- case CTR_POINT_2D:
- if (_use_wolf_autodiff)
- return createAutoDiffCostFunctionWrapper<ConstraintPoint2D>(_ctr_ptr);
- else
- return createAutoDiffCostFunctionCeres<ConstraintPoint2D>(_ctr_ptr);
-
- case CTR_POINT_TO_LINE_2D:
- if (_use_wolf_autodiff)
- return createAutoDiffCostFunctionWrapper<ConstraintPointToLine2D>(_ctr_ptr);
- else
- return createAutoDiffCostFunctionCeres<ConstraintPointToLine2D>(_ctr_ptr);
-#ifdef CV_VERSION
- case CTR_EPIPOLAR:
- if (_use_wolf_autodiff)
- return createAutoDiffCostFunctionWrapper<ConstraintAHP>(_ctr_ptr);
- else
- return createAutoDiffCostFunctionCeres<ConstraintAHP>(_ctr_ptr);
-
- case CTR_AHP:
- if (_use_wolf_autodiff)
- return createAutoDiffCostFunctionWrapper<ConstraintAHP>(_ctr_ptr);
- else
- return createAutoDiffCostFunctionCeres<ConstraintAHP>(_ctr_ptr);
-#endif
- case CTR_IMU:
- if (_use_wolf_autodiff)
- return createAutoDiffCostFunctionWrapper<ConstraintIMU>(_ctr_ptr);
- else
- return createAutoDiffCostFunctionCeres<ConstraintIMU>(_ctr_ptr);
-
- /* For adding a new constraint, add the #include and a case:
- case CTR_ENUM:
- if (_use_wolf_autodiff)
- return createAutoDiffCostFunctionWrapper<ConstraintType>(_ctr_ptr);
- else
- return createAutoDiffCostFunctionCeres<ConstraintType>(_ctr_ptr);
- */
-
- default:
- throw std::invalid_argument( "Unknown constraint type! Please add it in the file: ceres_wrapper/create_auto_diff_cost_function.cpp" );
- }
-}
-
-} // namespace wolf
diff --git a/src/ceres_wrapper/create_auto_diff_cost_function.h b/src/ceres_wrapper/create_auto_diff_cost_function.h
deleted file mode 100644
index d50bdb97fe266eaef6a19d919a7dd31605df56a7..0000000000000000000000000000000000000000
--- a/src/ceres_wrapper/create_auto_diff_cost_function.h
+++ /dev/null
@@ -1,19 +0,0 @@
-/*
- * create_auto_diff_cost_function.h
- *
- * Created on: Apr 4, 2016
- * Author: jvallve
- */
-
-#ifndef SRC_CERES_WRAPPER_CREATE_AUTO_DIFF_COST_FUNCTION_H_
-#define SRC_CERES_WRAPPER_CREATE_AUTO_DIFF_COST_FUNCTION_H_
-
-#include "../constraint_base.h"
-#include "ceres/cost_function.h"
-
-namespace wolf {
- ceres::CostFunction* createAutoDiffCostFunction(ConstraintBasePtr _ctr_ptr, bool _use_wolf_autodiff);
-
-}
-
-#endif /* SRC_CERES_WRAPPER_CREATE_AUTO_DIFF_COST_FUNCTION_H_ */
diff --git a/src/ceres_wrapper/create_auto_diff_cost_function_ceres.h b/src/ceres_wrapper/create_auto_diff_cost_function_ceres.h
deleted file mode 100644
index 3d6182a426ef1e3e5466034d7ac452f70cadde8f..0000000000000000000000000000000000000000
--- a/src/ceres_wrapper/create_auto_diff_cost_function_ceres.h
+++ /dev/null
@@ -1,44 +0,0 @@
-/*
- * create_auto_diff_cost_function_ceres.h
- *
- * Created on: Apr 5, 2016
- * Author: jvallve
- */
-
-#ifndef SRC_CERES_WRAPPER_CREATE_AUTO_DIFF_COST_FUNCTION_CERES_H_
-#define SRC_CERES_WRAPPER_CREATE_AUTO_DIFF_COST_FUNCTION_CERES_H_
-
-#include "ceres/autodiff_cost_function.h"
-
-namespace wolf {
-
-template <class CtrType>
-ceres::AutoDiffCostFunction<CtrType,
- CtrType::residualSize,
- CtrType::block0Size,CtrType::block1Size,
- CtrType::block2Size,CtrType::block3Size,
- CtrType::block4Size,CtrType::block5Size,
- CtrType::block6Size,CtrType::block7Size,
- CtrType::block8Size,CtrType::block9Size>* createAutoDiffCostFunctionCeres(ConstraintBasePtr _constraint_ptr)
-{
- static_assert(CtrType::residualSize != 0,"Measurement size cannot be null!");
- static_assert(!(CtrType::block0Size == 0 ||
- (CtrType::block1Size > 0 && CtrType::block0Size == 0) ||
- (CtrType::block2Size > 0 && (CtrType::block0Size == 0 || CtrType::block1Size == 0)) ||
- (CtrType::block3Size > 0 && (CtrType::block0Size == 0 || CtrType::block1Size == 0 || CtrType::block2Size == 0)) ||
- (CtrType::block4Size > 0 && (CtrType::block0Size == 0 || CtrType::block1Size == 0 || CtrType::block2Size == 0 || CtrType::block3Size == 0)) ||
- (CtrType::block5Size > 0 && (CtrType::block0Size == 0 || CtrType::block1Size == 0 || CtrType::block2Size == 0 || CtrType::block3Size == 0 || CtrType::block4Size == 0)) ||
- (CtrType::block6Size > 0 && (CtrType::block0Size == 0 || CtrType::block1Size == 0 || CtrType::block2Size == 0 || CtrType::block3Size == 0 || CtrType::block4Size == 0 || CtrType::block5Size == 0)) ||
- (CtrType::block7Size > 0 && (CtrType::block0Size == 0 || CtrType::block1Size == 0 || CtrType::block2Size == 0 || CtrType::block3Size == 0 || CtrType::block4Size == 0 || CtrType::block5Size == 0 || CtrType::block6Size == 0)) ||
- (CtrType::block8Size > 0 && (CtrType::block0Size == 0 || CtrType::block1Size == 0 || CtrType::block2Size == 0 || CtrType::block3Size == 0 || CtrType::block4Size == 0 || CtrType::block5Size == 0 || CtrType::block6Size == 0 || CtrType::block7Size == 0)) ||
- (CtrType::block9Size > 0 && (CtrType::block0Size == 0 || CtrType::block1Size == 0 || CtrType::block2Size == 0 || CtrType::block3Size == 0 || CtrType::block4Size == 0 || CtrType::block5Size == 0 || CtrType::block6Size == 0 || CtrType::block7Size == 0 || CtrType::block8Size == 0))),
- "bad block sizes numbers!");
-
- return new ceres::AutoDiffCostFunction<CtrType, CtrType::residualSize,
- CtrType::block0Size,CtrType::block1Size,CtrType::block2Size,CtrType::block3Size,CtrType::block4Size,
- CtrType::block5Size,CtrType::block6Size,CtrType::block7Size,CtrType::block8Size,CtrType::block9Size>(std::static_pointer_cast<CtrType>(_constraint_ptr).get()); // TODO revise pointer types
-};
-
-} // namespace wolf
-
-#endif /* SRC_CERES_WRAPPER_CREATE_AUTO_DIFF_COST_FUNCTION_CERES_H_ */
diff --git a/src/ceres_wrapper/create_auto_diff_cost_function_wrapper.h b/src/ceres_wrapper/create_auto_diff_cost_function_wrapper.h
deleted file mode 100644
index 32b3cec5212258d0bc2b1f2a184a4eaa41079fe2..0000000000000000000000000000000000000000
--- a/src/ceres_wrapper/create_auto_diff_cost_function_wrapper.h
+++ /dev/null
@@ -1,44 +0,0 @@
-/*
- * create_auto_diff_cost_function.h
- *
- * Created on: Apr 5, 2016
- * Author: jvallve
- */
-
-#ifndef SRC_CERES_WRAPPER_CREATE_AUTO_DIFF_COST_FUNCTION_WRAPPER_H_
-#define SRC_CERES_WRAPPER_CREATE_AUTO_DIFF_COST_FUNCTION_WRAPPER_H_
-
-#include "auto_diff_cost_function_wrapper.h"
-
-namespace wolf {
-
-template <class CtrType>
-AutoDiffCostFunctionWrapperBase<CtrType,
- CtrType::residualSize,
- CtrType::block0Size,CtrType::block1Size,
- CtrType::block2Size,CtrType::block3Size,
- CtrType::block4Size,CtrType::block5Size,
- CtrType::block6Size,CtrType::block7Size,
- CtrType::block8Size,CtrType::block9Size>* createAutoDiffCostFunctionWrapper(ConstraintBasePtr _constraint_ptr)
-{
- static_assert(CtrType::residualSize != 0,"Measurement size cannot be null!");
- static_assert(!(CtrType::block0Size == 0 ||
- (CtrType::block1Size > 0 && CtrType::block0Size == 0) ||
- (CtrType::block2Size > 0 && (CtrType::block0Size == 0 || CtrType::block1Size == 0)) ||
- (CtrType::block3Size > 0 && (CtrType::block0Size == 0 || CtrType::block1Size == 0 || CtrType::block2Size == 0)) ||
- (CtrType::block4Size > 0 && (CtrType::block0Size == 0 || CtrType::block1Size == 0 || CtrType::block2Size == 0 || CtrType::block3Size == 0)) ||
- (CtrType::block5Size > 0 && (CtrType::block0Size == 0 || CtrType::block1Size == 0 || CtrType::block2Size == 0 || CtrType::block3Size == 0 || CtrType::block4Size == 0)) ||
- (CtrType::block6Size > 0 && (CtrType::block0Size == 0 || CtrType::block1Size == 0 || CtrType::block2Size == 0 || CtrType::block3Size == 0 || CtrType::block4Size == 0 || CtrType::block5Size == 0)) ||
- (CtrType::block7Size > 0 && (CtrType::block0Size == 0 || CtrType::block1Size == 0 || CtrType::block2Size == 0 || CtrType::block3Size == 0 || CtrType::block4Size == 0 || CtrType::block5Size == 0 || CtrType::block6Size == 0)) ||
- (CtrType::block8Size > 0 && (CtrType::block0Size == 0 || CtrType::block1Size == 0 || CtrType::block2Size == 0 || CtrType::block3Size == 0 || CtrType::block4Size == 0 || CtrType::block5Size == 0 || CtrType::block6Size == 0 || CtrType::block7Size == 0)) ||
- (CtrType::block9Size > 0 && (CtrType::block0Size == 0 || CtrType::block1Size == 0 || CtrType::block2Size == 0 || CtrType::block3Size == 0 || CtrType::block4Size == 0 || CtrType::block5Size == 0 || CtrType::block6Size == 0 || CtrType::block7Size == 0 || CtrType::block8Size == 0))),
- "bad block sizes numbers!");
-
- return new AutoDiffCostFunctionWrapperBase<CtrType, CtrType::residualSize,
- CtrType::block0Size,CtrType::block1Size,CtrType::block2Size,CtrType::block3Size,CtrType::block4Size,
- CtrType::block5Size,CtrType::block6Size,CtrType::block7Size,CtrType::block8Size,CtrType::block9Size>(std::static_pointer_cast<CtrType>(_constraint_ptr).get()); // TODO revise pointer types
-};
-
-} // namespace wolf
-
-#endif /* SRC_CERES_WRAPPER_CREATE_AUTO_DIFF_COST_FUNCTION_WRAPPER_H_ */
diff --git a/src/ceres_wrapper/create_numeric_diff_cost_function.cpp b/src/ceres_wrapper/create_numeric_diff_cost_function.cpp
deleted file mode 100644
index cfadab44bfe16aa05bd37c6c16c51bc867f1ee7f..0000000000000000000000000000000000000000
--- a/src/ceres_wrapper/create_numeric_diff_cost_function.cpp
+++ /dev/null
@@ -1,39 +0,0 @@
-/*
- * create_numerical_diff_cost_function.cpp
- *
- * Created on: May 18, 2016
- * Author: jvallve
- */
-
-#include "create_numeric_diff_cost_function.h"
-
-// Constraints
-#include "../constraint_odom_2D.h"
-
-// Wolf and ceres auto_diff creators
-#include "create_numeric_diff_cost_function_ceres.h"
-
-namespace wolf {
-
-ceres::CostFunction* createNumericDiffCostFunction(ConstraintBasePtr _ctr_ptr, bool _use_wolf_numericdiff)
-{
- if (_use_wolf_numericdiff)
- throw std::invalid_argument( "Numeric differentiation not implemented in wolf" );
-
- switch (_ctr_ptr->getTypeId())
- {
- // just for testing
- case CTR_ODOM_2D:
- return createNumericDiffCostFunctionCeres<ConstraintOdom2D>(_ctr_ptr);
-
- /* For adding a new constraint, add the #include and a case:
- case CTR_ENUM:
- return createNumericDiffCostFunctionCeres<ConstraintType>(_ctr_ptr);
- */
-
- default:
- throw std::invalid_argument( "Unknown constraint type! Please add it in the file: ceres_wrapper/create_Numeric_diff_cost_function.h" );
- }
-}
-
-} // namespace wolf
diff --git a/src/ceres_wrapper/create_numeric_diff_cost_function.h b/src/ceres_wrapper/create_numeric_diff_cost_function.h
index e53e84f966c60a97ac0d38e0160816d34deda28e..ff9d34088fb2479e63de7f19ff3806389b049969 100644
--- a/src/ceres_wrapper/create_numeric_diff_cost_function.h
+++ b/src/ceres_wrapper/create_numeric_diff_cost_function.h
@@ -1,5 +1,5 @@
/*
- * create_numerical_diff_cost_function.h
+ * create_numeric_diff_cost_function.h
*
* Created on: Apr 5, 2016
* Author: jvallve
@@ -9,11 +9,44 @@
#define SRC_CERES_WRAPPER_CREATE_NUMERIC_DIFF_COST_FUNCTION_H_
#include "ceres/cost_function.h"
+#include "ceres/numeric_diff_cost_function.h"
+
+// Constraints
+#include "../constraint_odom_2D.h"
#include "../constraint_base.h"
+
namespace wolf {
-ceres::CostFunction* createNumericDiffCostFunction(ConstraintBasePtr _ctr_ptr, bool _use_wolf_numericdiff);
+// 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(ConstraintBasePtr _constraint_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>(_constraint_ptr).get());
+};
+
+
+std::shared_ptr<ceres::CostFunction> createNumericDiffCostFunction(ConstraintBasePtr _ctr_ptr)
+{
+ switch (_ctr_ptr->getTypeId())
+ {
+ // just for testing
+ case CTR_ODOM_2D:
+ return createNumericDiffCostFunctionCeres<ConstraintOdom2D>(_ctr_ptr);
+
+ /* For adding a new constraint, add the #include and a case:
+ case CTR_ENUM:
+ return createNumericDiffCostFunctionCeres<ConstraintType>(_ctr_ptr);
+ */
+
+ default:
+ throw std::invalid_argument( "Unknown constraint type! Please add it in the file: ceres_wrapper/create_Numeric_diff_cost_function.h" );
+ }
+}
} // namespace wolf
diff --git a/src/ceres_wrapper/create_numeric_diff_cost_function_ceres.h b/src/ceres_wrapper/create_numeric_diff_cost_function_ceres.h
deleted file mode 100644
index 88876221a5289eeba7685e430982d765e9e6b5e2..0000000000000000000000000000000000000000
--- a/src/ceres_wrapper/create_numeric_diff_cost_function_ceres.h
+++ /dev/null
@@ -1,47 +0,0 @@
-/*
- * create_numric_cost_function_ceres.h
- *
- * Created on: Apr 5, 2016
- * Author: jvallve
- */
-
-#ifndef SRC_CERES_WRAPPER_CREATE_NUMERIC_DIFF_COST_FUNCTION_CERES_H_
-#define SRC_CERES_WRAPPER_CREATE_NUMERIC_DIFF_COST_FUNCTION_CERES_H_
-
-#include "ceres/numeric_diff_cost_function.h"
-
-namespace wolf {
-
-template <class CtrType>
-ceres::NumericDiffCostFunction<CtrType, ceres::CENTRAL,
- CtrType::residualSize,
- CtrType::block0Size,CtrType::block1Size,
- CtrType::block2Size,CtrType::block3Size,
- CtrType::block4Size,CtrType::block5Size,
- CtrType::block6Size,CtrType::block7Size,
- CtrType::block8Size,CtrType::block9Size>* createNumericDiffCostFunctionCeres(ConstraintBasePtr _constraint_ptr)
-{
- static_assert(CtrType::residualSize != 0,"Measurement size cannot be null!");
- static_assert(!(CtrType::block0Size == 0 ||
- (CtrType::block1Size > 0 && CtrType::block0Size == 0) ||
- (CtrType::block2Size > 0 && (CtrType::block0Size == 0 || CtrType::block1Size == 0)) ||
- (CtrType::block3Size > 0 && (CtrType::block0Size == 0 || CtrType::block1Size == 0 || CtrType::block2Size == 0)) ||
- (CtrType::block4Size > 0 && (CtrType::block0Size == 0 || CtrType::block1Size == 0 || CtrType::block2Size == 0 || CtrType::block3Size == 0)) ||
- (CtrType::block5Size > 0 && (CtrType::block0Size == 0 || CtrType::block1Size == 0 || CtrType::block2Size == 0 || CtrType::block3Size == 0 || CtrType::block4Size == 0)) ||
- (CtrType::block6Size > 0 && (CtrType::block0Size == 0 || CtrType::block1Size == 0 || CtrType::block2Size == 0 || CtrType::block3Size == 0 || CtrType::block4Size == 0 || CtrType::block5Size == 0)) ||
- (CtrType::block7Size > 0 && (CtrType::block0Size == 0 || CtrType::block1Size == 0 || CtrType::block2Size == 0 || CtrType::block3Size == 0 || CtrType::block4Size == 0 || CtrType::block5Size == 0 || CtrType::block6Size == 0)) ||
- (CtrType::block8Size > 0 && (CtrType::block0Size == 0 || CtrType::block1Size == 0 || CtrType::block2Size == 0 || CtrType::block3Size == 0 || CtrType::block4Size == 0 || CtrType::block5Size == 0 || CtrType::block6Size == 0 || CtrType::block7Size == 0)) ||
- (CtrType::block9Size > 0 && (CtrType::block0Size == 0 || CtrType::block1Size == 0 || CtrType::block2Size == 0 || CtrType::block3Size == 0 || CtrType::block4Size == 0 || CtrType::block5Size == 0 || CtrType::block6Size == 0 || CtrType::block7Size == 0 || CtrType::block8Size == 0))),
- "bad block sizes numbers!");
-
- return new ceres::NumericDiffCostFunction<CtrType, ceres::CENTRAL, CtrType::residualSize,
- CtrType::block0Size,CtrType::block1Size,CtrType::block2Size,CtrType::block3Size,CtrType::block4Size,
- CtrType::block5Size,CtrType::block6Size,CtrType::block7Size,CtrType::block8Size,CtrType::block9Size>(std::static_pointer_cast<CtrType>(_constraint_ptr).get()); // TODO revise pointer type
-};
-
-
-
-
-} // namespace wolf
-
-#endif /* SRC_CERES_WRAPPER_CREATE_NUMERIC_DIFF_COST_FUNCTION_CERES_H_ */
diff --git a/src/ceres_wrapper/qr_manager.cpp b/src/ceres_wrapper/qr_manager.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..e11b7369d8eb3176d992ddbe71ef1403838486ff
--- /dev/null
+++ b/src/ceres_wrapper/qr_manager.cpp
@@ -0,0 +1,232 @@
+/*
+ * qr_manager.cpp
+ *
+ * Created on: Jun 7, 2017
+ * Author: jvallve
+ */
+
+#include "qr_manager.h"
+
+namespace wolf {
+
+QRManager::QRManager(ProblemPtr _wolf_problem, const unsigned int& _N_batch) :
+ SolverManager(_wolf_problem),
+ A_(), // empty matrix
+ b_(),
+ any_state_block_removed_(false),
+ new_state_blocks_(0),
+ N_batch_(_N_batch),
+ pending_changes_(false)
+{
+ //
+}
+
+QRManager::~QRManager()
+{
+ sb_2_col_.clear();
+ ctr_2_row_.clear();
+}
+
+std::string QRManager::solve(const unsigned int& _report_level)
+{
+ // check for update notifications
+ update();
+
+ // Decomposition
+ if (!computeDecomposition())
+ return std::string("decomposition failed\n");
+
+ // Solve
+ Eigen::VectorXs x_incr = solver_.solve(-b_);
+ b_.setZero();
+
+ // 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()));
+
+ if (_report_level == 1)
+ return std::string("Success!\n");
+ else if (_report_level == 2)
+ return std::string("Success!\n");
+
+ return std::string();
+}
+
+void QRManager::computeCovariances(CovarianceBlocksToBeComputed _blocks)
+{
+ // TODO
+}
+
+void QRManager::computeCovariances(const StateBlockList& _sb_list)
+{
+ //std::cout << "computing covariances.." << std::endl;
+ update();
+ computeDecomposition();
+
+// std::cout << "R is " << solver_.matrixR().rows() << "x" << solver_.matrixR().cols() << std::endl;
+// std::cout << Eigen::MatrixXs(solver_.matrixR()) << std::endl;
+
+ covariance_solver_.compute(solver_.matrixR().topRows(solver_.matrixR().cols()));
+
+ Eigen::SparseMatrix<Scalar, Eigen::ColMajor> I(A_.cols(),A_.cols());
+ I.setIdentity();
+ Eigen::SparseMatrix<Scalar, Eigen::ColMajor> iR = covariance_solver_.solve(I);
+ Eigen::MatrixXs Sigma_full = solver_.colsPermutation() * iR * iR.transpose() * solver_.colsPermutation().transpose();
+
+// std::cout << "A' A = \n" << Eigen::MatrixXs(A_.transpose() * A_)<< std::endl;
+// std::cout << "P iR iR' P' = \n" << Eigen::MatrixXs(P * iR * iR.transpose() * P.transpose()) << std::endl;
+// std::cout << "Sigma * Lambda = \n" << Eigen::MatrixXs(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++)
+ {
+ //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()));
+ }
+}
+
+bool QRManager::computeDecomposition()
+{
+ if (pending_changes_)
+ {
+ // Rebuild problem
+ if (any_state_block_removed_)
+ {
+ // rebuild maps
+ unsigned int state_size = 0;
+ for (auto sb_pair : sb_2_col_)
+ {
+ sb_2_col_[sb_pair.first] = state_size;
+ state_size += sb_pair.first->getSize();
+ }
+
+ unsigned int meas_size = 0;
+ for (auto ctr_pair : ctr_2_row_)
+ {
+ ctr_2_row_[ctr_pair.first] = meas_size;
+ meas_size += ctr_pair.first->getSize();
+ }
+
+ // resize and setZero A, b
+ A_.resize(meas_size,state_size);
+ b_.resize(meas_size);
+ }
+
+ if (any_state_block_removed_ || new_state_blocks_ >= N_batch_)
+ {
+ // relinearize all constraints
+ for (auto ctr_pair : ctr_2_row_)
+ relinearizeConstraint(ctr_pair.first);
+
+ any_state_block_removed_ = false;
+ new_state_blocks_ = 0;
+ }
+
+ // Decomposition
+ solver_.compute(A_);
+ if (solver_.info() != Eigen::Success)
+ return false;
+ }
+
+ pending_changes_ = false;
+
+ return true;
+}
+
+void QRManager::addConstraint(ConstraintBasePtr _ctr_ptr)
+{
+ //std::cout << "add constraint " << _ctr_ptr->id() << std::endl;
+ assert(ctr_2_row_.find(_ctr_ptr) == ctr_2_row_.end() && "adding existing constraint");
+ ctr_2_row_[_ctr_ptr] = A_.rows();
+ A_.conservativeResize(A_.rows() + _ctr_ptr->getSize(), A_.cols());
+ b_.conservativeResize(b_.size() + _ctr_ptr->getSize());
+
+ assert(A_.rows() >= ctr_2_row_[_ctr_ptr] + _ctr_ptr->getSize() - 1 && "bad A number of rows");
+ assert(b_.rows() >= ctr_2_row_[_ctr_ptr] + _ctr_ptr->getSize() - 1 && "bad b number of rows");
+
+ relinearizeConstraint(_ctr_ptr);
+
+ pending_changes_ = true;
+}
+
+
+void QRManager::removeConstraint(ConstraintBasePtr _ctr_ptr)
+{
+ //std::cout << "remove constraint " << _ctr_ptr->id() << std::endl;
+ assert(ctr_2_row_.find(_ctr_ptr) != ctr_2_row_.end() && "removing unknown constraint");
+ eraseBlockRow(A_, ctr_2_row_[_ctr_ptr], _ctr_ptr->getSize());
+ b_.segment(ctr_2_row_[_ctr_ptr], _ctr_ptr->getSize()).setZero();
+ ctr_2_row_.erase(_ctr_ptr);
+ pending_changes_ = true;
+}
+
+void QRManager::addStateBlock(StateBlockPtr _st_ptr)
+{
+ //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());
+
+ new_state_blocks_++;
+ pending_changes_ = true;
+}
+
+void QRManager::removeStateBlock(StateBlockPtr _st_ptr)
+{
+ //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());
+
+ // flag to rebuild problem
+ any_state_block_removed_ = true;
+ // TODO: insert identity while problem is not re-built?
+
+ sb_2_col_.erase(_st_ptr);
+ pending_changes_ = true;
+}
+
+void QRManager::updateStateBlockStatus(StateBlockPtr _st_ptr)
+{
+ //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);
+ }
+ else
+ if (sb_2_col_.find(_st_ptr) == sb_2_col_.end())
+ addStateBlock(_st_ptr);
+}
+
+void QRManager::relinearizeConstraint(ConstraintBasePtr _ctr_ptr)
+{
+ // evaluate constraint
+ std::vector<const Scalar*> ctr_states_ptr;
+ for (auto sb : _ctr_ptr->getStateBlockPtrVector())
+ ctr_states_ptr.push_back(sb->getPtr());
+ Eigen::VectorXs residual(_ctr_ptr->getSize());
+ std::vector<Eigen::MatrixXs> jacobians;
+ _ctr_ptr->evaluate(ctr_states_ptr,residual,jacobians);
+
+ // Fill jacobians
+ Eigen::SparseMatrixs A_block_row(_ctr_ptr->getSize(), A_.cols());
+ for (auto i = 0; i < jacobians.size(); i++)
+ if (!_ctr_ptr->getStateBlockPtrVector()[i]->isFixed())
+ {
+ assert(sb_2_col_.find(_ctr_ptr->getStateBlockPtrVector()[i]) != sb_2_col_.end() && "constraint involving a state block not added");
+ assert(A_.cols() >= sb_2_col_[_ctr_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_[_ctr_ptr->getStateBlockPtrVector()[i]]);
+ }
+ assignBlockRow(A_, A_block_row, ctr_2_row_[_ctr_ptr]);
+
+ // Fill residual
+ b_.segment(ctr_2_row_[_ctr_ptr], _ctr_ptr->getSize()) = residual;
+}
+
+} /* namespace wolf */
diff --git a/src/ceres_wrapper/qr_manager.h b/src/ceres_wrapper/qr_manager.h
new file mode 100644
index 0000000000000000000000000000000000000000..ae7fae7a1fa475e7521bd94438e7e351be24d42b
--- /dev/null
+++ b/src/ceres_wrapper/qr_manager.h
@@ -0,0 +1,62 @@
+/*
+ * qr_manager.h
+ *
+ * Created on: Jun 7, 2017
+ * Author: jvallve
+ */
+
+#ifndef SRC_CERES_WRAPPER_QR_MANAGER_H_
+#define SRC_CERES_WRAPPER_QR_MANAGER_H_
+
+#include "solver_manager.h"
+#include "sparse_utils.h"
+
+namespace wolf
+{
+
+class QRManager : public SolverManager
+{
+ protected:
+ Eigen::SparseQR<Eigen::SparseMatrixs, Eigen::COLAMDOrdering<int>> solver_;
+ Eigen::SparseQR<Eigen::SparseMatrixs, Eigen::NaturalOrdering<int>> covariance_solver_;
+ Eigen::SparseMatrixs A_;
+ Eigen::VectorXs b_;
+ std::map<StateBlockPtr, unsigned int> sb_2_col_;
+ std::map<ConstraintBasePtr, unsigned int> ctr_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);
+
+ virtual ~QRManager();
+
+ virtual std::string solve(const unsigned int& _report_level);
+
+ virtual void computeCovariances(CovarianceBlocksToBeComputed _blocks = ROBOT_LANDMARKS);
+
+ virtual void computeCovariances(const StateBlockList& _sb_list);
+
+ private:
+
+ bool computeDecomposition();
+
+ virtual void addConstraint(ConstraintBasePtr _ctr_ptr);
+
+ virtual void removeConstraint(ConstraintBasePtr _ctr_ptr);
+
+ virtual void addStateBlock(StateBlockPtr _st_ptr);
+
+ virtual void removeStateBlock(StateBlockPtr _st_ptr);
+
+ virtual void updateStateBlockStatus(StateBlockPtr _st_ptr);
+
+ void relinearizeConstraint(ConstraintBasePtr _ctr_ptr);
+};
+
+} /* namespace wolf */
+
+#endif /* SRC_CERES_WRAPPER_QR_MANAGER_H_ */
diff --git a/src/ceres_wrapper/solver_manager.cpp b/src/ceres_wrapper/solver_manager.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..5b495d52fb23056c14cf308f8ceb8d69eade01f9
--- /dev/null
+++ b/src/ceres_wrapper/solver_manager.cpp
@@ -0,0 +1,93 @@
+#include "solver_manager.h"
+#include "../trajectory_base.h"
+#include "../map_base.h"
+#include "../landmark_base.h"
+
+namespace wolf {
+
+SolverManager::SolverManager(ProblemPtr _wolf_problem) :
+ wolf_problem_(_wolf_problem)
+{
+}
+
+SolverManager::~SolverManager()
+{
+}
+
+void SolverManager::update()
+{
+ //std::cout << "SolverManager: updating... " << std::endl;
+ //std::cout << wolf_problem_->getStateBlockNotificationList().size() << " state block notifications" << std::endl;
+ //std::cout << wolf_problem_->getConstraintNotificationList().size() << " constraint notifications" << std::endl;
+
+ // REMOVE CONSTRAINTS
+ auto ctr_notification_it = wolf_problem_->getConstraintNotificationList().begin();
+ while ( ctr_notification_it != wolf_problem_->getConstraintNotificationList().end() )
+ if (ctr_notification_it->notification_ == REMOVE)
+ {
+ removeConstraint(ctr_notification_it->constraint_ptr_);
+ ctr_notification_it = wolf_problem_->getConstraintNotificationList().erase(ctr_notification_it);
+ }
+ else
+ ctr_notification_it++;
+
+ // REMOVE STATE BLOCKS
+ auto state_notification_it = wolf_problem_->getStateBlockNotificationList().begin();
+ while ( state_notification_it != wolf_problem_->getStateBlockNotificationList().end() )
+ if (state_notification_it->notification_ == REMOVE)
+ {
+ removeStateBlock(state_notification_it->state_block_ptr_);
+ state_notification_it = wolf_problem_->getStateBlockNotificationList().erase(state_notification_it);
+ }
+ else
+ state_notification_it++;
+
+ // ADD/UPDATE STATE BLOCKS
+ while (!wolf_problem_->getStateBlockNotificationList().empty())
+ {
+ switch (wolf_problem_->getStateBlockNotificationList().front().notification_)
+ {
+ case ADD:
+ {
+ addStateBlock(wolf_problem_->getStateBlockNotificationList().front().state_block_ptr_);
+ if (wolf_problem_->getStateBlockNotificationList().front().state_block_ptr_->isFixed())
+ updateStateBlockStatus(wolf_problem_->getStateBlockNotificationList().front().state_block_ptr_);
+ break;
+ }
+ case UPDATE:
+ {
+ updateStateBlockStatus(wolf_problem_->getStateBlockNotificationList().front().state_block_ptr_);
+ break;
+ }
+ default:
+ throw std::runtime_error("SolverManager::update: State Block notification must be ADD, UPATE or REMOVE.");
+ }
+ wolf_problem_->getStateBlockNotificationList().pop_front();
+ }
+ // ADD CONSTRAINTS
+ while (!wolf_problem_->getConstraintNotificationList().empty())
+ {
+ switch (wolf_problem_->getConstraintNotificationList().front().notification_)
+ {
+ case ADD:
+ {
+ addConstraint(wolf_problem_->getConstraintNotificationList().front().constraint_ptr_);
+ break;
+ }
+ default:
+ throw std::runtime_error("SolverManager::update: Constraint notification must be ADD or REMOVE.");
+ }
+ wolf_problem_->getConstraintNotificationList().pop_front();
+ }
+
+ assert(wolf_problem_->getConstraintNotificationList().empty() && "wolf problem's constraints notification list not empty after update");
+ assert(wolf_problem_->getStateBlockNotificationList().empty() && "wolf problem's state_blocks notification list not empty after update");
+}
+
+wolf::ProblemPtr SolverManager::getProblemPtr()
+{
+ return wolf_problem_;
+}
+
+} // namespace wolf
+
diff --git a/src/ceres_wrapper/solver_manager.h b/src/ceres_wrapper/solver_manager.h
new file mode 100644
index 0000000000000000000000000000000000000000..f7725df4ab188d0ef9f15245ff7a5dbd48a9ec91
--- /dev/null
+++ b/src/ceres_wrapper/solver_manager.h
@@ -0,0 +1,64 @@
+#ifndef SOLVER_MANAGER_H_
+#define SOLVER_MANAGER_H_
+
+//wolf includes
+#include "../wolf.h"
+#include "../state_block.h"
+#include "../constraint_base.h"
+
+namespace wolf {
+
+/** \brief Enumeration of covariance blocks to be computed
+ *
+ * Enumeration of covariance blocks to be computed
+ *
+ */
+typedef enum
+{
+ ALL, ///< All blocks and all cross-covariances
+ ALL_MARGINALS, ///< All marginals
+ ROBOT_LANDMARKS ///< marginals of landmarks and current robot pose plus cross covariances of current robot and all landmarks
+} CovarianceBlocksToBeComputed;
+
+WOLF_PTR_TYPEDEFS(SolverManager);
+
+/** \brief Solver manager for WOLF
+ *
+ */
+
+class SolverManager
+{
+ protected:
+ ProblemPtr wolf_problem_;
+
+ public:
+ SolverManager(ProblemPtr _wolf_problem);
+
+ virtual ~SolverManager();
+
+ virtual std::string solve(const unsigned int& _report_level) = 0;
+
+ virtual void computeCovariances(CovarianceBlocksToBeComputed _blocks = ROBOT_LANDMARKS) = 0;
+
+ virtual void computeCovariances(const StateBlockList& st_list) = 0;
+
+ virtual void update();
+
+ virtual ProblemPtr getProblemPtr();
+
+ private:
+
+ virtual void addConstraint(ConstraintBasePtr _ctr_ptr) = 0;
+
+ virtual void removeConstraint(ConstraintBasePtr _ctr_ptr) = 0;
+
+ virtual void addStateBlock(StateBlockPtr _st_ptr) = 0;
+
+ virtual void removeStateBlock(StateBlockPtr _st_ptr) = 0;
+
+ virtual void updateStateBlockStatus(StateBlockPtr _st_ptr) = 0;
+};
+
+} // namespace wolf
+
+#endif
diff --git a/src/ceres_wrapper/sparse_utils.h b/src/ceres_wrapper/sparse_utils.h
new file mode 100644
index 0000000000000000000000000000000000000000..66f49b9c8f868e84f4223408ef6f85980e89bf1b
--- /dev/null
+++ b/src/ceres_wrapper/sparse_utils.h
@@ -0,0 +1,107 @@
+/*
+ * sparse_utils.h
+ *
+ * Created on: Jul 2, 2015
+ * Author: jvallve
+ */
+
+#ifndef SPARSE_UTILS_H_
+#define SPARSE_UTILS_H_
+
+// eigen includes
+//#include <eigen3/Eigen/Sparse>
+
+namespace wolf
+{
+
+void eraseBlockRow(Eigen::SparseMatrix<Scalar, Eigen::RowMajor>& A, const unsigned int& _row, const unsigned int& _n_rows)
+{
+ A.middleRows(_row,_n_rows) = Eigen::SparseMatrixs(_n_rows,A.cols());
+ A.makeCompressed();
+}
+
+void eraseBlockRow(Eigen::SparseMatrix<Scalar, Eigen::ColMajor>& A, const unsigned int& _row, const unsigned int& _n_rows)
+{
+ A.prune([&](int i, int, Scalar) { return i >= _row && i < _row + _n_rows; });
+ A.makeCompressed();
+}
+
+void eraseBlockCol(Eigen::SparseMatrix<Scalar, Eigen::ColMajor>& A, const unsigned int& _col, const unsigned int& _n_cols)
+{
+ A.middleCols(_col,_n_cols) = Eigen::SparseMatrixs(A.rows(),_n_cols);
+ A.makeCompressed();
+}
+
+void eraseBlockCol(Eigen::SparseMatrix<Scalar, Eigen::RowMajor>& A, const unsigned int& _col, const unsigned int& _n_cols)
+{
+ A.prune([&](int, int j, Scalar) { return j >= _col && j < _col + _n_cols; });
+ A.makeCompressed();
+}
+
+template<int _Options, typename _StorageIndex>
+void assignSparseBlock(const Eigen::MatrixXs& ins, Eigen::SparseMatrix<Scalar,_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.makeCompressed();
+}
+
+template<int _Options, typename _StorageIndex>
+void addSparseBlock(const Eigen::MatrixXs& ins, Eigen::SparseMatrix<Scalar,_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.makeCompressed();
+}
+
+template<int _Options, typename _StorageIndex>
+void insertSparseBlock(const Eigen::MatrixXs& ins, Eigen::SparseMatrix<Scalar,_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.makeCompressed();
+}
+
+void assignBlockRow(Eigen::SparseMatrix<Scalar, Eigen::RowMajor>& A, const Eigen::SparseMatrix<Scalar, Eigen::RowMajor>& ins, const unsigned int& _row)
+{
+ assert(A.rows() >= _row + ins.rows() && A.cols() == ins.cols());
+ A.middleRows(_row, ins.rows()) = ins;
+ A.makeCompressed();
+}
+
+Eigen::SparseMatrixs createBlockDiagonal(const std::vector<Eigen::MatrixXs>& _diag_blocs)
+{
+ unsigned int dim = _diag_blocs.front().rows();
+ unsigned int size = dim * _diag_blocs.size();
+
+ Eigen::SparseMatrixs M(size,size);
+
+ unsigned int pos = 0;
+ for (const Eigen::MatrixXs& omega_k : _diag_blocs)
+ {
+ insertSparseBlock(omega_k, M, pos, pos);
+ pos += dim;
+ }
+
+ M.makeCompressed();
+
+ return M;
+}
+
+template<int _Options, typename _StorageIndex>
+void getDiagonalBlocks(const Eigen::SparseMatrix<Scalar,_Options,_StorageIndex>& _M, std::vector<Eigen::MatrixXs>& 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::MatrixXs(_M.block(i,i,dim,dim)));
+}
+
+} // namespace wolf
+#endif /* SPARSE_UTILS_H_ */
diff --git a/src/constraint_AHP.h b/src/constraint_AHP.h
index f39c097b196d40a70d1151af5b7ff256c7d6850c..9af047fad2470d19fce4eb42adfc3d2f66bc1745 100644
--- a/src/constraint_AHP.h
+++ b/src/constraint_AHP.h
@@ -2,7 +2,7 @@
#define CONSTRAINT_AHP_H
//Wolf includes
-#include "constraint_sparse.h"
+#include "constraint_autodiff.h"
#include "landmark_AHP.h"
#include "sensor_camera.h"
#include "pinholeTools.h"
@@ -15,7 +15,7 @@ namespace wolf {
WOLF_PTR_TYPEDEFS(ConstraintAHP);
//class
-class ConstraintAHP : public ConstraintSparse<2, 3, 4, 3, 4, 4>
+class ConstraintAHP : public ConstraintAutodiff<ConstraintAHP, 2, 3, 4, 3, 4, 4>
{
protected:
Eigen::Vector3s anchor_sensor_extrinsics_p_;
@@ -70,7 +70,7 @@ inline ConstraintAHP::ConstraintAHP(const FeatureBasePtr& _ftr_ptr,
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function,
ConstraintStatus _status) :
- ConstraintSparse<2, 3, 4, 3, 4, 4>(CTR_AHP,
+ ConstraintAutodiff<ConstraintAHP, 2, 3, 4, 3, 4, 4>(CTR_AHP,
_landmark_ptr->getAnchorFrame(),
nullptr,
_landmark_ptr,
diff --git a/src/constraint_analytic.cpp b/src/constraint_analytic.cpp
index 87714278a757ecf03bcad1cebfb21a3f7f35472d..9fc6a2f8187d871bf9501e8ba583ce942ce5e42d 100644
--- a/src/constraint_analytic.cpp
+++ b/src/constraint_analytic.cpp
@@ -47,103 +47,19 @@ ConstraintAnalytic::ConstraintAnalytic(ConstraintType _tp, const LandmarkBasePtr
resizeVectors();
}
-const std::vector<Scalar*> ConstraintAnalytic::getStateScalarPtrVector()
+std::vector<Scalar*> ConstraintAnalytic::getStateScalarPtrVector() const
{
assert(state_ptr_vector_.size() > 0 && state_ptr_vector_.size() <= 10 && "Wrong state vector size in constraint, it should be between 1 and 10");
- switch (state_ptr_vector_.size())
- {
- case 1:
- {
- return std::vector<Scalar*>({state_ptr_vector_[0]->getPtr()});
- }
- case 2:
- {
- return std::vector<Scalar*>({state_ptr_vector_[0]->getPtr(),
- state_ptr_vector_[1]->getPtr()});
- }
- case 3:
- {
- return std::vector<Scalar*>({state_ptr_vector_[0]->getPtr(),
- state_ptr_vector_[1]->getPtr(),
- state_ptr_vector_[2]->getPtr()});
- }
- case 4:
- {
- return std::vector<Scalar*>({state_ptr_vector_[0]->getPtr(),
- state_ptr_vector_[1]->getPtr(),
- state_ptr_vector_[2]->getPtr(),
- state_ptr_vector_[3]->getPtr()});
- }
- case 5:
- {
- return std::vector<Scalar*>({state_ptr_vector_[0]->getPtr(),
- state_ptr_vector_[1]->getPtr(),
- state_ptr_vector_[2]->getPtr(),
- state_ptr_vector_[3]->getPtr(),
- state_ptr_vector_[4]->getPtr()});
- }
- case 6:
- {
- return std::vector<Scalar*>({state_ptr_vector_[0]->getPtr(),
- state_ptr_vector_[1]->getPtr(),
- state_ptr_vector_[2]->getPtr(),
- state_ptr_vector_[3]->getPtr(),
- state_ptr_vector_[4]->getPtr(),
- state_ptr_vector_[5]->getPtr()});
- }
- case 7:
- {
- return std::vector<Scalar*>({state_ptr_vector_[0]->getPtr(),
- state_ptr_vector_[1]->getPtr(),
- state_ptr_vector_[2]->getPtr(),
- state_ptr_vector_[3]->getPtr(),
- state_ptr_vector_[4]->getPtr(),
- state_ptr_vector_[5]->getPtr(),
- state_ptr_vector_[6]->getPtr()});
- }
- case 8:
- {
- return std::vector<Scalar*>({state_ptr_vector_[0]->getPtr(),
- state_ptr_vector_[1]->getPtr(),
- state_ptr_vector_[2]->getPtr(),
- state_ptr_vector_[3]->getPtr(),
- state_ptr_vector_[4]->getPtr(),
- state_ptr_vector_[5]->getPtr(),
- state_ptr_vector_[6]->getPtr(),
- state_ptr_vector_[7]->getPtr()});
- }
- case 9:
- {
- return std::vector<Scalar*>({state_ptr_vector_[0]->getPtr(),
- state_ptr_vector_[1]->getPtr(),
- state_ptr_vector_[2]->getPtr(),
- state_ptr_vector_[3]->getPtr(),
- state_ptr_vector_[4]->getPtr(),
- state_ptr_vector_[5]->getPtr(),
- state_ptr_vector_[6]->getPtr(),
- state_ptr_vector_[7]->getPtr(),
- state_ptr_vector_[8]->getPtr()});
- }
- case 10:
- {
- return std::vector<Scalar*>({state_ptr_vector_[0]->getPtr(),
- state_ptr_vector_[1]->getPtr(),
- state_ptr_vector_[2]->getPtr(),
- state_ptr_vector_[3]->getPtr(),
- state_ptr_vector_[4]->getPtr(),
- state_ptr_vector_[5]->getPtr(),
- state_ptr_vector_[6]->getPtr(),
- state_ptr_vector_[7]->getPtr(),
- state_ptr_vector_[8]->getPtr(),
- state_ptr_vector_[9]->getPtr()});
- }
- }
+ std::vector<Scalar*> state_scalar_ptr_vector(state_ptr_vector_.size());
+
+ for (auto i = 0; i < state_scalar_ptr_vector.size(); i++)
+ state_scalar_ptr_vector[i] = state_ptr_vector_[i]->getPtr();
- return std::vector<Scalar*>(0); //Not going to happen
+ return state_scalar_ptr_vector;
}
-const std::vector<StateBlockPtr> ConstraintAnalytic::getStateBlockPtrVector() const
+std::vector<StateBlockPtr> ConstraintAnalytic::getStateBlockPtrVector() const
{
return state_ptr_vector_;
}
diff --git a/src/constraint_analytic.h b/src/constraint_analytic.h
index 6c9ddc7f95ea104c93a9383f4425e0d159fcf8d0..279394b652498a4001579516e507159bd6fa3205 100644
--- a/src/constraint_analytic.h
+++ b/src/constraint_analytic.h
@@ -103,21 +103,21 @@ class ConstraintAnalytic: public ConstraintBase
* Returns a vector of pointers to the state blocks in which this constraint depends
*
**/
- virtual const std::vector<Scalar*> getStateScalarPtrVector() override;
+ virtual std::vector<Scalar*> getStateScalarPtrVector() const override;
/** \brief Returns a vector of pointers to the states
*
* Returns a vector of pointers to the state in which this constraint depends
*
**/
- virtual const std::vector<StateBlockPtr> getStateBlockPtrVector() const override;
+ virtual std::vector<StateBlockPtr> getStateBlockPtrVector() const override;
/** \brief Returns a vector of sizes of the state blocks
*
* Returns a vector of sizes of the state blocks
*
**/
- virtual std::vector<unsigned int> getStateSizes() const;
+ virtual std::vector<unsigned int> getStateSizes() const override;
/** \brief Returns the constraint residual size
*
@@ -126,6 +126,50 @@ class ConstraintAnalytic: public ConstraintBase
**/
// virtual unsigned int getSize() const = 0;
+ /** \brief Evaluate the constraint given the input parameters and returning the residuals and jacobians
+ **/
+ // TODO
+ virtual bool evaluate(double const* const* parameters, double* residuals, double** jacobians) const
+ {
+ // load parameters evaluation value
+ std::vector<Eigen::Map<const Eigen::VectorXs>> state_blocks_map_;
+ for (unsigned int i = 0; i < state_block_sizes_vector_.size(); i++)
+ state_blocks_map_.push_back(Eigen::Map<const Eigen::VectorXs>((Scalar*)parameters[i], state_block_sizes_vector_[i]));
+
+ // residuals
+ Eigen::Map<Eigen::VectorXs> residuals_map((Scalar*)residuals, getSize());
+ residuals_map = evaluateResiduals(state_blocks_map_);
+
+ // also compute jacobians
+ if (jacobians != nullptr)
+ {
+ std::vector<Eigen::Map<Eigen::MatrixXs>> jacobians_map_;
+ std::vector<bool> compute_jacobians_(state_block_sizes_vector_.size());
+
+ for (unsigned int i = 0; i < state_block_sizes_vector_.size(); i++)
+ {
+ compute_jacobians_[i] = (jacobians[i] != nullptr);
+ if (jacobians[i] != nullptr)
+ jacobians_map_.push_back(Eigen::Map<Eigen::MatrixXs>((Scalar*)jacobians[i], getSize(), state_block_sizes_vector_[i]));
+ else
+ jacobians_map_.push_back(Eigen::Map<Eigen::MatrixXs>(nullptr, 0, 0)); //TODO: check if it can be done
+ }
+
+ // evaluate jacobians
+ evaluateJacobians(state_blocks_map_, jacobians_map_, compute_jacobians_);
+ }
+ return true;
+
+ return true;
+ };
+
+ /** Returns the residual vetor and a vector of Jacobian matrix corresponding to each state block evaluated in the point provided in _states_ptr
+ **/
+ // TODO
+ virtual void evaluate(const std::vector<const Scalar*>& _states_ptr, Eigen::VectorXs& residual_, std::vector<Eigen::MatrixXs>& jacobians_) const
+ {
+ };
+
/** \brief Returns the residual evaluated in the states provided
*
* Returns the residual evaluated in the states provided in std::vector of mapped Eigen::VectorXs
diff --git a/src/constraint_autodiff.h b/src/constraint_autodiff.h
new file mode 100644
index 0000000000000000000000000000000000000000..3e00d4d4c7a2d8a365ca2516c90d1025c14b5004
--- /dev/null
+++ b/src/constraint_autodiff.h
@@ -0,0 +1,2392 @@
+
+#ifndef CONSTRAINT_AUTODIFF_H_
+#define CONSTRAINT_AUTODIFF_H_
+
+//Wolf includes
+#include "constraint_base.h"
+#include "state_block.h"
+
+// CERES
+#include "ceres/jet.h"
+
+// GENERAL
+#include <array>
+
+namespace wolf {
+
+//template class ConstraintAutodiff
+template <class CtrT, 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>
+class ConstraintAutodiff: public ConstraintBase
+{
+ public:
+
+ static const unsigned int residualSize = RES;
+ static const unsigned int block0Size = B0;
+ static const unsigned int block1Size = B1;
+ static const unsigned int block2Size = B2;
+ static const unsigned int block3Size = B3;
+ static const unsigned int block4Size = B4;
+ static const unsigned int block5Size = B5;
+ static const unsigned int block6Size = B6;
+ static const unsigned int block7Size = B7;
+ static const unsigned int block8Size = B8;
+ static const unsigned int block9Size = B9;
+ static const unsigned int n_blocks = 10;
+
+ static const std::vector<unsigned int> state_block_sizes_;
+
+ typedef ceres::Jet<Scalar, B0 + B1 + B2 + B3 + B4 +
+ B5 + B6 + B7 + B8 + B9> WolfJet;
+
+ protected:
+
+ std::vector<StateBlockPtr> state_ptrs_;
+
+ static const std::vector<unsigned int> jacobian_locations_;
+ std::array<WolfJet, RES>* residuals_jets_;
+ std::array<WolfJet, B0>* jets_0_;
+ std::array<WolfJet, B1>* jets_1_;
+ std::array<WolfJet, B2>* jets_2_;
+ std::array<WolfJet, B3>* jets_3_;
+ std::array<WolfJet, B4>* jets_4_;
+ std::array<WolfJet, B5>* jets_5_;
+ std::array<WolfJet, B6>* jets_6_;
+ std::array<WolfJet, B7>* jets_7_;
+ std::array<WolfJet, B8>* jets_8_;
+ std::array<WolfJet, B9>* jets_9_;
+
+ public:
+ /** \brief Constructor valid for all categories (ABSOLUTE, FRAME, FEATURE, LANDMARK)
+ **/
+ ConstraintAutodiff(ConstraintType _tp,
+ const FrameBasePtr& _frame_other_ptr,
+ const FeatureBasePtr& _feature_other_ptr,
+ const LandmarkBasePtr& _landmark_other_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ ConstraintStatus _status,
+ StateBlockPtr _state0Ptr,
+ StateBlockPtr _state1Ptr,
+ StateBlockPtr _state2Ptr,
+ StateBlockPtr _state3Ptr,
+ StateBlockPtr _state4Ptr,
+ StateBlockPtr _state5Ptr,
+ StateBlockPtr _state6Ptr,
+ StateBlockPtr _state7Ptr,
+ StateBlockPtr _state8Ptr,
+ StateBlockPtr _state9Ptr) :
+ ConstraintBase(_tp, _frame_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
+ state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr,_state5Ptr,_state6Ptr,_state7Ptr,_state8Ptr,_state9Ptr}),
+ residuals_jets_(new std::array<WolfJet, RES>),
+ jets_0_(new std::array<WolfJet, B0>),
+ jets_1_(new std::array<WolfJet, B1>),
+ jets_2_(new std::array<WolfJet, B2>),
+ jets_3_(new std::array<WolfJet, B3>),
+ jets_4_(new std::array<WolfJet, B4>),
+ jets_5_(new std::array<WolfJet, B5>),
+ jets_6_(new std::array<WolfJet, B6>),
+ jets_7_(new std::array<WolfJet, B7>),
+ jets_8_(new std::array<WolfJet, B8>),
+ jets_9_(new std::array<WolfJet, B9>)
+ {
+ // initialize jets
+ unsigned int last_jet_idx = 0;
+ for (auto i = 0; i < B0; i++)
+ (*jets_0_)[i] = WolfJet(0, last_jet_idx++);
+ for (auto i = 0; i < B1; i++)
+ (*jets_1_)[i] = WolfJet(0, last_jet_idx++);
+ for (auto i = 0; i < B2; i++)
+ (*jets_2_)[i] = WolfJet(0, last_jet_idx++);
+ for (auto i = 0; i < B3; i++)
+ (*jets_3_)[i] = WolfJet(0, last_jet_idx++);
+ for (auto i = 0; i < B4; i++)
+ (*jets_4_)[i] = WolfJet(0, last_jet_idx++);
+ for (auto i = 0; i < B5; i++)
+ (*jets_5_)[i] = WolfJet(0, last_jet_idx++);
+ for (auto i = 0; i < B6; i++)
+ (*jets_6_)[i] = WolfJet(0, last_jet_idx++);
+ for (auto i = 0; i < B7; i++)
+ (*jets_7_)[i] = WolfJet(0, last_jet_idx++);
+ for (auto i = 0; i < B8; i++)
+ (*jets_8_)[i] = WolfJet(0, last_jet_idx++);
+ for (auto i = 0; i < B9; i++)
+ (*jets_9_)[i] = WolfJet(0, last_jet_idx++);
+ }
+
+ virtual ~ConstraintAutodiff()
+ {
+ delete jets_0_;
+ delete jets_1_;
+ delete jets_2_;
+ delete jets_3_;
+ delete jets_4_;
+ delete jets_5_;
+ delete jets_6_;
+ delete jets_7_;
+ delete jets_8_;
+ delete jets_9_;
+ delete residuals_jets_;
+ }
+
+ virtual JacobianMethod getJacobianMethod() const
+ {
+ return JAC_AUTO;
+ }
+
+ /** \brief Returns the residual and jacobians given the state values
+ *
+ * Returns the residual and jacobians given the state values
+ *
+ **/
+ virtual bool evaluate(double const* const* parameters, double* residuals, double** jacobians) const
+ {
+ // only residuals
+ if (jacobians == nullptr)
+ {
+ (*static_cast<CtrT const*>(this))(parameters[0],
+ parameters[1],
+ parameters[2],
+ parameters[3],
+ parameters[4],
+ parameters[5],
+ parameters[6],
+ parameters[7],
+ parameters[8],
+ parameters[9],
+ residuals);
+ }
+ // also compute jacobians
+ else
+ {
+ // update jets real part
+ std::vector<double const*> param_vec;
+ param_vec.assign(parameters,parameters+n_blocks);
+ updateJetsRealPart(param_vec);
+
+ // call functor
+ (*static_cast<CtrT const*>(this))(jets_0_->data(),
+ jets_1_->data(),
+ jets_2_->data(),
+ jets_3_->data(),
+ jets_4_->data(),
+ jets_5_->data(),
+ jets_6_->data(),
+ jets_7_->data(),
+ jets_8_->data(),
+ jets_9_->data(),
+ residuals_jets_->data());
+
+ // fill the residual array
+ for (auto i = 0; i < RES; i++)
+ residuals[i] = (*residuals_jets_)[i].a;
+
+ // fill the jacobian matrices
+ for (auto i = 0; i<n_blocks; i++)
+ if (jacobians[i] != nullptr)
+ for (unsigned int row = 0; row < RES; row++)
+ std::copy((*residuals_jets_)[row].v.data() + jacobian_locations_.at(i),
+ (*residuals_jets_)[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
+ jacobians[i] + row * state_block_sizes_.at(i));
+ }
+ return true;
+ }
+
+ /** \brief Updates all jets real part with values of parameters
+ *
+ **/
+ void updateJetsRealPart(const std::vector<double const*>& parameters) const
+ {
+ // update jets real part
+ for (auto i = 0; i < B0; i++)
+ (*jets_0_)[i].a = parameters[0][i];
+ for (auto i = 0; i < B1; i++)
+ (*jets_1_)[i].a = parameters[1][i];
+ for (auto i = 0; i < B2; i++)
+ (*jets_2_)[i].a = parameters[2][i];
+ for (auto i = 0; i < B3; i++)
+ (*jets_3_)[i].a = parameters[3][i];
+ for (auto i = 0; i < B4; i++)
+ (*jets_4_)[i].a = parameters[4][i];
+ for (auto i = 0; i < B5; i++)
+ (*jets_5_)[i].a = parameters[5][i];
+ for (auto i = 0; i < B6; i++)
+ (*jets_6_)[i].a = parameters[6][i];
+ for (auto i = 0; i < B7; i++)
+ (*jets_7_)[i].a = parameters[7][i];
+ for (auto i = 0; i < B8; i++)
+ (*jets_8_)[i].a = parameters[8][i];
+ for (auto i = 0; i < B9; i++)
+ (*jets_9_)[i].a = parameters[9][i];
+ }
+
+ /** \brief Returns a vector of Jacobian matrix corresponding to each state block evaluated in the point provided in _states_ptr
+ *
+ **/
+ virtual void evaluate(const std::vector<const Scalar*>& _states_ptr, Eigen::VectorXs& residual_, std::vector<Eigen::MatrixXs>& jacobians_) const
+ {
+ assert(residual_.size() == RES);
+ jacobians_.clear();
+
+ assert(_states_ptr.size() == n_blocks);
+
+ // init jacobian
+ for(auto i = 0; i < n_blocks; ++i)
+ {
+ Eigen::MatrixXs Ji = Eigen::MatrixXs::Zero(RES, state_block_sizes_[i]);
+ jacobians_.push_back(Ji);
+ }
+
+ // update jets real part
+ updateJetsRealPart(_states_ptr);
+
+ // call functor
+ (*static_cast<CtrT const*>(this))(jets_0_->data(),
+ jets_1_->data(),
+ jets_2_->data(),
+ jets_3_->data(),
+ jets_4_->data(),
+ jets_5_->data(),
+ jets_6_->data(),
+ jets_7_->data(),
+ jets_8_->data(),
+ jets_9_->data(),
+ residuals_jets_->data());
+
+ // fill the residual vector
+ for (auto i = 0; i < RES; i++)
+ residual_(i) = (*residuals_jets_)[i].a;
+
+ // fill the jacobian matrices
+ for (auto i = 0; i<n_blocks; i++)
+ if (!state_ptrs_[i]->isFixed())
+ for (unsigned int row = 0; row < RES; row++)
+ std::copy((*residuals_jets_)[row].v.data() + jacobian_locations_.at(i),
+ (*residuals_jets_)[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
+ jacobians_[i].data() + row * state_block_sizes_.at(i));
+
+ // print jacobian matrices
+// for (auto i = 0; i < n_blocks; i++)
+// std::cout << jacobians_[i] << std::endl << std::endl;
+ }
+
+ /** \brief Returns a vector of pointers to the state blocks
+ *
+ * Returns a vector of pointers to the state blocks in which this constraint depends
+ *
+ **/
+ virtual std::vector<Scalar*> getStateScalarPtrVector() const
+ {
+ return std::vector<Scalar*>({state_ptrs_[0]->getPtr(),
+ state_ptrs_[1]->getPtr(),
+ state_ptrs_[2]->getPtr(),
+ state_ptrs_[3]->getPtr(),
+ state_ptrs_[4]->getPtr(),
+ state_ptrs_[5]->getPtr(),
+ state_ptrs_[6]->getPtr(),
+ state_ptrs_[7]->getPtr(),
+ state_ptrs_[8]->getPtr(),
+ state_ptrs_[9]->getPtr()
+ });
+ }
+
+ /** \brief Returns a vector of pointers to the states
+ *
+ * Returns a vector of pointers to the state in which this constraint depends
+ *
+ **/
+ virtual std::vector<StateBlockPtr> getStateBlockPtrVector() const
+ {
+ return state_ptrs_;
+ }
+
+ /** \brief Returns a vector of the states sizes
+ *
+ **/
+ virtual std::vector<unsigned int> getStateSizes() const
+ {
+ return state_block_sizes_;
+ }
+
+ /** \brief Returns the residual size
+ *
+ * Returns the residual size
+ *
+ **/
+ virtual unsigned int getSize() const
+ {
+ return RES;
+ }
+};
+
+
+////////////////// SPECIALIZATION 9 BLOCKS ////////////////////////////////////////////////////////////////////////
+
+template <class CtrT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6,unsigned int B7,unsigned int B8>
+class ConstraintAutodiff<CtrT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,0> : public ConstraintBase
+{
+ public:
+
+ static const unsigned int residualSize = RES;
+ static const unsigned int block0Size = B0;
+ static const unsigned int block1Size = B1;
+ static const unsigned int block2Size = B2;
+ static const unsigned int block3Size = B3;
+ static const unsigned int block4Size = B4;
+ static const unsigned int block5Size = B5;
+ static const unsigned int block6Size = B6;
+ static const unsigned int block7Size = B7;
+ static const unsigned int block8Size = B8;
+ static const unsigned int block9Size = 0;
+ static const unsigned int n_blocks = 9;
+
+ static const std::vector<unsigned int> state_block_sizes_;
+
+ typedef ceres::Jet<Scalar, B0 + B1 + B2 + B3 + B4 +
+ B5 + B6 + B7 + B8> WolfJet;
+
+ protected:
+
+ std::vector<StateBlockPtr> state_ptrs_;
+
+ static const std::vector<unsigned int> jacobian_locations_;
+ std::array<WolfJet, RES>* residuals_jets_;
+
+ std::array<WolfJet, B0>* jets_0_;
+ std::array<WolfJet, B1>* jets_1_;
+ std::array<WolfJet, B2>* jets_2_;
+ std::array<WolfJet, B3>* jets_3_;
+ std::array<WolfJet, B4>* jets_4_;
+ std::array<WolfJet, B5>* jets_5_;
+ std::array<WolfJet, B6>* jets_6_;
+ std::array<WolfJet, B7>* jets_7_;
+ std::array<WolfJet, B8>* jets_8_;
+
+ public:
+
+ ConstraintAutodiff(ConstraintType _tp,
+ const FrameBasePtr& _frame_other_ptr,
+ const FeatureBasePtr& _feature_other_ptr,
+ const LandmarkBasePtr& _landmark_other_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ ConstraintStatus _status,
+ StateBlockPtr _state0Ptr,
+ StateBlockPtr _state1Ptr,
+ StateBlockPtr _state2Ptr,
+ StateBlockPtr _state3Ptr,
+ StateBlockPtr _state4Ptr,
+ StateBlockPtr _state5Ptr,
+ StateBlockPtr _state6Ptr,
+ StateBlockPtr _state7Ptr,
+ StateBlockPtr _state8Ptr) :
+ ConstraintBase(_tp, _frame_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
+ state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr,_state5Ptr,_state6Ptr,_state7Ptr,_state8Ptr}),
+ residuals_jets_(new std::array<WolfJet, RES>),
+ jets_0_(new std::array<WolfJet, B0>),
+ jets_1_(new std::array<WolfJet, B1>),
+ jets_2_(new std::array<WolfJet, B2>),
+ jets_3_(new std::array<WolfJet, B3>),
+ jets_4_(new std::array<WolfJet, B4>),
+ jets_5_(new std::array<WolfJet, B5>),
+ jets_6_(new std::array<WolfJet, B6>),
+ jets_7_(new std::array<WolfJet, B7>),
+ jets_8_(new std::array<WolfJet, B8>)
+ {
+ // initialize jets
+ unsigned int last_jet_idx = 0;
+ for (auto i = 0; i < B0; i++)
+ (*jets_0_)[i] = WolfJet(0, last_jet_idx++);
+ for (auto i = 0; i < B1; i++)
+ (*jets_1_)[i] = WolfJet(0, last_jet_idx++);
+ for (auto i = 0; i < B2; i++)
+ (*jets_2_)[i] = WolfJet(0, last_jet_idx++);
+ for (auto i = 0; i < B3; i++)
+ (*jets_3_)[i] = WolfJet(0, last_jet_idx++);
+ for (auto i = 0; i < B4; i++)
+ (*jets_4_)[i] = WolfJet(0, last_jet_idx++);
+ for (auto i = 0; i < B5; i++)
+ (*jets_5_)[i] = WolfJet(0, last_jet_idx++);
+ for (auto i = 0; i < B6; i++)
+ (*jets_6_)[i] = WolfJet(0, last_jet_idx++);
+ for (auto i = 0; i < B7; i++)
+ (*jets_7_)[i] = WolfJet(0, last_jet_idx++);
+ for (auto i = 0; i < B8; i++)
+ (*jets_8_)[i] = WolfJet(0, last_jet_idx++);
+ state_ptrs_.resize(n_blocks);
+ }
+
+ virtual ~ConstraintAutodiff()
+ {
+ delete jets_0_;
+ delete jets_1_;
+ delete jets_2_;
+ delete jets_3_;
+ delete jets_4_;
+ delete jets_5_;
+ delete jets_6_;
+ delete jets_7_;
+ delete jets_8_;
+ delete residuals_jets_;
+ }
+
+ virtual JacobianMethod getJacobianMethod() const
+ {
+ return JAC_AUTO;
+ }
+
+ virtual bool evaluate(double const* const* parameters, double* residuals, double** jacobians) const
+ {
+ // only residuals
+ if (jacobians == nullptr)
+ {
+ (*static_cast<CtrT const*>(this))(parameters[0],
+ parameters[1],
+ parameters[2],
+ parameters[3],
+ parameters[4],
+ parameters[5],
+ parameters[6],
+ parameters[7],
+ parameters[8],
+ residuals);
+ }
+ // also compute jacobians
+ else
+ {
+ // update jets real part
+ std::vector<double const*> param_vec;
+ param_vec.assign(parameters,parameters+n_blocks);
+ updateJetsRealPart(param_vec);
+
+ // call functor
+ (*static_cast<CtrT const*>(this))(jets_0_->data(),
+ jets_1_->data(),
+ jets_2_->data(),
+ jets_3_->data(),
+ jets_4_->data(),
+ jets_5_->data(),
+ jets_6_->data(),
+ jets_7_->data(),
+ jets_8_->data(),
+ residuals_jets_->data());
+
+ // fill the residual array
+ for (auto i = 0; i < RES; i++)
+ residuals[i] = (*residuals_jets_)[i].a;
+
+ // fill the jacobian matrices
+ for (auto i = 0; i<n_blocks; i++)
+ if (jacobians[i] != nullptr)
+ for (unsigned int row = 0; row < RES; row++)
+ std::copy((*residuals_jets_)[row].v.data() + jacobian_locations_.at(i),
+ (*residuals_jets_)[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
+ jacobians[i] + row * state_block_sizes_.at(i));
+ }
+ return true;
+ }
+
+ void updateJetsRealPart(const std::vector<double const*>& parameters) const
+ {
+ // update jets real part
+ for (auto i = 0; i < B0; i++)
+ (*jets_0_)[i].a = parameters[0][i];
+ for (auto i = 0; i < B1; i++)
+ (*jets_1_)[i].a = parameters[1][i];
+ for (auto i = 0; i < B2; i++)
+ (*jets_2_)[i].a = parameters[2][i];
+ for (auto i = 0; i < B3; i++)
+ (*jets_3_)[i].a = parameters[3][i];
+ for (auto i = 0; i < B4; i++)
+ (*jets_4_)[i].a = parameters[4][i];
+ for (auto i = 0; i < B5; i++)
+ (*jets_5_)[i].a = parameters[5][i];
+ for (auto i = 0; i < B6; i++)
+ (*jets_6_)[i].a = parameters[6][i];
+ for (auto i = 0; i < B7; i++)
+ (*jets_7_)[i].a = parameters[7][i];
+ for (auto i = 0; i < B8; i++)
+ (*jets_8_)[i].a = parameters[8][i];
+ }
+
+ virtual void evaluate(const std::vector<const Scalar*>& _states_ptr, Eigen::VectorXs& residual_, std::vector<Eigen::MatrixXs>& jacobians_) const
+ {
+ assert(residual_.size() == RES);
+ jacobians_.clear();
+
+ assert(_states_ptr.size() == n_blocks);
+
+ // init jacobian
+ for(auto i = 0; i < n_blocks; ++i)
+ {
+ Eigen::MatrixXs Ji = Eigen::MatrixXs::Zero(RES, state_block_sizes_[i]);
+ jacobians_.push_back(Ji);
+ }
+
+ // update jets real part
+ updateJetsRealPart(_states_ptr);
+
+ // call functor
+ (*static_cast<CtrT const*>(this))(jets_0_->data(),
+ jets_1_->data(),
+ jets_2_->data(),
+ jets_3_->data(),
+ jets_4_->data(),
+ jets_5_->data(),
+ jets_6_->data(),
+ jets_7_->data(),
+ jets_8_->data(),
+ residuals_jets_->data());
+
+ // fill the residual vector
+ for (auto i = 0; i < RES; i++)
+ residual_(i) = (*residuals_jets_)[i].a;
+
+ // fill the jacobian matrices
+ for (auto i = 0; i<n_blocks; i++)
+ if (!state_ptrs_[i]->isFixed())
+ for (unsigned int row = 0; row < RES; row++)
+ std::copy((*residuals_jets_)[row].v.data() + jacobian_locations_.at(i),
+ (*residuals_jets_)[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
+ jacobians_[i].data() + row * state_block_sizes_.at(i));
+
+ // print jacobian matrices
+// for (auto i = 0; i < n_blocks; i++)
+// std::cout << jacobians_[i] << std::endl << std::endl;
+ }
+
+ virtual std::vector<Scalar*> getStateScalarPtrVector() const
+ {
+ return std::vector<Scalar*>({state_ptrs_[0]->getPtr(),
+ state_ptrs_[1]->getPtr(),
+ state_ptrs_[2]->getPtr(),
+ state_ptrs_[3]->getPtr(),
+ state_ptrs_[4]->getPtr(),
+ state_ptrs_[5]->getPtr(),
+ state_ptrs_[6]->getPtr(),
+ state_ptrs_[7]->getPtr(),
+ state_ptrs_[8]->getPtr()
+ });
+ }
+
+ virtual std::vector<StateBlockPtr> getStateBlockPtrVector() const
+ {
+ return state_ptrs_;
+ }
+
+ virtual std::vector<unsigned int> getStateSizes() const
+ {
+ return state_block_sizes_;
+ }
+
+ virtual unsigned int getSize() const
+ {
+ return RES;
+ }
+};
+
+////////////////// SPECIALIZATION 8 BLOCKS ////////////////////////////////////////////////////////////////////////
+
+template <class CtrT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6,unsigned int B7>
+class ConstraintAutodiff<CtrT,RES,B0,B1,B2,B3,B4,B5,B6,B7,0,0> : public ConstraintBase
+{
+ public:
+
+ static const unsigned int residualSize = RES;
+ static const unsigned int block0Size = B0;
+ static const unsigned int block1Size = B1;
+ static const unsigned int block2Size = B2;
+ static const unsigned int block3Size = B3;
+ static const unsigned int block4Size = B4;
+ static const unsigned int block5Size = B5;
+ static const unsigned int block6Size = B6;
+ static const unsigned int block7Size = B7;
+ static const unsigned int block8Size = 0;
+ static const unsigned int block9Size = 0;
+ static const unsigned int n_blocks = 8;
+
+ static const std::vector<unsigned int> state_block_sizes_;
+
+ typedef ceres::Jet<Scalar, B0 + B1 + B2 + B3 + B4 +
+ B5 + B6 + B7> WolfJet;
+
+ protected:
+
+ std::vector<StateBlockPtr> state_ptrs_;
+
+ static const std::vector<unsigned int> jacobian_locations_;
+ std::array<WolfJet, RES>* residuals_jets_;
+
+ std::array<WolfJet, B0>* jets_0_;
+ std::array<WolfJet, B1>* jets_1_;
+ std::array<WolfJet, B2>* jets_2_;
+ std::array<WolfJet, B3>* jets_3_;
+ std::array<WolfJet, B4>* jets_4_;
+ std::array<WolfJet, B5>* jets_5_;
+ std::array<WolfJet, B6>* jets_6_;
+ std::array<WolfJet, B7>* jets_7_;
+
+ public:
+
+ ConstraintAutodiff(ConstraintType _tp,
+ const FrameBasePtr& _frame_other_ptr,
+ const FeatureBasePtr& _feature_other_ptr,
+ const LandmarkBasePtr& _landmark_other_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ ConstraintStatus _status,
+ StateBlockPtr _state0Ptr,
+ StateBlockPtr _state1Ptr,
+ StateBlockPtr _state2Ptr,
+ StateBlockPtr _state3Ptr,
+ StateBlockPtr _state4Ptr,
+ StateBlockPtr _state5Ptr,
+ StateBlockPtr _state6Ptr,
+ StateBlockPtr _state7Ptr) :
+ ConstraintBase(_tp, _frame_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
+ state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr,_state5Ptr,_state6Ptr,_state7Ptr}),
+ residuals_jets_(new std::array<WolfJet, RES>),
+ jets_0_(new std::array<WolfJet, B0>),
+ jets_1_(new std::array<WolfJet, B1>),
+ jets_2_(new std::array<WolfJet, B2>),
+ jets_3_(new std::array<WolfJet, B3>),
+ jets_4_(new std::array<WolfJet, B4>),
+ jets_5_(new std::array<WolfJet, B5>),
+ jets_6_(new std::array<WolfJet, B6>),
+ jets_7_(new std::array<WolfJet, B7>)
+ {
+ // initialize jets
+ unsigned int last_jet_idx = 0;
+ for (auto i = 0; i < B0; i++)
+ (*jets_0_)[i] = WolfJet(0, last_jet_idx++);
+ for (auto i = 0; i < B1; i++)
+ (*jets_1_)[i] = WolfJet(0, last_jet_idx++);
+ for (auto i = 0; i < B2; i++)
+ (*jets_2_)[i] = WolfJet(0, last_jet_idx++);
+ for (auto i = 0; i < B3; i++)
+ (*jets_3_)[i] = WolfJet(0, last_jet_idx++);
+ for (auto i = 0; i < B4; i++)
+ (*jets_4_)[i] = WolfJet(0, last_jet_idx++);
+ for (auto i = 0; i < B5; i++)
+ (*jets_5_)[i] = WolfJet(0, last_jet_idx++);
+ for (auto i = 0; i < B6; i++)
+ (*jets_6_)[i] = WolfJet(0, last_jet_idx++);
+ for (auto i = 0; i < B7; i++)
+ (*jets_7_)[i] = WolfJet(0, last_jet_idx++);
+ state_ptrs_.resize(n_blocks);
+ }
+
+ virtual ~ConstraintAutodiff()
+ {
+ delete jets_0_;
+ delete jets_1_;
+ delete jets_2_;
+ delete jets_3_;
+ delete jets_4_;
+ delete jets_5_;
+ delete jets_6_;
+ delete jets_7_;
+ delete residuals_jets_;
+ }
+
+ virtual JacobianMethod getJacobianMethod() const
+ {
+ return JAC_AUTO;
+ }
+
+ virtual bool evaluate(double const* const* parameters, double* residuals, double** jacobians) const
+ {
+ // only residuals
+ if (jacobians == nullptr)
+ {
+ (*static_cast<CtrT const*>(this))(parameters[0],
+ parameters[1],
+ parameters[2],
+ parameters[3],
+ parameters[4],
+ parameters[5],
+ parameters[6],
+ parameters[7],
+ residuals);
+ }
+ // also compute jacobians
+ else
+ {
+ // update jets real part
+ std::vector<double const*> param_vec;
+ param_vec.assign(parameters,parameters+n_blocks);
+ updateJetsRealPart(param_vec);
+
+ // call functor
+ (*static_cast<CtrT const*>(this))(jets_0_->data(),
+ jets_1_->data(),
+ jets_2_->data(),
+ jets_3_->data(),
+ jets_4_->data(),
+ jets_5_->data(),
+ jets_6_->data(),
+ jets_7_->data(),
+ residuals_jets_->data());
+
+ // fill the residual array
+ for (auto i = 0; i < RES; i++)
+ residuals[i] = (*residuals_jets_)[i].a;
+
+ // fill the jacobian matrices
+ for (auto i = 0; i<n_blocks; i++)
+ if (jacobians[i] != nullptr)
+ for (unsigned int row = 0; row < RES; row++)
+ std::copy((*residuals_jets_)[row].v.data() + jacobian_locations_.at(i),
+ (*residuals_jets_)[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
+ jacobians[i] + row * state_block_sizes_.at(i));
+ }
+ return true;
+ }
+
+ void updateJetsRealPart(const std::vector<double const*>& parameters) const
+ {
+ // update jets real part
+ for (auto i = 0; i < B0; i++)
+ (*jets_0_)[i].a = parameters[0][i];
+ for (auto i = 0; i < B1; i++)
+ (*jets_1_)[i].a = parameters[1][i];
+ for (auto i = 0; i < B2; i++)
+ (*jets_2_)[i].a = parameters[2][i];
+ for (auto i = 0; i < B3; i++)
+ (*jets_3_)[i].a = parameters[3][i];
+ for (auto i = 0; i < B4; i++)
+ (*jets_4_)[i].a = parameters[4][i];
+ for (auto i = 0; i < B5; i++)
+ (*jets_5_)[i].a = parameters[5][i];
+ for (auto i = 0; i < B6; i++)
+ (*jets_6_)[i].a = parameters[6][i];
+ for (auto i = 0; i < B7; i++)
+ (*jets_7_)[i].a = parameters[7][i];
+ }
+
+ virtual void evaluate(const std::vector<const Scalar*>& _states_ptr, Eigen::VectorXs& residual_, std::vector<Eigen::MatrixXs>& jacobians_) const
+ {
+ assert(residual_.size() == RES);
+ jacobians_.clear();
+
+ assert(_states_ptr.size() == n_blocks);
+
+ // init jacobian
+ for(auto i = 0; i < n_blocks; ++i)
+ {
+ Eigen::MatrixXs Ji = Eigen::MatrixXs::Zero(RES, state_block_sizes_[i]);
+ jacobians_.push_back(Ji);
+ }
+
+ // update jets real part
+ updateJetsRealPart(_states_ptr);
+
+ // call functor
+ (*static_cast<CtrT const*>(this))(jets_0_->data(),
+ jets_1_->data(),
+ jets_2_->data(),
+ jets_3_->data(),
+ jets_4_->data(),
+ jets_5_->data(),
+ jets_6_->data(),
+ jets_7_->data(),
+ residuals_jets_->data());
+
+ // fill the residual vector
+ for (auto i = 0; i < RES; i++)
+ residual_(i) = (*residuals_jets_)[i].a;
+
+ // fill the jacobian matrices
+ for (auto i = 0; i<n_blocks; i++)
+ if (!state_ptrs_[i]->isFixed())
+ for (unsigned int row = 0; row < RES; row++)
+ std::copy((*residuals_jets_)[row].v.data() + jacobian_locations_.at(i),
+ (*residuals_jets_)[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
+ jacobians_[i].data() + row * state_block_sizes_.at(i));
+
+ // print jacobian matrices
+// for (auto i = 0; i < n_blocks; i++)
+// std::cout << jacobians_[i] << std::endl << std::endl;
+ }
+
+ virtual std::vector<Scalar*> getStateScalarPtrVector() const
+ {
+ return std::vector<Scalar*>({state_ptrs_[0]->getPtr(),
+ state_ptrs_[1]->getPtr(),
+ state_ptrs_[2]->getPtr(),
+ state_ptrs_[3]->getPtr(),
+ state_ptrs_[4]->getPtr(),
+ state_ptrs_[5]->getPtr(),
+ state_ptrs_[6]->getPtr(),
+ state_ptrs_[7]->getPtr()
+ });
+ }
+
+ virtual std::vector<StateBlockPtr> getStateBlockPtrVector() const
+ {
+ return state_ptrs_;
+ }
+
+ virtual std::vector<unsigned int> getStateSizes() const
+ {
+ return state_block_sizes_;
+ }
+
+ virtual unsigned int getSize() const
+ {
+ return RES;
+ }
+};
+
+////////////////// SPECIALIZATION 7 BLOCKS ////////////////////////////////////////////////////////////////////////
+
+template <class CtrT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6>
+class ConstraintAutodiff<CtrT,RES,B0,B1,B2,B3,B4,B5,B6,0,0,0> : public ConstraintBase
+{
+ public:
+
+ static const unsigned int residualSize = RES;
+ static const unsigned int block0Size = B0;
+ static const unsigned int block1Size = B1;
+ static const unsigned int block2Size = B2;
+ static const unsigned int block3Size = B3;
+ static const unsigned int block4Size = B4;
+ static const unsigned int block5Size = B5;
+ static const unsigned int block6Size = B6;
+ static const unsigned int block7Size = 0;
+ static const unsigned int block8Size = 0;
+ static const unsigned int block9Size = 0;
+ static const unsigned int n_blocks = 7;
+
+ static const std::vector<unsigned int> state_block_sizes_;
+
+ typedef ceres::Jet<Scalar, B0 + B1 + B2 + B3 + B4 +
+ B5 + B6> WolfJet;
+
+ protected:
+
+ std::vector<StateBlockPtr> state_ptrs_;
+
+ static const std::vector<unsigned int> jacobian_locations_;
+ std::array<WolfJet, RES>* residuals_jets_;
+
+ std::array<WolfJet, B0>* jets_0_;
+ std::array<WolfJet, B1>* jets_1_;
+ std::array<WolfJet, B2>* jets_2_;
+ std::array<WolfJet, B3>* jets_3_;
+ std::array<WolfJet, B4>* jets_4_;
+ std::array<WolfJet, B5>* jets_5_;
+ std::array<WolfJet, B6>* jets_6_;
+
+ public:
+
+ ConstraintAutodiff(ConstraintType _tp,
+ const FrameBasePtr& _frame_other_ptr,
+ const FeatureBasePtr& _feature_other_ptr,
+ const LandmarkBasePtr& _landmark_other_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ ConstraintStatus _status,
+ StateBlockPtr _state0Ptr,
+ StateBlockPtr _state1Ptr,
+ StateBlockPtr _state2Ptr,
+ StateBlockPtr _state3Ptr,
+ StateBlockPtr _state4Ptr,
+ StateBlockPtr _state5Ptr,
+ StateBlockPtr _state6Ptr) :
+ ConstraintBase(_tp, _frame_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
+ state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr,_state5Ptr,_state6Ptr}),
+ residuals_jets_(new std::array<WolfJet, RES>),
+ jets_0_(new std::array<WolfJet, B0>),
+ jets_1_(new std::array<WolfJet, B1>),
+ jets_2_(new std::array<WolfJet, B2>),
+ jets_3_(new std::array<WolfJet, B3>),
+ jets_4_(new std::array<WolfJet, B4>),
+ jets_5_(new std::array<WolfJet, B5>),
+ jets_6_(new std::array<WolfJet, B6>)
+ {
+ // initialize jets
+ unsigned int last_jet_idx = 0;
+ for (auto i = 0; i < B0; i++)
+ (*jets_0_)[i] = WolfJet(0, last_jet_idx++);
+ for (auto i = 0; i < B1; i++)
+ (*jets_1_)[i] = WolfJet(0, last_jet_idx++);
+ for (auto i = 0; i < B2; i++)
+ (*jets_2_)[i] = WolfJet(0, last_jet_idx++);
+ for (auto i = 0; i < B3; i++)
+ (*jets_3_)[i] = WolfJet(0, last_jet_idx++);
+ for (auto i = 0; i < B4; i++)
+ (*jets_4_)[i] = WolfJet(0, last_jet_idx++);
+ for (auto i = 0; i < B5; i++)
+ (*jets_5_)[i] = WolfJet(0, last_jet_idx++);
+ for (auto i = 0; i < B6; i++)
+ (*jets_6_)[i] = WolfJet(0, last_jet_idx++);
+ state_ptrs_.resize(n_blocks);
+ }
+
+ virtual ~ConstraintAutodiff()
+ {
+ delete jets_0_;
+ delete jets_1_;
+ delete jets_2_;
+ delete jets_3_;
+ delete jets_4_;
+ delete jets_5_;
+ delete jets_6_;
+ delete residuals_jets_;
+ }
+
+ virtual JacobianMethod getJacobianMethod() const
+ {
+ return JAC_AUTO;
+ }
+
+ virtual bool evaluate(double const* const* parameters, double* residuals, double** jacobians) const
+ {
+ // only residuals
+ if (jacobians == nullptr)
+ {
+ (*static_cast<CtrT const*>(this))(parameters[0],
+ parameters[1],
+ parameters[2],
+ parameters[3],
+ parameters[4],
+ parameters[5],
+ parameters[6],
+ residuals);
+ }
+ // also compute jacobians
+ else
+ {
+ // update jets real part
+ std::vector<double const*> param_vec;
+ param_vec.assign(parameters,parameters+n_blocks);
+ updateJetsRealPart(param_vec);
+
+ // call functor
+ (*static_cast<CtrT const*>(this))(jets_0_->data(),
+ jets_1_->data(),
+ jets_2_->data(),
+ jets_3_->data(),
+ jets_4_->data(),
+ jets_5_->data(),
+ jets_6_->data(),
+ residuals_jets_->data());
+
+ // fill the residual array
+ for (auto i = 0; i < RES; i++)
+ residuals[i] = (*residuals_jets_)[i].a;
+
+ // fill the jacobian matrices
+ for (auto i = 0; i<n_blocks; i++)
+ if (jacobians[i] != nullptr)
+ for (unsigned int row = 0; row < RES; row++)
+ std::copy((*residuals_jets_)[row].v.data() + jacobian_locations_.at(i),
+ (*residuals_jets_)[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
+ jacobians[i] + row * state_block_sizes_.at(i));
+ }
+ return true;
+ }
+
+ void updateJetsRealPart(const std::vector<double const*>& parameters) const
+ {
+ // update jets real part
+ for (auto i = 0; i < B0; i++)
+ (*jets_0_)[i].a = parameters[0][i];
+ for (auto i = 0; i < B1; i++)
+ (*jets_1_)[i].a = parameters[1][i];
+ for (auto i = 0; i < B2; i++)
+ (*jets_2_)[i].a = parameters[2][i];
+ for (auto i = 0; i < B3; i++)
+ (*jets_3_)[i].a = parameters[3][i];
+ for (auto i = 0; i < B4; i++)
+ (*jets_4_)[i].a = parameters[4][i];
+ for (auto i = 0; i < B5; i++)
+ (*jets_5_)[i].a = parameters[5][i];
+ for (auto i = 0; i < B6; i++)
+ (*jets_6_)[i].a = parameters[6][i];
+ }
+
+ virtual void evaluate(const std::vector<const Scalar*>& _states_ptr, Eigen::VectorXs& residual_, std::vector<Eigen::MatrixXs>& jacobians_) const
+ {
+ assert(residual_.size() == RES);
+ jacobians_.clear();
+
+ assert(_states_ptr.size() == n_blocks);
+
+ // init jacobian
+ for(auto i = 0; i < n_blocks; ++i)
+ {
+ Eigen::MatrixXs Ji = Eigen::MatrixXs::Zero(RES, state_block_sizes_[i]);
+ jacobians_.push_back(Ji);
+ }
+
+ // update jets real part
+ updateJetsRealPart(_states_ptr);
+
+ // call functor
+ (*static_cast<CtrT const*>(this))(jets_0_->data(),
+ jets_1_->data(),
+ jets_2_->data(),
+ jets_3_->data(),
+ jets_4_->data(),
+ jets_5_->data(),
+ jets_6_->data(),
+ residuals_jets_->data());
+
+ // fill the residual vector
+ for (auto i = 0; i < RES; i++)
+ residual_(i) = (*residuals_jets_)[i].a;
+
+ // fill the jacobian matrices
+ for (auto i = 0; i<n_blocks; i++)
+ if (!state_ptrs_[i]->isFixed())
+ for (unsigned int row = 0; row < RES; row++)
+ std::copy((*residuals_jets_)[row].v.data() + jacobian_locations_.at(i),
+ (*residuals_jets_)[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
+ jacobians_[i].data() + row * state_block_sizes_.at(i));
+
+ // print jacobian matrices
+// for (auto i = 0; i < n_blocks; i++)
+// std::cout << jacobians_[i] << std::endl << std::endl;
+ }
+
+ virtual std::vector<Scalar*> getStateScalarPtrVector() const
+ {
+ return std::vector<Scalar*>({state_ptrs_[0]->getPtr(),
+ state_ptrs_[1]->getPtr(),
+ state_ptrs_[2]->getPtr(),
+ state_ptrs_[3]->getPtr(),
+ state_ptrs_[4]->getPtr(),
+ state_ptrs_[5]->getPtr(),
+ state_ptrs_[6]->getPtr()
+ });
+ }
+
+ virtual std::vector<StateBlockPtr> getStateBlockPtrVector() const
+ {
+ return state_ptrs_;
+ }
+
+ virtual std::vector<unsigned int> getStateSizes() const
+ {
+ return state_block_sizes_;
+ }
+
+ virtual unsigned int getSize() const
+ {
+ return RES;
+ }
+};
+
+////////////////// SPECIALIZATION 6 BLOCKS ////////////////////////////////////////////////////////////////////////
+
+template <class CtrT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5>
+class ConstraintAutodiff<CtrT,RES,B0,B1,B2,B3,B4,B5,0,0,0,0> : public ConstraintBase
+{
+ public:
+
+ static const unsigned int residualSize = RES;
+ static const unsigned int block0Size = B0;
+ static const unsigned int block1Size = B1;
+ static const unsigned int block2Size = B2;
+ static const unsigned int block3Size = B3;
+ static const unsigned int block4Size = B4;
+ static const unsigned int block5Size = B5;
+ static const unsigned int block6Size = 0;
+ static const unsigned int block7Size = 0;
+ static const unsigned int block8Size = 0;
+ static const unsigned int block9Size = 0;
+ static const unsigned int n_blocks = 6;
+
+ static const std::vector<unsigned int> state_block_sizes_;
+
+ typedef ceres::Jet<Scalar, B0 + B1 + B2 + B3 + B4 +
+ B5> WolfJet;
+
+ protected:
+
+ std::vector<StateBlockPtr> state_ptrs_;
+
+ static const std::vector<unsigned int> jacobian_locations_;
+ std::array<WolfJet, RES>* residuals_jets_;
+
+ std::array<WolfJet, B0>* jets_0_;
+ std::array<WolfJet, B1>* jets_1_;
+ std::array<WolfJet, B2>* jets_2_;
+ std::array<WolfJet, B3>* jets_3_;
+ std::array<WolfJet, B4>* jets_4_;
+ std::array<WolfJet, B5>* jets_5_;
+
+ public:
+
+ ConstraintAutodiff(ConstraintType _tp,
+ const FrameBasePtr& _frame_other_ptr,
+ const FeatureBasePtr& _feature_other_ptr,
+ const LandmarkBasePtr& _landmark_other_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ ConstraintStatus _status,
+ StateBlockPtr _state0Ptr,
+ StateBlockPtr _state1Ptr,
+ StateBlockPtr _state2Ptr,
+ StateBlockPtr _state3Ptr,
+ StateBlockPtr _state4Ptr,
+ StateBlockPtr _state5Ptr) :
+ ConstraintBase(_tp, _frame_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
+ state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr,_state5Ptr}),
+ residuals_jets_(new std::array<WolfJet, RES>),
+ jets_0_(new std::array<WolfJet, B0>),
+ jets_1_(new std::array<WolfJet, B1>),
+ jets_2_(new std::array<WolfJet, B2>),
+ jets_3_(new std::array<WolfJet, B3>),
+ jets_4_(new std::array<WolfJet, B4>),
+ jets_5_(new std::array<WolfJet, B5>)
+ {
+ // initialize jets
+ unsigned int last_jet_idx = 0;
+ for (auto i = 0; i < B0; i++)
+ (*jets_0_)[i] = WolfJet(0, last_jet_idx++);
+ for (auto i = 0; i < B1; i++)
+ (*jets_1_)[i] = WolfJet(0, last_jet_idx++);
+ for (auto i = 0; i < B2; i++)
+ (*jets_2_)[i] = WolfJet(0, last_jet_idx++);
+ for (auto i = 0; i < B3; i++)
+ (*jets_3_)[i] = WolfJet(0, last_jet_idx++);
+ for (auto i = 0; i < B4; i++)
+ (*jets_4_)[i] = WolfJet(0, last_jet_idx++);
+ for (auto i = 0; i < B5; i++)
+ (*jets_5_)[i] = WolfJet(0, last_jet_idx++);
+ state_ptrs_.resize(n_blocks);
+ }
+
+ virtual ~ConstraintAutodiff()
+ {
+ delete jets_0_;
+ delete jets_1_;
+ delete jets_2_;
+ delete jets_3_;
+ delete jets_4_;
+ delete jets_5_;
+ delete residuals_jets_;
+ }
+
+ virtual JacobianMethod getJacobianMethod() const
+ {
+ return JAC_AUTO;
+ }
+
+ virtual bool evaluate(double const* const* parameters, double* residuals, double** jacobians) const
+ {
+ // only residuals
+ if (jacobians == nullptr)
+ {
+ (*static_cast<CtrT const*>(this))(parameters[0],
+ parameters[1],
+ parameters[2],
+ parameters[3],
+ parameters[4],
+ parameters[5],
+ residuals);
+ }
+ // also compute jacobians
+ else
+ {
+ // update jets real part
+ std::vector<double const*> param_vec;
+ param_vec.assign(parameters,parameters+n_blocks);
+ updateJetsRealPart(param_vec);
+
+ // call functor
+ (*static_cast<CtrT const*>(this))(jets_0_->data(),
+ jets_1_->data(),
+ jets_2_->data(),
+ jets_3_->data(),
+ jets_4_->data(),
+ jets_5_->data(),
+ residuals_jets_->data());
+
+ // fill the residual array
+ for (auto i = 0; i < RES; i++)
+ residuals[i] = (*residuals_jets_)[i].a;
+
+ // fill the jacobian matrices
+ for (auto i = 0; i<n_blocks; i++)
+ if (jacobians[i] != nullptr)
+ for (unsigned int row = 0; row < RES; row++)
+ std::copy((*residuals_jets_)[row].v.data() + jacobian_locations_.at(i),
+ (*residuals_jets_)[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
+ jacobians[i] + row * state_block_sizes_.at(i));
+ }
+ return true;
+ }
+
+ void updateJetsRealPart(const std::vector<double const*>& parameters) const
+ {
+ // update jets real part
+ for (auto i = 0; i < B0; i++)
+ (*jets_0_)[i].a = parameters[0][i];
+ for (auto i = 0; i < B1; i++)
+ (*jets_1_)[i].a = parameters[1][i];
+ for (auto i = 0; i < B2; i++)
+ (*jets_2_)[i].a = parameters[2][i];
+ for (auto i = 0; i < B3; i++)
+ (*jets_3_)[i].a = parameters[3][i];
+ for (auto i = 0; i < B4; i++)
+ (*jets_4_)[i].a = parameters[4][i];
+ for (auto i = 0; i < B5; i++)
+ (*jets_5_)[i].a = parameters[5][i];
+ }
+
+ virtual void evaluate(const std::vector<const Scalar*>& _states_ptr, Eigen::VectorXs& residual_, std::vector<Eigen::MatrixXs>& jacobians_) const
+ {
+ assert(residual_.size() == RES);
+ jacobians_.clear();
+
+ assert(_states_ptr.size() == n_blocks);
+
+ // init jacobian
+ for(auto i = 0; i < n_blocks; ++i)
+ {
+ Eigen::MatrixXs Ji = Eigen::MatrixXs::Zero(RES, state_block_sizes_[i]);
+ jacobians_.push_back(Ji);
+ }
+
+ // update jets real part
+ updateJetsRealPart(_states_ptr);
+
+ // call functor
+ (*static_cast<CtrT const*>(this))(jets_0_->data(),
+ jets_1_->data(),
+ jets_2_->data(),
+ jets_3_->data(),
+ jets_4_->data(),
+ jets_5_->data(),
+ residuals_jets_->data());
+
+ // fill the residual vector
+ for (auto i = 0; i < RES; i++)
+ residual_(i) = (*residuals_jets_)[i].a;
+
+ // fill the jacobian matrices
+ for (auto i = 0; i<n_blocks; i++)
+ if (!state_ptrs_[i]->isFixed())
+ for (unsigned int row = 0; row < RES; row++)
+ std::copy((*residuals_jets_)[row].v.data() + jacobian_locations_.at(i),
+ (*residuals_jets_)[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
+ jacobians_[i].data() + row * state_block_sizes_.at(i));
+
+ // print jacobian matrices
+// for (auto i = 0; i < n_blocks; i++)
+// std::cout << jacobians_[i] << std::endl << std::endl;
+ }
+
+ virtual std::vector<Scalar*> getStateScalarPtrVector() const
+ {
+ return std::vector<Scalar*>({state_ptrs_[0]->getPtr(),
+ state_ptrs_[1]->getPtr(),
+ state_ptrs_[2]->getPtr(),
+ state_ptrs_[3]->getPtr(),
+ state_ptrs_[4]->getPtr(),
+ state_ptrs_[5]->getPtr()
+ });
+ }
+
+ virtual std::vector<StateBlockPtr> getStateBlockPtrVector() const
+ {
+ return state_ptrs_;
+ }
+
+ virtual std::vector<unsigned int> getStateSizes() const
+ {
+ return state_block_sizes_;
+ }
+
+ virtual unsigned int getSize() const
+ {
+ return RES;
+ }
+};
+
+////////////////// SPECIALIZATION 5 BLOCKS ////////////////////////////////////////////////////////////////////////
+
+template <class CtrT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4>
+class ConstraintAutodiff<CtrT,RES,B0,B1,B2,B3,B4,0,0,0,0,0> : public ConstraintBase
+{
+ public:
+
+ static const unsigned int residualSize = RES;
+ static const unsigned int block0Size = B0;
+ static const unsigned int block1Size = B1;
+ static const unsigned int block2Size = B2;
+ static const unsigned int block3Size = B3;
+ static const unsigned int block4Size = B4;
+ static const unsigned int block5Size = 0;
+ static const unsigned int block6Size = 0;
+ static const unsigned int block7Size = 0;
+ static const unsigned int block8Size = 0;
+ static const unsigned int block9Size = 0;
+ static const unsigned int n_blocks = 5;
+
+ static const std::vector<unsigned int> state_block_sizes_;
+
+ typedef ceres::Jet<Scalar, B0 + B1 + B2 + B3 + B4> WolfJet;
+
+ protected:
+
+ std::vector<StateBlockPtr> state_ptrs_;
+
+ static const std::vector<unsigned int> jacobian_locations_;
+ std::array<WolfJet, RES>* residuals_jets_;
+
+ std::array<WolfJet, B0>* jets_0_;
+ std::array<WolfJet, B1>* jets_1_;
+ std::array<WolfJet, B2>* jets_2_;
+ std::array<WolfJet, B3>* jets_3_;
+ std::array<WolfJet, B4>* jets_4_;
+
+ public:
+
+ ConstraintAutodiff(ConstraintType _tp,
+ const FrameBasePtr& _frame_other_ptr,
+ const FeatureBasePtr& _feature_other_ptr,
+ const LandmarkBasePtr& _landmark_other_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ ConstraintStatus _status,
+ StateBlockPtr _state0Ptr,
+ StateBlockPtr _state1Ptr,
+ StateBlockPtr _state2Ptr,
+ StateBlockPtr _state3Ptr,
+ StateBlockPtr _state4Ptr) :
+ ConstraintBase(_tp, _frame_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
+ state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr}),
+ residuals_jets_(new std::array<WolfJet, RES>),
+ jets_0_(new std::array<WolfJet, B0>),
+ jets_1_(new std::array<WolfJet, B1>),
+ jets_2_(new std::array<WolfJet, B2>),
+ jets_3_(new std::array<WolfJet, B3>),
+ jets_4_(new std::array<WolfJet, B4>)
+ {
+ // initialize jets
+ unsigned int last_jet_idx = 0;
+ for (auto i = 0; i < B0; i++)
+ (*jets_0_)[i] = WolfJet(0, last_jet_idx++);
+ for (auto i = 0; i < B1; i++)
+ (*jets_1_)[i] = WolfJet(0, last_jet_idx++);
+ for (auto i = 0; i < B2; i++)
+ (*jets_2_)[i] = WolfJet(0, last_jet_idx++);
+ for (auto i = 0; i < B3; i++)
+ (*jets_3_)[i] = WolfJet(0, last_jet_idx++);
+ for (auto i = 0; i < B4; i++)
+ (*jets_4_)[i] = WolfJet(0, last_jet_idx++);
+ state_ptrs_.resize(n_blocks);
+ }
+
+ virtual ~ConstraintAutodiff()
+ {
+ delete jets_0_;
+ delete jets_1_;
+ delete jets_2_;
+ delete jets_3_;
+ delete jets_4_;
+ delete residuals_jets_;
+ }
+
+ virtual JacobianMethod getJacobianMethod() const
+ {
+ return JAC_AUTO;
+ }
+
+ virtual bool evaluate(double const* const* parameters, double* residuals, double** jacobians) const
+ {
+ // only residuals
+ if (jacobians == nullptr)
+ {
+ (*static_cast<CtrT const*>(this))(parameters[0],
+ parameters[1],
+ parameters[2],
+ parameters[3],
+ parameters[4],
+ residuals);
+ }
+ // also compute jacobians
+ else
+ {
+ // update jets real part
+ std::vector<double const*> param_vec;
+ param_vec.assign(parameters,parameters+n_blocks);
+ updateJetsRealPart(param_vec);
+
+ // call functor
+ (*static_cast<CtrT const*>(this))(jets_0_->data(),
+ jets_1_->data(),
+ jets_2_->data(),
+ jets_3_->data(),
+ jets_4_->data(),
+ residuals_jets_->data());
+
+ // fill the residual array
+ for (auto i = 0; i < RES; i++)
+ residuals[i] = (*residuals_jets_)[i].a;
+
+ // fill the jacobian matrices
+ for (auto i = 0; i<n_blocks; i++)
+ if (jacobians[i] != nullptr)
+ for (unsigned int row = 0; row < RES; row++)
+ std::copy((*residuals_jets_)[row].v.data() + jacobian_locations_.at(i),
+ (*residuals_jets_)[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
+ jacobians[i] + row * state_block_sizes_.at(i));
+ }
+ return true;
+ }
+
+ void updateJetsRealPart(const std::vector<double const*>& parameters) const
+ {
+ // update jets real part
+ for (auto i = 0; i < B0; i++)
+ (*jets_0_)[i].a = parameters[0][i];
+ for (auto i = 0; i < B1; i++)
+ (*jets_1_)[i].a = parameters[1][i];
+ for (auto i = 0; i < B2; i++)
+ (*jets_2_)[i].a = parameters[2][i];
+ for (auto i = 0; i < B3; i++)
+ (*jets_3_)[i].a = parameters[3][i];
+ for (auto i = 0; i < B4; i++)
+ (*jets_4_)[i].a = parameters[4][i];
+ }
+
+ virtual void evaluate(const std::vector<const Scalar*>& _states_ptr, Eigen::VectorXs& residual_, std::vector<Eigen::MatrixXs>& jacobians_) const
+ {
+ assert(residual_.size() == RES);
+ jacobians_.clear();
+
+ assert(_states_ptr.size() == n_blocks);
+
+ // init jacobian
+ for(auto i = 0; i < n_blocks; ++i)
+ {
+ Eigen::MatrixXs Ji = Eigen::MatrixXs::Zero(RES, state_block_sizes_[i]);
+ jacobians_.push_back(Ji);
+ }
+
+ // update jets real part
+ updateJetsRealPart(_states_ptr);
+
+ // call functor
+ (*static_cast<CtrT const*>(this))(jets_0_->data(),
+ jets_1_->data(),
+ jets_2_->data(),
+ jets_3_->data(),
+ jets_4_->data(),
+ residuals_jets_->data());
+
+ // fill the residual vector
+ for (auto i = 0; i < RES; i++)
+ residual_(i) = (*residuals_jets_)[i].a;
+
+ // fill the jacobian matrices
+ for (auto i = 0; i<n_blocks; i++)
+ if (!state_ptrs_[i]->isFixed())
+ for (unsigned int row = 0; row < RES; row++)
+ std::copy((*residuals_jets_)[row].v.data() + jacobian_locations_.at(i),
+ (*residuals_jets_)[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
+ jacobians_[i].data() + row * state_block_sizes_.at(i));
+
+ // print jacobian matrices
+// for (auto i = 0; i < n_blocks; i++)
+// std::cout << jacobians_[i] << std::endl << std::endl;
+ }
+
+ virtual std::vector<Scalar*> getStateScalarPtrVector() const
+ {
+ return std::vector<Scalar*>({state_ptrs_[0]->getPtr(),
+ state_ptrs_[1]->getPtr(),
+ state_ptrs_[2]->getPtr(),
+ state_ptrs_[3]->getPtr(),
+ state_ptrs_[4]->getPtr(),
+ });
+ }
+
+ virtual std::vector<StateBlockPtr> getStateBlockPtrVector() const
+ {
+ return state_ptrs_;
+ }
+
+ virtual std::vector<unsigned int> getStateSizes() const
+ {
+ return state_block_sizes_;
+ }
+
+ virtual unsigned int getSize() const
+ {
+ return RES;
+ }
+};
+
+////////////////// SPECIALIZATION 4 BLOCKS ////////////////////////////////////////////////////////////////////////
+
+template <class CtrT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3>
+class ConstraintAutodiff<CtrT,RES,B0,B1,B2,B3,0,0,0,0,0,0> : public ConstraintBase
+{
+ public:
+
+ static const unsigned int residualSize = RES;
+ static const unsigned int block0Size = B0;
+ static const unsigned int block1Size = B1;
+ static const unsigned int block2Size = B2;
+ static const unsigned int block3Size = B3;
+ static const unsigned int block4Size = 0;
+ static const unsigned int block5Size = 0;
+ static const unsigned int block6Size = 0;
+ static const unsigned int block7Size = 0;
+ static const unsigned int block8Size = 0;
+ static const unsigned int block9Size = 0;
+ static const unsigned int n_blocks = 4;
+
+ static const std::vector<unsigned int> state_block_sizes_;
+
+ typedef ceres::Jet<Scalar, B0 + B1 + B2 + B3> WolfJet;
+
+ protected:
+
+ std::vector<StateBlockPtr> state_ptrs_;
+
+ static const std::vector<unsigned int> jacobian_locations_;
+ std::array<WolfJet, RES>* residuals_jets_;
+
+ std::array<WolfJet, B0>* jets_0_;
+ std::array<WolfJet, B1>* jets_1_;
+ std::array<WolfJet, B2>* jets_2_;
+ std::array<WolfJet, B3>* jets_3_;
+
+ public:
+
+ ConstraintAutodiff(ConstraintType _tp,
+ const FrameBasePtr& _frame_other_ptr,
+ const FeatureBasePtr& _feature_other_ptr,
+ const LandmarkBasePtr& _landmark_other_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ ConstraintStatus _status,
+ StateBlockPtr _state0Ptr,
+ StateBlockPtr _state1Ptr,
+ StateBlockPtr _state2Ptr,
+ StateBlockPtr _state3Ptr) :
+ ConstraintBase(_tp, _frame_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
+ state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr}),
+ residuals_jets_(new std::array<WolfJet, RES>),
+ jets_0_(new std::array<WolfJet, B0>),
+ jets_1_(new std::array<WolfJet, B1>),
+ jets_2_(new std::array<WolfJet, B2>),
+ jets_3_(new std::array<WolfJet, B3>)
+ {
+ // initialize jets
+ unsigned int last_jet_idx = 0;
+ for (auto i = 0; i < B0; i++)
+ (*jets_0_)[i] = WolfJet(0, last_jet_idx++);
+ for (auto i = 0; i < B1; i++)
+ (*jets_1_)[i] = WolfJet(0, last_jet_idx++);
+ for (auto i = 0; i < B2; i++)
+ (*jets_2_)[i] = WolfJet(0, last_jet_idx++);
+ for (auto i = 0; i < B3; i++)
+ (*jets_3_)[i] = WolfJet(0, last_jet_idx++);
+ state_ptrs_.resize(n_blocks);
+ }
+
+ virtual ~ConstraintAutodiff()
+ {
+ delete jets_0_;
+ delete jets_1_;
+ delete jets_2_;
+ delete jets_3_;
+ delete residuals_jets_;
+ }
+
+ virtual JacobianMethod getJacobianMethod() const
+ {
+ return JAC_AUTO;
+ }
+
+ virtual bool evaluate(double const* const* parameters, double* residuals, double** jacobians) const
+ {
+ // only residuals
+ if (jacobians == nullptr)
+ {
+ (*static_cast<CtrT const*>(this))(parameters[0],
+ parameters[1],
+ parameters[2],
+ parameters[3],
+ residuals);
+ }
+ // also compute jacobians
+ else
+ {
+ // update jets real part
+ std::vector<double const*> param_vec;
+ param_vec.assign(parameters,parameters+n_blocks);
+ updateJetsRealPart(param_vec);
+
+ // call functor
+ (*static_cast<CtrT const*>(this))(jets_0_->data(),
+ jets_1_->data(),
+ jets_2_->data(),
+ jets_3_->data(),
+ residuals_jets_->data());
+
+ // fill the residual array
+ for (auto i = 0; i < RES; i++)
+ residuals[i] = (*residuals_jets_)[i].a;
+
+ // fill the jacobian matrices
+ for (auto i = 0; i<n_blocks; i++)
+ if (jacobians[i] != nullptr)
+ for (unsigned int row = 0; row < RES; row++)
+ std::copy((*residuals_jets_)[row].v.data() + jacobian_locations_.at(i),
+ (*residuals_jets_)[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
+ jacobians[i] + row * state_block_sizes_.at(i));
+ }
+ return true;
+ }
+
+ void updateJetsRealPart(const std::vector<double const*>& parameters) const
+ {
+ // update jets real part
+ for (auto i = 0; i < B0; i++)
+ (*jets_0_)[i].a = parameters[0][i];
+ for (auto i = 0; i < B1; i++)
+ (*jets_1_)[i].a = parameters[1][i];
+ for (auto i = 0; i < B2; i++)
+ (*jets_2_)[i].a = parameters[2][i];
+ for (auto i = 0; i < B3; i++)
+ (*jets_3_)[i].a = parameters[3][i];
+ }
+
+ virtual void evaluate(const std::vector<const Scalar*>& _states_ptr, Eigen::VectorXs& residual_, std::vector<Eigen::MatrixXs>& jacobians_) const
+ {
+ assert(residual_.size() == RES);
+ jacobians_.clear();
+
+ assert(_states_ptr.size() == n_blocks);
+
+ // init jacobian
+ for(auto i = 0; i < n_blocks; ++i)
+ {
+ Eigen::MatrixXs Ji = Eigen::MatrixXs::Zero(RES, state_block_sizes_[i]);
+ jacobians_.push_back(Ji);
+ }
+
+ // update jets real part
+ updateJetsRealPart(_states_ptr);
+
+ // call functor
+ (*static_cast<CtrT const*>(this))(jets_0_->data(),
+ jets_1_->data(),
+ jets_2_->data(),
+ jets_3_->data(),
+ residuals_jets_->data());
+
+ // fill the residual vector
+ for (auto i = 0; i < RES; i++)
+ residual_(i) = (*residuals_jets_)[i].a;
+
+ // fill the jacobian matrices
+ for (auto i = 0; i<n_blocks; i++)
+ if (!state_ptrs_[i]->isFixed())
+ for (unsigned int row = 0; row < RES; row++)
+ std::copy((*residuals_jets_)[row].v.data() + jacobian_locations_.at(i),
+ (*residuals_jets_)[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
+ jacobians_[i].data() + row * state_block_sizes_.at(i));
+
+ // print jacobian matrices
+// for (auto i = 0; i < n_blocks; i++)
+// std::cout << jacobians_[i] << std::endl << std::endl;
+ }
+
+ virtual std::vector<Scalar*> getStateScalarPtrVector() const
+ {
+ return std::vector<Scalar*>({state_ptrs_[0]->getPtr(),
+ state_ptrs_[1]->getPtr(),
+ state_ptrs_[2]->getPtr(),
+ state_ptrs_[3]->getPtr()
+ });
+ }
+
+ virtual std::vector<StateBlockPtr> getStateBlockPtrVector() const
+ {
+ return state_ptrs_;
+ }
+
+ virtual std::vector<unsigned int> getStateSizes() const
+ {
+ return state_block_sizes_;
+ }
+
+ virtual unsigned int getSize() const
+ {
+ return RES;
+ }
+};
+
+////////////////// SPECIALIZATION 3 BLOCKS ////////////////////////////////////////////////////////////////////////
+
+template <class CtrT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2>
+class ConstraintAutodiff<CtrT,RES,B0,B1,B2,0,0,0,0,0,0,0> : public ConstraintBase
+{
+ public:
+
+ static const unsigned int residualSize = RES;
+ static const unsigned int block0Size = B0;
+ static const unsigned int block1Size = B1;
+ static const unsigned int block2Size = B2;
+ static const unsigned int block3Size = 0;
+ static const unsigned int block4Size = 0;
+ static const unsigned int block5Size = 0;
+ static const unsigned int block6Size = 0;
+ static const unsigned int block7Size = 0;
+ static const unsigned int block8Size = 0;
+ static const unsigned int block9Size = 0;
+ static const unsigned int n_blocks = 3;
+
+ static const std::vector<unsigned int> state_block_sizes_;
+
+ typedef ceres::Jet<Scalar, B0 + B1 + B2> WolfJet;
+
+ protected:
+
+ std::vector<StateBlockPtr> state_ptrs_;
+
+ static const std::vector<unsigned int> jacobian_locations_;
+ std::array<WolfJet, RES>* residuals_jets_;
+
+ std::array<WolfJet, B0>* jets_0_;
+ std::array<WolfJet, B1>* jets_1_;
+ std::array<WolfJet, B2>* jets_2_;
+
+ public:
+
+ ConstraintAutodiff(ConstraintType _tp,
+ const FrameBasePtr& _frame_other_ptr,
+ const FeatureBasePtr& _feature_other_ptr,
+ const LandmarkBasePtr& _landmark_other_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ ConstraintStatus _status,
+ StateBlockPtr _state0Ptr,
+ StateBlockPtr _state1Ptr,
+ StateBlockPtr _state2Ptr) :
+ ConstraintBase(_tp, _frame_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
+ state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr}),
+ residuals_jets_(new std::array<WolfJet, RES>),
+ jets_0_(new std::array<WolfJet, B0>),
+ jets_1_(new std::array<WolfJet, B1>),
+ jets_2_(new std::array<WolfJet, B2>)
+ {
+ // initialize jets
+ unsigned int last_jet_idx = 0;
+ for (auto i = 0; i < B0; i++)
+ (*jets_0_)[i] = WolfJet(0, last_jet_idx++);
+ for (auto i = 0; i < B1; i++)
+ (*jets_1_)[i] = WolfJet(0, last_jet_idx++);
+ for (auto i = 0; i < B2; i++)
+ (*jets_2_)[i] = WolfJet(0, last_jet_idx++);
+ state_ptrs_.resize(n_blocks);
+ }
+
+ virtual ~ConstraintAutodiff()
+ {
+ delete jets_0_;
+ delete jets_1_;
+ delete jets_2_;
+ delete residuals_jets_;
+ }
+
+ virtual JacobianMethod getJacobianMethod() const
+ {
+ return JAC_AUTO;
+ }
+
+ virtual bool evaluate(double const* const* parameters, double* residuals, double** jacobians) const
+ {
+ // only residuals
+ if (jacobians == nullptr)
+ {
+ (*static_cast<CtrT const*>(this))(parameters[0],
+ parameters[1],
+ parameters[2],
+ residuals);
+ }
+ // also compute jacobians
+ else
+ {
+ // update jets real part
+ std::vector<double const*> param_vec;
+ param_vec.assign(parameters,parameters+n_blocks);
+ updateJetsRealPart(param_vec);
+
+ // call functor
+ (*static_cast<CtrT const*>(this))(jets_0_->data(),
+ jets_1_->data(),
+ jets_2_->data(),
+ residuals_jets_->data());
+
+ // fill the residual array
+ for (auto i = 0; i < RES; i++)
+ residuals[i] = (*residuals_jets_)[i].a;
+
+ // fill the jacobian matrices
+ for (auto i = 0; i<n_blocks; i++)
+ if (jacobians[i] != nullptr)
+ for (unsigned int row = 0; row < RES; row++)
+ std::copy((*residuals_jets_)[row].v.data() + jacobian_locations_.at(i),
+ (*residuals_jets_)[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
+ jacobians[i] + row * state_block_sizes_.at(i));
+ }
+ return true;
+ }
+
+ void updateJetsRealPart(const std::vector<double const*>& parameters) const
+ {
+ // update jets real part
+ for (auto i = 0; i < B0; i++)
+ (*jets_0_)[i].a = parameters[0][i];
+ for (auto i = 0; i < B1; i++)
+ (*jets_1_)[i].a = parameters[1][i];
+ for (auto i = 0; i < B2; i++)
+ (*jets_2_)[i].a = parameters[2][i];
+ }
+
+ virtual void evaluate(const std::vector<const Scalar*>& _states_ptr, Eigen::VectorXs& residual_, std::vector<Eigen::MatrixXs>& jacobians_) const
+ {
+ assert(residual_.size() == RES);
+ jacobians_.clear();
+
+ assert(_states_ptr.size() == n_blocks);
+
+ // init jacobian
+ for(auto i = 0; i < n_blocks; ++i)
+ {
+ Eigen::MatrixXs Ji = Eigen::MatrixXs::Zero(RES, state_block_sizes_[i]);
+ jacobians_.push_back(Ji);
+ }
+
+ // update jets real part
+ updateJetsRealPart(_states_ptr);
+
+ // call functor
+ (*static_cast<CtrT const*>(this))(jets_0_->data(),
+ jets_1_->data(),
+ jets_2_->data(),
+ residuals_jets_->data());
+
+ // fill the residual vector
+ for (auto i = 0; i < RES; i++)
+ residual_(i) = (*residuals_jets_)[i].a;
+
+ // fill the jacobian matrices
+ for (auto i = 0; i<n_blocks; i++)
+ if (!state_ptrs_[i]->isFixed())
+ for (unsigned int row = 0; row < RES; row++)
+ std::copy((*residuals_jets_)[row].v.data() + jacobian_locations_.at(i),
+ (*residuals_jets_)[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
+ jacobians_[i].data() + row * state_block_sizes_.at(i));
+
+ // print jacobian matrices
+// for (auto i = 0; i < n_blocks; i++)
+// std::cout << jacobians_[i] << std::endl << std::endl;
+ }
+
+ virtual std::vector<Scalar*> getStateScalarPtrVector() const
+ {
+ return std::vector<Scalar*>({state_ptrs_[0]->getPtr(),
+ state_ptrs_[1]->getPtr(),
+ state_ptrs_[2]->getPtr()
+ });
+ }
+
+ virtual std::vector<StateBlockPtr> getStateBlockPtrVector() const
+ {
+ return state_ptrs_;
+ }
+
+ virtual std::vector<unsigned int> getStateSizes() const
+ {
+ return state_block_sizes_;
+ }
+
+ virtual unsigned int getSize() const
+ {
+ return RES;
+ }
+};
+
+////////////////// SPECIALIZATION 2 BLOCKS ////////////////////////////////////////////////////////////////////////
+
+template <class CtrT,unsigned int RES,unsigned int B0,unsigned int B1>
+class ConstraintAutodiff<CtrT,RES,B0,B1,0,0,0,0,0,0,0,0> : public ConstraintBase
+{
+ public:
+
+ static const unsigned int residualSize = RES;
+ static const unsigned int block0Size = B0;
+ static const unsigned int block1Size = B1;
+ static const unsigned int block2Size = 0;
+ static const unsigned int block3Size = 0;
+ static const unsigned int block4Size = 0;
+ static const unsigned int block5Size = 0;
+ static const unsigned int block6Size = 0;
+ static const unsigned int block7Size = 0;
+ static const unsigned int block8Size = 0;
+ static const unsigned int block9Size = 0;
+ static const unsigned int n_blocks = 2;
+
+ static const std::vector<unsigned int> state_block_sizes_;
+
+ typedef ceres::Jet<Scalar, B0 + B1> WolfJet;
+
+ protected:
+
+ std::vector<StateBlockPtr> state_ptrs_;
+
+ static const std::vector<unsigned int> jacobian_locations_;
+ std::array<WolfJet, RES>* residuals_jets_;
+
+ std::array<WolfJet, B0>* jets_0_;
+ std::array<WolfJet, B1>* jets_1_;
+
+ public:
+
+ ConstraintAutodiff(ConstraintType _tp,
+ const FrameBasePtr& _frame_other_ptr,
+ const FeatureBasePtr& _feature_other_ptr,
+ const LandmarkBasePtr& _landmark_other_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ ConstraintStatus _status,
+ StateBlockPtr _state0Ptr,
+ StateBlockPtr _state1Ptr) :
+ ConstraintBase(_tp, _frame_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
+ state_ptrs_({_state0Ptr,_state1Ptr}),
+ residuals_jets_(new std::array<WolfJet, RES>),
+ jets_0_(new std::array<WolfJet, B0>),
+ jets_1_(new std::array<WolfJet, B1>)
+ {
+ // initialize jets
+ unsigned int last_jet_idx = 0;
+ for (auto i = 0; i < B0; i++)
+ (*jets_0_)[i] = WolfJet(0, last_jet_idx++);
+ for (auto i = 0; i < B1; i++)
+ (*jets_1_)[i] = WolfJet(0, last_jet_idx++);
+ state_ptrs_.resize(n_blocks);
+ }
+
+ virtual ~ConstraintAutodiff()
+ {
+ delete jets_0_;
+ delete jets_1_;
+ delete residuals_jets_;
+ }
+
+ virtual JacobianMethod getJacobianMethod() const
+ {
+ return JAC_AUTO;
+ }
+
+ virtual bool evaluate(double const* const* parameters, double* residuals, double** jacobians) const
+ {
+ // only residuals
+ if (jacobians == nullptr)
+ {
+ (*static_cast<CtrT const*>(this))(parameters[0],
+ parameters[1],
+ residuals);
+ }
+ // also compute jacobians
+ else
+ {
+ // update jets real part
+ std::vector<double const*> param_vec;
+ param_vec.assign(parameters,parameters+n_blocks);
+ updateJetsRealPart(param_vec);
+
+ // call functor
+ (*static_cast<CtrT const*>(this))(jets_0_->data(),
+ jets_1_->data(),
+ residuals_jets_->data());
+
+ // fill the residual array
+ for (auto i = 0; i < RES; i++)
+ residuals[i] = (*residuals_jets_)[i].a;
+
+ // fill the jacobian matrices
+ for (auto i = 0; i<n_blocks; i++)
+ if (jacobians[i] != nullptr)
+ for (unsigned int row = 0; row < RES; row++)
+ std::copy((*residuals_jets_)[row].v.data() + jacobian_locations_.at(i),
+ (*residuals_jets_)[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
+ jacobians[i] + row * state_block_sizes_.at(i));
+ }
+ return true;
+ }
+
+ void updateJetsRealPart(const std::vector<double const*>& parameters) const
+ {
+ // update jets real part
+ for (auto i = 0; i < B0; i++)
+ (*jets_0_)[i].a = parameters[0][i];
+ for (auto i = 0; i < B1; i++)
+ (*jets_1_)[i].a = parameters[1][i];
+ }
+
+ virtual void evaluate(const std::vector<const Scalar*>& _states_ptr, Eigen::VectorXs& residual_, std::vector<Eigen::MatrixXs>& jacobians_) const
+ {
+ assert(residual_.size() == RES);
+ jacobians_.clear();
+
+ assert(_states_ptr.size() == n_blocks);
+
+ // init jacobian
+ for(auto i = 0; i < n_blocks; ++i)
+ {
+ Eigen::MatrixXs Ji = Eigen::MatrixXs::Zero(RES, state_block_sizes_[i]);
+ jacobians_.push_back(Ji);
+ }
+
+ // update jets real part
+ updateJetsRealPart(_states_ptr);
+
+ // call functor
+ (*static_cast<CtrT const*>(this))(jets_0_->data(),
+ jets_1_->data(),
+ residuals_jets_->data());
+
+ // fill the residual vector
+ for (auto i = 0; i < RES; i++)
+ residual_(i) = (*residuals_jets_)[i].a;
+
+ // fill the jacobian matrices
+ for (auto i = 0; i<n_blocks; i++)
+ if (!state_ptrs_[i]->isFixed())
+ for (unsigned int row = 0; row < RES; row++)
+ std::copy((*residuals_jets_)[row].v.data() + jacobian_locations_.at(i),
+ (*residuals_jets_)[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
+ jacobians_[i].data() + row * state_block_sizes_.at(i));
+
+ // print jacobian matrices
+// for (auto i = 0; i < n_blocks; i++)
+// std::cout << jacobians_[i] << std::endl << std::endl;
+ }
+
+ virtual std::vector<Scalar*> getStateScalarPtrVector() const
+ {
+ return std::vector<Scalar*>({state_ptrs_[0]->getPtr(),
+ state_ptrs_[1]->getPtr()
+ });
+ }
+
+ virtual std::vector<StateBlockPtr> getStateBlockPtrVector() const
+ {
+ return state_ptrs_;
+ }
+
+ virtual std::vector<unsigned int> getStateSizes() const
+ {
+ return state_block_sizes_;
+ }
+
+ virtual unsigned int getSize() const
+ {
+ return RES;
+ }
+};
+
+////////////////// SPECIALIZATION 1 BLOCK ////////////////////////////////////////////////////////////////////////
+
+template <class CtrT,unsigned int RES,unsigned int B0>
+class ConstraintAutodiff<CtrT,RES,B0,0,0,0,0,0,0,0,0,0> : public ConstraintBase
+{
+ public:
+
+ static const unsigned int residualSize = RES;
+ static const unsigned int block0Size = B0;
+ static const unsigned int block1Size = 0;
+ static const unsigned int block2Size = 0;
+ static const unsigned int block3Size = 0;
+ static const unsigned int block4Size = 0;
+ static const unsigned int block5Size = 0;
+ static const unsigned int block6Size = 0;
+ static const unsigned int block7Size = 0;
+ static const unsigned int block8Size = 0;
+ static const unsigned int block9Size = 0;
+ static const unsigned int n_blocks = 1;
+
+ static const std::vector<unsigned int> state_block_sizes_;
+
+ typedef ceres::Jet<Scalar, B0> WolfJet;
+
+ protected:
+
+ std::vector<StateBlockPtr> state_ptrs_;
+
+ static const std::vector<unsigned int> jacobian_locations_;
+ std::array<WolfJet, RES>* residuals_jets_;
+
+ std::array<WolfJet, B0>* jets_0_;
+
+ public:
+
+ ConstraintAutodiff(ConstraintType _tp,
+ const FrameBasePtr& _frame_other_ptr,
+ const FeatureBasePtr& _feature_other_ptr,
+ const LandmarkBasePtr& _landmark_other_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ ConstraintStatus _status,
+ StateBlockPtr _state0Ptr) :
+ ConstraintBase(_tp, _frame_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
+ state_ptrs_({_state0Ptr}),
+ residuals_jets_(new std::array<WolfJet, RES>),
+ jets_0_(new std::array<WolfJet, B0>)
+ {
+ // initialize jets
+ unsigned int last_jet_idx = 0;
+ for (auto i = 0; i < B0; i++)
+ (*jets_0_)[i] = WolfJet(0, last_jet_idx++);
+ state_ptrs_.resize(n_blocks);
+ }
+
+ virtual ~ConstraintAutodiff()
+ {
+ delete jets_0_;
+ delete residuals_jets_;
+ }
+
+ virtual JacobianMethod getJacobianMethod() const
+ {
+ return JAC_AUTO;
+ }
+
+ virtual bool evaluate(double const* const* parameters, double* residuals, double** jacobians) const
+ {
+ // only residuals
+ if (jacobians == nullptr)
+ {
+ (*static_cast<CtrT const*>(this))(parameters[0],
+ residuals);
+ }
+ // also compute jacobians
+ else
+ {
+ // update jets real part
+ std::vector<double const*> param_vec;
+ param_vec.assign(parameters,parameters+n_blocks);
+ updateJetsRealPart(param_vec);
+
+ // call functor
+ (*static_cast<CtrT const*>(this))(jets_0_->data(),
+ residuals_jets_->data());
+
+ // fill the residual array
+ for (auto i = 0; i < RES; i++)
+ residuals[i] = (*residuals_jets_)[i].a;
+
+ // fill the jacobian matrices
+ for (auto i = 0; i<n_blocks; i++)
+ if (jacobians[i] != nullptr)
+ for (unsigned int row = 0; row < RES; row++)
+ std::copy((*residuals_jets_)[row].v.data() + jacobian_locations_.at(i),
+ (*residuals_jets_)[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
+ jacobians[i] + row * state_block_sizes_.at(i));
+ }
+ return true;
+ }
+
+ void updateJetsRealPart(const std::vector<double const*>& parameters) const
+ {
+ // update jets real part
+ for (auto i = 0; i < B0; i++)
+ (*jets_0_)[i].a = parameters[0][i];
+ }
+
+ virtual void evaluate(const std::vector<const Scalar*>& _states_ptr, Eigen::VectorXs& residual_, std::vector<Eigen::MatrixXs>& jacobians_) const
+ {
+ assert(residual_.size() == RES);
+ jacobians_.clear();
+
+ assert(_states_ptr.size() == n_blocks);
+
+ // init jacobian
+ for(auto i = 0; i < n_blocks; ++i)
+ {
+ Eigen::MatrixXs Ji = Eigen::MatrixXs::Zero(RES, state_block_sizes_[i]);
+ jacobians_.push_back(Ji);
+ }
+
+ // update jets real part
+ updateJetsRealPart(_states_ptr);
+
+ // call functor
+ (*static_cast<CtrT const*>(this))(jets_0_->data(),
+ residuals_jets_->data());
+
+ // fill the residual vector
+ for (auto i = 0; i < RES; i++)
+ residual_(i) = (*residuals_jets_)[i].a;
+
+ // fill the jacobian matrices
+ for (auto i = 0; i<n_blocks; i++)
+ if (!state_ptrs_[i]->isFixed())
+ for (unsigned int row = 0; row < RES; row++)
+ std::copy((*residuals_jets_)[row].v.data() + jacobian_locations_.at(i),
+ (*residuals_jets_)[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
+ jacobians_[i].data() + row * state_block_sizes_.at(i));
+
+ // print jacobian matrices
+// for (auto i = 0; i < n_blocks; i++)
+// std::cout << jacobians_[i] << std::endl << std::endl;
+ }
+
+ virtual std::vector<Scalar*> getStateScalarPtrVector() const
+ {
+ return std::vector<Scalar*>({state_ptrs_[0]->getPtr()
+ });
+ }
+
+ virtual std::vector<StateBlockPtr> getStateBlockPtrVector() const
+ {
+ return state_ptrs_;
+ }
+
+ virtual std::vector<unsigned int> getStateSizes() const
+ {
+ return state_block_sizes_;
+ }
+
+ virtual unsigned int getSize() const
+ {
+ return RES;
+ }
+};
+
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+// STATIC CONST VECTORS INITIALIZATION
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+// state_block_sizes_
+// 10 BLOCKS
+template <class CtrT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6,unsigned int B7,unsigned int B8,unsigned int B9>
+const std::vector<unsigned int> ConstraintAutodiff<CtrT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,B9>::state_block_sizes_ = {B0,B1,B2,B3,B4,B5,B6,B7,B8,B9};
+// 9 BLOCKS
+template <class CtrT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6,unsigned int B7,unsigned int B8>
+const std::vector<unsigned int> ConstraintAutodiff<CtrT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,0>::state_block_sizes_ = {B0,B1,B2,B3,B4,B5,B6,B7,B8};
+// 8 BLOCKS
+template <class CtrT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6,unsigned int B7>
+const std::vector<unsigned int> ConstraintAutodiff<CtrT,RES,B0,B1,B2,B3,B4,B5,B6,B7,0,0>::state_block_sizes_ = {B0,B1,B2,B3,B4,B5,B6,B7};
+// 7 BLOCKS
+template <class CtrT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6>
+const std::vector<unsigned int> ConstraintAutodiff<CtrT,RES,B0,B1,B2,B3,B4,B5,B6,0,0,0>::state_block_sizes_ = {B0,B1,B2,B3,B4,B5,B6};
+// 6 BLOCKS
+template <class CtrT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5>
+const std::vector<unsigned int> ConstraintAutodiff<CtrT,RES,B0,B1,B2,B3,B4,B5,0,0,0,0>::state_block_sizes_ = {B0,B1,B2,B3,B4,B5};
+// 5 BLOCKS
+template <class CtrT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4>
+const std::vector<unsigned int> ConstraintAutodiff<CtrT,RES,B0,B1,B2,B3,B4,0,0,0,0,0>::state_block_sizes_ = {B0,B1,B2,B3,B4};
+// 4 BLOCKS
+template <class CtrT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3>
+const std::vector<unsigned int> ConstraintAutodiff<CtrT,RES,B0,B1,B2,B3,0,0,0,0,0,0>::state_block_sizes_ = {B0,B1,B2,B3};
+// 3 BLOCKS
+template <class CtrT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2>
+const std::vector<unsigned int> ConstraintAutodiff<CtrT,RES,B0,B1,B2,0,0,0,0,0,0,0>::state_block_sizes_ = {B0,B1,B2};
+// 2 BLOCKS
+template <class CtrT,unsigned int RES,unsigned int B0,unsigned int B1>
+const std::vector<unsigned int> ConstraintAutodiff<CtrT,RES,B0,B1,0,0,0,0,0,0,0,0>::state_block_sizes_ = {B0,B1};
+// 1 BLOCK
+template <class CtrT,unsigned int RES,unsigned int B0>
+const std::vector<unsigned int> ConstraintAutodiff<CtrT,RES,B0,0,0,0,0,0,0,0,0,0>::state_block_sizes_ = {B0};
+
+// jacobian_locations_
+// 10 BLOCKS
+template <class CtrT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6,unsigned int B7,unsigned int B8,unsigned int B9>
+const std::vector<unsigned int> ConstraintAutodiff<CtrT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,B9>::jacobian_locations_ = {0,
+ B0,
+ B0+B1,
+ B0+B1+B2,
+ B0+B1+B2+B3,
+ B0+B1+B2+B3+B4,
+ B0+B1+B2+B3+B4+B5,
+ B0+B1+B2+B3+B4+B5+B6,
+ B0+B1+B2+B3+B4+B5+B6+B7,
+ B0+B1+B2+B3+B4+B5+B6+B7+B8};
+// 9 BLOCKS
+template <class CtrT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6,unsigned int B7,unsigned int B8>
+const std::vector<unsigned int> ConstraintAutodiff<CtrT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,0>::jacobian_locations_ = {0,
+ B0,
+ B0+B1,
+ B0+B1+B2,
+ B0+B1+B2+B3,
+ B0+B1+B2+B3+B4,
+ B0+B1+B2+B3+B4+B5,
+ B0+B1+B2+B3+B4+B5+B6,
+ B0+B1+B2+B3+B4+B5+B6+B7};
+// 8 BLOCKS
+template <class CtrT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6,unsigned int B7>
+const std::vector<unsigned int> ConstraintAutodiff<CtrT,RES,B0,B1,B2,B3,B4,B5,B6,B7,0,0>::jacobian_locations_ = {0,
+ B0,
+ B0+B1,
+ B0+B1+B2,
+ B0+B1+B2+B3,
+ B0+B1+B2+B3+B4,
+ B0+B1+B2+B3+B4+B5,
+ B0+B1+B2+B3+B4+B5+B6};
+// 7 BLOCKS
+template <class CtrT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6>
+const std::vector<unsigned int> ConstraintAutodiff<CtrT,RES,B0,B1,B2,B3,B4,B5,B6,0,0,0>::jacobian_locations_ = {0,
+ B0,
+ B0+B1,
+ B0+B1+B2,
+ B0+B1+B2+B3,
+ B0+B1+B2+B3+B4,
+ B0+B1+B2+B3+B4+B5};
+// 6 BLOCKS
+template <class CtrT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5>
+const std::vector<unsigned int> ConstraintAutodiff<CtrT,RES,B0,B1,B2,B3,B4,B5,0,0,0,0>::jacobian_locations_ = {0,
+ B0,
+ B0+B1,
+ B0+B1+B2,
+ B0+B1+B2+B3,
+ B0+B1+B2+B3+B4};
+// 5 BLOCKS
+template <class CtrT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4>
+const std::vector<unsigned int> ConstraintAutodiff<CtrT,RES,B0,B1,B2,B3,B4,0,0,0,0,0>::jacobian_locations_ = {0,
+ B0,
+ B0+B1,
+ B0+B1+B2,
+ B0+B1+B2+B3};
+// 4 BLOCKS
+template <class CtrT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3>
+const std::vector<unsigned int> ConstraintAutodiff<CtrT,RES,B0,B1,B2,B3,0,0,0,0,0,0>::jacobian_locations_ = {0,
+ B0,
+ B0+B1,
+ B0+B1+B2};
+// 3 BLOCKS
+template <class CtrT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2>
+const std::vector<unsigned int> ConstraintAutodiff<CtrT,RES,B0,B1,B2,0,0,0,0,0,0,0>::jacobian_locations_ = {0,
+ B0,
+ B0+B1};
+// 2 BLOCKS
+template <class CtrT,unsigned int RES,unsigned int B0,unsigned int B1>
+const std::vector<unsigned int> ConstraintAutodiff<CtrT,RES,B0,B1,0,0,0,0,0,0,0,0>::jacobian_locations_ = {0,
+ B0};
+// 1 BLOCK
+template <class CtrT,unsigned int RES,unsigned int B0>
+const std::vector<unsigned int> ConstraintAutodiff<CtrT,RES,B0,0,0,0,0,0,0,0,0,0>::jacobian_locations_ = {0};
+
+} // namespace wolf
+
+#endif
diff --git a/src/constraint_base.h b/src/constraint_base.h
index ba85eaef7f605dd8930b01e62b04730e919d51d0..bd5cd88f0fec4b8cfa48bcbb0c0e9692c14fefc2 100644
--- a/src/constraint_base.h
+++ b/src/constraint_base.h
@@ -59,17 +59,29 @@ class ConstraintBase : public NodeBase, public std::enable_shared_from_this<Cons
**/
ConstraintType getTypeId() const;
+ /** \brief Evaluate the constraint given the input parameters and returning the residuals and jacobians
+ **/
+ virtual bool evaluate(double const* const* parameters, double* residuals, double** jacobians) const = 0;
+
+ /** Returns a vector of Jacobian matrix corresponding to each state block evaluated in the point provided in _states_ptr and the residual vector
+ **/
+ virtual void evaluate(const std::vector<const Scalar*>& _states_ptr, Eigen::VectorXs& residual_, std::vector<Eigen::MatrixXs>& jacobians_) const = 0;
+
/** \brief Returns the jacobians computation method
**/
virtual JacobianMethod getJacobianMethod() const = 0;
/** \brief Returns a vector of scalar pointers to the first element of all state blocks involved in the constraint
**/
- virtual const std::vector<Scalar*> getStateScalarPtrVector() = 0;
+ virtual std::vector<Scalar*> getStateScalarPtrVector() const = 0;
/** \brief Returns a vector of pointers to the states in which this constraint depends
**/
- virtual const std::vector<StateBlockPtr> getStateBlockPtrVector() const = 0;
+ virtual std::vector<StateBlockPtr> getStateBlockPtrVector() const = 0;
+
+ /** \brief Returns a vector of the states sizes
+ **/
+ virtual std::vector<unsigned int> getStateSizes() const = 0;
/** \brief Returns a reference to the feature measurement
**/
diff --git a/src/constraint_container.h b/src/constraint_container.h
index 41f7f3f3a93a56cc2f580a30107add1192de3964..7ec89a5a791810ddfb275d0b19dd6238a142d1d7 100644
--- a/src/constraint_container.h
+++ b/src/constraint_container.h
@@ -3,14 +3,14 @@
//Wolf includes
#include "wolf.h"
-#include "constraint_sparse.h"
+#include "constraint_autodiff.h"
#include "landmark_container.h"
namespace wolf {
WOLF_PTR_TYPEDEFS(ConstraintContainer);
-class ConstraintContainer: public ConstraintSparse<3,2,1,2,1>
+class ConstraintContainer: public ConstraintAutodiff<ConstraintContainer,3,2,1,2,1>
{
protected:
LandmarkContainerWPtr lmk_ptr_;
@@ -23,7 +23,7 @@ class ConstraintContainer: public ConstraintSparse<3,2,1,2,1>
const ProcessorBasePtr& _processor_ptr,
const unsigned int _corner,
bool _apply_loss_function = false, ConstraintStatus _status = CTR_ACTIVE) :
- ConstraintSparse<3,2,1,2,1>(CTR_CONTAINER, nullptr, nullptr, _lmk_ptr, _processor_ptr, _apply_loss_function, _status, _ftr_ptr->getFramePtr()->getPPtr(),_ftr_ptr->getFramePtr()->getOPtr(), _lmk_ptr->getPPtr(), _lmk_ptr->getOPtr()),
+ ConstraintAutodiff<ConstraintContainer,3,2,1,2,1>(CTR_CONTAINER, nullptr, nullptr, _lmk_ptr, _processor_ptr, _apply_loss_function, _status, _ftr_ptr->getFramePtr()->getPPtr(),_ftr_ptr->getFramePtr()->getOPtr(), _lmk_ptr->getPPtr(), _lmk_ptr->getOPtr()),
lmk_ptr_(_lmk_ptr),
corner_(_corner)
{
diff --git a/src/constraint_corner_2D.h b/src/constraint_corner_2D.h
index 6a5ea8a06aa76d3523dfcacfea4acc542578df9d..d3702dc58bf17dc250f6c5687a4422eb07829eb7 100644
--- a/src/constraint_corner_2D.h
+++ b/src/constraint_corner_2D.h
@@ -2,14 +2,14 @@
#define CONSTRAINT_CORNER_2D_THETA_H_
//Wolf includes
-#include "constraint_sparse.h"
+#include "constraint_autodiff.h"
#include "landmark_corner_2D.h"
namespace wolf {
WOLF_PTR_TYPEDEFS(ConstraintCorner2D);
-class ConstraintCorner2D: public ConstraintSparse<3,2,1,2,1>
+class ConstraintCorner2D: public ConstraintAutodiff<ConstraintCorner2D, 3,2,1,2,1>
{
public:
@@ -18,7 +18,7 @@ class ConstraintCorner2D: public ConstraintSparse<3,2,1,2,1>
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function = false,
ConstraintStatus _status = CTR_ACTIVE) :
- ConstraintSparse<3,2,1,2,1>(CTR_CORNER_2D, nullptr, nullptr, _lmk_ptr, _processor_ptr, _apply_loss_function, _status, _ftr_ptr->getFramePtr()->getPPtr(),_ftr_ptr->getFramePtr()->getOPtr(), _lmk_ptr->getPPtr(), _lmk_ptr->getOPtr())
+ ConstraintAutodiff<ConstraintCorner2D,3,2,1,2,1>(CTR_CORNER_2D, nullptr, nullptr, _lmk_ptr, _processor_ptr, _apply_loss_function, _status, _ftr_ptr->getFramePtr()->getPPtr(),_ftr_ptr->getFramePtr()->getOPtr(), _lmk_ptr->getPPtr(), _lmk_ptr->getOPtr())
{
setType("CORNER 2D");
}
@@ -35,16 +35,15 @@ class ConstraintCorner2D: public ConstraintSparse<3,2,1,2,1>
const T* const _landmarkO, T* _residuals) const;
/** \brief Returns the jacobians computation method
- *
- * Returns the jacobians computation method
- *
- **/
+ *
+ * Returns the jacobians computation method
+ *
+ **/
virtual JacobianMethod getJacobianMethod() const override
{
return JAC_AUTO;
}
-public:
static ConstraintBasePtr create(const FeatureBasePtr& _feature_ptr, const NodeBasePtr& _correspondant_ptr, const ProcessorBasePtr& _processor_ptr = nullptr)
{
return std::make_shared<ConstraintCorner2D>(_feature_ptr, std::static_pointer_cast<LandmarkCorner2D>(_correspondant_ptr), _processor_ptr);
diff --git a/src/constraint_epipolar.h b/src/constraint_epipolar.h
index 199888a9a2751c428e4102cc77ec6a14b69f7f76..bb6d1f86f074ed913a583ee7cc3e322780b497e5 100644
--- a/src/constraint_epipolar.h
+++ b/src/constraint_epipolar.h
@@ -19,22 +19,34 @@ class ConstraintEpipolar : public ConstraintBase
virtual ~ConstraintEpipolar() = default;
+
+ /** \brief Evaluate the constraint given the input parameters and returning the residuals and jacobians
+ **/
+ virtual bool evaluate(double const* const* parameters, double* residuals, double** jacobians) const{return true;};
+
+ /** Returns a residual vector and a vector of Jacobian matrix corresponding to each state block evaluated in the point provided in _states_ptr
+ **/
+ virtual void evaluate(const std::vector<const Scalar*>& _states_ptr, Eigen::VectorXs& residual_, std::vector<Eigen::MatrixXs>& jacobians_) const{};
/** \brief Returns the jacobians computation method
**/
virtual JacobianMethod getJacobianMethod() const override {return JAC_ANALYTIC;}
/** \brief Returns a vector of scalar pointers to the first element of all state blocks involved in the constraint
**/
- virtual const std::vector<Scalar*> getStateScalarPtrVector() override {return std::vector<Scalar*>(0);}
+ virtual std::vector<Scalar*> getStateScalarPtrVector() const override {return std::vector<Scalar*>(0);}
/** \brief Returns a vector of pointers to the states in which this constraint depends
**/
- virtual const std::vector<StateBlockPtr> getStateBlockPtrVector() const override {return std::vector<StateBlockPtr>(0);}
+ virtual std::vector<StateBlockPtr> getStateBlockPtrVector() const override {return std::vector<StateBlockPtr>(0);}
/** \brief Returns the constraint residual size
**/
virtual unsigned int getSize() const override {return 0;}
+ /** \brief Returns the constraint states sizes
+ **/
+ virtual std::vector<unsigned int> getStateSizes() const{return std::vector<unsigned int>({1});}
+
public:
static wolf::ConstraintBasePtr create(const FeatureBasePtr& _feature_ptr,
const NodeBasePtr& _correspondant_ptr,
diff --git a/src/constraint_fix.h b/src/constraint_fix.h
index 77bfe34330a429f5f660f658647674e8cb33f1bc..8b140749947b8b88007d193150a63ede0d7d5eab 100644
--- a/src/constraint_fix.h
+++ b/src/constraint_fix.h
@@ -3,7 +3,7 @@
#define CONSTRAINT_FIX_H_
//Wolf includes
-#include "constraint_sparse.h"
+#include "constraint_autodiff.h"
#include "frame_base.h"
#include "rotations.h"
@@ -15,12 +15,11 @@ namespace wolf {
WOLF_PTR_TYPEDEFS(ConstraintFix);
//class
-class ConstraintFix: public ConstraintSparse<3,2,1>
+class ConstraintFix: public ConstraintAutodiff<ConstraintFix,3,2,1>
{
public:
ConstraintFix(FeatureBasePtr _ftr_ptr, bool _apply_loss_function = false, ConstraintStatus _status = CTR_ACTIVE) :
- ConstraintSparse<3, 2, 1>(CTR_FIX, _apply_loss_function, _status, _ftr_ptr->getFramePtr()->getPPtr(),
- _ftr_ptr->getFramePtr()->getOPtr())
+ ConstraintAutodiff<ConstraintFix, 3, 2, 1>(CTR_FIX, nullptr, nullptr, nullptr, nullptr,_apply_loss_function, _status, _ftr_ptr->getFramePtr()->getPPtr(), _ftr_ptr->getFramePtr()->getOPtr())
{
setType("FIX");
// std::cout << "created ConstraintFix " << std::endl;
diff --git a/src/constraint_fix_3D.h b/src/constraint_fix_3D.h
index bd5cb3750984c4d56535b338db339bfc96d44f31..b64f63e699a6501543a7fdbd74c323e00a0c460e 100644
--- a/src/constraint_fix_3D.h
+++ b/src/constraint_fix_3D.h
@@ -3,7 +3,7 @@
#define CONSTRAINT_FIX_3D_H_
//Wolf includes
-#include "constraint_sparse.h"
+#include "constraint_autodiff.h"
#include "frame_base.h"
#include "rotations.h"
@@ -13,13 +13,12 @@ namespace wolf {
WOLF_PTR_TYPEDEFS(ConstraintFix3D);
//class
-class ConstraintFix3D: public ConstraintSparse<6,3,4>
+class ConstraintFix3D: public ConstraintAutodiff<ConstraintFix3D,6,3,4>
{
public:
ConstraintFix3D(FeatureBasePtr _ftr_ptr, bool _apply_loss_function = false, ConstraintStatus _status = CTR_ACTIVE) :
- ConstraintSparse<6,3,4>(CTR_FIX_3D, _apply_loss_function, _status, _ftr_ptr->getFramePtr()->getPPtr(),
- _ftr_ptr->getFramePtr()->getOPtr())
+ ConstraintAutodiff<ConstraintFix3D,6,3,4>(CTR_FIX_3D, nullptr, nullptr, nullptr, nullptr, _apply_loss_function, _status, _ftr_ptr->getFramePtr()->getPPtr(), _ftr_ptr->getFramePtr()->getOPtr())
{
setType("FIX3D");
}
diff --git a/src/constraint_fix_bias.h b/src/constraint_fix_bias.h
index 3ccdc2377e45f96de99016ae02380ea1ba50966f..6afa4adc69435469cbdf3a8b3e0279164cc711d1 100644
--- a/src/constraint_fix_bias.h
+++ b/src/constraint_fix_bias.h
@@ -3,7 +3,7 @@
#define CONSTRAINT_FIX_BIAS_H_
//Wolf includes
-#include "constraint_sparse.h"
+#include "constraint_autodiff.h"
#include "frame_base.h"
#include "frame_imu.h"
#include "rotations.h"
@@ -17,11 +17,11 @@ namespace wolf {
WOLF_PTR_TYPEDEFS(ConstraintFixBias);
//class
-class ConstraintFixBias: public ConstraintSparse<6,3,3>
+class ConstraintFixBias: public ConstraintAutodiff<ConstraintFixBias,6,3,3>
{
public:
ConstraintFixBias(FeatureBasePtr _ftr_ptr, bool _apply_loss_function = false, ConstraintStatus _status = CTR_ACTIVE) :
- ConstraintSparse<6, 3, 3>(CTR_FIX_BIAS, _apply_loss_function, _status, std::static_pointer_cast<FrameIMU>(_ftr_ptr->getFramePtr())->getAccBiasPtr(),
+ ConstraintAutodiff<ConstraintFixBias, 6, 3, 3>(CTR_FIX_BIAS, nullptr, nullptr, nullptr, nullptr, _apply_loss_function, _status, std::static_pointer_cast<FrameIMU>(_ftr_ptr->getFramePtr())->getAccBiasPtr(),
std::static_pointer_cast<FrameIMU>(_ftr_ptr->getFramePtr())->getGyroBiasPtr())
{
setType("FIX BIAS");
diff --git a/src/constraint_gps_2D.h b/src/constraint_gps_2D.h
index 874cba0f5271ab046af4e651c28d9594eb450c59..6a5948efb83027c2512c5be221d43f87c0829d69 100644
--- a/src/constraint_gps_2D.h
+++ b/src/constraint_gps_2D.h
@@ -4,19 +4,19 @@
//Wolf includes
#include "wolf.h"
-#include "constraint_sparse.h"
+#include "constraint_autodiff.h"
#include "frame_base.h"
namespace wolf {
WOLF_PTR_TYPEDEFS(ConstraintGPS2D);
-class ConstraintGPS2D : public ConstraintSparse<2, 2>
+class ConstraintGPS2D : public ConstraintAutodiff<ConstraintGPS2D, 2, 2>
{
public:
ConstraintGPS2D(FeatureBasePtr _ftr_ptr, bool _apply_loss_function = false, ConstraintStatus _status = CTR_ACTIVE) :
- ConstraintSparse<2, 2>(CTR_GPS_FIX_2D, _apply_loss_function, _status, _ftr_ptr->getFramePtr()->getPPtr())
+ ConstraintAutodiff<ConstraintGPS2D, 2, 2>(CTR_GPS_FIX_2D, nullptr, nullptr, nullptr, nullptr, _apply_loss_function, _status, _ftr_ptr->getFramePtr()->getPPtr())
{
setType("GPS FIX 2D");
}
diff --git a/src/constraint_gps_pseudorange_2D.h b/src/constraint_gps_pseudorange_2D.h
index 409dc35699f5e10a1dbe2c7702fb8106e9b0b2b6..0873355892d0d82b74c78a98fe238bbda034a2df 100644
--- a/src/constraint_gps_pseudorange_2D.h
+++ b/src/constraint_gps_pseudorange_2D.h
@@ -6,7 +6,7 @@
//Wolf includes
#include "sensor_gps.h"
#include "feature_gps_pseudorange.h"
-#include "constraint_sparse.h"
+#include "constraint_autodiff.h"
//std
#include <string>
@@ -24,13 +24,13 @@ WOLF_PTR_TYPEDEFS(ConstraintGPSPseudorange2D);
*
* TODO maybe is no more true
*/
-class ConstraintGPSPseudorange2D : public ConstraintSparse<1, 2, 1, 3, 1, 3, 1>
+class ConstraintGPSPseudorange2D : public ConstraintAutodiff<ConstraintGPSPseudorange2D, 1, 2, 1, 3, 1, 3, 1>
{
public:
ConstraintGPSPseudorange2D(const FeatureBasePtr& _ftr_ptr,
bool _apply_loss_function = false,
ConstraintStatus _status = CTR_ACTIVE) :
- ConstraintSparse<1, 2, 1, 3, 1, 3, 1>(CTR_GPS_PR_2D,
+ ConstraintAutodiff<ConstraintGPSPseudorange2D, 1, 2, 1, 3, 1, 3, 1>(CTR_GPS_PR_2D,
_apply_loss_function,
_status,
_ftr_ptr->getFramePtr()->getPPtr(), // position of the vehicle's frame with respect to the initial pos frame
diff --git a/src/constraint_gps_pseudorange_3D.h b/src/constraint_gps_pseudorange_3D.h
index c87cb87933717f9ef30fc25097e012982710afa7..47e4d95ffc0a263ec2215742e473efa3165fac4d 100644
--- a/src/constraint_gps_pseudorange_3D.h
+++ b/src/constraint_gps_pseudorange_3D.h
@@ -6,7 +6,7 @@
//Wolf includes
#include "sensor_gps.h"
#include "feature_gps_pseudorange.h"
-#include "constraint_sparse.h"
+#include "constraint_autodiff.h"
namespace wolf {
@@ -21,7 +21,7 @@ WOLF_PTR_TYPEDEFS(ConstraintGPSPseudorange3D);
*
* TODO maybe is no more true
*/
-class ConstraintGPSPseudorange3D: public ConstraintSparse<1, 3, 4, 3, 1, 3, 4>
+class ConstraintGPSPseudorange3D: public ConstraintAutodiff<ConstraintGPSPseudorange3D, 1, 3, 4, 3, 1, 3, 4>
{
public:
@@ -29,7 +29,7 @@ class ConstraintGPSPseudorange3D: public ConstraintSparse<1, 3, 4, 3, 1, 3, 4>
ConstraintGPSPseudorange3D(FeatureBasePtr _ftr_ptr,
bool _apply_loss_function = false,
ConstraintStatus _status = CTR_ACTIVE) :
- ConstraintSparse<1, 3, 4, 3, 1, 3, 4>(CTR_GPS_PR_3D, _apply_loss_function, _status,
+ ConstraintAutodiff<ConstraintGPSPseudorange3D, 1, 3, 4, 3, 1, 3, 4>(CTR_GPS_PR_3D, nullptr, nullptr, nullptr, _apply_loss_function, _status,
_ftr_ptr->getFramePtr()->getPPtr(), // position of the vehicle's frame with respect to map frame
_ftr_ptr->getFramePtr()->getOPtr(), // orientation of the vehicle's frame wrt map frame
_ftr_ptr->getCapturePtr()->getSensorPPtr(), // position of the sensor (gps antenna) with respect to base frame
diff --git a/src/constraint_imu.h b/src/constraint_imu.h
index 7ae0c07b7408ff6dbdb86d88a137ae39cb33ce07..2a21202a2a8f38c3fc47342cdd244ed10b6a61a6 100644
--- a/src/constraint_imu.h
+++ b/src/constraint_imu.h
@@ -2,7 +2,7 @@
#define CONSTRAINT_IMU_THETA_H_
//Wolf includes
-#include "constraint_sparse.h"
+#include "constraint_autodiff.h"
#include "feature_imu.h"
#include "frame_imu.h"
#include "sensor_imu.h"
@@ -16,7 +16,7 @@ namespace wolf {
WOLF_PTR_TYPEDEFS(ConstraintIMU);
//class
-class ConstraintIMU : public ConstraintSparse<15, 3, 4, 3, 3, 3, 3, 4, 3, 3, 3>
+class ConstraintIMU : public ConstraintAutodiff<ConstraintIMU, 15, 3, 4, 3, 3, 3, 3, 4, 3, 3, 3>
{
public:
ConstraintIMU(const FeatureIMUPtr& _ftr_ptr, const FrameIMUPtr& _frame_ptr,
@@ -145,7 +145,7 @@ inline ConstraintIMU::ConstraintIMU(const FeatureIMUPtr& _ftr_ptr,
const FrameIMUPtr& _frame_ptr,
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function, ConstraintStatus _status) :
- ConstraintSparse<15, 3, 4, 3, 3, 3, 3, 4, 3, 3, 3>(CTR_IMU, _frame_ptr, nullptr, nullptr, _processor_ptr, _apply_loss_function, _status,
+ ConstraintAutodiff<ConstraintIMU,15, 3, 4, 3, 3, 3, 3, 4, 3, 3, 3>(CTR_IMU, _frame_ptr, nullptr, nullptr, _processor_ptr, _apply_loss_function, _status,
_frame_ptr->getPPtr(), _frame_ptr->getOPtr(), _frame_ptr->getVPtr(),
_frame_ptr->getAccBiasPtr(), _frame_ptr->getGyroBiasPtr(),
_ftr_ptr->getFramePtr()->getPPtr(),
diff --git a/src/constraint_odom_2D.h b/src/constraint_odom_2D.h
index 2314f9179d774c7f582889c8b69a463490afff9d..e592e322eea96a477d402816203f5e2140ec8c86 100644
--- a/src/constraint_odom_2D.h
+++ b/src/constraint_odom_2D.h
@@ -2,7 +2,7 @@
#define CONSTRAINT_ODOM_2D_THETA_H_
//Wolf includes
-#include "constraint_sparse.h"
+#include "constraint_autodiff.h"
#include "frame_base.h"
//#include "ceres/jet.h"
@@ -10,16 +10,16 @@
namespace wolf {
WOLF_PTR_TYPEDEFS(ConstraintOdom2D);
-
+
//class
-class ConstraintOdom2D : public ConstraintSparse<3, 2, 1, 2, 1>
+class ConstraintOdom2D : public ConstraintAutodiff<ConstraintOdom2D, 3, 2, 1, 2, 1>
{
public:
ConstraintOdom2D(const FeatureBasePtr& _ftr_ptr,
const FrameBasePtr& _frame_ptr,
const ProcessorBasePtr& _processor_ptr = nullptr,
bool _apply_loss_function = false, ConstraintStatus _status = CTR_ACTIVE) :
- ConstraintSparse<3, 2, 1, 2, 1>(CTR_ODOM_2D, _frame_ptr, nullptr, nullptr, _processor_ptr, _apply_loss_function, _status, _frame_ptr->getPPtr(), _frame_ptr->getOPtr(), _ftr_ptr->getFramePtr()->getPPtr(), _ftr_ptr->getFramePtr()->getOPtr())
+ ConstraintAutodiff<ConstraintOdom2D, 3, 2, 1, 2, 1>(CTR_ODOM_2D, _frame_ptr, nullptr, nullptr, _processor_ptr, _apply_loss_function, _status, _frame_ptr->getPPtr(), _frame_ptr->getOPtr(), _ftr_ptr->getFramePtr()->getPPtr(), _ftr_ptr->getFramePtr()->getOPtr())
{
setType("ODOM 2D");
// std::cout << "created ConstraintOdom2D " << std::endl;
diff --git a/src/constraint_odom_3D.h b/src/constraint_odom_3D.h
index 2062bbba75e10762a694637977df63ade038bee6..e315489b89a5196323b3837cfc9611295b2ecb6e 100644
--- a/src/constraint_odom_3D.h
+++ b/src/constraint_odom_3D.h
@@ -8,7 +8,7 @@
#ifndef CONSTRAINT_ODOM_3D_H_
#define CONSTRAINT_ODOM_3D_H_
-#include "constraint_sparse.h"
+#include "constraint_autodiff.h"
#include "rotations.h"
namespace wolf
@@ -17,7 +17,7 @@ namespace wolf
WOLF_PTR_TYPEDEFS(ConstraintOdom3D);
//class
-class ConstraintOdom3D : public ConstraintSparse<6,3,4,3,4>
+class ConstraintOdom3D : public ConstraintAutodiff<ConstraintOdom3D,6,3,4,3,4>
{
public:
ConstraintOdom3D(const FeatureBasePtr& _ftr_current_ptr,
@@ -79,7 +79,7 @@ inline ConstraintOdom3D::ConstraintOdom3D(const FeatureBasePtr& _ftr_current_ptr
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function,
ConstraintStatus _status) :
- ConstraintSparse<6, 3, 4, 3, 4>(CTR_ODOM_3D, // type
+ ConstraintAutodiff<ConstraintOdom3D, 6, 3, 4, 3, 4>(CTR_ODOM_3D, // type
_frame_past_ptr, // frame other
nullptr, // feature other
nullptr, // landmark other
diff --git a/src/constraint_point_2D.h b/src/constraint_point_2D.h
index 9a092e1e045d96839e2c7ccaf02bda706cba2262..927743721fe640149df46d3a47931a4b1a7645dd 100644
--- a/src/constraint_point_2D.h
+++ b/src/constraint_point_2D.h
@@ -2,7 +2,7 @@
#define CONSTRAINT_POINT_2D_THETA_H_
//Wolf includes
-#include "constraint_sparse.h"
+#include "constraint_autodiff.h"
#include "feature_polyline_2D.h"
#include "landmark_polyline_2D.h"
@@ -13,7 +13,7 @@ WOLF_PTR_TYPEDEFS(ConstraintPoint2D);
/**
* @brief The ConstraintPoint2D class
*/
-class ConstraintPoint2D: public ConstraintSparse<2,2,1,2,1,2>
+class ConstraintPoint2D: public ConstraintAutodiff<ConstraintPoint2D, 2,2,1,2,1,2>
{
protected:
unsigned int feature_point_id_;
@@ -29,16 +29,16 @@ class ConstraintPoint2D: public ConstraintSparse<2,2,1,2,1,2>
const LandmarkPolyline2DPtr& _lmk_ptr,
const ProcessorBasePtr& _processor_ptr,
unsigned int _ftr_point_id, int _lmk_point_id, bool _apply_loss_function = false, ConstraintStatus _status = CTR_ACTIVE) :
- ConstraintSparse<2,2,1,2,1,2>(CTR_POINT_2D, nullptr, nullptr, _lmk_ptr, _processor_ptr, _apply_loss_function, _status, _ftr_ptr->getFramePtr()->getPPtr(), _ftr_ptr->getFramePtr()->getOPtr(), _lmk_ptr->getPPtr(), _lmk_ptr->getOPtr(), _lmk_ptr->getPointStateBlockPtr(_lmk_point_id)),
- feature_point_id_(_ftr_point_id), landmark_point_id_(_lmk_point_id), point_state_ptr_(_lmk_ptr->getPointStateBlockPtr(_lmk_point_id)), measurement_(_ftr_ptr->getPoints().col(_ftr_point_id)), measurement_covariance_(_ftr_ptr->getPointsCov().middleCols(_ftr_point_id*2,2))
- {
- //std::cout << "Constriant point: feature " << _ftr_ptr->id() << " landmark " << _lmk_ptr->id() << "(point " << _lmk_point_id << ")" << std::endl;
- //std::cout << "landmark state block " << _lmk_ptr->getPointStateBlockPtr(_lmk_point_id)->getVector().transpose() << std::endl;
- setType("POINT TO POINT 2D");
- Eigen::LLT<Eigen::MatrixXs> lltOfA(measurement_covariance_); // compute the Cholesky decomposition of A
- Eigen::MatrixXs measurement_sqrt_covariance = lltOfA.matrixU();
- measurement_sqrt_information_ = measurement_sqrt_covariance.inverse().transpose(); // retrieve factor U in the decomposition
- }
+ ConstraintAutodiff<ConstraintPoint2D,2,2,1,2,1,2>(CTR_POINT_2D, nullptr, nullptr, _lmk_ptr, _processor_ptr, _apply_loss_function, _status, _ftr_ptr->getFramePtr()->getPPtr(), _ftr_ptr->getFramePtr()->getOPtr(), _lmk_ptr->getPPtr(), _lmk_ptr->getOPtr(), _lmk_ptr->getPointStateBlockPtr(_lmk_point_id)),
+ feature_point_id_(_ftr_point_id), landmark_point_id_(_lmk_point_id), point_state_ptr_(_lmk_ptr->getPointStateBlockPtr(_lmk_point_id)), measurement_(_ftr_ptr->getPoints().col(_ftr_point_id)), measurement_covariance_(_ftr_ptr->getPointsCov().middleCols(_ftr_point_id*2,2))
+ {
+ //std::cout << "Constriant point: feature " << _ftr_ptr->id() << " landmark " << _lmk_ptr->id() << "(point " << _lmk_point_id << ")" << std::endl;
+ //std::cout << "landmark state block " << _lmk_ptr->getPointStateBlockPtr(_lmk_point_id)->getVector().transpose() << std::endl;
+ setType("POINT TO POINT 2D");
+ Eigen::LLT<Eigen::MatrixXs> lltOfA(measurement_covariance_); // compute the Cholesky decomposition of A
+ Eigen::MatrixXs measurement_sqrt_covariance = lltOfA.matrixU();
+ measurement_sqrt_information_ = measurement_sqrt_covariance.inverse().transpose(); // retrieve factor U in the decomposition
+ }
virtual ~ConstraintPoint2D() = default;
@@ -47,9 +47,9 @@ class ConstraintPoint2D: public ConstraintSparse<2,2,1,2,1,2>
* @return
*/
LandmarkPolyline2DPtr getLandmarkPtr()
- {
- return std::static_pointer_cast<LandmarkPolyline2D>(landmark_other_ptr_.lock());
- }
+ {
+ return std::static_pointer_cast<LandmarkPolyline2D>(landmark_other_ptr_.lock());
+ }
/**
* @brief getLandmarkPointId
diff --git a/src/constraint_point_to_line_2D.h b/src/constraint_point_to_line_2D.h
index 55eb97f4d5e9f04707f5447a839fc5a93da7d53a..c54b6ae50cb63ca1723817d688571bedd7f1f960 100644
--- a/src/constraint_point_to_line_2D.h
+++ b/src/constraint_point_to_line_2D.h
@@ -2,7 +2,7 @@
#define CONSTRAINT_POINT_TO_LINE_2D_H_
//Wolf includes
-#include "constraint_sparse.h"
+#include "constraint_autodiff.h"
#include "feature_polyline_2D.h"
#include "landmark_polyline_2D.h"
@@ -11,7 +11,7 @@ namespace wolf {
WOLF_PTR_TYPEDEFS(ConstraintPointToLine2D);
//class
-class ConstraintPointToLine2D: public ConstraintSparse<1,2,1,2,1,2,2>
+class ConstraintPointToLine2D: public ConstraintAutodiff<ConstraintPointToLine2D, 1,2,1,2,1,2,2>
{
protected:
int landmark_point_id_;
@@ -30,15 +30,15 @@ class ConstraintPointToLine2D: public ConstraintSparse<1,2,1,2,1,2,2>
const ProcessorBasePtr& _processor_ptr,
unsigned int _ftr_point_id, int _lmk_point_id, int _lmk_point_aux_id,
bool _apply_loss_function = false, ConstraintStatus _status = CTR_ACTIVE) :
- ConstraintSparse<1,2,1,2,1,2,2>(CTR_POINT_TO_LINE_2D, nullptr, nullptr, _lmk_ptr, _processor_ptr, _apply_loss_function, _status, _ftr_ptr->getFramePtr()->getPPtr(), _ftr_ptr->getFramePtr()->getOPtr(), _lmk_ptr->getPPtr(), _lmk_ptr->getOPtr(), _lmk_ptr->getPointStateBlockPtr(_lmk_point_id), _lmk_ptr->getPointStateBlockPtr(_lmk_point_aux_id)),
- landmark_point_id_(_lmk_point_id), landmark_point_aux_id_(_lmk_point_aux_id), feature_point_id_(_ftr_point_id), point_state_ptr_(_lmk_ptr->getPointStateBlockPtr(_lmk_point_id)), point_aux_state_ptr_(_lmk_ptr->getPointStateBlockPtr(_lmk_point_aux_id)), measurement_(_ftr_ptr->getPoints().col(_ftr_point_id)), measurement_covariance_(_ftr_ptr->getPointsCov().middleCols(_ftr_point_id*2,2))
- {
- //std::cout << "ConstraintPointToLine2D" << std::endl;
- setType("POINT TO LINE 2D");
- Eigen::LLT<Eigen::MatrixXs> lltOfA(measurement_covariance_); // compute the Cholesky decomposition of A
- Eigen::MatrixXs measurement_sqrt_covariance = lltOfA.matrixU();
- measurement_sqrt_information_ = measurement_sqrt_covariance.inverse().transpose(); // retrieve factor U in the decomposition
- }
+ ConstraintAutodiff<ConstraintPointToLine2D, 1,2,1,2,1,2,2>(CTR_POINT_TO_LINE_2D, nullptr, nullptr, _lmk_ptr, _processor_ptr, _apply_loss_function, _status, _ftr_ptr->getFramePtr()->getPPtr(), _ftr_ptr->getFramePtr()->getOPtr(), _lmk_ptr->getPPtr(), _lmk_ptr->getOPtr(), _lmk_ptr->getPointStateBlockPtr(_lmk_point_id), _lmk_ptr->getPointStateBlockPtr(_lmk_point_aux_id)),
+ landmark_point_id_(_lmk_point_id), landmark_point_aux_id_(_lmk_point_aux_id), feature_point_id_(_ftr_point_id), point_state_ptr_(_lmk_ptr->getPointStateBlockPtr(_lmk_point_id)), point_aux_state_ptr_(_lmk_ptr->getPointStateBlockPtr(_lmk_point_aux_id)), measurement_(_ftr_ptr->getPoints().col(_ftr_point_id)), measurement_covariance_(_ftr_ptr->getPointsCov().middleCols(_ftr_point_id*2,2))
+ {
+ //std::cout << "ConstraintPointToLine2D" << std::endl;
+ setType("POINT TO LINE 2D");
+ Eigen::LLT<Eigen::MatrixXs> lltOfA(measurement_covariance_); // compute the Cholesky decomposition of A
+ Eigen::MatrixXs measurement_sqrt_covariance = lltOfA.matrixU();
+ measurement_sqrt_information_ = measurement_sqrt_covariance.inverse().transpose(); // retrieve factor U in the decomposition
+ }
virtual ~ConstraintPointToLine2D() = default;
diff --git a/src/constraint_sparse.h b/src/constraint_sparse.h
deleted file mode 100644
index 3ba6ecac2a267d09c6dc7826b853f5c9456c8682..0000000000000000000000000000000000000000
--- a/src/constraint_sparse.h
+++ /dev/null
@@ -1,410 +0,0 @@
-
-#ifndef CONSTRAINT_SPARSE_H_
-#define CONSTRAINT_SPARSE_H_
-
-//Wolf includes
-#include "constraint_base.h"
-#include "state_block.h"
-
-namespace wolf {
-
-//TODO: change class name (and file name!->includes) to ConstraintNumericalAutoDiff
-//template class ConstraintSparse
-template <const unsigned int RESIDUAL_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>
-class ConstraintSparse: public ConstraintBase
-{
- protected:
- std::vector<StateBlockPtr> state_ptr_vector_;
- std::vector<unsigned int> state_block_sizes_vector_;
-
- public:
- static const unsigned int residualSize = RESIDUAL_SIZE;
- static const unsigned int block0Size = BLOCK_0_SIZE;
- static const unsigned int block1Size = BLOCK_1_SIZE;
- static const unsigned int block2Size = BLOCK_2_SIZE;
- static const unsigned int block3Size = BLOCK_3_SIZE;
- static const unsigned int block4Size = BLOCK_4_SIZE;
- static const unsigned int block5Size = BLOCK_5_SIZE;
- static const unsigned int block6Size = BLOCK_6_SIZE;
- static const unsigned int block7Size = BLOCK_7_SIZE;
- static const unsigned int block8Size = BLOCK_8_SIZE;
- static const unsigned int block9Size = BLOCK_9_SIZE;
-
- /** \brief Constructor of category ABSOLUTE
- *
- * Constructor of category ABSOLUTE
- *
- **/
- ConstraintSparse(ConstraintType _tp, bool _apply_loss_function, ConstraintStatus _status,
- StateBlockPtr _state0Ptr,
- StateBlockPtr _state1Ptr = nullptr,
- StateBlockPtr _state2Ptr = nullptr,
- StateBlockPtr _state3Ptr = nullptr,
- StateBlockPtr _state4Ptr = nullptr,
- StateBlockPtr _state5Ptr = nullptr,
- StateBlockPtr _state6Ptr = nullptr,
- StateBlockPtr _state7Ptr = nullptr,
- StateBlockPtr _state8Ptr = nullptr,
- StateBlockPtr _state9Ptr = nullptr ) ;
-
- /** \brief Constructor valid for all categories (FRAME, FEATURE, LANDMARK)
- **/
- ConstraintSparse(ConstraintType _tp,
- const FrameBasePtr& _frame_other_ptr, const FeatureBasePtr& _feature_other_ptr,
- const LandmarkBasePtr& _landmark_other_ptr,
- const ProcessorBasePtr& _processor_ptr,
- bool _apply_loss_function, ConstraintStatus _status,
- StateBlockPtr _state0Ptr,
- StateBlockPtr _state1Ptr = nullptr,
- StateBlockPtr _state2Ptr = nullptr,
- StateBlockPtr _state3Ptr = nullptr,
- StateBlockPtr _state4Ptr = nullptr,
- StateBlockPtr _state5Ptr = nullptr,
- StateBlockPtr _state6Ptr = nullptr,
- StateBlockPtr _state7Ptr = nullptr,
- StateBlockPtr _state8Ptr = nullptr,
- StateBlockPtr _state9Ptr = nullptr );
-
- virtual ~ConstraintSparse() = default;
-
- /** \brief Returns a vector of pointers to the state blocks
- *
- * Returns a vector of pointers to the state blocks in which this constraint depends
- *
- **/
- virtual const std::vector<Scalar*> getStateScalarPtrVector() override;
-
- /** \brief Returns a vector of pointers to the states
- *
- * Returns a vector of pointers to the state in which this constraint depends
- *
- **/
- virtual const std::vector<StateBlockPtr> getStateBlockPtrVector() const override;
-
- /** \brief Returns the residual size
- *
- * Returns the residual size
- *
- **/
- virtual unsigned int getSize() const override;
-
- private:
- void resizeVectors();
-};
-
-
-//////////////////////////////////////////
-// IMPLEMENTATION
-//////////////////////////////////////////
-template <const unsigned int RESIDUAL_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>
-ConstraintSparse<RESIDUAL_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>::ConstraintSparse(ConstraintType _tp,
- bool _apply_loss_function,
- ConstraintStatus _status,
- StateBlockPtr _state0Ptr,
- StateBlockPtr _state1Ptr,
- StateBlockPtr _state2Ptr,
- StateBlockPtr _state3Ptr,
- StateBlockPtr _state4Ptr,
- StateBlockPtr _state5Ptr,
- StateBlockPtr _state6Ptr,
- StateBlockPtr _state7Ptr,
- StateBlockPtr _state8Ptr,
- StateBlockPtr _state9Ptr ) :
- ConstraintBase(_tp, _apply_loss_function, _status),
- state_ptr_vector_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr,_state5Ptr,_state6Ptr,_state7Ptr,_state8Ptr,_state9Ptr}),
- state_block_sizes_vector_({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})
- {
- resizeVectors();
- }
-
-template <const unsigned int RESIDUAL_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>
-ConstraintSparse<RESIDUAL_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>::ConstraintSparse(ConstraintType _tp,
- const FrameBasePtr& _frame_other_ptr,
- const FeatureBasePtr& _feature_other_ptr,
- const LandmarkBasePtr& _landmark_other_ptr,
- const ProcessorBasePtr& _processor_ptr,
- bool _apply_loss_function,
- ConstraintStatus _status,
- StateBlockPtr _state0Ptr,
- StateBlockPtr _state1Ptr,
- StateBlockPtr _state2Ptr,
- StateBlockPtr _state3Ptr,
- StateBlockPtr _state4Ptr,
- StateBlockPtr _state5Ptr,
- StateBlockPtr _state6Ptr,
- StateBlockPtr _state7Ptr,
- StateBlockPtr _state8Ptr,
- StateBlockPtr _state9Ptr ) :
- ConstraintBase(_tp, _frame_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
- state_ptr_vector_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr,_state5Ptr,_state6Ptr,_state7Ptr,_state8Ptr,_state9Ptr}),
- state_block_sizes_vector_({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})
- {
- resizeVectors();
- }
-
-template <const unsigned int RESIDUAL_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>
-const std::vector<Scalar*> ConstraintSparse<RESIDUAL_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>::getStateScalarPtrVector()
-{
- assert(state_ptr_vector_.size() > 0 && state_ptr_vector_.size() <= 10 && "Wrong state vector size in constraint, it should be between 1 and 10");
-
- switch (state_ptr_vector_.size())
- {
- case 1:
- {
- return std::vector<Scalar*>({state_ptr_vector_[0]->getPtr()});
- }
- case 2:
- {
- return std::vector<Scalar*>({state_ptr_vector_[0]->getPtr(),
- state_ptr_vector_[1]->getPtr()});
- }
- case 3:
- {
- return std::vector<Scalar*>({state_ptr_vector_[0]->getPtr(),
- state_ptr_vector_[1]->getPtr(),
- state_ptr_vector_[2]->getPtr()});
- }
- case 4:
- {
- return std::vector<Scalar*>({state_ptr_vector_[0]->getPtr(),
- state_ptr_vector_[1]->getPtr(),
- state_ptr_vector_[2]->getPtr(),
- state_ptr_vector_[3]->getPtr()});
- }
- case 5:
- {
- return std::vector<Scalar*>({state_ptr_vector_[0]->getPtr(),
- state_ptr_vector_[1]->getPtr(),
- state_ptr_vector_[2]->getPtr(),
- state_ptr_vector_[3]->getPtr(),
- state_ptr_vector_[4]->getPtr()});
- }
- case 6:
- {
- return std::vector<Scalar*>({state_ptr_vector_[0]->getPtr(),
- state_ptr_vector_[1]->getPtr(),
- state_ptr_vector_[2]->getPtr(),
- state_ptr_vector_[3]->getPtr(),
- state_ptr_vector_[4]->getPtr(),
- state_ptr_vector_[5]->getPtr()});
- }
- case 7:
- {
- return std::vector<Scalar*>({state_ptr_vector_[0]->getPtr(),
- state_ptr_vector_[1]->getPtr(),
- state_ptr_vector_[2]->getPtr(),
- state_ptr_vector_[3]->getPtr(),
- state_ptr_vector_[4]->getPtr(),
- state_ptr_vector_[5]->getPtr(),
- state_ptr_vector_[6]->getPtr()});
- }
- case 8:
- {
- return std::vector<Scalar*>({state_ptr_vector_[0]->getPtr(),
- state_ptr_vector_[1]->getPtr(),
- state_ptr_vector_[2]->getPtr(),
- state_ptr_vector_[3]->getPtr(),
- state_ptr_vector_[4]->getPtr(),
- state_ptr_vector_[5]->getPtr(),
- state_ptr_vector_[6]->getPtr(),
- state_ptr_vector_[7]->getPtr()});
- }
- case 9:
- {
- return std::vector<Scalar*>({state_ptr_vector_[0]->getPtr(),
- state_ptr_vector_[1]->getPtr(),
- state_ptr_vector_[2]->getPtr(),
- state_ptr_vector_[3]->getPtr(),
- state_ptr_vector_[4]->getPtr(),
- state_ptr_vector_[5]->getPtr(),
- state_ptr_vector_[6]->getPtr(),
- state_ptr_vector_[7]->getPtr(),
- state_ptr_vector_[8]->getPtr()});
- }
- case 10:
- {
- return std::vector<Scalar*>({state_ptr_vector_[0]->getPtr(),
- state_ptr_vector_[1]->getPtr(),
- state_ptr_vector_[2]->getPtr(),
- state_ptr_vector_[3]->getPtr(),
- state_ptr_vector_[4]->getPtr(),
- state_ptr_vector_[5]->getPtr(),
- state_ptr_vector_[6]->getPtr(),
- state_ptr_vector_[7]->getPtr(),
- state_ptr_vector_[8]->getPtr(),
- state_ptr_vector_[9]->getPtr()});
- }
- }
-
- return std::vector<Scalar*>(0); //Not going to happen
-}
-
-template <const unsigned int RESIDUAL_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>
-const std::vector<StateBlockPtr> ConstraintSparse<RESIDUAL_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>::getStateBlockPtrVector() const
-{
- return state_ptr_vector_;
-}
-
-template <const unsigned int RESIDUAL_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 ConstraintSparse<RESIDUAL_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>::getSize() const
-{
- return RESIDUAL_SIZE;
-}
-
-
-template <const unsigned int RESIDUAL_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 ConstraintSparse<RESIDUAL_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>::resizeVectors()
-{
- for (unsigned int ii = 1; ii<state_block_sizes_vector_.size(); ii++)
- {
- if (state_block_sizes_vector_.at(ii) != 0)
- {
- for (unsigned int jj = 0; jj < ii; jj++)
- assert(state_ptr_vector_.at(ii) != state_ptr_vector_.at(jj) && "ConstraintSparse: Repeated state block.");
- assert(state_ptr_vector_.at(ii) != nullptr && "ConstraintSparse: Null state pointer in a non-zero sized block!");
- assert(state_ptr_vector_[ii]->getSize() == state_block_sizes_vector_[ii] && "ConstraintSparse: Incoherent state block size and template block size");
- }
-
- else
- {
- assert(state_ptr_vector_.at(ii) == nullptr && "ConstraintSparse: Non-null state pointer in a zero sized block!");
- state_ptr_vector_.resize(ii);
- state_block_sizes_vector_.resize(ii);
- break;
- }
- }
-}
-
-} // namespace wolf
-
-#endif
diff --git a/src/examples/CMakeLists.txt b/src/examples/CMakeLists.txt
index c614b93723c22f70d73f529ca4f61ea392a748ff..b9a4b728a9554500b9a85ebe89d48651bce9470d 100644
--- a/src/examples/CMakeLists.txt
+++ b/src/examples/CMakeLists.txt
@@ -96,6 +96,10 @@ ENDIF(Suitesparse_FOUND)
#ADD_EXECUTABLE(test_wolf_prunning test_wolf_prunning.cpp)
#TARGET_LINK_LIBRARIES(test_wolf_prunning ${PROJECT_NAME})
+# Sparsification from wolf tree from .graph file (TORO)
+ADD_EXECUTABLE(test_sparsification test_sparsification.cpp)
+TARGET_LINK_LIBRARIES(test_sparsification ${PROJECT_NAME})
+
# Comparing analytic and autodiff odometry constraints
#ADD_EXECUTABLE(test_analytic_odom_constraint test_analytic_odom_constraint.cpp)
#TARGET_LINK_LIBRARIES(test_analytic_odom_constraint ${PROJECT_NAME})
@@ -110,6 +114,11 @@ IF(OpenCV_FOUND)
# Processor Image Landmark test
ADD_EXECUTABLE(test_processor_image_landmark test_processor_image_landmark.cpp)
TARGET_LINK_LIBRARIES(test_processor_image_landmark ${PROJECT_NAME})
+
+ # Simple AHP test
+ ADD_EXECUTABLE(test_simple_AHP test_simple_AHP.cpp)
+ TARGET_LINK_LIBRARIES(test_simple_AHP ${PROJECT_NAME})
+
ENDIF(Ceres_FOUND)
# Testing opencv matching and fundamental matrix with ransac
@@ -131,15 +140,8 @@ IF(OpenCV_FOUND)
# ORB tracker test
ADD_EXECUTABLE(test_ROI_ORB test_ROI_ORB.cpp)
TARGET_LINK_LIBRARIES(test_ROI_ORB ${PROJECT_NAME})
-ENDIF(OpenCV_FOUND)
-IF(Ceres_FOUND)
- # Simple AHP test
- IF(OpenCV_FOUND)
- ADD_EXECUTABLE(test_simple_AHP test_simple_AHP.cpp)
- TARGET_LINK_LIBRARIES(test_simple_AHP ${PROJECT_NAME})
- ENDIF(OpenCV_FOUND)
-ENDIF(Ceres_FOUND)
+ENDIF(OpenCV_FOUND)
# Processor Tracker Feature test
ADD_EXECUTABLE(test_processor_tracker_feature test_processor_tracker_feature.cpp)
diff --git a/src/examples/test_ceres_2_lasers_polylines.cpp b/src/examples/test_ceres_2_lasers_polylines.cpp
index b2be568b0bd2fca63d110a178dbed35660e3d8f0..9c5f592ec5728d999f66cc688f73739d080eef86 100644
--- a/src/examples/test_ceres_2_lasers_polylines.cpp
+++ b/src/examples/test_ceres_2_lasers_polylines.cpp
@@ -328,8 +328,8 @@ int main(int argc, char** argv)
// SOLVE OPTIMIZATION ---------------------------
std::cout << "SOLVING..." << std::endl;
t1 = clock();
- ceres::Solver::Summary summary = ceres_manager.solve();
- std::cout << summary.FullReport() << std::endl;
+ std::string summary = ceres_manager.solve(2);// 0: nothing, 1: BriefReport, 2: FullReport
+ std::cout << summary << std::endl;
mean_times(3) += ((double) clock() - t1) / CLOCKS_PER_SEC;
// COMPUTE COVARIANCES ---------------------------
diff --git a/src/examples/test_imuDock.cpp b/src/examples/test_imuDock.cpp
index 40c63e71b83a8550079c4f309c9c371fd94a7d9c..17d39a10c1d3544c5b317bab83827d41b2281837 100644
--- a/src/examples/test_imuDock.cpp
+++ b/src/examples/test_imuDock.cpp
@@ -240,7 +240,7 @@ int main(int argc, char** argv)
// ___Solve + compute covariances___
problem->print(4,0,1,0);
- ceres::Solver::Summary summary = ceres_manager->solve();
+ std::string report = ceres_manager->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport
ceres_manager->computeCovariances(ALL_MARGINALS);
problem->print(1,0,1,0);
diff --git a/src/examples/test_imuDock_autoKFs.cpp b/src/examples/test_imuDock_autoKFs.cpp
index 1cbe8723eed660b93d3d27de9afb7ee053d04950..2f4b67fd8046e1950801ddd24bb10457e338ece6 100644
--- a/src/examples/test_imuDock_autoKFs.cpp
+++ b/src/examples/test_imuDock_autoKFs.cpp
@@ -249,7 +249,7 @@ int main(int argc, char** argv)
// ___Solve + compute covariances___
problem->print(4,0,1,0);
- ceres::Solver::Summary summary = ceres_manager->solve();
+ std::string report = ceres_manager->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
ceres_manager->computeCovariances(ALL_MARGINALS);
problem->print(1,0,1,0);
diff --git a/src/examples/test_imuPlateform_Offline.cpp b/src/examples/test_imuPlateform_Offline.cpp
index b77c82b6e6ccf49f6ce31788b1cc3a0bc52c0375..074e6ddb27c7ecc0223ca08352e51a9087e53a70 100644
--- a/src/examples/test_imuPlateform_Offline.cpp
+++ b/src/examples/test_imuPlateform_Offline.cpp
@@ -79,8 +79,7 @@ int main(int argc, char** argv)
ceres_options.minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;ceres::LINE_SEARCH
ceres_options.max_line_search_step_contraction = 1e-3;
ceres_options.max_num_iterations = 1e4;
- CeresManager* ceres_manager_wolf_diff = new CeresManager(wolf_problem_ptr_, ceres_options, true);
-
+ CeresManager* ceres_manager_wolf_diff = new CeresManager(wolf_problem_ptr_, ceres_options);
// SENSOR + PROCESSOR IMU
SensorBasePtr sen0_ptr = wolf_problem_ptr_->installSensor("IMU", "Main IMU", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/src/examples/sensor_imu.yaml");
@@ -202,8 +201,8 @@ int main(int argc, char** argv)
origin_KF->getVPtr()->fix();
std::cout << "\t\t\t ______solving______" << std::endl;
- ceres::Solver::Summary summary = ceres_manager_wolf_diff->solve();
- std::cout << summary.FullReport() << std::endl;
+ std::string report = ceres_manager_wolf_diff->solve(2);// 0: nothing, 1: BriefReport, 2: FullReport
+ std::cout << report << std::endl;
ceres_manager_wolf_diff->computeCovariances(ALL);
std::cout << "\t\t\t ______solved______" << std::endl;
diff --git a/src/examples/test_imu_constrained0.cpp b/src/examples/test_imu_constrained0.cpp
index 3e5e81c0f97df7c2160f4546b3e281c18b43b7d7..5869c6bfc71bc38ea645c22224e428d4067526a1 100644
--- a/src/examples/test_imu_constrained0.cpp
+++ b/src/examples/test_imu_constrained0.cpp
@@ -101,8 +101,7 @@ int main(int argc, char** argv)
ceres_options.minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;ceres::LINE_SEARCH
ceres_options.max_line_search_step_contraction = 1e-3;
ceres_options.max_num_iterations = 1e4;
- CeresManager* ceres_manager_wolf_diff = new CeresManager(wolf_problem_ptr_, ceres_options, true);
-
+ CeresManager* ceres_manager_wolf_diff = new CeresManager(wolf_problem_ptr_, ceres_options);
// SENSOR + PROCESSOR IMU
SensorBasePtr sen0_ptr = wolf_problem_ptr_->installSensor("IMU", "Main IMU", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/src/examples/sensor_imu.yaml");
@@ -194,7 +193,7 @@ int main(int argc, char** argv)
if( (ts.get() - t.get()) >= 0.05 )
{
t = ts;
- //ceres::Solver::Summary summary = ceres_manager_wolf_diff->solve();
+ //std::string report = ceres_manager_wolf_diff->solve(1); //0: nothing, 1: BriefReport, 2: FullReport
Eigen::VectorXs frm_state(16);
@@ -260,15 +259,15 @@ int main(int argc, char** argv)
std::cout << "\t\t\t ______solving______" << std::endl;
- ceres::Solver::Summary summary = ceres_manager_wolf_diff->solve();
- std::cout << summary.FullReport() << std::endl;
+ std::string report = ceres_manager_wolf_diff->solve(2); //0: nothing, 1: BriefReport, 2: FullReport
+ std::cout << report << std::endl;
last_KF->getAccBiasPtr()->fix();
last_KF->getGyroBiasPtr()->fix();
std::cout << "\t\t\t solving after fixBias" << std::endl;
- summary = ceres_manager_wolf_diff->solve();
- std::cout << summary.BriefReport() << std::endl;
+ report = ceres_manager_wolf_diff->solve(1); //0: nothing, 1: BriefReport, 2: FullReport
+ std::cout << report << std::endl;
ceres_manager_wolf_diff->computeCovariances(ALL);
std::cout << "\t\t\t ______solved______" << std::endl;
diff --git a/src/examples/test_processor_image_landmark.cpp b/src/examples/test_processor_image_landmark.cpp
index 646160b61a84ca379f6bfb04778a3a760852e746..06495426a1de3b9ec4df946bf318589ce06902d4 100644
--- a/src/examples/test_processor_image_landmark.cpp
+++ b/src/examples/test_processor_image_landmark.cpp
@@ -264,8 +264,8 @@ int main(int argc, char** argv)
if (problem->getTrajectoryPtr()->getFrameList().size() > number_of_KFs)
{
number_of_KFs = problem->getTrajectoryPtr()->getFrameList().size();
- ceres::Solver::Summary summary = ceres_manager.solve();
- std::cout << summary.BriefReport() << std::endl;
+ std::string summary = ceres_manager.solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
+ std::cout << summary << std::endl;
}
diff --git a/src/examples/test_processor_odom_3D.cpp b/src/examples/test_processor_odom_3D.cpp
index 9e17469d10b5465431bebad5a2b5989d57b3c88d..c43b1f50807d259e8f8a13da6ea8a9701a7771b9 100644
--- a/src/examples/test_processor_odom_3D.cpp
+++ b/src/examples/test_processor_odom_3D.cpp
@@ -89,9 +89,8 @@ int main (int argc, char** argv)
// frm->setState(problem->zeroState());
// }
// problem->print(1,0,1,0);
- ceres::Solver::Summary summary = ceres_manager.solve();
- std::cout << summary.BriefReport() << std::endl;
- summary.FullReport();
+ std::string brief_report = ceres_manager.solve(1);// 0, 1 or 2
+ std::cout << brief_report << std::endl;
problem->print(1,0,1,0);
problem.reset();
diff --git a/src/examples/test_simple_AHP.cpp b/src/examples/test_simple_AHP.cpp
index 052b528865d927611356ae38b80f682f48ffdbc5..d31eba5ab0db4735847ec354b13dcb6ddb206812 100644
--- a/src/examples/test_simple_AHP.cpp
+++ b/src/examples/test_simple_AHP.cpp
@@ -244,8 +244,8 @@ int main(int argc, char** argv)
CeresManager ceres_manager(problem, ceres_options);
- ceres::Solver::Summary summary = ceres_manager.solve();
- std::cout << summary.FullReport() << std::endl;
+ std::string summary = ceres_manager.solve(2);// 0: nothing, 1: BriefReport, 2: FullReport
+ std::cout << summary << std::endl;
// Test of convergence over the lmk params
bool pass = (lmk_2->point() - lmk_1->point()).isMuchSmallerThan(1,Constants::EPS);
diff --git a/src/examples/test_sparsification.cpp b/src/examples/test_sparsification.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..bf797869098964ffe95683a7155e8657d6622b4f
--- /dev/null
+++ b/src/examples/test_sparsification.cpp
@@ -0,0 +1,315 @@
+// Sparsification example creating wolf tree from imported graph from .txt file
+
+//C includes for sleep, time and main args
+#include "unistd.h"
+
+//std includes
+#include <cstdlib>
+#include <fstream>
+#include <string>
+#include <iostream>
+#include <memory>
+#include <random>
+#include <cmath>
+#include <queue>
+
+//Wolf includes
+#include "capture_void.h"
+#include "feature_odom_2D.h"
+#include "constraint_base.h"
+#include "ceres_wrapper/ceres_manager.h"
+
+// EIGEN
+//#include <Eigen/CholmodSupport>
+#include <Eigen/StdVector> // Eigen in std vector
+
+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<Scalar>& ins, Eigen::SparseMatrix<Scalar>& original, const unsigned int& row, const unsigned int& col)
+{
+ for (int k=0; k<ins.outerSize(); ++k)
+ for (Eigen::SparseMatrix<Scalar>::InnerIterator iti(ins,k); iti; ++iti)
+ original.coeffRef(iti.row() + row, iti.col() + col) = iti.value();
+
+ original.makeCompressed();
+}
+
+void decodeEdge(const std::string& buffer, unsigned int& edge_from, unsigned int& edge_to, Eigen::Vector3s& measurement, Eigen::Matrix3s& covariance)
+{
+ std::string str_num;
+
+ unsigned int i = 0;
+
+ // only decode edges
+ if (buffer.at(0) == 'E')
+ {
+ //skip rest of EDGE word
+ while (buffer.at(i) != ' ') i++;
+ //skip white spaces
+ while (buffer.at(i) == ' ') i++;
+
+ // FROM ID
+ while (buffer.at(i) != ' ')
+ str_num.push_back(buffer.at(i++));
+ edge_from = atoi(str_num.c_str())+1;
+ str_num.clear();
+
+ //skip white spaces
+ while (buffer.at(i) == ' ') i++;
+
+ // TO ID
+ while (buffer.at(i) != ' ')
+ str_num.push_back(buffer.at(i++));
+ edge_to = atoi(str_num.c_str())+1;
+ str_num.clear();
+
+ //skip white spaces
+ while (buffer.at(i) == ' ') i++;
+
+ // MEASUREMENT
+ // X
+ while (buffer.at(i) != ' ')
+ str_num.push_back(buffer.at(i++));
+ measurement(0) = atof(str_num.c_str());
+ str_num.clear();
+ //skip white spaces
+ while (buffer.at(i) == ' ') i++;
+ // Y
+ while (buffer.at(i) != ' ')
+ str_num.push_back(buffer.at(i++));
+ measurement(1) = atof(str_num.c_str());
+ str_num.clear();
+ //skip white spaces
+ while (buffer.at(i) == ' ') i++;
+ // THETA
+ while (buffer.at(i) != ' ')
+ str_num.push_back(buffer.at(i++));
+ measurement(2) = atof(str_num.c_str());
+ str_num.clear();
+ //skip white spaces
+ while (buffer.at(i) == ' ') i++;
+
+ // INFORMATION
+ Eigen::Matrix3s information;
+ // XX
+ while (buffer.at(i) != ' ')
+ str_num.push_back(buffer.at(i++));
+ information(0,0) = atof(str_num.c_str());
+ str_num.clear();
+ //skip white spaces
+ while (buffer.at(i) == ' ') i++;
+ // XY
+ while (buffer.at(i) != ' ')
+ str_num.push_back(buffer.at(i++));
+ information(0,1) = atof(str_num.c_str());
+ information(1,0) = atof(str_num.c_str());
+ str_num.clear();
+ //skip white spaces
+ while (buffer.at(i) == ' ') i++;
+ // YY
+ while (buffer.at(i) != ' ')
+ str_num.push_back(buffer.at(i++));
+ information(1,1) = atof(str_num.c_str());
+ str_num.clear();
+ //skip white spaces
+ while (buffer.at(i) == ' ') i++;
+ // THETATHETA
+ while (buffer.at(i) != ' ')
+ str_num.push_back(buffer.at(i++));
+ information(2,2) = atof(str_num.c_str());
+ str_num.clear();
+ //skip white spaces
+ while (buffer.at(i) == ' ') i++;
+ // XTHETA
+ while (buffer.at(i) != ' ')
+ str_num.push_back(buffer.at(i++));
+ information(0,2) = atof(str_num.c_str());
+ information(2,0) = atof(str_num.c_str());
+ str_num.clear();
+ //skip white spaces
+ while (buffer.at(i) == ' ') i++;
+ // YTHETA
+ while (i < buffer.size() && buffer.at(i) != ' ')
+ str_num.push_back(buffer.at(i++));
+ information(1,2) = atof(str_num.c_str());
+ information(2,1) = atof(str_num.c_str());
+ str_num.clear();
+
+ // COVARIANCE
+ covariance = information.inverse();
+ }
+ else
+ {
+ edge_from = 0;
+ edge_to = 0;
+ }
+}
+
+}
+
+int main(int argc, char** argv)
+{
+ using namespace wolf;
+
+ //Welcome message
+ std::cout << std::endl << " ========= WOLF IMPORTED .graph TEST ===========" << std::endl << std::endl;
+
+ bool wrong_input = false;
+ if (argc < 3)
+ wrong_input = true;
+ else if (argc > 4)
+ wrong_input = true;
+ else if (argc > 2 && (atoi(argv[2]) < 2 || atoi(argv[2]) > 5))
+ wrong_input = true;
+ else if (argc > 3 && atoi(argv[3]) < 0 )
+ wrong_input = true;
+
+ if (wrong_input)
+ {
+ std::cout << "Please call me with: [./test_wolf_imported_graph DATASET T (MAX_VERTICES)], where:" << std::endl;
+ std::cout << " DATASET: manhattan, killian or intel" << std::endl;
+ std::cout << " T keep one node each T: 2, 3, 4 or 5" << std::endl;
+ std::cout << " optional: MAX_VERTICES max edges to be loaded" << std::endl;
+ std::cout << "EXIT due to bad user input" << std::endl << std::endl;
+ return -1;
+ }
+
+ // input variables
+ char const * dataset_path = std::getenv("DATASET_PATH");
+ unsigned int pruning_T = atoi(argv[2]);
+ std::string file_path(dataset_path);
+ file_path = file_path + "/graphs/redirected_" + std::to_string(pruning_T) + "_" + argv[1] + ".graph";
+ unsigned int MAX_VERTEX = 1e9;
+ if (argc > 3 && atoi(argv[3]) != 0)
+ MAX_VERTEX = atoi(argv[3]);
+
+ // Wolf problem
+ FrameBasePtr last_frame_ptr, frame_from_ptr, frame_to_ptr;
+ ProblemPtr bl_problem_ptr = Problem::create("PO_2D");
+ SensorBasePtr sensor_ptr = bl_problem_ptr->installSensor("ODOM 2D", "Odometry", Eigen::VectorXs::Zero(3), IntrinsicsBasePtr());
+
+ // Ceres wrapper
+ std::string bl_summary, sp_summary;
+ ceres::Solver::Options ceres_options;
+ 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 = 10;
+ CeresManagerPtr bl_ceres_manager = std::make_shared<CeresManager>(bl_problem_ptr, ceres_options);
+
+ // load graph from .txt
+ std::ifstream graph_file;
+ graph_file.open(file_path.c_str(), std::ifstream::in);
+ if (!graph_file.is_open())
+ {
+ printf("\nError opening file: %s\n",file_path.c_str());
+ return -1;
+ }
+
+ // auxiliar variables
+ std::string line_string;
+ unsigned int edge_from, edge_to;
+ Eigen::Vector3s meas;
+ Eigen::Matrix3s meas_cov;
+ Eigen::Matrix3s R = Eigen::Matrix3s::Identity();
+ //clock_t t1;
+
+ // ------------------------ START EXPERIMENT ------------------------
+ // First frame FIXED
+ last_frame_ptr = bl_problem_ptr->emplaceFrame(KEY_FRAME, Eigen::Vector3s::Zero(),TimeStamp(0));
+ last_frame_ptr->fix();
+ bl_problem_ptr->print(4, true, false, true);
+
+ while (std::getline(graph_file, line_string) && last_frame_ptr->id() <= MAX_VERTEX)
+ {
+ std::cout << "new line:" << line_string << std::endl;
+ decodeEdge(line_string, edge_from, edge_to, meas, meas_cov);
+
+ // only factors
+ if (edge_from != 0)
+ {
+
+ // ODOMETRY -------------------
+ if (edge_to > last_frame_ptr->id())
+ {
+ frame_from_ptr = last_frame_ptr;
+
+ // NEW KEYFRAME
+ Eigen::Vector3s from_pose = frame_from_ptr->getState();
+ R.topLeftCorner(2,2) = Eigen::Rotation2Ds(from_pose(2)).matrix();
+ Eigen::Vector3s new_frame_pose = from_pose + R*meas;
+ last_frame_ptr = bl_problem_ptr->emplaceFrame(KEY_FRAME, new_frame_pose, TimeStamp(double(edge_to)));
+
+ frame_to_ptr = last_frame_ptr;
+
+ std::cout << "NEW FRAME " << last_frame_ptr->id() << " - ts = " << last_frame_ptr->getTimeStamp().get() << std::endl;
+
+ // REMOVE PREVIOUS NODES
+ }
+ // LOOP CLOSURE ---------------
+ else
+ {
+ if (edge_from == last_frame_ptr->id())
+ frame_from_ptr = last_frame_ptr;
+ else
+ for (auto frm_rit = bl_problem_ptr->getTrajectoryPtr()->getFrameList().rbegin(); frm_rit != bl_problem_ptr->getTrajectoryPtr()->getFrameList().rend(); frm_rit++)
+ if ((*frm_rit)->id() == edge_from)
+ {
+ frame_from_ptr = *frm_rit;
+ break;
+ }
+ if (edge_to == last_frame_ptr->id())
+ frame_to_ptr = last_frame_ptr;
+ else
+ for (auto frm_rit = bl_problem_ptr->getTrajectoryPtr()->getFrameList().rbegin(); frm_rit != bl_problem_ptr->getTrajectoryPtr()->getFrameList().rend(); frm_rit++)
+ if ((*frm_rit)->id() == edge_to)
+ {
+ frame_to_ptr = *frm_rit;
+ break;
+ }
+ }
+// std::cout << "frame_from " << frame_from_ptr->id() << std::endl;
+// std::cout << "edge_from " << edge_from << std::endl;
+// std::cout << "frame_to " << frame_to_ptr->id() << std::endl;
+// std::cout << "edge_to " << edge_to << std::endl;
+
+ assert(frame_from_ptr->id() == edge_from && "frame from id and edge from idx must be the same");
+ assert(frame_to_ptr->id() == edge_to && "frame to id and edge to idx must be the same");
+
+ // CAPTURE
+ CaptureVoidPtr capture_ptr = std::make_shared<CaptureVoid>(TimeStamp(0), sensor_ptr);
+ frame_from_ptr->addCapture(capture_ptr);
+
+ // FEATURE
+ FeatureBasePtr feature_ptr = std::make_shared<FeatureOdom2D>(meas, meas_cov);
+ capture_ptr->addFeature(feature_ptr);
+
+ // CONSTRAINT
+ ConstraintOdom2DPtr constraint_ptr = std::make_shared<ConstraintOdom2D>(feature_ptr, frame_to_ptr);
+ feature_ptr->addConstraint(constraint_ptr);
+ frame_to_ptr->addConstrainedBy(constraint_ptr);
+
+ // SOLVE
+ // solution
+ bl_summary = bl_ceres_manager->solve(1);
+ std::cout << bl_summary << std::endl;
+
+ // covariance
+ bl_ceres_manager->computeCovariances(ALL);//ALL_MARGINALS
+
+
+ // t1 = clock();
+ // double t_sigma_manual = 0;
+ // t_sigma_manual += ((double) clock() - t1) / CLOCKS_PER_SEC;
+
+ }
+ }
+
+ //bl_problem_ptr->print(4, true, false, true);
+
+ //End message
+ std::cout << " =========================== END ===============================" << std::endl << std::endl;
+
+ //exit
+ return 0;
+}
diff --git a/src/feature_base.cpp b/src/feature_base.cpp
index f5dd0d1e444b36078c6f144caca526382aa0dd42..13b9987f5c8adcdb8add385aae6c3e10c584707f 100644
--- a/src/feature_base.cpp
+++ b/src/feature_base.cpp
@@ -13,11 +13,9 @@ FeatureBase::FeatureBase(const std::string& _type, const Eigen::VectorXs& _measu
feature_id_(++feature_id_count_),
track_id_(0),
landmark_id_(0),
- measurement_(_measurement),
- measurement_covariance_(_meas_covariance)
+ measurement_(_measurement)
{
- measurement_sqrt_information_upper_ = computeSqrtInformationUpper(measurement_covariance_);
-
+ setMeasurementCovariance(_meas_covariance);
// std::cout << "constructed +f" << id() << std::endl;
}
@@ -84,10 +82,18 @@ void FeatureBase::setMeasurementCovariance(const Eigen::MatrixXs & _meas_cov)
assert(_meas_cov.determinant() > 0 && "Not positive definite measurement covariance");
measurement_covariance_ = _meas_cov;
- measurement_sqrt_information_upper_ = computeSqrtInformationUpper(_meas_cov);
+ measurement_sqrt_information_upper_ = computeSqrtUpper(_meas_cov);
+}
+
+void FeatureBase::setMeasurementInfo(const Eigen::MatrixXs & _meas_info)
+{
+ assert(_meas_info.determinant() > 0 && "Not positive definite measurement information");
+
+ measurement_covariance_ = _meas_info.inverse();
+ measurement_sqrt_information_upper_ = computeSqrtUpper(_meas_info.inverse());
}
-Eigen::MatrixXs FeatureBase::computeSqrtInformationUpper(const Eigen::MatrixXs & _covariance) const
+Eigen::MatrixXs FeatureBase::computeSqrtUpper(const Eigen::MatrixXs & _covariance) const
{
if (_covariance.determinant() < Constants::EPS_SMALL)
{
diff --git a/src/feature_base.h b/src/feature_base.h
index 2b92f537e9053e0bf4b635b03dd5533b4736323a..2e700eab9dac0c55aaf4b4c76010f507e8eedcba 100644
--- a/src/feature_base.h
+++ b/src/feature_base.h
@@ -64,6 +64,7 @@ class FeatureBase : public NodeBase, public std::enable_shared_from_this<Feature
const Eigen::VectorXs& getMeasurement() const;
void setMeasurement(const Eigen::VectorXs& _meas);
void setMeasurementCovariance(const Eigen::MatrixXs & _meas_cov);
+ void setMeasurementInfo(const Eigen::MatrixXs & _meas_info);
const Eigen::MatrixXs& getMeasurementCovariance() const;
const Eigen::MatrixXs& getMeasurementSquareRootInformationUpper() const;
@@ -89,7 +90,7 @@ class FeatureBase : public NodeBase, public std::enable_shared_from_this<Feature
void getConstraintList(ConstraintBaseList & _ctr_list);
private:
- Eigen::MatrixXs computeSqrtInformationUpper(const Eigen::MatrixXs& covariance) const;
+ Eigen::MatrixXs computeSqrtUpper(const Eigen::MatrixXs& _M) const;
};
diff --git a/src/frame_base.cpp b/src/frame_base.cpp
index 0ba15c7449ca2dcf48900c5f3cf5537aae3906b8..50f6e8bbf0f71afe99e0a7e38f71a9fca4ea246f 100644
--- a/src/frame_base.cpp
+++ b/src/frame_base.cpp
@@ -182,6 +182,15 @@ void FrameBase::getState(Eigen::VectorXs& state) const
}
}
+unsigned int FrameBase::getSize() const
+{
+ unsigned int size = 0;
+ for (auto st : state_block_vec_)
+ if (st)
+ size += st->getSize();
+ return size;
+}
+
FrameBasePtr FrameBase::getPreviousFrame() const
{
//std::cout << "finding previous frame of " << this->frame_id_ << std::endl;
diff --git a/src/frame_base.h b/src/frame_base.h
index 4ffb5aa03f868ecd5adf06c348690748353d2dfe..408b6b0bc9f061fd3e6bb703bd829a88f80bef09 100644
--- a/src/frame_base.h
+++ b/src/frame_base.h
@@ -118,7 +118,7 @@ class FrameBase : public NodeBase, public std::enable_shared_from_this<FrameBase
void setState(const Eigen::VectorXs& _st);
Eigen::VectorXs getState() const;
void getState(Eigen::VectorXs& state) const;
-
+ unsigned int getSize() const;
// Wolf tree access ---------------------------------------------------
public:
diff --git a/src/problem.cpp b/src/problem.cpp
index c9eeb21c041a8643f97808a45a316a88f4661873..080e392fe4647a0f608ba8b002ff5a932402f041 100644
--- a/src/problem.cpp
+++ b/src/problem.cpp
@@ -359,72 +359,70 @@ void Problem::addLandmarkList(LandmarkBaseList& _lmk_list)
StateBlockPtr Problem::addStateBlock(StateBlockPtr _state_ptr)
{
- //std::cout << "addStateBlockPtr" << std::endl;
+ //std::cout << "Problem::addStateBlockPtr " << _state_ptr.get() << std::endl;
+
// add the state unit to the list
state_block_list_.push_back(_state_ptr);
// queue for solver manager
state_block_notification_list_.push_back(StateBlockNotification({ADD,_state_ptr}));
-
return _state_ptr;
}
void Problem::updateStateBlockPtr(StateBlockPtr _state_ptr)
{
+ //std::cout << "Problem::updateStateBlockPtr " << _state_ptr.get() << std::endl;
+
// queue for solver manager
state_block_notification_list_.push_back(StateBlockNotification({UPDATE,_state_ptr}));
}
void Problem::removeStateBlockPtr(StateBlockPtr _state_ptr)
{
+ //std::cout << "Problem::removeStateBlockPtr " << _state_ptr.get() << std::endl;
+
// add the state unit to the list
state_block_list_.remove(_state_ptr);
// Check if the state addition is still as a notification
- auto state_found_it = state_block_notification_list_.end();
- for (auto state_notif_it = state_block_notification_list_.begin(); state_notif_it != state_block_notification_list_.end(); state_notif_it++)
- {
+ auto state_notif_it = state_block_notification_list_.begin();
+ for (; state_notif_it != state_block_notification_list_.end(); state_notif_it++)
if (state_notif_it->notification_ == ADD && state_notif_it->state_block_ptr_ == _state_ptr)
- {
- state_found_it = state_notif_it;
break;
- }
- }
+
// Remove addition notification
- if (state_found_it != state_block_notification_list_.end())
- state_block_notification_list_.erase(state_found_it);
+ if (state_notif_it != state_block_notification_list_.end())
+ state_block_notification_list_.erase(state_notif_it);
// Add remove notification
else
- state_block_notification_list_.push_back(StateBlockNotification({REMOVE, nullptr, _state_ptr->getPtr()}));
+ state_block_notification_list_.push_back(StateBlockNotification({REMOVE, _state_ptr}));
}
ConstraintBasePtr Problem::addConstraintPtr(ConstraintBasePtr _constraint_ptr)
{
- //std::cout << "addConstraintPtr" << std::endl;
+ //std::cout << "Problem::addConstraintPtr " << _constraint_ptr->id() << std::endl;
// queue for solver manager
- constraint_notification_list_.push_back(ConstraintNotification({ADD, _constraint_ptr, _constraint_ptr->id()}));
+ constraint_notification_list_.push_back(ConstraintNotification({ADD, _constraint_ptr}));
return _constraint_ptr;
}
void Problem::removeConstraintPtr(ConstraintBasePtr _constraint_ptr)
{
+ //std::cout << "Problem::removeConstraintPtr " << _constraint_ptr->id() << std::endl;
+
// Check if the constraint addition is still as a notification
- auto ctr_found_it = constraint_notification_list_.end();
- for (auto ctr_notif_it = constraint_notification_list_.begin(); ctr_notif_it != constraint_notification_list_.end(); ctr_notif_it++)
- {
+ auto ctr_notif_it = constraint_notification_list_.begin();
+ for (; ctr_notif_it != constraint_notification_list_.end(); ctr_notif_it++)
if (ctr_notif_it->notification_ == ADD && ctr_notif_it->constraint_ptr_ == _constraint_ptr)
- {
- ctr_found_it = ctr_notif_it;
break;
- }
- }
+
// Remove addition notification
- if (ctr_found_it != constraint_notification_list_.end())
- constraint_notification_list_.erase(ctr_found_it);
+ if (ctr_notif_it != constraint_notification_list_.end())
+ constraint_notification_list_.erase(ctr_notif_it); // CHECKED shared_ptr is not active after erase
// Add remove notification
else
- constraint_notification_list_.push_back(ConstraintNotification({REMOVE, nullptr, _constraint_ptr->id()}));
+ constraint_notification_list_.push_back(ConstraintNotification({REMOVE, _constraint_ptr}));
}
void Problem::clearCovariance()
@@ -468,6 +466,45 @@ bool Problem::getCovarianceBlock(StateBlockPtr _state1, StateBlockPtr _state2, E
return true;
}
+bool Problem::getCovarianceBlock(std::map<StateBlockPtr, unsigned int> _sb_2_idx, Eigen::MatrixXs& _cov)
+{
+ // fill covariance
+ for (auto it1 = _sb_2_idx.begin(); it1 != _sb_2_idx.end(); it1++)
+ for (auto it2 = it1; it2 != _sb_2_idx.end(); it2++)
+ {
+ StateBlockPtr sb1 = it1->first;
+ StateBlockPtr sb2 = it2->first;
+ std::pair<StateBlockPtr, StateBlockPtr> pair_12(sb1, sb2);
+ std::pair<StateBlockPtr, StateBlockPtr> pair_21(sb2, sb1);
+
+ // search st1 & st2
+ if (covariances_.find(pair_12) != covariances_.end())
+ {
+ assert(_sb_2_idx[sb1] + sb1->getSize() <= _cov.rows() &&
+ _sb_2_idx[sb2] + sb2->getSize() <= _cov.cols() && "Problem::getCovarianceBlock: Bad matrix covariance size!");
+ assert(_sb_2_idx[sb2] + sb2->getSize() <= _cov.rows() &&
+ _sb_2_idx[sb1] + sb1->getSize() <= _cov.cols() && "Problem::getCovarianceBlock: Bad matrix covariance size!");
+
+ _cov.block(_sb_2_idx[sb1], _sb_2_idx[sb2], sb1->getSize(), sb2->getSize()) = covariances_[pair_12];
+ _cov.block(_sb_2_idx[sb2], _sb_2_idx[sb1], sb2->getSize(), sb1->getSize()) = covariances_[pair_12].transpose();
+ }
+ else if (covariances_.find(pair_21) != covariances_.end())
+ {
+ assert(_sb_2_idx[sb1] + sb1->getSize() <= _cov.rows() &&
+ _sb_2_idx[sb2] + sb2->getSize() <= _cov.cols() && "Problem::getCovarianceBlock: Bad matrix covariance size!");
+ assert(_sb_2_idx[sb2] + sb2->getSize() <= _cov.rows() &&
+ _sb_2_idx[sb1] + sb1->getSize() <= _cov.cols() && "Problem::getCovarianceBlock: Bad matrix covariance size!");
+
+ _cov.block(_sb_2_idx[sb1], _sb_2_idx[sb2], sb1->getSize(), sb2->getSize()) = covariances_[pair_21].transpose();
+ _cov.block(_sb_2_idx[sb2], _sb_2_idx[sb1], sb2->getSize(), sb1->getSize()) = covariances_[pair_21];
+ }
+ else
+ return false;
+ }
+
+ return true;
+}
+
bool Problem::getCovarianceBlock(StateBlockPtr _state, Eigen::MatrixXs& _cov, const int _row_and_col)
{
return getCovarianceBlock(_state, _state, _cov, _row_and_col, _row_and_col);
diff --git a/src/problem.h b/src/problem.h
index d4f10e6474d230668cc50ea0c774699f3af9bc0c..48b68ed6452ab76ec739dc834a922472fb55d069 100644
--- a/src/problem.h
+++ b/src/problem.h
@@ -30,15 +30,13 @@ enum Notification
};
struct StateBlockNotification
{
- Notification notification_;
- StateBlockPtr state_block_ptr_;
- Scalar* scalar_ptr_;
+ Notification notification_;
+ StateBlockPtr state_block_ptr_;
};
struct ConstraintNotification
{
- Notification notification_;
- ConstraintBasePtr constraint_ptr_; // TODO check pointer type
- unsigned int id_;
+ Notification notification_;
+ ConstraintBasePtr constraint_ptr_;
};
@@ -245,8 +243,8 @@ class Problem : public std::enable_shared_from_this<Problem>
// Covariances -------------------------------------------
void clearCovariance();
void addCovarianceBlock(StateBlockPtr _state1, StateBlockPtr _state2, const Eigen::MatrixXs& _cov);
- bool getCovarianceBlock(StateBlockPtr _state1, StateBlockPtr _state2, Eigen::MatrixXs& _cov, const int _row = 0,
- const int _col=0);
+ bool getCovarianceBlock(StateBlockPtr _state1, StateBlockPtr _state2, Eigen::MatrixXs& _cov, const int _row = 0, const int _col=0);
+ bool getCovarianceBlock(std::map<StateBlockPtr, unsigned int> _sb_2_idx, Eigen::MatrixXs& _cov);
bool getCovarianceBlock(StateBlockPtr _state, Eigen::MatrixXs& _cov, const int _row_and_col = 0);
bool getFrameCovariance(FrameBasePtr _frame_ptr, Eigen::MatrixXs& _covariance);
Eigen::MatrixXs getFrameCovariance(FrameBasePtr _frame_ptr);
@@ -254,7 +252,6 @@ class Problem : public std::enable_shared_from_this<Problem>
bool getLandmarkCovariance(LandmarkBasePtr _landmark_ptr, Eigen::MatrixXs& _covariance);
Eigen::MatrixXs getLandmarkCovariance(LandmarkBasePtr _landmark_ptr);
-
// Solver management ----------------------------------------
/** \brief Gets a reference to the state units list
diff --git a/src/solver/sparse_utils.h b/src/solver/sparse_utils.h
index 04dd88db8afd910841c90c93a5485fd2ef5a4e4d..280aa2310c648ca360a491de6be6784826404f8a 100644
--- a/src/solver/sparse_utils.h
+++ b/src/solver/sparse_utils.h
@@ -5,45 +5,42 @@
* Author: jvallve
*/
-#ifndef TRUNK_SRC_SOLVER_SPARSE_UTILS_H_
-#define TRUNK_SRC_SOLVER_SPARSE_UTILS_H_
+#ifndef SPARSE_UTILS_H_
+#define SPARSE_UTILS_H_
// eigen includes
#include <eigen3/Eigen/Sparse>
-class SparseBlockPruning
+namespace wolf
{
- public:
- unsigned int col, row, Nrows, Ncols;
- SparseBlockPruning(unsigned int _col, unsigned int _row, unsigned int _Nrows, unsigned int _Ncols) :
- col(_col),
- row(_row),
- Nrows(_Nrows),
- Ncols(_Ncols)
- {
- //
- }
- bool operator()(unsigned int i, unsigned int j, double) const
- {
- return (i < row || i > row + Nrows-1) || (j < col || j > col + Ncols-1);
- }
-};
-
-void eraseSparseBlock(Eigen::SparseMatrix<double>& original, const unsigned int& row, const unsigned int& col, const unsigned int& Nrows, const unsigned int& Ncols)
+
+void eraseBlockRow(Eigen::SparseMatrixs& A, const unsigned int& _row, const unsigned int& _n_rows)
{
- // 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
+ A.prune([](int i, int, Scalar) { return i >= _row && i < _row + _n_rows; });
+}
- SparseBlockPruning bp(row, col, Nrows, Ncols);
- original.prune(bp);
+void eraseBlockCol(Eigen::SparseMatrixs& A, const unsigned int& _col, const unsigned int& _n_cols)
+{
+ A.prune([](int, int j, Scalar) { return j >= _col && j < _col + _n_cols; });
}
-void addSparseBlock(const Eigen::MatrixXd& ins, Eigen::SparseMatrix<double>& original, const unsigned int& row, const unsigned int& col)
+void addSparseBlock(const Eigen::MatrixXs& ins, Eigen::SparseMatrixs& 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 (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.makeCompressed();
}
-#endif /* TRUNK_SRC_SOLVER_SPARSE_UTILS_H_ */
+void insertSparseBlock(const Eigen::MatrixXs& ins, Eigen::SparseMatrixs& 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.makeCompressed();
+}
+
+}
+#endif /* SPARSE_UTILS_H_ */
diff --git a/src/test/CMakeLists.txt b/src/test/CMakeLists.txt
index 9f5f51f7a02255d99fc87b944f933a43a268b784..3763631a09f9297e2c7758383fe92d93ef9aaf33 100644
--- a/src/test/CMakeLists.txt
+++ b/src/test/CMakeLists.txt
@@ -45,8 +45,8 @@ wolf_add_gtest(gtest_capture_base gtest_capture_base.cpp)
target_link_libraries(gtest_capture_base ${PROJECT_NAME})
# CaptureBase class test
-wolf_add_gtest(gtest_constraint_sparse gtest_constraint_sparse.cpp)
-target_link_libraries(gtest_constraint_sparse ${PROJECT_NAME})
+#wolf_add_gtest(gtest_constraint_sparse gtest_constraint_sparse.cpp)
+#target_link_libraries(gtest_constraint_sparse ${PROJECT_NAME})
# FeatureBase classes test
wolf_add_gtest(gtest_feature_base gtest_feature_base.cpp)
@@ -134,6 +134,10 @@ target_link_libraries(gtest_odom_2D ${PROJECT_NAME})
wolf_add_gtest(gtest_odom_3D gtest_odom_3D.cpp)
target_link_libraries(gtest_odom_3D ${PROJECT_NAME})
+# ConstraintAutodiff class test
+wolf_add_gtest(gtest_constraint_autodiff gtest_constraint_autodiff.cpp)
+target_link_libraries(gtest_constraint_autodiff ${PROJECT_NAME})
+
# ROI test
IF(OpenCV_FOUND)
# Problem class test
diff --git a/src/test/gtest_constraint_autodiff.cpp b/src/test/gtest_constraint_autodiff.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..52b5d1c121987779fb20661319f5b8f1a27fd8ad
--- /dev/null
+++ b/src/test/gtest_constraint_autodiff.cpp
@@ -0,0 +1,209 @@
+/*
+ * gtest_constraint_autodiff.cpp
+ *
+ * Created on: May 24 2017
+ * Author: jvallve
+ */
+
+
+#include "utils_gtest.h"
+
+#include "sensor_odom_2D.h"
+#include "capture_void.h"
+#include "feature_odom_2D.h"
+#include "constraint_odom_2D.h"
+#include "constraint_odom_2D_analytic.h"
+#include "constraint_autodiff.h"
+
+using namespace wolf;
+using namespace Eigen;
+
+TEST(CaptureAutodiff, ConstructorOdom2d)
+{
+ FrameBasePtr fr1_ptr(std::make_shared<FrameBase>(TimeStamp(0), std::make_shared<StateBlock>(2), std::make_shared<StateBlock>(1)));
+ FrameBasePtr fr2_ptr(std::make_shared<FrameBase>(TimeStamp(0), std::make_shared<StateBlock>(2), std::make_shared<StateBlock>(1)));
+
+ // SENSOR
+ SensorOdom2DPtr sensor_ptr = std::make_shared<SensorOdom2D>(std::make_shared<StateBlock>(2), std::make_shared<StateBlock>(1), 0.1, 0.1);
+
+ // CAPTURE
+ CaptureVoidPtr capture_ptr = std::make_shared<CaptureVoid>(TimeStamp(0), sensor_ptr);
+ fr2_ptr->addCapture(capture_ptr);
+
+ // FEATURE
+ FeatureBasePtr feature_ptr = std::make_shared<FeatureOdom2D>(Eigen::Vector3s::Zero(), Eigen::Matrix3s::Identity());
+ capture_ptr->addFeature(feature_ptr);
+
+ // CONSTRAINT
+ ConstraintOdom2DPtr constraint_ptr = std::make_shared<ConstraintOdom2D>(feature_ptr, fr1_ptr);
+ feature_ptr->addConstraint(constraint_ptr);
+ fr1_ptr->addConstrainedBy(constraint_ptr);
+
+ ASSERT_TRUE(constraint_ptr->getFeaturePtr());
+ ASSERT_TRUE(constraint_ptr->getFeaturePtr()->getCapturePtr());
+ ASSERT_TRUE(constraint_ptr->getFeaturePtr()->getCapturePtr()->getFramePtr());
+ ASSERT_TRUE(constraint_ptr->getFeaturePtr()->getCapturePtr()->getSensorPtr());
+ ASSERT_TRUE(constraint_ptr->getFrameOtherPtr());
+}
+
+TEST(CaptureAutodiff, ResidualOdom2d)
+{
+ Eigen::Vector3s f1_pose, f2_pose;
+ f1_pose << 2,1,M_PI;
+ f2_pose << 3,5,3*M_PI/2;
+
+ FrameBasePtr fr1_ptr(std::make_shared<FrameBase>(TimeStamp(0), std::make_shared<StateBlock>(f1_pose.head<2>()), std::make_shared<StateBlock>(f1_pose.tail<1>())));
+ FrameBasePtr fr2_ptr(std::make_shared<FrameBase>(TimeStamp(0), std::make_shared<StateBlock>(f2_pose.head<2>()), std::make_shared<StateBlock>(f2_pose.tail<1>())));
+
+ // SENSOR
+ SensorOdom2DPtr sensor_ptr = std::make_shared<SensorOdom2D>(std::make_shared<StateBlock>(2), std::make_shared<StateBlock>(1), 0.1, 0.1);
+
+ // CAPTURE
+ CaptureVoidPtr capture_ptr = std::make_shared<CaptureVoid>(TimeStamp(0), sensor_ptr);
+ fr2_ptr->addCapture(capture_ptr);
+
+ // FEATURE
+ Eigen::Vector3s d;
+ d << -1, -4, M_PI/2;
+ FeatureBasePtr feature_ptr = std::make_shared<FeatureOdom2D>(d, Eigen::Matrix3s::Identity());
+ capture_ptr->addFeature(feature_ptr);
+
+ // CONSTRAINT
+ ConstraintOdom2DPtr constraint_ptr = std::make_shared<ConstraintOdom2D>(feature_ptr, fr1_ptr);
+ feature_ptr->addConstraint(constraint_ptr);
+ fr1_ptr->addConstrainedBy(constraint_ptr);
+
+ // EVALUATE
+ std::vector<Scalar*> states_ptr({fr1_ptr->getPPtr()->getPtr(), fr1_ptr->getOPtr()->getPtr(), fr2_ptr->getPPtr()->getPtr(), fr2_ptr->getOPtr()->getPtr()});
+ double const* const* parameters = states_ptr.data();
+ Eigen::VectorXs residuals(constraint_ptr->getSize());
+ constraint_ptr->evaluate(parameters, residuals.data(), nullptr);
+
+ ASSERT_TRUE(residuals.maxCoeff() < 1e-9);
+ ASSERT_TRUE(residuals.minCoeff() > -1e-9);
+}
+
+TEST(CaptureAutodiff, JacobianOdom2d)
+{
+ Eigen::Vector3s f1_pose, f2_pose;
+ f1_pose << 2,1,M_PI;
+ f2_pose << 3,5,3*M_PI/2;
+
+ FrameBasePtr fr1_ptr(std::make_shared<FrameBase>(TimeStamp(0), std::make_shared<StateBlock>(f1_pose.head<2>()), std::make_shared<StateBlock>(f1_pose.tail<1>())));
+ FrameBasePtr fr2_ptr(std::make_shared<FrameBase>(TimeStamp(0), std::make_shared<StateBlock>(f2_pose.head<2>()), std::make_shared<StateBlock>(f2_pose.tail<1>())));
+
+ // SENSOR
+ SensorOdom2DPtr sensor_ptr = std::make_shared<SensorOdom2D>(std::make_shared<StateBlock>(2), std::make_shared<StateBlock>(1), 0.1, 0.1);
+
+ // CAPTURE
+ CaptureVoidPtr capture_ptr = std::make_shared<CaptureVoid>(TimeStamp(0), sensor_ptr);
+ fr2_ptr->addCapture(capture_ptr);
+
+ // FEATURE
+ Eigen::Vector3s d;
+ d << -1, -4, M_PI/2;
+ FeatureBasePtr feature_ptr = std::make_shared<FeatureOdom2D>(d, Eigen::Matrix3s::Identity());
+ capture_ptr->addFeature(feature_ptr);
+
+ // CONSTRAINT
+ ConstraintOdom2DPtr constraint_ptr = std::make_shared<ConstraintOdom2D>(feature_ptr, fr1_ptr);
+ feature_ptr->addConstraint(constraint_ptr);
+ fr1_ptr->addConstrainedBy(constraint_ptr);
+
+ // COMPUTE JACOBIANS
+ std::vector<const Scalar*> states_ptr({fr1_ptr->getPPtr()->getPtr(), fr1_ptr->getOPtr()->getPtr(), fr2_ptr->getPPtr()->getPtr(), fr2_ptr->getOPtr()->getPtr()});
+ std::vector<Eigen::MatrixXs> Jauto;
+ Eigen::VectorXs residuals(constraint_ptr->getSize());
+ constraint_ptr->evaluate(states_ptr, residuals, Jauto);
+
+ std::cout << Jauto.size() << std::endl;
+
+ // ANALYTIC JACOBIANS
+ Eigen::MatrixXs J0(3,2);
+ J0 << -cos(f1_pose(2)), -sin(f1_pose(2)),
+ sin(f1_pose(2)), -cos(f1_pose(2)),
+ 0, 0;
+ ASSERT_MATRIX_APPROX(J0, Jauto[0], wolf::Constants::EPS);
+ //ASSERT_TRUE((J0-Jauto[0]).maxCoeff() < 1e-9 && (J0-Jauto[0]).minCoeff() > -1e-9);
+
+ Eigen::MatrixXs J1(3,1);
+ J1 << -(f2_pose(0) - f1_pose(0)) * sin(f1_pose(2)) + (f2_pose(1) - f1_pose(1)) * cos(f1_pose(2)),
+ -(f2_pose(0) - f1_pose(0)) * cos(f1_pose(2)) - (f2_pose(1) - f1_pose(1)) * sin(f1_pose(2)),
+ -1;
+ ASSERT_MATRIX_APPROX(J1, Jauto[1], wolf::Constants::EPS);
+ //ASSERT_TRUE((J1-Jauto[1]).maxCoeff() < 1e-9 && (J1-Jauto[1]).minCoeff() > -1e-9);
+
+ Eigen::MatrixXs J2(3,2);
+ J2 << cos(f1_pose(2)), sin(f1_pose(2)),
+ -sin(f1_pose(2)), cos(f1_pose(2)),
+ 0, 0;
+ ASSERT_MATRIX_APPROX(J2, Jauto[2], wolf::Constants::EPS);
+ //ASSERT_TRUE((J2-Jauto[2]).maxCoeff() < 1e-9 && (J2-Jauto[2]).minCoeff() > -1e-9);
+
+ Eigen::MatrixXs J3(3,1);
+ J3 << 0, 0, 1;
+ ASSERT_MATRIX_APPROX(J3, Jauto[3], wolf::Constants::EPS);
+ //ASSERT_TRUE((J3-Jauto[3]).maxCoeff() < 1e-9 && (J3-Jauto[3]).minCoeff() > -1e-9);
+
+// std::cout << "autodiff J " << 0 << ":\n" << Jauto[0] << std::endl;
+// std::cout << "analytic J " << 0 << ":\n" << J0 << std::endl;
+// std::cout << "autodiff J " << 1 << ":\n" << Jauto[1] << std::endl;
+// std::cout << "analytic J " << 1 << ":\n" << J1 << std::endl;
+// std::cout << "autodiff J " << 2 << ":\n" << Jauto[2] << std::endl;
+// std::cout << "analytic J " << 2 << ":\n" << J2 << std::endl;
+// std::cout << "autodiff J " << 3 << ":\n" << Jauto[3] << std::endl;
+// std::cout << "analytic J " << 3 << ":\n" << J3 << std::endl;
+}
+
+TEST(CaptureAutodiff, AutodiffVsAnalytic)
+{
+ Eigen::Vector3s f1_pose, f2_pose;
+ f1_pose << 2,1,M_PI;
+ f2_pose << 3,5,3*M_PI/2;
+
+ FrameBasePtr fr1_ptr(std::make_shared<FrameBase>(TimeStamp(0), std::make_shared<StateBlock>(f1_pose.head<2>()), std::make_shared<StateBlock>(f1_pose.tail<1>())));
+ FrameBasePtr fr2_ptr(std::make_shared<FrameBase>(TimeStamp(0), std::make_shared<StateBlock>(f2_pose.head<2>()), std::make_shared<StateBlock>(f2_pose.tail<1>())));
+
+ // SENSOR
+ SensorOdom2DPtr sensor_ptr = std::make_shared<SensorOdom2D>(std::make_shared<StateBlock>(2), std::make_shared<StateBlock>(1), 0.1, 0.1);
+
+ // CAPTURE
+ CaptureVoidPtr capture_ptr = std::make_shared<CaptureVoid>(TimeStamp(0), sensor_ptr);
+ fr2_ptr->addCapture(capture_ptr);
+
+ // FEATURE
+ Eigen::Vector3s d;
+ d << -1, -4, M_PI/2;
+ FeatureBasePtr feature_ptr = std::make_shared<FeatureOdom2D>(d, Eigen::Matrix3s::Identity());
+ capture_ptr->addFeature(feature_ptr);
+
+ // CONSTRAINTS
+ ConstraintOdom2DPtr ctr_autodiff_ptr = std::make_shared<ConstraintOdom2D>(feature_ptr, fr1_ptr);
+ feature_ptr->addConstraint(ctr_autodiff_ptr);
+ fr1_ptr->addConstrainedBy(ctr_autodiff_ptr);
+ ConstraintOdom2DAnalyticPtr ctr_analytic_ptr = std::make_shared<ConstraintOdom2DAnalytic>(feature_ptr, fr1_ptr);
+ feature_ptr->addConstraint(ctr_analytic_ptr);
+ fr1_ptr->addConstrainedBy(ctr_analytic_ptr);
+
+ // COMPUTE JACOBIANS
+ std::vector<const Scalar*> states_ptr({fr1_ptr->getPPtr()->getPtr(), fr1_ptr->getOPtr()->getPtr(), fr2_ptr->getPPtr()->getPtr(), fr2_ptr->getOPtr()->getPtr()});
+ std::vector<Eigen::MatrixXs> Jautodiff, Janalytic;
+ Eigen::VectorXs residuals(ctr_autodiff_ptr->getSize());
+ clock_t t = clock();
+ ctr_autodiff_ptr->evaluate(states_ptr, residuals, Jautodiff);
+ std::cout << "autodiff evaluate: " << ((double) clock() - t) / CLOCKS_PER_SEC << "s" << std::endl;
+ t = clock();
+ //TODO ConstraintAnalytic::evaluate
+// ctr_analytic_ptr->evaluate(states_ptr, residuals, Janalytic);
+// std::cout << "analytic evaluate: " << ((double) clock() - t) / CLOCKS_PER_SEC << "s" << std::endl;
+//
+// for (auto i = 0; i < Jautodiff.size(); i++)
+// ASSERT_MATRIX_APPROX(Jautodiff[i], Janalytic[i], wolf::Constants::EPS);
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
+
diff --git a/src/test/gtest_constraint_fix.cpp b/src/test/gtest_constraint_fix.cpp
index 4bafe16545e6768e21985dbe5567b18278b662f2..eaa454d6aec4a8a0a7857587940d7bc9ea66b3eb 100644
--- a/src/test/gtest_constraint_fix.cpp
+++ b/src/test/gtest_constraint_fix.cpp
@@ -59,7 +59,7 @@ TEST(ConstraintFix, solve)
frm0->setState(x0);
// solve for frm0
- ceres::Solver::Summary summary = ceres_mgr.solve();
+ std::string report = ceres_mgr.solve(0); // 0: nothing, 1: BriefReport, 2: FullReport
ASSERT_MATRIX_APPROX(frm0->getState(), pose, 1e-6);
diff --git a/src/test/gtest_constraint_fix_3D.cpp b/src/test/gtest_constraint_fix_3D.cpp
index b2bcc8d9332d53bbd672c98903699440d01bc598..126b1a081f5a8c731a8355dba8106ddd9ff5a0a7 100644
--- a/src/test/gtest_constraint_fix_3D.cpp
+++ b/src/test/gtest_constraint_fix_3D.cpp
@@ -67,7 +67,7 @@ TEST(ConstraintFix3D, solve)
frm0->setState(x0);
// solve for frm0
- ceres::Solver::Summary summary = ceres_mgr.solve();
+ std::string brief_report = ceres_mgr.solve(1);
ASSERT_MATRIX_APPROX(frm0->getState(), pose7, 1e-6);
diff --git a/src/test/gtest_constraint_imu.cpp b/src/test/gtest_constraint_imu.cpp
index 174e4783a48e9313c5ce3fc08c8e326de55ab6b0..4c19834f3af6c1edcf8ae4951d67a49f3ea4167a 100644
--- a/src/test/gtest_constraint_imu.cpp
+++ b/src/test/gtest_constraint_imu.cpp
@@ -69,7 +69,7 @@ class ConstraintIMU_biasTest_Static_NullBias : public testing::Test
ceres_options.minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;ceres::LINE_SEARCH
ceres_options.max_line_search_step_contraction = 1e-3;
ceres_options.max_num_iterations = 1e4;
- ceres_manager_wolf_diff = new CeresManager(wolf_problem_ptr_, ceres_options, true);
+ ceres_manager_wolf_diff = new CeresManager(wolf_problem_ptr_, ceres_options);
// SENSOR + PROCESSOR IMU
SensorBasePtr sen0_ptr = wolf_problem_ptr_->installSensor("IMU", "Main IMU", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/src/examples/sensor_imu.yaml");
@@ -154,7 +154,7 @@ class ConstraintIMU_biasTest_Static_NonNullAccBias : public testing::Test
ceres_options.minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;ceres::LINE_SEARCH
ceres_options.max_line_search_step_contraction = 1e-3;
ceres_options.max_num_iterations = 1e4;
- ceres_manager_wolf_diff = new CeresManager(wolf_problem_ptr_, ceres_options, true);
+ ceres_manager_wolf_diff = new CeresManager(wolf_problem_ptr_, ceres_options);
// SENSOR + PROCESSOR IMU
SensorBasePtr sen0_ptr = wolf_problem_ptr_->installSensor("IMU", "Main IMU", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/src/examples/sensor_imu.yaml");
@@ -239,7 +239,7 @@ class ConstraintIMU_biasTest_Static_NonNullGyroBias : public testing::Test
ceres_options.minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;ceres::LINE_SEARCH
ceres_options.max_line_search_step_contraction = 1e-3;
ceres_options.max_num_iterations = 1e4;
- ceres_manager_wolf_diff = new CeresManager(wolf_problem_ptr_, ceres_options, true);
+ ceres_manager_wolf_diff = new CeresManager(wolf_problem_ptr_, ceres_options);
// SENSOR + PROCESSOR IMU
SensorBasePtr sen0_ptr = wolf_problem_ptr_->installSensor("IMU", "Main IMU", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/src/examples/sensor_imu.yaml");
@@ -325,7 +325,7 @@ class ConstraintIMU_biasTest_Static_NonNullBias : public testing::Test
ceres_options.minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;ceres::LINE_SEARCH
ceres_options.max_line_search_step_contraction = 1e-3;
ceres_options.max_num_iterations = 1e4;
- ceres_manager_wolf_diff = new CeresManager(wolf_problem_ptr_, ceres_options, true);
+ ceres_manager_wolf_diff = new CeresManager(wolf_problem_ptr_, ceres_options);
// SENSOR + PROCESSOR IMU
SensorBasePtr sen0_ptr = wolf_problem_ptr_->installSensor("IMU", "Main IMU", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/src/examples/sensor_imu.yaml");
@@ -410,7 +410,7 @@ class ConstraintIMU_biasTest_Move_NullBias : public testing::Test
ceres_options.minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;ceres::LINE_SEARCH
ceres_options.max_line_search_step_contraction = 1e-3;
ceres_options.max_num_iterations = 1e4;
- ceres_manager_wolf_diff = new CeresManager(wolf_problem_ptr_, ceres_options, true);
+ ceres_manager_wolf_diff = new CeresManager(wolf_problem_ptr_, ceres_options);
// SENSOR + PROCESSOR IMU
SensorBasePtr sen0_ptr = wolf_problem_ptr_->installSensor("IMU", "Main IMU", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/src/examples/sensor_imu.yaml");
@@ -499,7 +499,7 @@ class ConstraintIMU_biasTest_Move_NonNullBias : public testing::Test
ceres_options.minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;ceres::LINE_SEARCH
ceres_options.max_line_search_step_contraction = 1e-3;
ceres_options.max_num_iterations = 1e4;
- ceres_manager_wolf_diff = new CeresManager(wolf_problem_ptr_, ceres_options, true);
+ ceres_manager_wolf_diff = new CeresManager(wolf_problem_ptr_, ceres_options);
// SENSOR + PROCESSOR IMU
SensorBasePtr sen0_ptr = wolf_problem_ptr_->installSensor("IMU", "Main IMU", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/src/examples/sensor_imu.yaml");
@@ -584,7 +584,7 @@ class ConstraintIMU_biasTest_Move_NonNullBiasRotCst : public testing::Test
ceres_options.minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;ceres::LINE_SEARCH
ceres_options.max_line_search_step_contraction = 1e-3;
ceres_options.max_num_iterations = 1e4;
- ceres_manager_wolf_diff = new CeresManager(wolf_problem_ptr_, ceres_options, true);
+ ceres_manager_wolf_diff = new CeresManager(wolf_problem_ptr_, ceres_options);
// SENSOR + PROCESSOR IMU
SensorBasePtr sen0_ptr = wolf_problem_ptr_->installSensor("IMU", "Main IMU", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/src/examples/sensor_imu.yaml");
@@ -678,7 +678,7 @@ class ConstraintIMU_biasTest_Move_NonNullBiasRotAndVCst : public testing::Test
ceres_options.minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;ceres::LINE_SEARCH
ceres_options.max_line_search_step_contraction = 1e-3;
ceres_options.max_num_iterations = 1e4;
- ceres_manager_wolf_diff = new CeresManager(wolf_problem_ptr_, ceres_options, true);
+ ceres_manager_wolf_diff = new CeresManager(wolf_problem_ptr_, ceres_options);
// SENSOR + PROCESSOR IMU
SensorBasePtr sen0_ptr = wolf_problem_ptr_->installSensor("IMU", "Main IMU", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/src/examples/sensor_imu.yaml");
@@ -780,7 +780,7 @@ class ConstraintIMU_biasTest_Move_NonNullBiasRot : public testing::Test
ceres_options.minimizer_type = ceres::TRUST_REGION;
ceres_options.max_line_search_step_contraction = 1e-3;
ceres_options.max_num_iterations = 1e4;
- ceres_manager_wolf_diff = new CeresManager(wolf_problem_ptr_, ceres_options, true);
+ ceres_manager_wolf_diff = new CeresManager(wolf_problem_ptr_, ceres_options);
// SENSOR + PROCESSOR IMU
SensorBasePtr sen0_ptr = wolf_problem_ptr_->installSensor("IMU", "Main IMU", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/src/examples/sensor_imu.yaml");
@@ -881,7 +881,7 @@ class ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRot : public testing::Test
ceres_options.minimizer_type = ceres::TRUST_REGION;
ceres_options.max_line_search_step_contraction = 1e-3;
ceres_options.max_num_iterations = 1e4;
- ceres_manager_wolf_diff = new CeresManager(wolf_problem_ptr_, ceres_options, true);
+ ceres_manager_wolf_diff = new CeresManager(wolf_problem_ptr_, ceres_options);
// SENSOR + PROCESSOR IMU
SensorBasePtr sen0_ptr = wolf_problem_ptr_->installSensor("IMU", "Main IMU", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/src/examples/sensor_imu.yaml");
@@ -1017,7 +1017,7 @@ class ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRot2 : public testing::Test
ceres_options.minimizer_type = ceres::TRUST_REGION;
ceres_options.max_line_search_step_contraction = 1e-3;
ceres_options.max_num_iterations = 1e4;
- ceres_manager_wolf_diff = new CeresManager(wolf_problem_ptr_, ceres_options, true);
+ ceres_manager_wolf_diff = new CeresManager(wolf_problem_ptr_, ceres_options);
// SENSOR + PROCESSOR IMU
SensorBasePtr sen0_ptr = wolf_problem_ptr_->installSensor("IMU", "Main IMU", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/src/examples/sensor_imu.yaml");
@@ -1156,7 +1156,7 @@ class ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRotY : public testing::Test
ceres_options.minimizer_type = ceres::TRUST_REGION;
ceres_options.max_line_search_step_contraction = 1e-3;
ceres_options.max_num_iterations = 1e4;
- ceres_manager_wolf_diff = new CeresManager(wolf_problem_ptr_, ceres_options, true);
+ ceres_manager_wolf_diff = new CeresManager(wolf_problem_ptr_, ceres_options);
// SENSOR + PROCESSOR IMU
SensorBasePtr sen0_ptr = wolf_problem_ptr_->installSensor("IMU", "Main IMU", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/src/examples/sensor_imu.yaml");
@@ -1291,7 +1291,7 @@ class ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRotXY : public testing::Test
ceres_options.minimizer_type = ceres::TRUST_REGION;
ceres_options.max_line_search_step_contraction = 1e-3;
ceres_options.max_num_iterations = 1e4;
- ceres_manager_wolf_diff = new CeresManager(wolf_problem_ptr_, ceres_options, true);
+ ceres_manager_wolf_diff = new CeresManager(wolf_problem_ptr_, ceres_options);
// SENSOR + PROCESSOR IMU
SensorBasePtr sen0_ptr = wolf_problem_ptr_->installSensor("IMU", "Main IMU", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/src/examples/sensor_imu.yaml");
@@ -1491,7 +1491,7 @@ class ConstraintIMU_ODOM_biasTest_Move_NonNullBiasComplex : public testing::Test
ceres_options.minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;ceres::LINE_SEARCH
ceres_options.max_line_search_step_contraction = 1e-3;
ceres_options.max_num_iterations = 1e4;
- ceres_manager_wolf_diff = new CeresManager(wolf_problem_ptr_, ceres_options, true);
+ ceres_manager_wolf_diff = new CeresManager(wolf_problem_ptr_, ceres_options);
// SENSOR + PROCESSOR IMU
SensorBasePtr sen0_ptr = wolf_problem_ptr_->installSensor("IMU", "Main IMU", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/src/examples/sensor_imu.yaml");
@@ -1650,7 +1650,7 @@ class ConstraintIMU_ODOM_biasTest_Move_NonNullBiasComplex_initOK : public testin
ceres_options.minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;ceres::LINE_SEARCH
ceres_options.max_line_search_step_contraction = 1e-3;
ceres_options.max_num_iterations = 1e4;
- ceres_manager_wolf_diff = new CeresManager(wolf_problem_ptr_, ceres_options, true);
+ ceres_manager_wolf_diff = new CeresManager(wolf_problem_ptr_, ceres_options);
// SENSOR + PROCESSOR IMU
SensorBasePtr sen0_ptr = wolf_problem_ptr_->installSensor("IMU", "Main IMU", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/src/examples/sensor_imu.yaml");
@@ -1809,7 +1809,7 @@ class ConstraintIMU_ODOM_biasTest_Static_NullBiasNoisyComplex_initOK : public te
ceres_options.minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;ceres::LINE_SEARCH
ceres_options.max_line_search_step_contraction = 1e-3;
ceres_options.max_num_iterations = 1e4;
- ceres_manager_wolf_diff = new CeresManager(wolf_problem_ptr_, ceres_options, true);
+ ceres_manager_wolf_diff = new CeresManager(wolf_problem_ptr_, ceres_options);
// SENSOR + PROCESSOR IMU
SensorBasePtr sen0_ptr = wolf_problem_ptr_->installSensor("IMU", "Main IMU", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/src/examples/sensor_imu.yaml");
@@ -1968,7 +1968,7 @@ class ConstraintIMU_ODOM_biasTest_Move_BiasedNoisyComplex_initOK : public testin
ceres_options.minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;ceres::LINE_SEARCH
ceres_options.max_line_search_step_contraction = 1e-3;
ceres_options.max_num_iterations = 1e4;
- ceres_manager_wolf_diff = new CeresManager(wolf_problem_ptr_, ceres_options, true);
+ ceres_manager_wolf_diff = new CeresManager(wolf_problem_ptr_, ceres_options);
// SENSOR + PROCESSOR IMU
SensorBasePtr sen0_ptr = wolf_problem_ptr_->installSensor("IMU", "Main IMU", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/src/examples/sensor_imu.yaml");
@@ -2091,7 +2091,7 @@ TEST_F(ConstraintIMU_biasTest_Static_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_initOK)
//wolf_problem_ptr_->print(1,1,1,1);
- ceres::Solver::Summary summary = ceres_manager_wolf_diff->solve();
+ std::string report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport;
//wolf_problem_ptr_->print(1,1,1,1);
@@ -2115,7 +2115,7 @@ TEST_F(ConstraintIMU_biasTest_Static_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
wolf::Scalar epsilon_bias = 0.0000001;
Eigen::VectorXs perturbated_origin_state(x_origin);
- ceres::Solver::Summary summary;
+ std::string report;
//==============================================================
//WOLF_INFO("Starting error bias 1e-6")
@@ -2127,7 +2127,7 @@ TEST_F(ConstraintIMU_biasTest_Static_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
origin_KF->setState(perturbated_origin_state);
last_KF->setState(expected_final_state);
- summary = ceres_manager_wolf_diff->solve();
+ report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport;
//Only biases are unfixed
ASSERT_MATRIX_APPROX(origin_KF->getAccBiasPtr()->getState(), origin_bias.head(3), wolf::Constants::EPS*100)
@@ -2145,7 +2145,7 @@ TEST_F(ConstraintIMU_biasTest_Static_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
origin_KF->setState(perturbated_origin_state);
last_KF->setState(expected_final_state);
- summary = ceres_manager_wolf_diff->solve();
+ report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport;
//Only biases are unfixed
ASSERT_MATRIX_APPROX(origin_KF->getAccBiasPtr()->getState(), origin_bias.head(3), wolf::Constants::EPS*100)
@@ -2163,7 +2163,7 @@ TEST_F(ConstraintIMU_biasTest_Static_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
origin_KF->setState(perturbated_origin_state);
last_KF->setState(expected_final_state);
- summary = ceres_manager_wolf_diff->solve();
+ report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport;
//Only biases are unfixed
ASSERT_MATRIX_APPROX(origin_KF->getAccBiasPtr()->getState(), origin_bias.head(3), wolf::Constants::EPS*100)
@@ -2183,7 +2183,7 @@ TEST_F(ConstraintIMU_biasTest_Static_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
origin_KF->setState(perturbated_origin_state);
last_KF->setState(expected_final_state);
- summary = ceres_manager_wolf_diff->solve();
+ report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport;
//Only biases are unfixed
ASSERT_MATRIX_APPROX(origin_KF->getAccBiasPtr()->getState(), origin_bias.head(3), wolf::Constants::EPS*100)
@@ -2207,7 +2207,7 @@ TEST_F(ConstraintIMU_biasTest_Static_NonNullAccBias,VarB1B2_InvarP1Q1V1P2Q2V2_in
last_KF->getOPtr()->fix();
last_KF->getVPtr()->fix();
- ceres::Solver::Summary summary = ceres_manager_wolf_diff->solve();
+ std::string report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport;
//Only biases are unfixed
ASSERT_MATRIX_APPROX(origin_KF->getAccBiasPtr()->getState(), origin_bias.head(3), wolf::Constants::EPS*100)
@@ -2229,7 +2229,7 @@ TEST_F(ConstraintIMU_biasTest_Static_NonNullAccBias,VarB1B2_InvarP1Q1V1P2Q2V2_Er
wolf::Scalar epsilon_bias = 0.0000001;
Eigen::VectorXs perturbated_origin_state(x_origin);
- ceres::Solver::Summary summary;
+ std::string report;
//==============================================================
//WOLF_INFO("Starting error bias 1e-6")
@@ -2241,7 +2241,7 @@ TEST_F(ConstraintIMU_biasTest_Static_NonNullAccBias,VarB1B2_InvarP1Q1V1P2Q2V2_Er
origin_KF->setState(perturbated_origin_state);
last_KF->setState(expected_final_state);
- summary = ceres_manager_wolf_diff->solve();
+ report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport;
//Only biases are unfixed
ASSERT_MATRIX_APPROX(origin_KF->getAccBiasPtr()->getState(), origin_bias.head(3), wolf::Constants::EPS*100)
@@ -2259,7 +2259,7 @@ TEST_F(ConstraintIMU_biasTest_Static_NonNullAccBias,VarB1B2_InvarP1Q1V1P2Q2V2_Er
origin_KF->setState(perturbated_origin_state);
last_KF->setState(expected_final_state);
- summary = ceres_manager_wolf_diff->solve();
+ report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport;
//Only biases are unfixed
ASSERT_MATRIX_APPROX(origin_KF->getAccBiasPtr()->getState(), origin_bias.head(3), wolf::Constants::EPS*100)
@@ -2277,7 +2277,7 @@ TEST_F(ConstraintIMU_biasTest_Static_NonNullAccBias,VarB1B2_InvarP1Q1V1P2Q2V2_Er
origin_KF->setState(perturbated_origin_state);
last_KF->setState(expected_final_state);
- summary = ceres_manager_wolf_diff->solve();
+ report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport;
//Only biases are unfixed
ASSERT_MATRIX_APPROX(origin_KF->getAccBiasPtr()->getState(), origin_bias.head(3), wolf::Constants::EPS*100)
@@ -2297,7 +2297,7 @@ TEST_F(ConstraintIMU_biasTest_Static_NonNullAccBias,VarB1B2_InvarP1Q1V1P2Q2V2_Er
origin_KF->setState(perturbated_origin_state);
last_KF->setState(expected_final_state);
- summary = ceres_manager_wolf_diff->solve();
+ report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport;
//Only biases are unfixed
ASSERT_MATRIX_APPROX(origin_KF->getAccBiasPtr()->getState(), origin_bias.head(3), wolf::Constants::EPS*100)
@@ -2321,7 +2321,7 @@ TEST_F(ConstraintIMU_biasTest_Static_NonNullGyroBias,VarB1B2_InvarP1Q1V1P2Q2V2_i
last_KF->getOPtr()->fix();
last_KF->getVPtr()->fix();
- ceres::Solver::Summary summary = ceres_manager_wolf_diff->solve();
+ std::string report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport;
//Only biases are unfixed
ASSERT_MATRIX_APPROX(origin_KF->getAccBiasPtr()->getState(), origin_bias.head(3), wolf::Constants::EPS*100)
@@ -2343,7 +2343,7 @@ TEST_F(ConstraintIMU_biasTest_Static_NonNullGyroBias,VarB1B2_InvarP1Q1V1P2Q2V2_E
wolf::Scalar epsilon_bias = 0.0000001;
Eigen::VectorXs perturbated_origin_state(x_origin);
- ceres::Solver::Summary summary;
+ std::string report;
//==============================================================
//WOLF_INFO("Starting error bias 1e-6")
@@ -2355,7 +2355,7 @@ TEST_F(ConstraintIMU_biasTest_Static_NonNullGyroBias,VarB1B2_InvarP1Q1V1P2Q2V2_E
origin_KF->setState(perturbated_origin_state);
last_KF->setState(expected_final_state);
- summary = ceres_manager_wolf_diff->solve();
+ report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport;
//Only biases are unfixed
ASSERT_MATRIX_APPROX(origin_KF->getAccBiasPtr()->getState(), origin_bias.head(3), wolf::Constants::EPS*100)
@@ -2373,7 +2373,7 @@ TEST_F(ConstraintIMU_biasTest_Static_NonNullGyroBias,VarB1B2_InvarP1Q1V1P2Q2V2_E
origin_KF->setState(perturbated_origin_state);
last_KF->setState(expected_final_state);
- summary = ceres_manager_wolf_diff->solve();
+ report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport;
//Only biases are unfixed
ASSERT_MATRIX_APPROX(origin_KF->getAccBiasPtr()->getState(), origin_bias.head(3), wolf::Constants::EPS*100)
@@ -2391,7 +2391,7 @@ TEST_F(ConstraintIMU_biasTest_Static_NonNullGyroBias,VarB1B2_InvarP1Q1V1P2Q2V2_E
origin_KF->setState(perturbated_origin_state);
last_KF->setState(expected_final_state);
- summary = ceres_manager_wolf_diff->solve();
+ report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport;
//Only biases are unfixed
ASSERT_MATRIX_APPROX(origin_KF->getAccBiasPtr()->getState(), origin_bias.head(3), wolf::Constants::EPS*100)
@@ -2411,7 +2411,7 @@ TEST_F(ConstraintIMU_biasTest_Static_NonNullGyroBias,VarB1B2_InvarP1Q1V1P2Q2V2_E
origin_KF->setState(perturbated_origin_state);
last_KF->setState(expected_final_state);
- summary = ceres_manager_wolf_diff->solve();
+ report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport;
//Only biases are unfixed
ASSERT_MATRIX_APPROX(origin_KF->getAccBiasPtr()->getState(), origin_bias.head(3), wolf::Constants::EPS*100)
@@ -2434,7 +2434,7 @@ TEST_F(ConstraintIMU_biasTest_Static_NonNullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBi
wolf::Scalar epsilon_bias = 0.0000001;
Eigen::VectorXs perturbated_origin_state(x_origin);
- ceres::Solver::Summary summary;
+ std::string report;
//==============================================================
//WOLF_INFO("Starting error bias 1e-6")
@@ -2446,7 +2446,7 @@ TEST_F(ConstraintIMU_biasTest_Static_NonNullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBi
origin_KF->setState(perturbated_origin_state);
last_KF->setState(expected_final_state);
- summary = ceres_manager_wolf_diff->solve();
+ report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport;
//Only biases are unfixed
ASSERT_MATRIX_APPROX(origin_KF->getAccBiasPtr()->getState(), origin_bias.head(3), wolf::Constants::EPS*100)
@@ -2464,7 +2464,7 @@ TEST_F(ConstraintIMU_biasTest_Static_NonNullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBi
origin_KF->setState(perturbated_origin_state);
last_KF->setState(expected_final_state);
- summary = ceres_manager_wolf_diff->solve();
+ report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport;
//Only biases are unfixed
ASSERT_MATRIX_APPROX(origin_KF->getAccBiasPtr()->getState(), origin_bias.head(3), wolf::Constants::EPS*100)
@@ -2482,7 +2482,7 @@ TEST_F(ConstraintIMU_biasTest_Static_NonNullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBi
origin_KF->setState(perturbated_origin_state);
last_KF->setState(expected_final_state);
- summary = ceres_manager_wolf_diff->solve();
+ report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport;
//Only biases are unfixed
ASSERT_MATRIX_APPROX(origin_KF->getAccBiasPtr()->getState(), origin_bias.head(3), wolf::Constants::EPS*100)
@@ -2502,7 +2502,7 @@ TEST_F(ConstraintIMU_biasTest_Static_NonNullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBi
origin_KF->setState(perturbated_origin_state);
last_KF->setState(expected_final_state);
- summary = ceres_manager_wolf_diff->solve();
+ report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport;
//Only biases are unfixed
ASSERT_MATRIX_APPROX(origin_KF->getAccBiasPtr()->getState(), origin_bias.head(3), wolf::Constants::EPS*100)
@@ -2526,7 +2526,7 @@ TEST_F(ConstraintIMU_biasTest_Move_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_initOK)
last_KF->getOPtr()->fix();
last_KF->getVPtr()->fix();
- ceres::Solver::Summary summary = ceres_manager_wolf_diff->solve();
+ std::string report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport
//Only biases are unfixed
ASSERT_MATRIX_APPROX(origin_KF->getAccBiasPtr()->getState(), origin_bias.head(3), wolf::Constants::EPS*100)
@@ -2548,7 +2548,6 @@ TEST_F(ConstraintIMU_biasTest_Move_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
wolf::Scalar epsilon_bias = 0.0000001;
Eigen::VectorXs perturbated_origin_state(x_origin);
- ceres::Solver::Summary summary;
//==============================================================
//WOLF_INFO("Starting error bias 1e-6")
@@ -2560,7 +2559,7 @@ TEST_F(ConstraintIMU_biasTest_Move_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
origin_KF->setState(perturbated_origin_state);
last_KF->setState(expected_final_state);
- summary = ceres_manager_wolf_diff->solve();
+ std::string report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport
//Only biases are unfixed
ASSERT_MATRIX_APPROX(origin_KF->getAccBiasPtr()->getState(), origin_bias.head(3), wolf::Constants::EPS*100)
@@ -2578,7 +2577,7 @@ TEST_F(ConstraintIMU_biasTest_Move_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
origin_KF->setState(perturbated_origin_state);
last_KF->setState(expected_final_state);
- summary = ceres_manager_wolf_diff->solve();
+ report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport
//Only biases are unfixed
ASSERT_MATRIX_APPROX(origin_KF->getAccBiasPtr()->getState(), origin_bias.head(3), wolf::Constants::EPS*100)
@@ -2596,7 +2595,7 @@ TEST_F(ConstraintIMU_biasTest_Move_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
origin_KF->setState(perturbated_origin_state);
last_KF->setState(expected_final_state);
- summary = ceres_manager_wolf_diff->solve();
+ report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport
//Only biases are unfixed
ASSERT_MATRIX_APPROX(origin_KF->getAccBiasPtr()->getState(), origin_bias.head(3), wolf::Constants::EPS*100)
@@ -2616,7 +2615,7 @@ TEST_F(ConstraintIMU_biasTest_Move_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
origin_KF->setState(perturbated_origin_state);
last_KF->setState(expected_final_state);
- summary = ceres_manager_wolf_diff->solve();
+ report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport
//Only biases are unfixed
ASSERT_MATRIX_APPROX(origin_KF->getAccBiasPtr()->getState(), origin_bias.head(3), wolf::Constants::EPS*100)
@@ -2642,7 +2641,7 @@ TEST_F(ConstraintIMU_biasTest_Move_NonNullBias,VarB1B2_InvarP1Q1V1P2Q2V2_initOK)
//wolf_problem_ptr_->print(4,1,1,1);
- ceres::Solver::Summary summary = ceres_manager_wolf_diff->solve();
+ std::string report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport
// wolf_problem_ptr_->print(4,1,1,1);
@@ -2666,7 +2665,6 @@ TEST_F(ConstraintIMU_biasTest_Move_NonNullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias
wolf::Scalar epsilon_bias = 0.0000001;
Eigen::VectorXs perturbated_origin_state(x_origin);
- ceres::Solver::Summary summary;
//==============================================================
//WOLF_INFO("Starting error bias 1e-6")
@@ -2678,7 +2676,7 @@ TEST_F(ConstraintIMU_biasTest_Move_NonNullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias
origin_KF->setState(perturbated_origin_state);
last_KF->setState(expected_final_state);
- summary = ceres_manager_wolf_diff->solve();
+ std::string report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport
//Only biases are unfixed
ASSERT_MATRIX_APPROX(origin_KF->getAccBiasPtr()->getState(), origin_bias.head(3), wolf::Constants::EPS*100)
@@ -2696,7 +2694,7 @@ TEST_F(ConstraintIMU_biasTest_Move_NonNullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias
origin_KF->setState(perturbated_origin_state);
last_KF->setState(expected_final_state);
- summary = ceres_manager_wolf_diff->solve();
+ report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
//Only biases are unfixed
ASSERT_MATRIX_APPROX(origin_KF->getAccBiasPtr()->getState(), origin_bias.head(3), wolf::Constants::EPS*100)
@@ -2714,7 +2712,7 @@ TEST_F(ConstraintIMU_biasTest_Move_NonNullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias
origin_KF->setState(perturbated_origin_state);
last_KF->setState(expected_final_state);
- summary = ceres_manager_wolf_diff->solve();
+ report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
//Only biases are unfixed
ASSERT_MATRIX_APPROX(origin_KF->getAccBiasPtr()->getState(), origin_bias.head(3), wolf::Constants::EPS*100)
@@ -2734,7 +2732,7 @@ TEST_F(ConstraintIMU_biasTest_Move_NonNullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias
origin_KF->setState(perturbated_origin_state);
last_KF->setState(expected_final_state);
- summary = ceres_manager_wolf_diff->solve();
+ report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
//Only biases are unfixed
ASSERT_MATRIX_APPROX(origin_KF->getAccBiasPtr()->getState(), origin_bias.head(3), wolf::Constants::EPS*100)
@@ -2760,7 +2758,7 @@ TEST_F(ConstraintIMU_biasTest_Move_NonNullBiasRotCst,VarB1B2_InvarP1Q1V1P2Q2V2_i
//wolf_problem_ptr_->print(4,1,1,1);
- ceres::Solver::Summary summary = ceres_manager_wolf_diff->solve();
+ std::string report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
//wolf_problem_ptr_->print(4,1,1,1);
@@ -2784,7 +2782,6 @@ TEST_F(ConstraintIMU_biasTest_Move_NonNullBiasRotCst,VarB1B2_InvarP1Q1V1P2Q2V2_E
wolf::Scalar epsilon_bias = 0.0000001;
Eigen::VectorXs perturbated_origin_state(x_origin);
- ceres::Solver::Summary summary;
//==============================================================
//WOLF_INFO("Starting error bias 1e-6")
@@ -2796,7 +2793,7 @@ TEST_F(ConstraintIMU_biasTest_Move_NonNullBiasRotCst,VarB1B2_InvarP1Q1V1P2Q2V2_E
origin_KF->setState(perturbated_origin_state);
last_KF->setState(expected_final_state);
- summary = ceres_manager_wolf_diff->solve();
+ std::string report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
//Only biases are unfixed
ASSERT_MATRIX_APPROX(origin_KF->getAccBiasPtr()->getState(), origin_bias.head(3), wolf::Constants::EPS*100)
@@ -2814,7 +2811,7 @@ TEST_F(ConstraintIMU_biasTest_Move_NonNullBiasRotCst,VarB1B2_InvarP1Q1V1P2Q2V2_E
origin_KF->setState(perturbated_origin_state);
last_KF->setState(expected_final_state);
- summary = ceres_manager_wolf_diff->solve();
+ report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
//Only biases are unfixed
ASSERT_MATRIX_APPROX(origin_KF->getAccBiasPtr()->getState(), origin_bias.head(3), wolf::Constants::EPS*100)
@@ -2832,7 +2829,7 @@ TEST_F(ConstraintIMU_biasTest_Move_NonNullBiasRotCst,VarB1B2_InvarP1Q1V1P2Q2V2_E
origin_KF->setState(perturbated_origin_state);
last_KF->setState(expected_final_state);
- summary = ceres_manager_wolf_diff->solve();
+ report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
//Only biases are unfixed
ASSERT_MATRIX_APPROX(origin_KF->getAccBiasPtr()->getState(), origin_bias.head(3), wolf::Constants::EPS*100)
@@ -2852,7 +2849,7 @@ TEST_F(ConstraintIMU_biasTest_Move_NonNullBiasRotCst,VarB1B2_InvarP1Q1V1P2Q2V2_E
origin_KF->setState(perturbated_origin_state);
last_KF->setState(expected_final_state);
- summary = ceres_manager_wolf_diff->solve();
+ report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
//Only biases are unfixed
ASSERT_MATRIX_APPROX(origin_KF->getAccBiasPtr()->getState(), origin_bias.head(3), wolf::Constants::EPS*100)
@@ -2878,7 +2875,7 @@ TEST_F(ConstraintIMU_biasTest_Move_NonNullBiasRotAndVCst,VarB1B2_InvarP1Q1V1P2Q2
//wolf_problem_ptr_->print(4,1,1,1);
- ceres::Solver::Summary summary = ceres_manager_wolf_diff->solve();
+ std::string report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
//wolf_problem_ptr_->print(4,1,1,1);
@@ -2902,7 +2899,6 @@ TEST_F(ConstraintIMU_biasTest_Move_NonNullBiasRotAndVCst,VarB1B2_InvarP1Q1V1P2Q2
wolf::Scalar epsilon_bias = 0.0000001;
Eigen::VectorXs perturbated_origin_state(x_origin);
- ceres::Solver::Summary summary;
//==============================================================
//WOLF_INFO("Starting error bias 1e-6")
@@ -2914,7 +2910,7 @@ TEST_F(ConstraintIMU_biasTest_Move_NonNullBiasRotAndVCst,VarB1B2_InvarP1Q1V1P2Q2
origin_KF->setState(perturbated_origin_state);
last_KF->setState(expected_final_state);
- summary = ceres_manager_wolf_diff->solve();
+ std::string report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
//Only biases are unfixed
ASSERT_MATRIX_APPROX(origin_KF->getAccBiasPtr()->getState(), origin_bias.head(3), wolf::Constants::EPS*100)
@@ -2932,7 +2928,7 @@ TEST_F(ConstraintIMU_biasTest_Move_NonNullBiasRotAndVCst,VarB1B2_InvarP1Q1V1P2Q2
origin_KF->setState(perturbated_origin_state);
last_KF->setState(expected_final_state);
- summary = ceres_manager_wolf_diff->solve();
+ report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
//Only biases are unfixed
ASSERT_MATRIX_APPROX(origin_KF->getAccBiasPtr()->getState(), origin_bias.head(3), wolf::Constants::EPS*100)
@@ -2950,7 +2946,7 @@ TEST_F(ConstraintIMU_biasTest_Move_NonNullBiasRotAndVCst,VarB1B2_InvarP1Q1V1P2Q2
origin_KF->setState(perturbated_origin_state);
last_KF->setState(expected_final_state);
- summary = ceres_manager_wolf_diff->solve();
+ report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
//Only biases are unfixed
ASSERT_MATRIX_APPROX(origin_KF->getAccBiasPtr()->getState(), origin_bias.head(3), wolf::Constants::EPS*100)
@@ -2970,7 +2966,7 @@ TEST_F(ConstraintIMU_biasTest_Move_NonNullBiasRotAndVCst,VarB1B2_InvarP1Q1V1P2Q2
origin_KF->setState(perturbated_origin_state);
last_KF->setState(expected_final_state);
- summary = ceres_manager_wolf_diff->solve();
+ report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
//Only biases are unfixed
ASSERT_MATRIX_APPROX(origin_KF->getAccBiasPtr()->getState(), origin_bias.head(3), wolf::Constants::EPS*100)
@@ -2994,7 +2990,7 @@ TEST_F(ConstraintIMU_biasTest_Move_NonNullBiasRotAndVCst, VarB1B2V1P2V2_InvarP1Q
last_KF->getOPtr()->fix();
last_KF->getVPtr()->unfix();
- ceres::Solver::Summary summary = ceres_manager_wolf_diff->solve();
+ std::string report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
//Only biases are unfixed
ASSERT_MATRIX_APPROX(origin_KF->getAccBiasPtr()->getState(), origin_bias.head(3), wolf::Constants::EPS*100)
@@ -3019,7 +3015,7 @@ TEST_F(ConstraintIMU_biasTest_Move_NonNullBiasRot, VarB1B2V1P2V2_InvarP1Q1Q2_ini
//wolf_problem_ptr_->print(4,1,1,1);
- ceres::Solver::Summary summary = ceres_manager_wolf_diff->solve();
+ std::string report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
//wolf_problem_ptr_->print(4,1,1,1);
@@ -3053,7 +3049,7 @@ TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2_InvarP1Q1V1P2Q2
//wolf_problem_ptr_->print(4,1,1,1);
- ceres::Solver::Summary summary = ceres_manager_wolf_diff->solve();
+ std::string report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
ceres_manager_wolf_diff->computeCovariances(ALL);
//wolf_problem_ptr_->print(4,1,1,1);
@@ -3105,7 +3101,7 @@ TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2V2_InvarP1Q1V1P2
origin_KF->getAccBiasPtr()->setState(accBias + random_err);
origin_KF->getGyroBiasPtr()->setState(gyroBias + random_err);
- ceres::Solver::Summary summary = ceres_manager_wolf_diff->solve();
+ std::string report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
ceres_manager_wolf_diff->computeCovariances(ALL);
ASSERT_MATRIX_APPROX(origin_KF->getAccBiasPtr()->getState(), origin_bias.head(3), wolf::Constants::EPS*100)
@@ -3156,7 +3152,7 @@ TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2V1V2_InvarP1Q1P2
origin_KF->getAccBiasPtr()->setState(accBias + random_err);
origin_KF->getGyroBiasPtr()->setState(gyroBias + random_err);
- ceres::Solver::Summary summary = ceres_manager_wolf_diff->solve();
+ std::string report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
//ceres_manager_wolf_diff->computeCovariances(ALL);
ASSERT_MATRIX_APPROX(origin_KF->getVPtr()->getState(), x_origin.segment(7,3), wolf::Constants::EPS*1000)
@@ -3189,7 +3185,7 @@ TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2V1Q2V2_InvarP1Q1
origin_KF->getAccBiasPtr()->setState(accBias + random_err);
origin_KF->getGyroBiasPtr()->setState(gyroBias + random_err);
- ceres::Solver::Summary summary = ceres_manager_wolf_diff->solve();
+ std::string report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
ASSERT_MATRIX_APPROX(origin_KF->getVPtr()->getState(), x_origin.segment(7,3), wolf::Constants::EPS*1000)
ASSERT_MATRIX_APPROX(origin_KF->getAccBiasPtr()->getState(), origin_bias.head(3), 0.00001)
@@ -3223,7 +3219,7 @@ TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2V1P2V2_InvarP1Q1
origin_KF->getAccBiasPtr()->setState(accBias + random_err);
origin_KF->getGyroBiasPtr()->setState(gyroBias + random_err);
- ceres::Solver::Summary summary = ceres_manager_wolf_diff->solve();
+ std::string report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
ASSERT_MATRIX_APPROX(origin_KF->getVPtr()->getState(), x_origin.segment(7,3), wolf::Constants::EPS*1000)
ASSERT_MATRIX_APPROX(origin_KF->getAccBiasPtr()->getState(), origin_bias.head(3), 0.00001)
@@ -3256,7 +3252,7 @@ TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2V1P2Q2V2_InvarP1
origin_KF->getGyroBiasPtr()->setState(gyroBias + random_err);
//wolf_problem_ptr_->print(4,1,1,1);
- ceres::Solver::Summary summary = ceres_manager_wolf_diff->solve();
+ std::string report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
//wolf_problem_ptr_->print(4,1,1,1);
ASSERT_MATRIX_APPROX(origin_KF->getVPtr()->getState(), x_origin.segment(7,3), wolf::Constants::EPS*10000)
@@ -3290,7 +3286,7 @@ TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2P2Q2V2_InvarP1Q1
origin_KF->getAccBiasPtr()->setState(accBias + random_err);
origin_KF->getGyroBiasPtr()->setState(gyroBias + random_err);
- ceres::Solver::Summary summary = ceres_manager_wolf_diff->solve();
+ std::string report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
ceres_manager_wolf_diff->computeCovariances(ALL);
ASSERT_MATRIX_APPROX(origin_KF->getVPtr()->getState(), x_origin.segment(7,3), wolf::Constants::EPS*1000)
@@ -3352,7 +3348,7 @@ TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarQ1B1B2P2Q2_InvarP1V1
origin_KF->getAccBiasPtr()->setState(accBias + random_err);
origin_KF->getGyroBiasPtr()->setState(gyroBias + random_err);
- ceres::Solver::Summary summary = ceres_manager_wolf_diff->solve();
+ std::string report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
ceres_manager_wolf_diff->computeCovariances(ALL);
ASSERT_MATRIX_APPROX(origin_KF->getVPtr()->getState(), x_origin.segment(7,3), wolf::Constants::EPS*1000)
@@ -3414,7 +3410,7 @@ TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRotXY, VarQ1B1B2P2Q2_InvarP1V
origin_KF->getAccBiasPtr()->setState(accBias + random_err);
origin_KF->getGyroBiasPtr()->setState(gyroBias + random_err);
- ceres::Solver::Summary summary = ceres_manager_wolf_diff->solve();
+ std::string report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
ceres_manager_wolf_diff->computeCovariances(ALL);
ASSERT_MATRIX_APPROX(origin_KF->getVPtr()->getState(), x_origin.segment(7,3), wolf::Constants::EPS*1000)
@@ -3470,7 +3466,7 @@ TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2_InvarP1Q1V1P2Q2V
//wolf_problem_ptr_->print(4,1,1,1);
- ceres::Solver::Summary summary = ceres_manager_wolf_diff->solve();
+ std::string report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
ceres_manager_wolf_diff->computeCovariances(ALL);
//wolf_problem_ptr_->print(4,1,1,1);
@@ -3522,7 +3518,7 @@ TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2V2_InvarP1Q1V1P2Q
origin_KF->getAccBiasPtr()->setState(accBias + random_err);
origin_KF->getGyroBiasPtr()->setState(gyroBias + random_err);
- ceres::Solver::Summary summary = ceres_manager_wolf_diff->solve();
+ std::string report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
ceres_manager_wolf_diff->computeCovariances(ALL);
ASSERT_MATRIX_APPROX(origin_KF->getAccBiasPtr()->getState(), origin_bias.head(3), wolf::Constants::EPS*100)
@@ -3572,7 +3568,7 @@ TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2V1V2_InvarP1Q1P2Q
origin_KF->getAccBiasPtr()->setState(accBias + random_err);
origin_KF->getGyroBiasPtr()->setState(gyroBias + random_err);
- ceres::Solver::Summary summary = ceres_manager_wolf_diff->solve();
+ std::string report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
ASSERT_MATRIX_APPROX(origin_KF->getVPtr()->getState(), x_origin.segment(7,3), wolf::Constants::EPS*1000)
ASSERT_MATRIX_APPROX(origin_KF->getAccBiasPtr()->getState(), origin_bias.head(3), 0.00001)
@@ -3603,7 +3599,7 @@ TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2V1Q2V2_InvarP1Q1P
origin_KF->getAccBiasPtr()->setState(accBias + random_err);
origin_KF->getGyroBiasPtr()->setState(gyroBias + random_err);
- ceres::Solver::Summary summary = ceres_manager_wolf_diff->solve();
+ std::string report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
ASSERT_MATRIX_APPROX(origin_KF->getVPtr()->getState(), x_origin.segment(7,3), wolf::Constants::EPS*1000)
ASSERT_MATRIX_APPROX(origin_KF->getAccBiasPtr()->getState(), origin_bias.head(3), 0.00001)
@@ -3636,7 +3632,7 @@ TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2V1P2V2_InvarP1Q1Q
origin_KF->getAccBiasPtr()->setState(accBias + random_err);
origin_KF->getGyroBiasPtr()->setState(gyroBias + random_err);
- ceres::Solver::Summary summary = ceres_manager_wolf_diff->solve();
+ std::string report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
ASSERT_MATRIX_APPROX(origin_KF->getVPtr()->getState(), x_origin.segment(7,3), wolf::Constants::EPS*1000)
ASSERT_MATRIX_APPROX(origin_KF->getAccBiasPtr()->getState(), origin_bias.head(3), 0.00001)
@@ -3668,7 +3664,7 @@ TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2V1P2Q2V2_InvarP1Q
origin_KF->getGyroBiasPtr()->setState(gyroBias + random_err);
//wolf_problem_ptr_->print(4,1,1,1);
- ceres::Solver::Summary summary = ceres_manager_wolf_diff->solve();
+ std::string report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
//wolf_problem_ptr_->print(4,1,1,1);
ASSERT_MATRIX_APPROX(origin_KF->getVPtr()->getState(), x_origin.segment(7,3), wolf::Constants::EPS*10000)
@@ -3702,7 +3698,7 @@ TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2P2Q2V2_InvarP1Q1V
origin_KF->getAccBiasPtr()->setState(accBias + random_err);
origin_KF->getGyroBiasPtr()->setState(gyroBias + random_err);
- ceres::Solver::Summary summary = ceres_manager_wolf_diff->solve();
+ std::string report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
ceres_manager_wolf_diff->computeCovariances(ALL);
ASSERT_MATRIX_APPROX(origin_KF->getVPtr()->getState(), x_origin.segment(7,3), wolf::Constants::EPS*1000)
@@ -3750,7 +3746,7 @@ TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2P2Q2V2_InvarP1Q1V
last_KF->getOPtr()->unfix();
last_KF->getVPtr()->unfix();
- ceres::Solver::Summary summary = ceres_manager_wolf_diff->solve();
+ std::string report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
ASSERT_MATRIX_APPROX(origin_KF->getPPtr()->getState(), x_origin.head(3), wolf::Constants::EPS*100)
Eigen::Map<const Eigen::Quaternions> estimatedOriginQuat(origin_KF->getOPtr()->getState().data()), expectedOriginQuat(x_origin.segment(3,4).data());
@@ -3788,7 +3784,7 @@ TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasComplex_initOK, VarB1B2_Invar
origin_KF->setState(perturbated_origin_state);
last_KF->setState(expected_final_state);
- ceres::Solver::Summary summary = ceres_manager_wolf_diff->solve();
+ std::string report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
WOLF_WARN("Precision set to ", 0.001)
ASSERT_MATRIX_APPROX(origin_KF->getAccBiasPtr()->getState(), origin_bias.head(3), 0.001)
@@ -3819,7 +3815,7 @@ TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasComplex_initOK, VarB1B2P2Q2_I
origin_KF->setState(perturbated_origin_state);
last_KF->setState(expected_final_state);
- ceres::Solver::Summary summary = ceres_manager_wolf_diff->solve();
+ std::string report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
WOLF_WARN("Precision set to ", 0.001)
ASSERT_MATRIX_APPROX(origin_KF->getAccBiasPtr()->getState(), origin_bias.head(3), 0.001)
@@ -3853,7 +3849,7 @@ TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasComplex_initOK, VarB1B2P2Q2V2
origin_KF->setState(perturbated_origin_state);
last_KF->setState(expected_final_state);
- ceres::Solver::Summary summary = ceres_manager_wolf_diff->solve();
+ std::string report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
WOLF_WARN("Precision set to ", 0.0001)
@@ -3889,7 +3885,7 @@ TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasComplex_initOK, VarAll)
origin_KF->setState(perturbated_origin_state);
last_KF->setState(expected_final_state);
- ceres::Solver::Summary summary = ceres_manager_wolf_diff->solve();
+ std::string report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
WOLF_WARN("Precision set to ", 0.001)
@@ -3929,7 +3925,7 @@ TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasComplex, VarB1B2_InvarP1Q1V1P
origin_KF->setState(perturbated_origin_state);
last_KF->setState(expected_final_state);
- ceres::Solver::Summary summary = ceres_manager_wolf_diff->solve();
+ std::string report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
//ASSERT_MATRIX_APPROX(origin_KF->getAccBiasPtr()->getState(), origin_bias.head(3), wolf::Constants::EPS*100)
//ASSERT_MATRIX_APPROX(origin_KF->getGyroBiasPtr()->getState(), origin_bias.tail(3), wolf::Constants::EPS*100)
@@ -3976,7 +3972,7 @@ TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasComplex, VarB1B2P2Q2V2_InvarP
origin_KF->setState(perturbated_origin_state);
last_KF->setState(expected_final_state);
- ceres::Solver::Summary summary = ceres_manager_wolf_diff->solve();
+ std::string report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
ceres_manager_wolf_diff->computeCovariances(ALL);
ASSERT_MATRIX_APPROX(origin_KF->getAccBiasPtr()->getState(), origin_bias.head(3), wolf::Constants::EPS*100)
@@ -4028,7 +4024,7 @@ TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasComplex, VarB1B2_InvarP1Q1V1P
origin_KF->setState(perturbated_origin_state);
last_KF->setState(expected_final_state);
- ceres::Solver::Summary summary = ceres_manager_wolf_diff->solve();
+ std::string report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
ceres_manager_wolf_diff->computeCovariances(ALL);
/*ASSERT_MATRIX_APPROX(origin_KF->getAccBiasPtr()->getState(), origin_bias.head(3), wolf::Constants::EPS*100)
@@ -4067,7 +4063,6 @@ TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasComplex_initOK,VarB1B2_InvarP
wolf::Scalar epsilon_bias = 0.0000001;
Eigen::VectorXs perturbated_origin_state(x_origin);
- ceres::Solver::Summary summary;
//==============================================================
//WOLF_INFO("Starting error bias 1e-6")
@@ -4079,7 +4074,7 @@ TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasComplex_initOK,VarB1B2_InvarP
origin_KF->setState(perturbated_origin_state);
last_KF->setState(expected_final_state);
- summary = ceres_manager_wolf_diff->solve();
+ std::string report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
WOLF_WARN("Precision set to ", 0.001)
//Only biases are unfixed
@@ -4098,7 +4093,7 @@ TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasComplex_initOK,VarB1B2_InvarP
origin_KF->setState(perturbated_origin_state);
last_KF->setState(expected_final_state);
- summary = ceres_manager_wolf_diff->solve();
+ report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
//Only biases are unfixed
ASSERT_MATRIX_APPROX(origin_KF->getAccBiasPtr()->getState(), origin_bias.head(3), 0.001)
@@ -4116,7 +4111,7 @@ TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasComplex_initOK,VarB1B2_InvarP
origin_KF->setState(perturbated_origin_state);
last_KF->setState(expected_final_state);
- summary = ceres_manager_wolf_diff->solve();
+ report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
//Only biases are unfixed
ASSERT_MATRIX_APPROX(origin_KF->getAccBiasPtr()->getState(), origin_bias.head(3), 0.001)
@@ -4136,7 +4131,7 @@ TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasComplex_initOK,VarB1B2_InvarP
origin_KF->setState(perturbated_origin_state);
last_KF->setState(expected_final_state);
- summary = ceres_manager_wolf_diff->solve();
+ report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
//Only biases are unfixed
ASSERT_MATRIX_APPROX(origin_KF->getAccBiasPtr()->getState(), origin_bias.head(3), 0.001)
@@ -4172,9 +4167,9 @@ TEST_F(ConstraintIMU_ODOM_biasTest_Static_NullBiasNoisyComplex_initOK, varB1B2P2
//wolf_problem_ptr_->print(4,1,1,1);
- ceres::Solver::Summary summary = ceres_manager_wolf_diff->solve();
+ std::string report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
ceres_manager_wolf_diff->computeCovariances(ALL);
- std::cout << summary.BriefReport() << std::endl;
+ std::cout << report << std::endl;
//wolf_problem_ptr_->print(4,1,1,1);
@@ -4229,9 +4224,9 @@ TEST_F(ConstraintIMU_ODOM_biasTest_Static_NullBiasNoisyComplex_initOK, varQ1B1P2
origin_KF->setState(perturbated_origin_state);
last_KF->setState(expected_final_state);
- ceres::Solver::Summary summary = ceres_manager_wolf_diff->solve();
+ std::string report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
ceres_manager_wolf_diff->computeCovariances(ALL);
- std::cout << summary.BriefReport() << std::endl;
+ std::cout << report << std::endl;
//wolf_problem_ptr_->print(4,1,1,1);
@@ -4285,10 +4280,9 @@ TEST_F(ConstraintIMU_ODOM_biasTest_Static_NullBiasNoisyComplex_initOK, varB1B2_i
last_KF->setState(expected_final_state);
//wolf_problem_ptr_->print(4,1,1,1);
-
- ceres::Solver::Summary summary = ceres_manager_wolf_diff->solve();
+ std::string report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
ceres_manager_wolf_diff->computeCovariances(ALL);
- std::cout << summary.BriefReport() << std::endl;
+ std::cout << report << std::endl;
//wolf_problem_ptr_->print(4,1,1,1);
@@ -4340,9 +4334,9 @@ TEST_F(ConstraintIMU_ODOM_biasTest_Move_BiasedNoisyComplex_initOK, varB1P2Q2V2B2
origin_KF->setState(perturbated_origin_state);
last_KF->setState(expected_final_state);
- ceres::Solver::Summary summary = ceres_manager_wolf_diff->solve();
+ std::string report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
ceres_manager_wolf_diff->computeCovariances(ALL);
- std::cout << summary.BriefReport() << std::endl;
+ std::cout << report << std::endl;
//These ASSERTS can be removed since we are more interested in using covariances to make sure that expected values are inside estimated +/- 2*std
WOLF_WARN("Precision set to ", 0.01)
@@ -4405,9 +4399,9 @@ TEST_F(ConstraintIMU_ODOM_biasTest_Move_BiasedNoisyComplex_initOK, varQ1B1P2Q2B2
origin_KF->setState(perturbated_origin_state);
last_KF->setState(expected_final_state);
- ceres::Solver::Summary summary = ceres_manager_wolf_diff->solve();
+ std::string report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
ceres_manager_wolf_diff->computeCovariances(ALL);
- std::cout << summary.BriefReport() << std::endl;
+ std::cout << report << std::endl;
//wolf_problem_ptr_->print(4,1,1,1);
diff --git a/src/test/gtest_constraint_odom_3D.cpp b/src/test/gtest_constraint_odom_3D.cpp
index 39afa2989534989c9f7c787e356ce91e9b98dc37..077568e260e28f83d9aabdc1e16699de827252d1 100644
--- a/src/test/gtest_constraint_odom_3D.cpp
+++ b/src/test/gtest_constraint_odom_3D.cpp
@@ -69,7 +69,7 @@ TEST(ConstraintOdom3D, fix_0_solve)
frm1->setState(x1);
// solve for frm1
- ceres::Solver::Summary summary = ceres_mgr.solve();
+ std::string report = ceres_mgr.solve(1);
ASSERT_MATRIX_APPROX(frm1->getState(), delta, 1e-6);
@@ -83,7 +83,7 @@ TEST(ConstraintOdom3D, fix_1_solve)
frm0->setState(x0);
// solve for frm0
- ceres::Solver::Summary summary = ceres_mgr.solve();
+ std::string brief_report = ceres_mgr.solve(1);
ASSERT_MATRIX_APPROX(frm0->getState(), problem->zeroState(), 1e-6);
}
diff --git a/src/test/gtest_odom_2D.cpp b/src/test/gtest_odom_2D.cpp
index 59f549cd20c60fe03e5949583a6ce92b6f0fedff..eac322008965686b4b63458258de20a1c62af99e 100644
--- a/src/test/gtest_odom_2D.cpp
+++ b/src/test/gtest_odom_2D.cpp
@@ -125,7 +125,6 @@ TEST(Odom2D, ConstraintFix_and_ConstraintOdom2D)
ProblemPtr Pr = Problem::create("PO 2D");
CeresManager ceres_manager(Pr);
- ceres::Solver::Summary summary;
// KF0 and absolute prior
FrameBasePtr F0 = Pr->setPrior(x0, P0,t0);
@@ -154,9 +153,9 @@ TEST(Odom2D, ConstraintFix_and_ConstraintOdom2D)
F0->setState(Vector3s(1,2,3));
F1->setState(Vector3s(2,3,1));
F2->setState(Vector3s(3,1,2));
- summary = ceres_manager.solve();
-// std::cout << summary.BriefReport() << std::endl;
-// std::cout << summary.FullReport() << std::endl;
+ std::string report = ceres_manager.solve(1);
+// std::cout << report << std::endl;
+
ceres_manager.computeCovariances(ALL_MARGINALS);
// show(Pr);
@@ -212,7 +211,7 @@ TEST(Odom2D, VoteForKfAndSolve)
// Origin Key Frame
FrameBasePtr origin_frame = problem->setPrior(x0, P0, t0);
- ceres_manager.solve();
+ ceres_manager.solve(0);
ceres_manager.computeCovariances(ALL_MARGINALS);
// std::cout << "Initial pose : " << problem->getCurrentState().transpose() << std::endl;
@@ -281,7 +280,8 @@ TEST(Odom2D, VoteForKfAndSolve)
}
// Solve
- ceres::Solver::Summary summary = ceres_manager.solve();
+ std::string report = ceres_manager.solve(1);
+// std::cout << report << std::endl;
ceres_manager.computeCovariances(ALL_MARGINALS);
ASSERT_MATRIX_APPROX(problem->getLastKeyFrameCovariance() , integrated_cov_vector[5], 1e-6);
@@ -326,7 +326,6 @@ TEST(Odom2D, KF_callback)
// Ceres wrapper
CeresManager ceres_manager(problem);
- ceres::Solver::Summary summary;
// Origin Key Frame
FrameBasePtr keyframe_0 = problem->setPrior(x0, x0_cov, t0);
@@ -399,7 +398,8 @@ TEST(Odom2D, KF_callback)
MotionBuffer key_buffer_n = key_capture_n->getBuffer();
- ceres_manager.solve();
+ std::string report = ceres_manager.solve(1);
+// std::cout << report << std::endl;
ceres_manager.computeCovariances(ALL_MARGINALS);
ASSERT_POSE2D_APPROX(problem->getLastKeyFramePtr()->getState() , integrated_pose_vector[n_split], 1e-6);
@@ -428,8 +428,9 @@ TEST(Odom2D, KF_callback)
keyframe_0->setState(Vector3s(1,2,3));
keyframe_1->setState(Vector3s(2,3,1));
keyframe_2->setState(Vector3s(3,1,2));
- summary = ceres_manager.solve();
-// std::cout << summary.BriefReport() << std::endl;
+
+ report = ceres_manager.solve(1);
+// std::cout << report << std::endl;
ceres_manager.computeCovariances(ALL_MARGINALS);
// show(problem);
diff --git a/src/wolf.h b/src/wolf.h
index 0e945f49d9ea47c7ed4c73657c4e9f44f77977c4..cb8094e01f79df77b34032191276f32bb493a968 100644
--- a/src/wolf.h
+++ b/src/wolf.h
@@ -109,6 +109,9 @@ typedef Transform<wolf::Scalar,3,Affine> Affine3ds; ///< Affine3d of rea
typedef Transform<wolf::Scalar,2,Isometry> Isometry2ds; ///< Isometry2d of real Scalar type
typedef Transform<wolf::Scalar,3,Isometry> Isometry3ds; ///< Isometry3d of real Scalar type
+
+// 3. Sparse matrix
+typedef SparseMatrix<wolf::Scalar, RowMajor, int> SparseMatrixs;
}
namespace wolf {