diff --git a/include/core/factor/factor_autodiff.h b/include/core/factor/factor_autodiff.h
index d5e86d2f168acce03feb9085420de1c6ad901ab8..71e09826bbc8f37b5b594e3c93d1041c7f83dcb5 100644
--- a/include/core/factor/factor_autodiff.h
+++ b/include/core/factor/factor_autodiff.h
@@ -15,7 +15,7 @@
namespace wolf {
//template class FactorAutodiff
-template <class FacT, unsigned int RES, unsigned int B0, unsigned int B1 = 0, unsigned int B2 = 0, unsigned int B3 = 0, unsigned int B4 = 0, unsigned int B5 = 0, unsigned int B6 = 0, unsigned int B7 = 0, unsigned int B8 = 0, unsigned int B9 = 0>
+template <class FacT, unsigned int RES, unsigned int B0, unsigned int B1 = 0, unsigned int B2 = 0, unsigned int B3 = 0, unsigned int B4 = 0, unsigned int B5 = 0, unsigned int B6 = 0, unsigned int B7 = 0, unsigned int B8 = 0, unsigned int B9 = 0, unsigned int B10 = 0, unsigned int B11 = 0>
class FactorAutodiff : public FactorBase
{
public:
@@ -31,12 +31,15 @@ class FactorAutodiff : public FactorBase
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 unsigned int block10Size = B10;
+ static const unsigned int block11Size = B11;
+ static const unsigned int n_blocks = 12;
static const std::vector<unsigned int> state_block_sizes_;
typedef ceres::Jet<double, B0 + B1 + B2 + B3 + B4 +
- B5 + B6 + B7 + B8 + B9> WolfJet;
+ B5 + B6 + B7 + B8 + B9 +
+ B10 + B11> WolfJet;
protected:
@@ -54,6 +57,8 @@ class FactorAutodiff : public FactorBase
std::array<WolfJet, B7>* jets_7_;
std::array<WolfJet, B8>* jets_8_;
std::array<WolfJet, B9>* jets_9_;
+ std::array<WolfJet, B10>* jets_10_;
+ std::array<WolfJet, B11>* jets_11_;
public:
/** \brief Constructor valid for all categories (ABSOLUTE, FRAME, FEATURE, LANDMARK)
@@ -75,9 +80,11 @@ class FactorAutodiff : public FactorBase
StateBlockPtr _state6Ptr,
StateBlockPtr _state7Ptr,
StateBlockPtr _state8Ptr,
- StateBlockPtr _state9Ptr) :
+ StateBlockPtr _state9Ptr,
+ StateBlockPtr _state10Ptr,
+ StateBlockPtr _state11Ptr) :
FactorBase(_tp, _frame_other_ptr, _capture_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}),
+ state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr,_state5Ptr,_state6Ptr,_state7Ptr,_state8Ptr,_state9Ptr,_state10Ptr,_state11Ptr}),
residuals_jets_(new std::array<WolfJet, RES>),
jets_0_(new std::array<WolfJet, B0>),
jets_1_(new std::array<WolfJet, B1>),
@@ -88,7 +95,9 @@ class FactorAutodiff : public FactorBase
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>)
+ jets_9_(new std::array<WolfJet, B9>),
+ jets_10_(new std::array<WolfJet, B10>),
+ jets_11_(new std::array<WolfJet, B11>)
{
// initialize jets
unsigned int last_jet_idx = 0;
@@ -111,7 +120,11 @@ class FactorAutodiff : public FactorBase
for (unsigned int i = 0; i < B8; i++)
(*jets_8_)[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B9; i++)
- (*jets_9_)[i] = WolfJet(0, last_jet_idx++);
+ (*jets_9_)[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B10; i++)
+ (*jets_10_)[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B11; i++)
+ (*jets_11_)[i] = WolfJet(0, last_jet_idx++);
}
virtual ~FactorAutodiff()
@@ -126,6 +139,8 @@ class FactorAutodiff : public FactorBase
delete jets_7_;
delete jets_8_;
delete jets_9_;
+ delete jets_10_;
+ delete jets_11_;
delete residuals_jets_;
}
@@ -154,6 +169,8 @@ class FactorAutodiff : public FactorBase
parameters[7],
parameters[8],
parameters[9],
+ parameters[10],
+ parameters[11],
residuals);
}
// also compute jacobians
@@ -175,6 +192,8 @@ class FactorAutodiff : public FactorBase
jets_7_->data(),
jets_8_->data(),
jets_9_->data(),
+ jets_10_->data(),
+ jets_11_->data(),
residuals_jets_->data());
// fill the residual array
@@ -218,6 +237,10 @@ class FactorAutodiff : public FactorBase
(*jets_8_)[i].a = parameters[8][i];
for (unsigned int i = 0; i < B9; i++)
(*jets_9_)[i].a = parameters[9][i];
+ for (unsigned int i = 0; i < B10; i++)
+ (*jets_10_)[i].a = parameters[10][i];
+ for (unsigned int i = 0; i < B11; i++)
+ (*jets_11_)[i].a = parameters[11][i];
}
/** \brief Returns a vector of Jacobian matrix corresponding to each state block evaluated in the point provided in _states_ptr
@@ -244,6 +267,8 @@ class FactorAutodiff : public FactorBase
jets_7_->data(),
jets_8_->data(),
jets_9_->data(),
+ jets_10_->data(),
+ jets_11_->data(),
residuals_jets_->data());
// fill the residual vector
@@ -299,10 +324,552 @@ class FactorAutodiff : public FactorBase
}
};
+
+////////////////// SPECIALIZATION 11 BLOCKS ////////////////////////////////////////////////////////////////////////
+
+template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6,unsigned int B7,unsigned int B8,unsigned int B9,unsigned int B10>
+class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,B9,B10,0> : public FactorBase
+{
+ public:
+
+ static const unsigned int residualSize = RES;
+ static const unsigned int block0Size = B0;
+ static const unsigned int block1Size = B1;
+ static const unsigned int block2Size = B2;
+ static const unsigned int block3Size = B3;
+ static const unsigned int block4Size = B4;
+ static const unsigned int block5Size = B5;
+ static const unsigned int block6Size = B6;
+ static const unsigned int block7Size = B7;
+ static const unsigned int block8Size = B8;
+ static const unsigned int block9Size = B9;
+ static const unsigned int block10Size = B10;
+ static const unsigned int block11Size = 0;
+ static const unsigned int n_blocks = 11;
+
+ static const std::vector<unsigned int> state_block_sizes_;
+
+ typedef ceres::Jet<double, B0 + B1 + B2 + B3 + B4 +
+ B5 + B6 + B7 + B8 + B9 + B10> 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_;
+ std::array<WolfJet, B10>* jets_10_;
+
+ public:
+
+ FactorAutodiff(const std::string& _tp,
+ const FrameBasePtr& _frame_other_ptr,
+ const CaptureBasePtr& _capture_other_ptr,
+ const FeatureBasePtr& _feature_other_ptr,
+ const LandmarkBasePtr& _landmark_other_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status,
+ StateBlockPtr _state0Ptr,
+ StateBlockPtr _state1Ptr,
+ StateBlockPtr _state2Ptr,
+ StateBlockPtr _state3Ptr,
+ StateBlockPtr _state4Ptr,
+ StateBlockPtr _state5Ptr,
+ StateBlockPtr _state6Ptr,
+ StateBlockPtr _state7Ptr,
+ StateBlockPtr _state8Ptr,
+ StateBlockPtr _state9Ptr,
+ StateBlockPtr _state10Ptr) :
+ FactorBase(_tp, _frame_other_ptr, _capture_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,_state10Ptr}),
+ 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>),
+ jets_10_(new std::array<WolfJet, B10>)
+ {
+ // initialize jets
+ unsigned int last_jet_idx = 0;
+ for (unsigned int i = 0; i < B0; i++)
+ (*jets_0_)[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B1; i++)
+ (*jets_1_)[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B2; i++)
+ (*jets_2_)[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B3; i++)
+ (*jets_3_)[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B4; i++)
+ (*jets_4_)[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B5; i++)
+ (*jets_5_)[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B6; i++)
+ (*jets_6_)[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B7; i++)
+ (*jets_7_)[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B8; i++)
+ (*jets_8_)[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B9; i++)
+ (*jets_9_)[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B10; i++)
+ (*jets_10_)[i] = WolfJet(0, last_jet_idx++);
+ state_ptrs_.resize(n_blocks);
+ }
+
+ virtual ~FactorAutodiff()
+ {
+ 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 jets_10_;
+ 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<FacT const*>(this))(parameters[0],
+ parameters[1],
+ parameters[2],
+ parameters[3],
+ parameters[4],
+ parameters[5],
+ parameters[6],
+ parameters[7],
+ parameters[8],
+ parameters[9],
+ parameters[10],
+ residuals);
+ }
+ // also compute jacobians
+ else
+ {
+ // update jets real part
+ std::vector<double const*> param_vec;
+ param_vec.assign(parameters,parameters+n_blocks);
+ updateJetsRealPart(param_vec);
+
+ // call functor
+ (*static_cast<FacT const*>(this))(jets_0_->data(),
+ jets_1_->data(),
+ jets_2_->data(),
+ jets_3_->data(),
+ jets_4_->data(),
+ jets_5_->data(),
+ jets_6_->data(),
+ jets_7_->data(),
+ jets_8_->data(),
+ jets_9_->data(),
+ jets_10_->data(),
+ residuals_jets_->data());
+
+ // fill the residual array
+ for (unsigned int i = 0; i < RES; i++)
+ residuals[i] = (*residuals_jets_)[i].a;
+
+ // fill the jacobian matrices
+ for (unsigned int i = 0; i<n_blocks; i++)
+ if (jacobians[i] != nullptr)
+ for (unsigned int row = 0; row < RES; row++)
+ std::copy((*residuals_jets_)[row].v.data() + jacobian_locations_.at(i),
+ (*residuals_jets_)[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
+ jacobians[i] + row * state_block_sizes_.at(i));
+ }
+ return true;
+ }
+
+ void updateJetsRealPart(const std::vector<double const*>& parameters) const
+ {
+ // update jets real part
+ for (unsigned int i = 0; i < B0; i++)
+ (*jets_0_)[i].a = parameters[0][i];
+ for (unsigned int i = 0; i < B1; i++)
+ (*jets_1_)[i].a = parameters[1][i];
+ for (unsigned int i = 0; i < B2; i++)
+ (*jets_2_)[i].a = parameters[2][i];
+ for (unsigned int i = 0; i < B3; i++)
+ (*jets_3_)[i].a = parameters[3][i];
+ for (unsigned int i = 0; i < B4; i++)
+ (*jets_4_)[i].a = parameters[4][i];
+ for (unsigned int i = 0; i < B5; i++)
+ (*jets_5_)[i].a = parameters[5][i];
+ for (unsigned int i = 0; i < B6; i++)
+ (*jets_6_)[i].a = parameters[6][i];
+ for (unsigned int i = 0; i < B7; i++)
+ (*jets_7_)[i].a = parameters[7][i];
+ for (unsigned int i = 0; i < B8; i++)
+ (*jets_8_)[i].a = parameters[8][i];
+ for (unsigned int i = 0; i < B9; i++)
+ (*jets_9_)[i].a = parameters[9][i];
+ for (unsigned int i = 0; i < B10; i++)
+ (*jets_10_)[i].a = parameters[10][i];
+ }
+
+ virtual void evaluate(const std::vector<const double*>& _states_ptr, Eigen::VectorXd& residual_, std::vector<Eigen::MatrixXd>& jacobians_) const
+ {
+ assert(residual_.size() == RES);
+ jacobians_.clear();
+
+ assert(_states_ptr.size() == n_blocks);
+
+ // update jets real part
+ updateJetsRealPart(_states_ptr);
+
+ // call functor
+ (*static_cast<FacT const*>(this))(jets_0_->data(),
+ jets_1_->data(),
+ jets_2_->data(),
+ jets_3_->data(),
+ jets_4_->data(),
+ jets_5_->data(),
+ jets_6_->data(),
+ jets_7_->data(),
+ jets_8_->data(),
+ jets_9_->data(),
+ jets_10_->data(),
+ residuals_jets_->data());
+
+ // fill the residual vector
+ for (unsigned int i = 0; i < RES; i++)
+ residual_(i) = (*residuals_jets_)[i].a;
+
+ // fill the jacobian matrices
+ for (unsigned int i = 0; i<n_blocks; i++)
+ {
+ // Create a row major Jacobian
+ Eigen::MatrixRowXd Ji(Eigen::MatrixRowXd::Zero(RES, state_block_sizes_[i]));
+ // Fill Jacobian rows from jets
+ if (!state_ptrs_[i]->isFixed())
+ for (unsigned int row = 0; row < RES; row++)
+ std::copy((*residuals_jets_)[row].v.data() + jacobian_locations_.at(i),
+ (*residuals_jets_)[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
+ Ji.data() + row * state_block_sizes_.at(i));
+ // add to the Jacobians vector
+ jacobians_.push_back(Ji);
+ }
+
+ // print jacobian matrices
+// for (unsigned int i = 0; i < n_blocks; i++)
+// std::cout << jacobians_[i] << std::endl << std::endl;
+ }
+
+ 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 10 BLOCKS ////////////////////////////////////////////////////////////////////////
+
+template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6,unsigned int B7,unsigned int B8,unsigned int B9>
+class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,B9,0,0> : public FactorBase
+{
+ public:
+
+ static const unsigned int residualSize = RES;
+ static const unsigned int block0Size = B0;
+ static const unsigned int block1Size = B1;
+ static const unsigned int block2Size = B2;
+ static const unsigned int block3Size = B3;
+ static const unsigned int block4Size = B4;
+ static const unsigned int block5Size = B5;
+ static const unsigned int block6Size = B6;
+ static const unsigned int block7Size = B7;
+ static const unsigned int block8Size = B8;
+ static const unsigned int block9Size = B9;
+ static const unsigned int block10Size = 0;
+ static const unsigned int block11Size = 0;
+ static const unsigned int n_blocks = 10;
+
+ static const std::vector<unsigned int> state_block_sizes_;
+
+ typedef ceres::Jet<double, 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:
+
+ FactorAutodiff(const std::string& _tp,
+ const FrameBasePtr& _frame_other_ptr,
+ const CaptureBasePtr& _capture_other_ptr,
+ const FeatureBasePtr& _feature_other_ptr,
+ const LandmarkBasePtr& _landmark_other_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status,
+ StateBlockPtr _state0Ptr,
+ StateBlockPtr _state1Ptr,
+ StateBlockPtr _state2Ptr,
+ StateBlockPtr _state3Ptr,
+ StateBlockPtr _state4Ptr,
+ StateBlockPtr _state5Ptr,
+ StateBlockPtr _state6Ptr,
+ StateBlockPtr _state7Ptr,
+ StateBlockPtr _state8Ptr,
+ StateBlockPtr _state9Ptr) :
+ FactorBase(_tp, _frame_other_ptr, _capture_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 (unsigned int i = 0; i < B0; i++)
+ (*jets_0_)[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B1; i++)
+ (*jets_1_)[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B2; i++)
+ (*jets_2_)[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B3; i++)
+ (*jets_3_)[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B4; i++)
+ (*jets_4_)[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B5; i++)
+ (*jets_5_)[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B6; i++)
+ (*jets_6_)[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B7; i++)
+ (*jets_7_)[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B8; i++)
+ (*jets_8_)[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B9; i++)
+ (*jets_9_)[i] = WolfJet(0, last_jet_idx++);
+ state_ptrs_.resize(n_blocks);
+ }
+
+ virtual ~FactorAutodiff()
+ {
+ 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;
+ }
+
+ virtual bool evaluate(double const* const* parameters, double* residuals, double** jacobians) const
+ {
+ // only residuals
+ if (jacobians == nullptr)
+ {
+ (*static_cast<FacT const*>(this))(parameters[0],
+ parameters[1],
+ parameters[2],
+ parameters[3],
+ parameters[4],
+ parameters[5],
+ parameters[6],
+ parameters[7],
+ parameters[8],
+ parameters[9],
+ residuals);
+ }
+ // also compute jacobians
+ else
+ {
+ // update jets real part
+ std::vector<double const*> param_vec;
+ param_vec.assign(parameters,parameters+n_blocks);
+ updateJetsRealPart(param_vec);
+
+ // call functor
+ (*static_cast<FacT const*>(this))(jets_0_->data(),
+ jets_1_->data(),
+ jets_2_->data(),
+ jets_3_->data(),
+ jets_4_->data(),
+ jets_5_->data(),
+ jets_6_->data(),
+ jets_7_->data(),
+ jets_8_->data(),
+ jets_9_->data(),
+ residuals_jets_->data());
+
+ // fill the residual array
+ for (unsigned int i = 0; i < RES; i++)
+ residuals[i] = (*residuals_jets_)[i].a;
+
+ // fill the jacobian matrices
+ for (unsigned int i = 0; i<n_blocks; i++)
+ if (jacobians[i] != nullptr)
+ for (unsigned int row = 0; row < RES; row++)
+ std::copy((*residuals_jets_)[row].v.data() + jacobian_locations_.at(i),
+ (*residuals_jets_)[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
+ jacobians[i] + row * state_block_sizes_.at(i));
+ }
+ return true;
+ }
+
+ void updateJetsRealPart(const std::vector<double const*>& parameters) const
+ {
+ // update jets real part
+ for (unsigned int i = 0; i < B0; i++)
+ (*jets_0_)[i].a = parameters[0][i];
+ for (unsigned int i = 0; i < B1; i++)
+ (*jets_1_)[i].a = parameters[1][i];
+ for (unsigned int i = 0; i < B2; i++)
+ (*jets_2_)[i].a = parameters[2][i];
+ for (unsigned int i = 0; i < B3; i++)
+ (*jets_3_)[i].a = parameters[3][i];
+ for (unsigned int i = 0; i < B4; i++)
+ (*jets_4_)[i].a = parameters[4][i];
+ for (unsigned int i = 0; i < B5; i++)
+ (*jets_5_)[i].a = parameters[5][i];
+ for (unsigned int i = 0; i < B6; i++)
+ (*jets_6_)[i].a = parameters[6][i];
+ for (unsigned int i = 0; i < B7; i++)
+ (*jets_7_)[i].a = parameters[7][i];
+ for (unsigned int i = 0; i < B8; i++)
+ (*jets_8_)[i].a = parameters[8][i];
+ for (unsigned int i = 0; i < B9; i++)
+ (*jets_9_)[i].a = parameters[9][i];
+ }
+
+ virtual void evaluate(const std::vector<const double*>& _states_ptr, Eigen::VectorXd& residual_, std::vector<Eigen::MatrixXd>& jacobians_) const
+ {
+ assert(residual_.size() == RES);
+ jacobians_.clear();
+
+ assert(_states_ptr.size() == n_blocks);
+
+ // update jets real part
+ updateJetsRealPart(_states_ptr);
+
+ // call functor
+ (*static_cast<FacT const*>(this))(jets_0_->data(),
+ jets_1_->data(),
+ jets_2_->data(),
+ jets_3_->data(),
+ jets_4_->data(),
+ jets_5_->data(),
+ jets_6_->data(),
+ jets_7_->data(),
+ jets_8_->data(),
+ jets_9_->data(),
+ residuals_jets_->data());
+
+ // fill the residual vector
+ for (unsigned int i = 0; i < RES; i++)
+ residual_(i) = (*residuals_jets_)[i].a;
+
+ // fill the jacobian matrices
+ for (unsigned int i = 0; i<n_blocks; i++)
+ {
+ // Create a row major Jacobian
+ Eigen::MatrixRowXd Ji(Eigen::MatrixRowXd::Zero(RES, state_block_sizes_[i]));
+ // Fill Jacobian rows from jets
+ if (!state_ptrs_[i]->isFixed())
+ for (unsigned int row = 0; row < RES; row++)
+ std::copy((*residuals_jets_)[row].v.data() + jacobian_locations_.at(i),
+ (*residuals_jets_)[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
+ Ji.data() + row * state_block_sizes_.at(i));
+ // add to the Jacobians vector
+ jacobians_.push_back(Ji);
+ }
+
+ // print jacobian matrices
+// for (unsigned int i = 0; i < n_blocks; i++)
+// std::cout << jacobians_[i] << std::endl << std::endl;
+ }
+
+ 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 9 BLOCKS ////////////////////////////////////////////////////////////////////////
template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6,unsigned int B7,unsigned int B8>
-class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,0> : public FactorBase
+class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,0,0,0> : public FactorBase
{
public:
@@ -317,6 +884,8 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,0> : public FactorBase
static const unsigned int block7Size = B7;
static const unsigned int block8Size = B8;
static const unsigned int block9Size = 0;
+ static const unsigned int block10Size = 0;
+ static const unsigned int block11Size = 0;
static const unsigned int n_blocks = 9;
static const std::vector<unsigned int> state_block_sizes_;
@@ -554,7 +1123,7 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,0> : public FactorBase
////////////////// SPECIALIZATION 8 BLOCKS ////////////////////////////////////////////////////////////////////////
template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6,unsigned int B7>
-class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,0,0> : public FactorBase
+class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,0,0,0,0> : public FactorBase
{
public:
@@ -569,6 +1138,8 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,0,0> : public FactorBase
static const unsigned int block7Size = B7;
static const unsigned int block8Size = 0;
static const unsigned int block9Size = 0;
+ static const unsigned int block10Size = 0;
+ static const unsigned int block11Size = 0;
static const unsigned int n_blocks = 8;
static const std::vector<unsigned int> state_block_sizes_;
@@ -795,7 +1366,7 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,0,0> : public FactorBase
////////////////// SPECIALIZATION 7 BLOCKS ////////////////////////////////////////////////////////////////////////
template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6>
-class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,0,0,0> : public FactorBase
+class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,0,0,0,0,0> : public FactorBase
{
public:
@@ -810,6 +1381,8 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,0,0,0> : public FactorBase
static const unsigned int block7Size = 0;
static const unsigned int block8Size = 0;
static const unsigned int block9Size = 0;
+ static const unsigned int block10Size = 0;
+ static const unsigned int block11Size = 0;
static const unsigned int n_blocks = 7;
static const std::vector<unsigned int> state_block_sizes_;
@@ -1025,7 +1598,7 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,0,0,0> : public FactorBase
////////////////// SPECIALIZATION 6 BLOCKS ////////////////////////////////////////////////////////////////////////
template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5>
-class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,0,0,0,0> : public FactorBase
+class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,0,0,0,0,0,0> : public FactorBase
{
public:
@@ -1040,6 +1613,8 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,0,0,0,0> : public FactorBase
static const unsigned int block7Size = 0;
static const unsigned int block8Size = 0;
static const unsigned int block9Size = 0;
+ static const unsigned int block10Size = 0;
+ static const unsigned int block11Size = 0;
static const unsigned int n_blocks = 6;
static const std::vector<unsigned int> state_block_sizes_;
@@ -1240,7 +1815,7 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,0,0,0,0> : public FactorBase
////////////////// SPECIALIZATION 5 BLOCKS ////////////////////////////////////////////////////////////////////////
template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4>
-class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,0,0,0,0,0> : public FactorBase
+class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,0,0,0,0,0,0,0> : public FactorBase
{
public:
@@ -1255,6 +1830,8 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,0,0,0,0,0> : public FactorBase
static const unsigned int block7Size = 0;
static const unsigned int block8Size = 0;
static const unsigned int block9Size = 0;
+ static const unsigned int block10Size = 0;
+ static const unsigned int block11Size = 0;
static const unsigned int n_blocks = 5;
static const std::vector<unsigned int> state_block_sizes_;
@@ -1443,7 +2020,7 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,0,0,0,0,0> : public FactorBase
////////////////// SPECIALIZATION 4 BLOCKS ////////////////////////////////////////////////////////////////////////
template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3>
-class FactorAutodiff<FacT,RES,B0,B1,B2,B3,0,0,0,0,0,0> : public FactorBase
+class FactorAutodiff<FacT,RES,B0,B1,B2,B3,0,0,0,0,0,0,0,0> : public FactorBase
{
public:
@@ -1458,6 +2035,8 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,0,0,0,0,0,0> : public FactorBase
static const unsigned int block7Size = 0;
static const unsigned int block8Size = 0;
static const unsigned int block9Size = 0;
+ static const unsigned int block10Size = 0;
+ static const unsigned int block11Size = 0;
static const unsigned int n_blocks = 4;
static const std::vector<unsigned int> state_block_sizes_;
@@ -1639,7 +2218,7 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,0,0,0,0,0,0> : public FactorBase
////////////////// SPECIALIZATION 3 BLOCKS ////////////////////////////////////////////////////////////////////////
template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2>
-class FactorAutodiff<FacT,RES,B0,B1,B2,0,0,0,0,0,0,0> : public FactorBase
+class FactorAutodiff<FacT,RES,B0,B1,B2,0,0,0,0,0,0,0,0,0> : public FactorBase
{
public:
@@ -1654,6 +2233,8 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,0,0,0,0,0,0,0> : public FactorBase
static const unsigned int block7Size = 0;
static const unsigned int block8Size = 0;
static const unsigned int block9Size = 0;
+ static const unsigned int block10Size = 0;
+ static const unsigned int block11Size = 0;
static const unsigned int n_blocks = 3;
static const std::vector<unsigned int> state_block_sizes_;
@@ -1824,7 +2405,7 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,0,0,0,0,0,0,0> : public FactorBase
////////////////// SPECIALIZATION 2 BLOCKS ////////////////////////////////////////////////////////////////////////
template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1>
-class FactorAutodiff<FacT,RES,B0,B1,0,0,0,0,0,0,0,0> : public FactorBase
+class FactorAutodiff<FacT,RES,B0,B1,0,0,0,0,0,0,0,0,0,0> : public FactorBase
{
public:
@@ -1839,6 +2420,8 @@ class FactorAutodiff<FacT,RES,B0,B1,0,0,0,0,0,0,0,0> : public FactorBase
static const unsigned int block7Size = 0;
static const unsigned int block8Size = 0;
static const unsigned int block9Size = 0;
+ static const unsigned int block10Size = 0;
+ static const unsigned int block11Size = 0;
static const unsigned int n_blocks = 2;
static const std::vector<unsigned int> state_block_sizes_;
@@ -1998,7 +2581,7 @@ class FactorAutodiff<FacT,RES,B0,B1,0,0,0,0,0,0,0,0> : public FactorBase
////////////////// SPECIALIZATION 1 BLOCK ////////////////////////////////////////////////////////////////////////
template <class FacT,unsigned int RES,unsigned int B0>
-class FactorAutodiff<FacT,RES,B0,0,0,0,0,0,0,0,0,0> : public FactorBase
+class FactorAutodiff<FacT,RES,B0,0,0,0,0,0,0,0,0,0,0,0> : public FactorBase
{
public:
@@ -2013,6 +2596,8 @@ class FactorAutodiff<FacT,RES,B0,0,0,0,0,0,0,0,0,0> : public FactorBase
static const unsigned int block7Size = 0;
static const unsigned int block8Size = 0;
static const unsigned int block9Size = 0;
+ static const unsigned int block10Size = 0;
+ static const unsigned int block11Size = 0;
static const unsigned int n_blocks = 1;
static const std::vector<unsigned int> state_block_sizes_;
@@ -2162,113 +2747,146 @@ class FactorAutodiff<FacT,RES,B0,0,0,0,0,0,0,0,0,0> : public FactorBase
// STATIC CONST VECTORS INITIALIZATION
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// state_block_sizes_
+// 12 BLOCKS
+template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6,unsigned int B7,unsigned int B8,unsigned int B9,unsigned int B10,unsigned int B11>
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,B9,B10,B11>::state_block_sizes_ = {B0,B1,B2,B3,B4,B5,B6,B7,B8,B9,B10,B11};
+// 11 BLOCKS
+template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6,unsigned int B7,unsigned int B8,unsigned int B9,unsigned int B10>
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,B9,B10,0>::state_block_sizes_ = {B0,B1,B2,B3,B4,B5,B6,B7,B8,B9,B10};
// 10 BLOCKS
template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6,unsigned int B7,unsigned int B8,unsigned int B9>
-const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,B9>::state_block_sizes_ = {B0,B1,B2,B3,B4,B5,B6,B7,B8,B9};
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,B9,0,0>::state_block_sizes_ = {B0,B1,B2,B3,B4,B5,B6,B7,B8,B9};
// 9 BLOCKS
template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6,unsigned int B7,unsigned int B8>
-const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,0>::state_block_sizes_ = {B0,B1,B2,B3,B4,B5,B6,B7,B8};
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,0,0,0>::state_block_sizes_ = {B0,B1,B2,B3,B4,B5,B6,B7,B8};
// 8 BLOCKS
template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6,unsigned int B7>
-const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,0,0>::state_block_sizes_ = {B0,B1,B2,B3,B4,B5,B6,B7};
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,0,0,0,0>::state_block_sizes_ = {B0,B1,B2,B3,B4,B5,B6,B7};
// 7 BLOCKS
template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6>
-const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,0,0,0>::state_block_sizes_ = {B0,B1,B2,B3,B4,B5,B6};
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,0,0,0,0,0>::state_block_sizes_ = {B0,B1,B2,B3,B4,B5,B6};
// 6 BLOCKS
template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5>
-const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,0,0,0,0>::state_block_sizes_ = {B0,B1,B2,B3,B4,B5};
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,0,0,0,0,0,0>::state_block_sizes_ = {B0,B1,B2,B3,B4,B5};
// 5 BLOCKS
template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4>
-const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,0,0,0,0,0>::state_block_sizes_ = {B0,B1,B2,B3,B4};
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,0,0,0,0,0,0,0>::state_block_sizes_ = {B0,B1,B2,B3,B4};
// 4 BLOCKS
template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3>
-const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,0,0,0,0,0,0>::state_block_sizes_ = {B0,B1,B2,B3};
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,0,0,0,0,0,0,0,0>::state_block_sizes_ = {B0,B1,B2,B3};
// 3 BLOCKS
template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2>
-const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,0,0,0,0,0,0,0>::state_block_sizes_ = {B0,B1,B2};
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,0,0,0,0,0,0,0,0,0>::state_block_sizes_ = {B0,B1,B2};
// 2 BLOCKS
template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1>
-const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,0,0,0,0,0,0,0,0>::state_block_sizes_ = {B0,B1};
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,0,0,0,0,0,0,0,0,0,0>::state_block_sizes_ = {B0,B1};
// 1 BLOCK
template <class FacT,unsigned int RES,unsigned int B0>
-const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,0,0,0,0,0,0,0,0,0>::state_block_sizes_ = {B0};
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,0,0,0,0,0,0,0,0,0,0,0>::state_block_sizes_ = {B0};
// jacobian_locations_
+// 12 BLOCKS
+template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6,unsigned int B7,unsigned int B8,unsigned int B9,unsigned int B10,unsigned int B11>
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,B9,B10,B11>::jacobian_locations_ = {0,
+ B0,
+ B0+B1,
+ B0+B1+B2,
+ B0+B1+B2+B3,
+ B0+B1+B2+B3+B4,
+ B0+B1+B2+B3+B4+B5,
+ B0+B1+B2+B3+B4+B5+B6,
+ B0+B1+B2+B3+B4+B5+B6+B7,
+ B0+B1+B2+B3+B4+B5+B6+B7+B8,
+ B0+B1+B2+B3+B4+B5+B6+B7+B8+B9,
+ B0+B1+B2+B3+B4+B5+B6+B7+B8+B9+B10};
+// 10 BLOCKS
+template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6,unsigned int B7,unsigned int B8,unsigned int B9,unsigned int B10>
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,B9,B10,0>::jacobian_locations_ = {0,
+ B0,
+ B0+B1,
+ B0+B1+B2,
+ B0+B1+B2+B3,
+ B0+B1+B2+B3+B4,
+ B0+B1+B2+B3+B4+B5,
+ B0+B1+B2+B3+B4+B5+B6,
+ B0+B1+B2+B3+B4+B5+B6+B7,
+ B0+B1+B2+B3+B4+B5+B6+B7+B8,
+ B0+B1+B2+B3+B4+B5+B6+B7+B8+B9};
// 10 BLOCKS
template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6,unsigned int B7,unsigned int B8,unsigned int B9>
-const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,B9>::jacobian_locations_ = {0,
- B0,
- B0+B1,
- B0+B1+B2,
- B0+B1+B2+B3,
- B0+B1+B2+B3+B4,
- B0+B1+B2+B3+B4+B5,
- B0+B1+B2+B3+B4+B5+B6,
- B0+B1+B2+B3+B4+B5+B6+B7,
- B0+B1+B2+B3+B4+B5+B6+B7+B8};
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,B9,0,0>::jacobian_locations_ = {0,
+ B0,
+ B0+B1,
+ B0+B1+B2,
+ B0+B1+B2+B3,
+ B0+B1+B2+B3+B4,
+ B0+B1+B2+B3+B4+B5,
+ B0+B1+B2+B3+B4+B5+B6,
+ B0+B1+B2+B3+B4+B5+B6+B7,
+ B0+B1+B2+B3+B4+B5+B6+B7+B8};
// 9 BLOCKS
template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6,unsigned int B7,unsigned int B8>
-const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,0>::jacobian_locations_ = {0,
- B0,
- B0+B1,
- B0+B1+B2,
- B0+B1+B2+B3,
- B0+B1+B2+B3+B4,
- B0+B1+B2+B3+B4+B5,
- B0+B1+B2+B3+B4+B5+B6,
- B0+B1+B2+B3+B4+B5+B6+B7};
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,0,0,0>::jacobian_locations_ = {0,
+ B0,
+ B0+B1,
+ B0+B1+B2,
+ B0+B1+B2+B3,
+ B0+B1+B2+B3+B4,
+ B0+B1+B2+B3+B4+B5,
+ B0+B1+B2+B3+B4+B5+B6,
+ B0+B1+B2+B3+B4+B5+B6+B7};
// 8 BLOCKS
template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6,unsigned int B7>
-const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,0,0>::jacobian_locations_ = {0,
- B0,
- B0+B1,
- B0+B1+B2,
- B0+B1+B2+B3,
- B0+B1+B2+B3+B4,
- B0+B1+B2+B3+B4+B5,
- B0+B1+B2+B3+B4+B5+B6};
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,0,0,0,0>::jacobian_locations_ = {0,
+ B0,
+ B0+B1,
+ B0+B1+B2,
+ B0+B1+B2+B3,
+ B0+B1+B2+B3+B4,
+ B0+B1+B2+B3+B4+B5,
+ B0+B1+B2+B3+B4+B5+B6};
// 7 BLOCKS
template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6>
-const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,0,0,0>::jacobian_locations_ = {0,
- B0,
- B0+B1,
- B0+B1+B2,
- B0+B1+B2+B3,
- B0+B1+B2+B3+B4,
- B0+B1+B2+B3+B4+B5};
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,0,0,0,0,0>::jacobian_locations_ = {0,
+ B0,
+ B0+B1,
+ B0+B1+B2,
+ B0+B1+B2+B3,
+ B0+B1+B2+B3+B4,
+ B0+B1+B2+B3+B4+B5};
// 6 BLOCKS
template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5>
-const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,0,0,0,0>::jacobian_locations_ = {0,
- B0,
- B0+B1,
- B0+B1+B2,
- B0+B1+B2+B3,
- B0+B1+B2+B3+B4};
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,0,0,0,0,0,0>::jacobian_locations_ = {0,
+ B0,
+ B0+B1,
+ B0+B1+B2,
+ B0+B1+B2+B3,
+ B0+B1+B2+B3+B4};
// 5 BLOCKS
template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4>
-const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,0,0,0,0,0>::jacobian_locations_ = {0,
- B0,
- B0+B1,
- B0+B1+B2,
- B0+B1+B2+B3};
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,0,0,0,0,0,0,0>::jacobian_locations_ = {0,
+ B0,
+ B0+B1,
+ B0+B1+B2,
+ B0+B1+B2+B3};
// 4 BLOCKS
template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3>
-const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,0,0,0,0,0,0>::jacobian_locations_ = {0,
- B0,
- B0+B1,
- B0+B1+B2};
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,0,0,0,0,0,0,0,0>::jacobian_locations_ = {0,
+ B0,
+ B0+B1,
+ B0+B1+B2};
// 3 BLOCKS
template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2>
-const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,0,0,0,0,0,0,0>::jacobian_locations_ = {0,
- B0,
- B0+B1};
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,0,0,0,0,0,0,0,0,0>::jacobian_locations_ = {0,
+ B0,
+ B0+B1};
// 2 BLOCKS
template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1>
-const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,0,0,0,0,0,0,0,0>::jacobian_locations_ = {0,
- B0};
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,0,0,0,0,0,0,0,0,0,0>::jacobian_locations_ = {0,
+ B0};
// 1 BLOCK
template <class FacT,unsigned int RES,unsigned int B0>
-const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,0,0,0,0,0,0,0,0,0>::jacobian_locations_ = {0};
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,0,0,0,0,0,0,0,0,0,0,0>::jacobian_locations_ = {0};
} // namespace wolf
diff --git a/test/dummy/factor_dummy_zero_1.h b/test/dummy/factor_dummy_zero_1.h
new file mode 100644
index 0000000000000000000000000000000000000000..ffe372f0cb9f5671cc491f295d38ec24b305da3b
--- /dev/null
+++ b/test/dummy/factor_dummy_zero_1.h
@@ -0,0 +1,45 @@
+#ifndef FACTOR_DUMMY_ZERO_1_H_
+#define FACTOR_DUMMY_ZERO_1_H_
+
+//Wolf includes
+#include "core/factor/factor_autodiff.h"
+
+//#include "ceres/jet.h"
+
+namespace wolf {
+
+WOLF_PTR_TYPEDEFS(FactorDummyZero1);
+
+//class
+class FactorDummyZero1 : public FactorAutodiff<FactorDummyZero1, 1, 1>
+{
+ public:
+ FactorDummyZero1(const StateBlockPtr& _sb_ptr) :
+ FactorAutodiff<FactorDummyZero1, 1, 1>("FactorDummy1",
+ nullptr, nullptr, nullptr, nullptr,
+ nullptr,
+ false, FAC_ACTIVE,
+ _sb_ptr)
+ {
+ //
+ }
+
+ virtual ~FactorDummyZero1() = default;
+
+ virtual std::string getTopology() const override
+ {
+ return std::string("MOTION");
+ }
+
+ template<typename T>
+ bool operator ()(const T* const _st1,
+ T* _residuals) const
+ {
+ _residuals[0] = _st1[0];
+ return true;
+ }
+};
+
+} // namespace wolf
+
+#endif
diff --git a/test/dummy/factor_dummy_zero_12.h b/test/dummy/factor_dummy_zero_12.h
new file mode 100644
index 0000000000000000000000000000000000000000..3a57aa386d20524fbbec30aa154f555af0fbb9a1
--- /dev/null
+++ b/test/dummy/factor_dummy_zero_12.h
@@ -0,0 +1,155 @@
+#ifndef FACTOR_DUMMY_ZERO_12_H_
+#define FACTOR_DUMMY_ZERO_12_H_
+
+//Wolf includes
+#include "core/factor/factor_autodiff.h"
+
+//#include "ceres/jet.h"
+
+namespace wolf {
+
+//class
+template <unsigned int ID>
+class FactorDummyZero12 : public FactorAutodiff<FactorDummyZero12<ID>, ID, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12>
+{
+ static const unsigned int id = ID;
+ public:
+ FactorDummyZero12(const StateBlockPtr& _sb1_ptr,
+ const StateBlockPtr& _sb2_ptr,
+ const StateBlockPtr& _sb3_ptr,
+ const StateBlockPtr& _sb4_ptr,
+ const StateBlockPtr& _sb5_ptr,
+ const StateBlockPtr& _sb6_ptr,
+ const StateBlockPtr& _sb7_ptr,
+ const StateBlockPtr& _sb8_ptr,
+ const StateBlockPtr& _sb9_ptr,
+ const StateBlockPtr& _sb10_ptr,
+ const StateBlockPtr& _sb11_ptr,
+ const StateBlockPtr& _sb12_ptr) :
+ FactorAutodiff<FactorDummyZero12<ID>, ID, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12>("FactorDummy12",
+ nullptr, nullptr, nullptr, nullptr,
+ nullptr,
+ false, FAC_ACTIVE,
+ _sb1_ptr,
+ _sb2_ptr,
+ _sb3_ptr,
+ _sb4_ptr,
+ _sb5_ptr,
+ _sb6_ptr,
+ _sb7_ptr,
+ _sb8_ptr,
+ _sb9_ptr,
+ _sb10_ptr,
+ _sb11_ptr,
+ _sb12_ptr)
+ {
+ assert(id > 0 && id <= 12);
+ }
+
+ virtual ~FactorDummyZero12() = default;
+
+ virtual std::string getTopology() const override
+ {
+ return std::string("MOTION");
+ }
+
+ template<typename T>
+ bool operator ()(const T* const _st1,
+ const T* const _st2,
+ const T* const _st3,
+ const T* const _st4,
+ const T* const _st5,
+ const T* const _st6,
+ const T* const _st7,
+ const T* const _st8,
+ const T* const _st9,
+ const T* const _st10,
+ const T* const _st11,
+ const T* const _st12,
+ T* _residuals) const
+ {
+ Eigen::Map<Eigen::Matrix<T,ID,1>> residuals(_residuals);
+ switch (id)
+ {
+ case 1:
+ {
+ Eigen::Map<const Eigen::Matrix<T,ID,1>> st1(_st1);
+ residuals = st1;
+ break;
+ }
+ case 2:
+ {
+ Eigen::Map<const Eigen::Matrix<T,ID,1>> st2(_st2);
+ residuals = st2;
+ break;
+ }
+ case 3:
+ {
+ Eigen::Map<const Eigen::Matrix<T,ID,1>> st3(_st3);
+ residuals = st3;
+ break;
+ }
+ case 4:
+ {
+ Eigen::Map<const Eigen::Matrix<T,ID,1>> st4(_st4);
+ residuals = st4;
+ break;
+ }
+ case 5:
+ {
+ Eigen::Map<const Eigen::Matrix<T,ID,1>> st5(_st5);
+ residuals = st5;
+ break;
+ }
+ case 6:
+ {
+ Eigen::Map<const Eigen::Matrix<T,ID,1>> st6(_st6);
+ residuals = st6;
+ break;
+ }
+ case 7:
+ {
+ Eigen::Map<const Eigen::Matrix<T,ID,1>> st7(_st7);
+ residuals = st7;
+ break;
+ }
+ case 8:
+ {
+ Eigen::Map<const Eigen::Matrix<T,ID,1>> st8(_st8);
+ residuals = st8;
+ break;
+ }
+ case 9:
+ {
+ Eigen::Map<const Eigen::Matrix<T,ID,1>> st9(_st9);
+ residuals = st9;
+ break;
+ }
+ case 10:
+ {
+ Eigen::Map<const Eigen::Matrix<T,ID,1>> st10(_st10);
+ residuals = st10;
+ break;
+ }
+ case 11:
+ {
+ Eigen::Map<const Eigen::Matrix<T,ID,1>> st11(_st11);
+ residuals = st11;
+ break;
+ }
+ case 12:
+ {
+ Eigen::Map<const Eigen::Matrix<T,ID,1>> st12(_st12);
+ residuals = st12;
+ break;
+ }
+ default:
+ throw std::runtime_error("ID not found");
+ }
+ return true;
+ }
+};
+
+} // namespace wolf
+
+#endif
diff --git a/test/gtest_factor_autodiff.cpp b/test/gtest_factor_autodiff.cpp
index 993908461d6b37cae1b24d8701a3612d4dc9c432..0ca75836e54c0661be560a623f484bee0e5c592a 100644
--- a/test/gtest_factor_autodiff.cpp
+++ b/test/gtest_factor_autodiff.cpp
@@ -13,11 +13,360 @@
#include "core/factor/factor_odom_2d.h"
#include "core/factor/factor_odom_2d_analytic.h"
#include "core/factor/factor_autodiff.h"
+#include "dummy/factor_dummy_zero_1.h"
+#include "dummy/factor_dummy_zero_12.h"
using namespace wolf;
using namespace Eigen;
-TEST(CaptureAutodiff, ConstructorOdom2d)
+template <int First, int Last>
+struct static_for
+{
+ template <typename Lambda>
+ static inline constexpr void apply(Lambda const& f)
+ {
+ if (First < Last)
+ {
+ f(std::integral_constant<int, First>{});
+ static_for<First + 1, Last>::apply(f);
+ }
+ }
+};
+template <int N>
+struct static_for<N, N>
+{
+ template <typename Lambda>
+ static inline constexpr void apply(Lambda const& f) {}
+};
+
+
+TEST(FactorAutodiff, AutodiffDummy1)
+{
+ StateBlockPtr sb = std::make_shared<StateBlock>(Eigen::Vector1d::Random());
+
+ auto fac = std::make_shared<FactorDummyZero1>(sb);
+
+ // COMPUTE JACOBIANS
+ std::vector<const double*> states_ptr({sb->getStateData()});
+
+ std::vector<Eigen::MatrixXd> J;
+ Eigen::VectorXd residuals(fac->getSize());
+ fac->evaluate(states_ptr, residuals, J);
+
+ ASSERT_MATRIX_APPROX(J[0], Eigen::Matrix1d::Ones(), wolf::Constants::EPS);
+}
+
+TEST(FactorAutodiff, AutodiffDummy12)
+{
+ StateBlockPtr sb1 = std::make_shared<StateBlock>(Eigen::Vector1d::Random());
+ StateBlockPtr sb2 = std::make_shared<StateBlock>(Eigen::Vector2d::Random());
+ StateBlockPtr sb3 = std::make_shared<StateBlock>(Eigen::Vector3d::Random());
+ StateBlockPtr sb4 = std::make_shared<StateBlock>(Eigen::Vector4d::Random());
+ StateBlockPtr sb5 = std::make_shared<StateBlock>(Eigen::Vector5d::Random());
+ StateBlockPtr sb6 = std::make_shared<StateBlock>(Eigen::Vector6d::Random());
+ StateBlockPtr sb7 = std::make_shared<StateBlock>(Eigen::Vector7d::Random());
+ StateBlockPtr sb8 = std::make_shared<StateBlock>(Eigen::Vector8d::Random());
+ StateBlockPtr sb9 = std::make_shared<StateBlock>(Eigen::Vector9d::Random());
+ StateBlockPtr sb10 = std::make_shared<StateBlock>(Eigen::Vector10d::Random());
+ StateBlockPtr sb11 = std::make_shared<StateBlock>(Eigen::VectorXd::Random(11));
+ StateBlockPtr sb12 = std::make_shared<StateBlock>(Eigen::VectorXd::Random(12));
+
+ std::vector<const double*> states_ptr({sb1->getStateData(),sb2->getStateData(),sb3->getStateData(),sb4->getStateData(),sb5->getStateData(),sb6->getStateData(),sb7->getStateData(),sb8->getStateData(),sb9->getStateData(),sb10->getStateData(),sb11->getStateData(),sb12->getStateData()});
+ std::vector<Eigen::MatrixXd> J;
+ Eigen::VectorXd residuals;
+ unsigned int i;
+ FactorBasePtr fac = nullptr;
+
+ // 1 ------------------------------------------------------------------------
+ fac = std::make_shared<FactorDummyZero12<1>>(sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12);
+ i = fac->getSize();
+
+ // Jacobian
+ J.clear();
+ residuals.resize(i);
+ fac->evaluate(states_ptr, residuals, J);
+
+ std::cout << "i = " << i << std::endl;
+ for (unsigned int j = 0; j < 12; j++)
+ {
+ std::cout << "j = " << j << std::endl;
+ std::cout << "Jacobian size: " << J[j].rows() << " " << J[j].cols() << std::endl;
+ if (j == i-1)
+ {
+ ASSERT_MATRIX_APPROX(J[j], Eigen::MatrixXd::Identity(i,i), wolf::Constants::EPS);
+ }
+ else
+ {
+ ASSERT_MATRIX_APPROX(J[j], Eigen::MatrixXd::Zero(i,j+1), wolf::Constants::EPS);
+ }
+ }
+
+ // 2 ------------------------------------------------------------------------
+ fac = std::make_shared<FactorDummyZero12<2>>(sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12);
+ i = fac->getSize();
+
+ // Jacobian
+ J.clear();
+ residuals.resize(i);
+ fac->evaluate(states_ptr, residuals, J);
+
+ std::cout << "i = " << i << std::endl;
+ for (unsigned int j = 0; j < 12; j++)
+ {
+ std::cout << "j = " << j << std::endl;
+ std::cout << "Jacobian size: " << J[j].rows() << " " << J[j].cols() << std::endl;
+ if (j == i-1)
+ {
+ ASSERT_MATRIX_APPROX(J[j], Eigen::MatrixXd::Identity(i,i), wolf::Constants::EPS);
+ }
+ else
+ {
+ ASSERT_MATRIX_APPROX(J[j], Eigen::MatrixXd::Zero(i,j+1), wolf::Constants::EPS);
+ }
+ }
+
+ // 3 ------------------------------------------------------------------------
+ fac = std::make_shared<FactorDummyZero12<3>>(sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12);
+ i = fac->getSize();
+
+ // Jacobian
+ J.clear();
+ residuals.resize(i);
+ fac->evaluate(states_ptr, residuals, J);
+
+ std::cout << "i = " << i << std::endl;
+ for (unsigned int j = 0; j < 12; j++)
+ {
+ std::cout << "j = " << j << std::endl;
+ std::cout << "Jacobian size: " << J[j].rows() << " " << J[j].cols() << std::endl;
+ if (j == i-1)
+ {
+ ASSERT_MATRIX_APPROX(J[j], Eigen::MatrixXd::Identity(i,i), wolf::Constants::EPS);
+ }
+ else
+ {
+ ASSERT_MATRIX_APPROX(J[j], Eigen::MatrixXd::Zero(i,j+1), wolf::Constants::EPS);
+ }
+ }
+
+ // 4 ------------------------------------------------------------------------
+ fac = std::make_shared<FactorDummyZero12<4>>(sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12);
+ i = fac->getSize();
+
+ // Jacobian
+ J.clear();
+ residuals.resize(i);
+ fac->evaluate(states_ptr, residuals, J);
+
+ std::cout << "i = " << i << std::endl;
+ for (unsigned int j = 0; j < 12; j++)
+ {
+ std::cout << "j = " << j << std::endl;
+ std::cout << "Jacobian size: " << J[j].rows() << " " << J[j].cols() << std::endl;
+ if (j == i-1)
+ {
+ ASSERT_MATRIX_APPROX(J[j], Eigen::MatrixXd::Identity(i,i), wolf::Constants::EPS);
+ }
+ else
+ {
+ ASSERT_MATRIX_APPROX(J[j], Eigen::MatrixXd::Zero(i,j+1), wolf::Constants::EPS);
+ }
+ }
+
+ // 5 ------------------------------------------------------------------------
+ fac = std::make_shared<FactorDummyZero12<5>>(sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12);
+ i = fac->getSize();
+
+ // Jacobian
+ J.clear();
+ residuals.resize(i);
+ fac->evaluate(states_ptr, residuals, J);
+
+ std::cout << "i = " << i << std::endl;
+ for (unsigned int j = 0; j < 12; j++)
+ {
+ std::cout << "j = " << j << std::endl;
+ std::cout << "Jacobian size: " << J[j].rows() << " " << J[j].cols() << std::endl;
+ if (j == i-1)
+ {
+ ASSERT_MATRIX_APPROX(J[j], Eigen::MatrixXd::Identity(i,i), wolf::Constants::EPS);
+ }
+ else
+ {
+ ASSERT_MATRIX_APPROX(J[j], Eigen::MatrixXd::Zero(i,j+1), wolf::Constants::EPS);
+ }
+ }
+
+ // 6 ------------------------------------------------------------------------
+ fac = std::make_shared<FactorDummyZero12<6>>(sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12);
+ i = fac->getSize();
+
+ // Jacobian
+ J.clear();
+ residuals.resize(i);
+ fac->evaluate(states_ptr, residuals, J);
+
+ std::cout << "i = " << i << std::endl;
+ for (unsigned int j = 0; j < 12; j++)
+ {
+ std::cout << "j = " << j << std::endl;
+ std::cout << "Jacobian size: " << J[j].rows() << " " << J[j].cols() << std::endl;
+ if (j == i-1)
+ {
+ ASSERT_MATRIX_APPROX(J[j], Eigen::MatrixXd::Identity(i,i), wolf::Constants::EPS);
+ }
+ else
+ {
+ ASSERT_MATRIX_APPROX(J[j], Eigen::MatrixXd::Zero(i,j+1), wolf::Constants::EPS);
+ }
+ }
+
+ // 7 ------------------------------------------------------------------------
+ fac = std::make_shared<FactorDummyZero12<7>>(sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12);
+ i = fac->getSize();
+
+ // Jacobian
+ J.clear();
+ residuals.resize(i);
+ fac->evaluate(states_ptr, residuals, J);
+
+ std::cout << "i = " << i << std::endl;
+ for (unsigned int j = 0; j < 12; j++)
+ {
+ std::cout << "j = " << j << std::endl;
+ std::cout << "Jacobian size: " << J[j].rows() << " " << J[j].cols() << std::endl;
+ if (j == i-1)
+ {
+ ASSERT_MATRIX_APPROX(J[j], Eigen::MatrixXd::Identity(i,i), wolf::Constants::EPS);
+ }
+ else
+ {
+ ASSERT_MATRIX_APPROX(J[j], Eigen::MatrixXd::Zero(i,j+1), wolf::Constants::EPS);
+ }
+ }
+
+ // 8 ------------------------------------------------------------------------
+ fac = std::make_shared<FactorDummyZero12<8>>(sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12);
+ i = fac->getSize();
+
+ // Jacobian
+ J.clear();
+ residuals.resize(i);
+ fac->evaluate(states_ptr, residuals, J);
+
+ std::cout << "i = " << i << std::endl;
+ for (unsigned int j = 0; j < 12; j++)
+ {
+ std::cout << "j = " << j << std::endl;
+ std::cout << "Jacobian size: " << J[j].rows() << " " << J[j].cols() << std::endl;
+ if (j == i-1)
+ {
+ ASSERT_MATRIX_APPROX(J[j], Eigen::MatrixXd::Identity(i,i), wolf::Constants::EPS);
+ }
+ else
+ {
+ ASSERT_MATRIX_APPROX(J[j], Eigen::MatrixXd::Zero(i,j+1), wolf::Constants::EPS);
+ }
+ }
+
+ // 9 ------------------------------------------------------------------------
+ fac = std::make_shared<FactorDummyZero12<9>>(sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12);
+ i = fac->getSize();
+
+ // Jacobian
+ J.clear();
+ residuals.resize(i);
+ fac->evaluate(states_ptr, residuals, J);
+
+ std::cout << "i = " << i << std::endl;
+ for (unsigned int j = 0; j < 12; j++)
+ {
+ std::cout << "j = " << j << std::endl;
+ std::cout << "Jacobian size: " << J[j].rows() << " " << J[j].cols() << std::endl;
+ if (j == i-1)
+ {
+ ASSERT_MATRIX_APPROX(J[j], Eigen::MatrixXd::Identity(i,i), wolf::Constants::EPS);
+ }
+ else
+ {
+ ASSERT_MATRIX_APPROX(J[j], Eigen::MatrixXd::Zero(i,j+1), wolf::Constants::EPS);
+ }
+ }
+
+ // 10 ------------------------------------------------------------------------
+ fac = std::make_shared<FactorDummyZero12<10>>(sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12);
+ i = fac->getSize();
+
+ // Jacobian
+ J.clear();
+ residuals.resize(i);
+ fac->evaluate(states_ptr, residuals, J);
+
+ std::cout << "i = " << i << std::endl;
+ for (unsigned int j = 0; j < 12; j++)
+ {
+ std::cout << "j = " << j << std::endl;
+ std::cout << "Jacobian size: " << J[j].rows() << " " << J[j].cols() << std::endl;
+ if (j == i-1)
+ {
+ ASSERT_MATRIX_APPROX(J[j], Eigen::MatrixXd::Identity(i,i), wolf::Constants::EPS);
+ }
+ else
+ {
+ ASSERT_MATRIX_APPROX(J[j], Eigen::MatrixXd::Zero(i,j+1), wolf::Constants::EPS);
+ }
+ }
+
+ // 11 ------------------------------------------------------------------------
+ fac = std::make_shared<FactorDummyZero12<11>>(sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12);
+ i = fac->getSize();
+
+ // Jacobian
+ J.clear();
+ residuals.resize(i);
+ fac->evaluate(states_ptr, residuals, J);
+
+ std::cout << "i = " << i << std::endl;
+ for (unsigned int j = 0; j < 12; j++)
+ {
+ std::cout << "j = " << j << std::endl;
+ std::cout << "Jacobian size: " << J[j].rows() << " " << J[j].cols() << std::endl;
+ if (j == i-1)
+ {
+ ASSERT_MATRIX_APPROX(J[j], Eigen::MatrixXd::Identity(i,i), wolf::Constants::EPS);
+ }
+ else
+ {
+ ASSERT_MATRIX_APPROX(J[j], Eigen::MatrixXd::Zero(i,j+1), wolf::Constants::EPS);
+ }
+ }
+
+ // 12 ------------------------------------------------------------------------
+ fac = std::make_shared<FactorDummyZero12<12>>(sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12);
+ i = fac->getSize();
+
+ // Jacobian
+ J.clear();
+ residuals.resize(i);
+ fac->evaluate(states_ptr, residuals, J);
+
+ std::cout << "i = " << i << std::endl;
+ for (unsigned int j = 0; j < 12; j++)
+ {
+ std::cout << "j = " << j << std::endl;
+ std::cout << "Jacobian size: " << J[j].rows() << " " << J[j].cols() << std::endl;
+ if (j == i-1)
+ {
+ ASSERT_MATRIX_APPROX(J[j], Eigen::MatrixXd::Identity(i,i), wolf::Constants::EPS);
+ }
+ else
+ {
+ ASSERT_MATRIX_APPROX(J[j], Eigen::MatrixXd::Zero(i,j+1), wolf::Constants::EPS);
+ }
+ }
+}
+
+TEST(FactorAutodiff, EmplaceOdom2d)
{
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)));
@@ -44,7 +393,7 @@ TEST(CaptureAutodiff, ConstructorOdom2d)
ASSERT_TRUE(factor_ptr->getFrameOther());
}
-TEST(CaptureAutodiff, ResidualOdom2d)
+TEST(FactorAutodiff, ResidualOdom2d)
{
Eigen::Vector3d f1_pose, f2_pose;
f1_pose << 2,1,M_PI;
@@ -88,7 +437,7 @@ TEST(CaptureAutodiff, ResidualOdom2d)
ASSERT_TRUE(residuals.minCoeff() > -1e-9);
}
-TEST(CaptureAutodiff, JacobianOdom2d)
+TEST(FactorAutodiff, JacobianOdom2d)
{
Eigen::Vector3d f1_pose, f2_pose;
f1_pose << 2,1,M_PI;
@@ -166,7 +515,7 @@ TEST(CaptureAutodiff, JacobianOdom2d)
// std::cout << "analytic J " << 3 << ":\n" << J3 << std::endl;
}
-TEST(CaptureAutodiff, AutodiffVsAnalytic)
+TEST(FactorAutodiff, AutodiffVsAnalytic)
{
Eigen::Vector3d f1_pose, f2_pose;
f1_pose << 2,1,M_PI;