diff --git a/include/core/factor/factor_autodiff.h b/include/core/factor/factor_autodiff.h
index 14e3660fa43bfb8c7892a4f225b5edf38592bb4d..03fc5fbb407c1f2c6914f871ede6834701af2bab 100644
--- a/include/core/factor/factor_autodiff.h
+++ b/include/core/factor/factor_autodiff.h
@@ -46,19 +46,19 @@ class FactorAutodiff : public FactorBase
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_;
- std::array<WolfJet, B11>* jets_11_;
+ mutable std::array<WolfJet, RES> residuals_jets_;
+ mutable std::array<WolfJet, B0> jets_0_;
+ mutable std::array<WolfJet, B1> jets_1_;
+ mutable std::array<WolfJet, B2> jets_2_;
+ mutable std::array<WolfJet, B3> jets_3_;
+ mutable std::array<WolfJet, B4> jets_4_;
+ mutable std::array<WolfJet, B5> jets_5_;
+ mutable std::array<WolfJet, B6> jets_6_;
+ mutable std::array<WolfJet, B7> jets_7_;
+ mutable std::array<WolfJet, B8> jets_8_;
+ mutable std::array<WolfJet, B9> jets_9_;
+ mutable std::array<WolfJet, B10> jets_10_;
+ mutable std::array<WolfJet, B11> jets_11_;
public:
/** \brief Constructor valid for all categories (ABSOLUTE, FRAME, FEATURE, LANDMARK)
@@ -84,20 +84,7 @@ class FactorAutodiff : public FactorBase
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,_state10Ptr,_state11Ptr}),
- 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>),
- jets_11_(new std::array<WolfJet, B11>)
+ state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr,_state5Ptr,_state6Ptr,_state7Ptr,_state8Ptr,_state9Ptr,_state10Ptr,_state11Ptr})
{
// asserts for null states
assert(_state0Ptr != nullptr && "nullptr state block");
@@ -116,46 +103,33 @@ class FactorAutodiff : public FactorBase
// initialize jets
unsigned int last_jet_idx = 0;
for (unsigned int i = 0; i < B0; i++)
- (*jets_0_)[i] = WolfJet(0, last_jet_idx++);
+ jets_0_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B1; i++)
- (*jets_1_)[i] = WolfJet(0, last_jet_idx++);
+ jets_1_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B2; i++)
- (*jets_2_)[i] = WolfJet(0, last_jet_idx++);
+ jets_2_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B3; i++)
- (*jets_3_)[i] = WolfJet(0, last_jet_idx++);
+ jets_3_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B4; i++)
- (*jets_4_)[i] = WolfJet(0, last_jet_idx++);
+ jets_4_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B5; i++)
- (*jets_5_)[i] = WolfJet(0, last_jet_idx++);
+ jets_5_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B6; i++)
- (*jets_6_)[i] = WolfJet(0, last_jet_idx++);
+ jets_6_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B7; i++)
- (*jets_7_)[i] = WolfJet(0, last_jet_idx++);
+ jets_7_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B8; i++)
- (*jets_8_)[i] = WolfJet(0, last_jet_idx++);
+ 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++);
+ jets_10_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B11; i++)
- (*jets_11_)[i] = WolfJet(0, last_jet_idx++);
+ jets_11_[i] = WolfJet(0, last_jet_idx++);
}
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 jets_11_;
- delete residuals_jets_;
}
virtual JacobianMethod getJacobianMethod() const
@@ -196,30 +170,30 @@ class FactorAutodiff : public FactorBase
updateJetsRealPart(param_vec);
// call functor
- (*static_cast<FacT const*>(this))(jets_0_->data(),
- jets_1_->data(),
- jets_2_->data(),
- jets_3_->data(),
- jets_4_->data(),
- jets_5_->data(),
- jets_6_->data(),
- jets_7_->data(),
- jets_8_->data(),
- jets_9_->data(),
- jets_10_->data(),
- jets_11_->data(),
- residuals_jets_->data());
+ (*static_cast<FacT const*>(this))(jets_0_.data(),
+ jets_1_.data(),
+ jets_2_.data(),
+ jets_3_.data(),
+ jets_4_.data(),
+ jets_5_.data(),
+ jets_6_.data(),
+ jets_7_.data(),
+ jets_8_.data(),
+ jets_9_.data(),
+ jets_10_.data(),
+ jets_11_.data(),
+ residuals_jets_.data());
// fill the residual array
for (unsigned int i = 0; i < RES; i++)
- residuals[i] = (*residuals_jets_)[i].a;
+ 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),
+ 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;
@@ -232,29 +206,29 @@ class FactorAutodiff : public FactorBase
{
// update jets real part
for (unsigned int i = 0; i < B0; i++)
- (*jets_0_)[i].a = parameters[0][i];
+ jets_0_[i].a = parameters[0][i];
for (unsigned int i = 0; i < B1; i++)
- (*jets_1_)[i].a = parameters[1][i];
+ jets_1_[i].a = parameters[1][i];
for (unsigned int i = 0; i < B2; i++)
- (*jets_2_)[i].a = parameters[2][i];
+ jets_2_[i].a = parameters[2][i];
for (unsigned int i = 0; i < B3; i++)
- (*jets_3_)[i].a = parameters[3][i];
+ jets_3_[i].a = parameters[3][i];
for (unsigned int i = 0; i < B4; i++)
- (*jets_4_)[i].a = parameters[4][i];
+ jets_4_[i].a = parameters[4][i];
for (unsigned int i = 0; i < B5; i++)
- (*jets_5_)[i].a = parameters[5][i];
+ jets_5_[i].a = parameters[5][i];
for (unsigned int i = 0; i < B6; i++)
- (*jets_6_)[i].a = parameters[6][i];
+ jets_6_[i].a = parameters[6][i];
for (unsigned int i = 0; i < B7; i++)
- (*jets_7_)[i].a = parameters[7][i];
+ jets_7_[i].a = parameters[7][i];
for (unsigned int i = 0; i < B8; i++)
- (*jets_8_)[i].a = parameters[8][i];
+ jets_8_[i].a = parameters[8][i];
for (unsigned int i = 0; i < B9; i++)
- (*jets_9_)[i].a = parameters[9][i];
+ jets_9_[i].a = parameters[9][i];
for (unsigned int i = 0; i < B10; i++)
- (*jets_10_)[i].a = parameters[10][i];
+ jets_10_[i].a = parameters[10][i];
for (unsigned int i = 0; i < B11; i++)
- (*jets_11_)[i].a = parameters[11][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
@@ -271,23 +245,23 @@ class FactorAutodiff : public FactorBase
updateJetsRealPart(_states_ptr);
// call functor
- (*static_cast<FacT const*>(this))(jets_0_->data(),
- jets_1_->data(),
- jets_2_->data(),
- jets_3_->data(),
- jets_4_->data(),
- jets_5_->data(),
- jets_6_->data(),
- jets_7_->data(),
- jets_8_->data(),
- jets_9_->data(),
- jets_10_->data(),
- jets_11_->data(),
- residuals_jets_->data());
+ (*static_cast<FacT const*>(this))(jets_0_.data(),
+ jets_1_.data(),
+ jets_2_.data(),
+ jets_3_.data(),
+ jets_4_.data(),
+ jets_5_.data(),
+ jets_6_.data(),
+ jets_7_.data(),
+ jets_8_.data(),
+ jets_9_.data(),
+ jets_10_.data(),
+ jets_11_.data(),
+ residuals_jets_.data());
// fill the residual vector
for (unsigned int i = 0; i < RES; i++)
- residual_(i) = (*residuals_jets_)[i].a;
+ residual_(i) = residuals_jets_[i].a;
// fill the jacobian matrices
for (unsigned int i = 0; i<n_blocks; i++)
@@ -297,8 +271,8 @@ class FactorAutodiff : public FactorBase
// 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),
+ 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);
@@ -371,19 +345,18 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,B9,B10,0> : public Fact
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_;
+ mutable std::array<WolfJet, RES> residuals_jets_;
+ mutable std::array<WolfJet, B0> jets_0_;
+ mutable std::array<WolfJet, B1> jets_1_;
+ mutable std::array<WolfJet, B2> jets_2_;
+ mutable std::array<WolfJet, B3> jets_3_;
+ mutable std::array<WolfJet, B4> jets_4_;
+ mutable std::array<WolfJet, B5> jets_5_;
+ mutable std::array<WolfJet, B6> jets_6_;
+ mutable std::array<WolfJet, B7> jets_7_;
+ mutable std::array<WolfJet, B8> jets_8_;
+ mutable std::array<WolfJet, B9> jets_9_;
+ mutable std::array<WolfJet, B10> jets_10_;
public:
@@ -407,19 +380,7 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,B9,B10,0> : public Fact
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>)
+ state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr,_state5Ptr,_state6Ptr,_state7Ptr,_state8Ptr,_state9Ptr,_state10Ptr})
{
// asserts for null states
assert(_state0Ptr != nullptr && "nullptr state block");
@@ -437,44 +398,32 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,B9,B10,0> : public Fact
// initialize jets
unsigned int last_jet_idx = 0;
for (unsigned int i = 0; i < B0; i++)
- (*jets_0_)[i] = WolfJet(0, last_jet_idx++);
+ jets_0_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B1; i++)
- (*jets_1_)[i] = WolfJet(0, last_jet_idx++);
+ jets_1_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B2; i++)
- (*jets_2_)[i] = WolfJet(0, last_jet_idx++);
+ jets_2_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B3; i++)
- (*jets_3_)[i] = WolfJet(0, last_jet_idx++);
+ jets_3_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B4; i++)
- (*jets_4_)[i] = WolfJet(0, last_jet_idx++);
+ jets_4_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B5; i++)
- (*jets_5_)[i] = WolfJet(0, last_jet_idx++);
+ jets_5_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B6; i++)
- (*jets_6_)[i] = WolfJet(0, last_jet_idx++);
+ jets_6_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B7; i++)
- (*jets_7_)[i] = WolfJet(0, last_jet_idx++);
+ jets_7_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B8; i++)
- (*jets_8_)[i] = WolfJet(0, last_jet_idx++);
+ 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++);
+ 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
@@ -509,29 +458,29 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,B9,B10,0> : public Fact
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());
+ (*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;
+ 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),
+ 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;
@@ -541,27 +490,27 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,B9,B10,0> : public Fact
{
// update jets real part
for (unsigned int i = 0; i < B0; i++)
- (*jets_0_)[i].a = parameters[0][i];
+ jets_0_[i].a = parameters[0][i];
for (unsigned int i = 0; i < B1; i++)
- (*jets_1_)[i].a = parameters[1][i];
+ jets_1_[i].a = parameters[1][i];
for (unsigned int i = 0; i < B2; i++)
- (*jets_2_)[i].a = parameters[2][i];
+ jets_2_[i].a = parameters[2][i];
for (unsigned int i = 0; i < B3; i++)
- (*jets_3_)[i].a = parameters[3][i];
+ jets_3_[i].a = parameters[3][i];
for (unsigned int i = 0; i < B4; i++)
- (*jets_4_)[i].a = parameters[4][i];
+ jets_4_[i].a = parameters[4][i];
for (unsigned int i = 0; i < B5; i++)
- (*jets_5_)[i].a = parameters[5][i];
+ jets_5_[i].a = parameters[5][i];
for (unsigned int i = 0; i < B6; i++)
- (*jets_6_)[i].a = parameters[6][i];
+ jets_6_[i].a = parameters[6][i];
for (unsigned int i = 0; i < B7; i++)
- (*jets_7_)[i].a = parameters[7][i];
+ jets_7_[i].a = parameters[7][i];
for (unsigned int i = 0; i < B8; i++)
- (*jets_8_)[i].a = parameters[8][i];
+ jets_8_[i].a = parameters[8][i];
for (unsigned int i = 0; i < B9; i++)
- (*jets_9_)[i].a = parameters[9][i];
+ jets_9_[i].a = parameters[9][i];
for (unsigned int i = 0; i < B10; i++)
- (*jets_10_)[i].a = parameters[10][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
@@ -575,22 +524,22 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,B9,B10,0> : public Fact
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());
+ (*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;
+ residual_(i) = residuals_jets_[i].a;
// fill the jacobian matrices
for (unsigned int i = 0; i<n_blocks; i++)
@@ -600,8 +549,8 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,B9,B10,0> : public Fact
// 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),
+ 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);
@@ -660,18 +609,17 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,B9,0,0> : public Factor
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_;
+ mutable std::array<WolfJet, RES> residuals_jets_;
+ mutable std::array<WolfJet, B0> jets_0_;
+ mutable std::array<WolfJet, B1> jets_1_;
+ mutable std::array<WolfJet, B2> jets_2_;
+ mutable std::array<WolfJet, B3> jets_3_;
+ mutable std::array<WolfJet, B4> jets_4_;
+ mutable std::array<WolfJet, B5> jets_5_;
+ mutable std::array<WolfJet, B6> jets_6_;
+ mutable std::array<WolfJet, B7> jets_7_;
+ mutable std::array<WolfJet, B8> jets_8_;
+ mutable std::array<WolfJet, B9> jets_9_;
public:
@@ -694,18 +642,7 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,B9,0,0> : public Factor
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>)
+ state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr,_state5Ptr,_state6Ptr,_state7Ptr,_state8Ptr,_state9Ptr})
{
// asserts for null states
assert(_state0Ptr != nullptr && "nullptr state block");
@@ -722,41 +659,30 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,B9,0,0> : public Factor
// initialize jets
unsigned int last_jet_idx = 0;
for (unsigned int i = 0; i < B0; i++)
- (*jets_0_)[i] = WolfJet(0, last_jet_idx++);
+ jets_0_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B1; i++)
- (*jets_1_)[i] = WolfJet(0, last_jet_idx++);
+ jets_1_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B2; i++)
- (*jets_2_)[i] = WolfJet(0, last_jet_idx++);
+ jets_2_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B3; i++)
- (*jets_3_)[i] = WolfJet(0, last_jet_idx++);
+ jets_3_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B4; i++)
- (*jets_4_)[i] = WolfJet(0, last_jet_idx++);
+ jets_4_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B5; i++)
- (*jets_5_)[i] = WolfJet(0, last_jet_idx++);
+ jets_5_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B6; i++)
- (*jets_6_)[i] = WolfJet(0, last_jet_idx++);
+ jets_6_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B7; i++)
- (*jets_7_)[i] = WolfJet(0, last_jet_idx++);
+ jets_7_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B8; i++)
- (*jets_8_)[i] = WolfJet(0, last_jet_idx++);
+ 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++);
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
@@ -790,28 +716,28 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,B9,0,0> : public Factor
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());
+ (*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;
+ 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),
+ 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;
@@ -821,25 +747,25 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,B9,0,0> : public Factor
{
// update jets real part
for (unsigned int i = 0; i < B0; i++)
- (*jets_0_)[i].a = parameters[0][i];
+ jets_0_[i].a = parameters[0][i];
for (unsigned int i = 0; i < B1; i++)
- (*jets_1_)[i].a = parameters[1][i];
+ jets_1_[i].a = parameters[1][i];
for (unsigned int i = 0; i < B2; i++)
- (*jets_2_)[i].a = parameters[2][i];
+ jets_2_[i].a = parameters[2][i];
for (unsigned int i = 0; i < B3; i++)
- (*jets_3_)[i].a = parameters[3][i];
+ jets_3_[i].a = parameters[3][i];
for (unsigned int i = 0; i < B4; i++)
- (*jets_4_)[i].a = parameters[4][i];
+ jets_4_[i].a = parameters[4][i];
for (unsigned int i = 0; i < B5; i++)
- (*jets_5_)[i].a = parameters[5][i];
+ jets_5_[i].a = parameters[5][i];
for (unsigned int i = 0; i < B6; i++)
- (*jets_6_)[i].a = parameters[6][i];
+ jets_6_[i].a = parameters[6][i];
for (unsigned int i = 0; i < B7; i++)
- (*jets_7_)[i].a = parameters[7][i];
+ jets_7_[i].a = parameters[7][i];
for (unsigned int i = 0; i < B8; i++)
- (*jets_8_)[i].a = parameters[8][i];
+ jets_8_[i].a = parameters[8][i];
for (unsigned int i = 0; i < B9; i++)
- (*jets_9_)[i].a = parameters[9][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
@@ -853,21 +779,21 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,B9,0,0> : public Factor
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());
+ (*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;
+ residual_(i) = residuals_jets_[i].a;
// fill the jacobian matrices
for (unsigned int i = 0; i<n_blocks; i++)
@@ -877,8 +803,8 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,B9,0,0> : public Factor
// 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),
+ 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);
@@ -937,17 +863,16 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,0,0,0> : public FactorB
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_;
+ mutable std::array<WolfJet, RES> residuals_jets_;
+ mutable std::array<WolfJet, B0> jets_0_;
+ mutable std::array<WolfJet, B1> jets_1_;
+ mutable std::array<WolfJet, B2> jets_2_;
+ mutable std::array<WolfJet, B3> jets_3_;
+ mutable std::array<WolfJet, B4> jets_4_;
+ mutable std::array<WolfJet, B5> jets_5_;
+ mutable std::array<WolfJet, B6> jets_6_;
+ mutable std::array<WolfJet, B7> jets_7_;
+ mutable std::array<WolfJet, B8> jets_8_;
public:
@@ -969,17 +894,7 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,0,0,0> : public FactorB
StateBlockPtr _state7Ptr,
StateBlockPtr _state8Ptr) :
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}),
- 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>)
+ state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr,_state5Ptr,_state6Ptr,_state7Ptr,_state8Ptr})
{
// asserts for null states
assert(_state0Ptr != nullptr && "nullptr state block");
@@ -995,38 +910,28 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,0,0,0> : public FactorB
// initialize jets
unsigned int last_jet_idx = 0;
for (unsigned int i = 0; i < B0; i++)
- (*jets_0_)[i] = WolfJet(0, last_jet_idx++);
+ jets_0_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B1; i++)
- (*jets_1_)[i] = WolfJet(0, last_jet_idx++);
+ jets_1_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B2; i++)
- (*jets_2_)[i] = WolfJet(0, last_jet_idx++);
+ jets_2_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B3; i++)
- (*jets_3_)[i] = WolfJet(0, last_jet_idx++);
+ jets_3_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B4; i++)
- (*jets_4_)[i] = WolfJet(0, last_jet_idx++);
+ jets_4_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B5; i++)
- (*jets_5_)[i] = WolfJet(0, last_jet_idx++);
+ jets_5_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B6; i++)
- (*jets_6_)[i] = WolfJet(0, last_jet_idx++);
+ jets_6_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B7; i++)
- (*jets_7_)[i] = WolfJet(0, last_jet_idx++);
+ jets_7_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B8; i++)
- (*jets_8_)[i] = WolfJet(0, last_jet_idx++);
+ jets_8_[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 residuals_jets_;
}
virtual JacobianMethod getJacobianMethod() const
@@ -1059,27 +964,27 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,0,0,0> : public FactorB
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(),
- residuals_jets_->data());
+ (*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(),
+ residuals_jets_.data());
// fill the residual array
for (unsigned int i = 0; i < RES; i++)
- residuals[i] = (*residuals_jets_)[i].a;
+ 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),
+ 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;
@@ -1089,23 +994,23 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,0,0,0> : public FactorB
{
// update jets real part
for (unsigned int i = 0; i < B0; i++)
- (*jets_0_)[i].a = parameters[0][i];
+ jets_0_[i].a = parameters[0][i];
for (unsigned int i = 0; i < B1; i++)
- (*jets_1_)[i].a = parameters[1][i];
+ jets_1_[i].a = parameters[1][i];
for (unsigned int i = 0; i < B2; i++)
- (*jets_2_)[i].a = parameters[2][i];
+ jets_2_[i].a = parameters[2][i];
for (unsigned int i = 0; i < B3; i++)
- (*jets_3_)[i].a = parameters[3][i];
+ jets_3_[i].a = parameters[3][i];
for (unsigned int i = 0; i < B4; i++)
- (*jets_4_)[i].a = parameters[4][i];
+ jets_4_[i].a = parameters[4][i];
for (unsigned int i = 0; i < B5; i++)
- (*jets_5_)[i].a = parameters[5][i];
+ jets_5_[i].a = parameters[5][i];
for (unsigned int i = 0; i < B6; i++)
- (*jets_6_)[i].a = parameters[6][i];
+ jets_6_[i].a = parameters[6][i];
for (unsigned int i = 0; i < B7; i++)
- (*jets_7_)[i].a = parameters[7][i];
+ jets_7_[i].a = parameters[7][i];
for (unsigned int i = 0; i < B8; i++)
- (*jets_8_)[i].a = parameters[8][i];
+ jets_8_[i].a = parameters[8][i];
}
virtual void evaluate(const std::vector<const double*>& _states_ptr, Eigen::VectorXd& residual_, std::vector<Eigen::MatrixXd>& jacobians_) const
@@ -1119,20 +1024,20 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,0,0,0> : public FactorB
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(),
- residuals_jets_->data());
+ (*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(),
+ residuals_jets_.data());
// fill the residual vector
for (unsigned int i = 0; i < RES; i++)
- residual_(i) = (*residuals_jets_)[i].a;
+ residual_(i) = residuals_jets_[i].a;
// fill the jacobian matrices
for (unsigned int i = 0; i<n_blocks; i++)
@@ -1142,8 +1047,8 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,0,0,0> : public FactorB
// 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),
+ 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);
@@ -1202,16 +1107,15 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,0,0,0,0> : public FactorBa
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_;
+ mutable std::array<WolfJet, RES> residuals_jets_;
+ mutable std::array<WolfJet, B0> jets_0_;
+ mutable std::array<WolfJet, B1> jets_1_;
+ mutable std::array<WolfJet, B2> jets_2_;
+ mutable std::array<WolfJet, B3> jets_3_;
+ mutable std::array<WolfJet, B4> jets_4_;
+ mutable std::array<WolfJet, B5> jets_5_;
+ mutable std::array<WolfJet, B6> jets_6_;
+ mutable std::array<WolfJet, B7> jets_7_;
public:
@@ -1232,16 +1136,7 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,0,0,0,0> : public FactorBa
StateBlockPtr _state6Ptr,
StateBlockPtr _state7Ptr) :
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}),
- 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>)
+ state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr,_state5Ptr,_state6Ptr,_state7Ptr})
{
// asserts for null states
assert(_state0Ptr != nullptr && "nullptr state block");
@@ -1256,35 +1151,26 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,0,0,0,0> : public FactorBa
// initialize jets
unsigned int last_jet_idx = 0;
for (unsigned int i = 0; i < B0; i++)
- (*jets_0_)[i] = WolfJet(0, last_jet_idx++);
+ jets_0_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B1; i++)
- (*jets_1_)[i] = WolfJet(0, last_jet_idx++);
+ jets_1_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B2; i++)
- (*jets_2_)[i] = WolfJet(0, last_jet_idx++);
+ jets_2_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B3; i++)
- (*jets_3_)[i] = WolfJet(0, last_jet_idx++);
+ jets_3_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B4; i++)
- (*jets_4_)[i] = WolfJet(0, last_jet_idx++);
+ jets_4_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B5; i++)
- (*jets_5_)[i] = WolfJet(0, last_jet_idx++);
+ jets_5_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B6; i++)
- (*jets_6_)[i] = WolfJet(0, last_jet_idx++);
+ jets_6_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B7; i++)
- (*jets_7_)[i] = WolfJet(0, last_jet_idx++);
+ jets_7_[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 residuals_jets_;
}
virtual JacobianMethod getJacobianMethod() const
@@ -1316,26 +1202,26 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,0,0,0,0> : public FactorBa
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(),
- residuals_jets_->data());
+ (*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(),
+ residuals_jets_.data());
// fill the residual array
for (unsigned int i = 0; i < RES; i++)
- residuals[i] = (*residuals_jets_)[i].a;
+ 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),
+ 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;
@@ -1345,21 +1231,21 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,0,0,0,0> : public FactorBa
{
// update jets real part
for (unsigned int i = 0; i < B0; i++)
- (*jets_0_)[i].a = parameters[0][i];
+ jets_0_[i].a = parameters[0][i];
for (unsigned int i = 0; i < B1; i++)
- (*jets_1_)[i].a = parameters[1][i];
+ jets_1_[i].a = parameters[1][i];
for (unsigned int i = 0; i < B2; i++)
- (*jets_2_)[i].a = parameters[2][i];
+ jets_2_[i].a = parameters[2][i];
for (unsigned int i = 0; i < B3; i++)
- (*jets_3_)[i].a = parameters[3][i];
+ jets_3_[i].a = parameters[3][i];
for (unsigned int i = 0; i < B4; i++)
- (*jets_4_)[i].a = parameters[4][i];
+ jets_4_[i].a = parameters[4][i];
for (unsigned int i = 0; i < B5; i++)
- (*jets_5_)[i].a = parameters[5][i];
+ jets_5_[i].a = parameters[5][i];
for (unsigned int i = 0; i < B6; i++)
- (*jets_6_)[i].a = parameters[6][i];
+ jets_6_[i].a = parameters[6][i];
for (unsigned int i = 0; i < B7; i++)
- (*jets_7_)[i].a = parameters[7][i];
+ jets_7_[i].a = parameters[7][i];
}
virtual void evaluate(const std::vector<const double*>& _states_ptr, Eigen::VectorXd& residual_, std::vector<Eigen::MatrixXd>& jacobians_) const
@@ -1373,19 +1259,19 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,0,0,0,0> : public FactorBa
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(),
- residuals_jets_->data());
+ (*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(),
+ residuals_jets_.data());
// fill the residual vector
for (unsigned int i = 0; i < RES; i++)
- residual_(i) = (*residuals_jets_)[i].a;
+ residual_(i) = residuals_jets_[i].a;
// fill the jacobian matrices
for (unsigned int i = 0; i<n_blocks; i++)
@@ -1395,8 +1281,8 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,0,0,0,0> : public FactorBa
// 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),
+ 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);
@@ -1455,15 +1341,14 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,0,0,0,0,0> : public FactorBas
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_;
+ mutable std::array<WolfJet, RES> residuals_jets_;
+ mutable std::array<WolfJet, B0> jets_0_;
+ mutable std::array<WolfJet, B1> jets_1_;
+ mutable std::array<WolfJet, B2> jets_2_;
+ mutable std::array<WolfJet, B3> jets_3_;
+ mutable std::array<WolfJet, B4> jets_4_;
+ mutable std::array<WolfJet, B5> jets_5_;
+ mutable std::array<WolfJet, B6> jets_6_;
public:
@@ -1483,15 +1368,7 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,0,0,0,0,0> : public FactorBas
StateBlockPtr _state5Ptr,
StateBlockPtr _state6Ptr) :
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}),
- 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>)
+ state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr,_state5Ptr,_state6Ptr})
{
// asserts for null states
assert(_state0Ptr != nullptr && "nullptr state block");
@@ -1505,32 +1382,24 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,0,0,0,0,0> : public FactorBas
// initialize jets
unsigned int last_jet_idx = 0;
for (unsigned int i = 0; i < B0; i++)
- (*jets_0_)[i] = WolfJet(0, last_jet_idx++);
+ jets_0_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B1; i++)
- (*jets_1_)[i] = WolfJet(0, last_jet_idx++);
+ jets_1_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B2; i++)
- (*jets_2_)[i] = WolfJet(0, last_jet_idx++);
+ jets_2_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B3; i++)
- (*jets_3_)[i] = WolfJet(0, last_jet_idx++);
+ jets_3_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B4; i++)
- (*jets_4_)[i] = WolfJet(0, last_jet_idx++);
+ jets_4_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B5; i++)
- (*jets_5_)[i] = WolfJet(0, last_jet_idx++);
+ jets_5_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B6; i++)
- (*jets_6_)[i] = WolfJet(0, last_jet_idx++);
+ jets_6_[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 residuals_jets_;
}
virtual JacobianMethod getJacobianMethod() const
@@ -1561,25 +1430,25 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,0,0,0,0,0> : public FactorBas
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(),
- residuals_jets_->data());
+ (*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(),
+ residuals_jets_.data());
// fill the residual array
for (unsigned int i = 0; i < RES; i++)
- residuals[i] = (*residuals_jets_)[i].a;
+ 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),
+ 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;
@@ -1589,19 +1458,19 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,0,0,0,0,0> : public FactorBas
{
// update jets real part
for (unsigned int i = 0; i < B0; i++)
- (*jets_0_)[i].a = parameters[0][i];
+ jets_0_[i].a = parameters[0][i];
for (unsigned int i = 0; i < B1; i++)
- (*jets_1_)[i].a = parameters[1][i];
+ jets_1_[i].a = parameters[1][i];
for (unsigned int i = 0; i < B2; i++)
- (*jets_2_)[i].a = parameters[2][i];
+ jets_2_[i].a = parameters[2][i];
for (unsigned int i = 0; i < B3; i++)
- (*jets_3_)[i].a = parameters[3][i];
+ jets_3_[i].a = parameters[3][i];
for (unsigned int i = 0; i < B4; i++)
- (*jets_4_)[i].a = parameters[4][i];
+ jets_4_[i].a = parameters[4][i];
for (unsigned int i = 0; i < B5; i++)
- (*jets_5_)[i].a = parameters[5][i];
+ jets_5_[i].a = parameters[5][i];
for (unsigned int i = 0; i < B6; i++)
- (*jets_6_)[i].a = parameters[6][i];
+ jets_6_[i].a = parameters[6][i];
}
virtual void evaluate(const std::vector<const double*>& _states_ptr, Eigen::VectorXd& residual_, std::vector<Eigen::MatrixXd>& jacobians_) const
@@ -1615,18 +1484,18 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,0,0,0,0,0> : public FactorBas
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(),
- residuals_jets_->data());
+ (*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(),
+ residuals_jets_.data());
// fill the residual vector
for (unsigned int i = 0; i < RES; i++)
- residual_(i) = (*residuals_jets_)[i].a;
+ residual_(i) = residuals_jets_[i].a;
// fill the jacobian matrices
for (unsigned int i = 0; i<n_blocks; i++)
@@ -1636,8 +1505,8 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,0,0,0,0,0> : public FactorBas
// 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),
+ 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);
@@ -1696,14 +1565,13 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,0,0,0,0,0,0> : public FactorBase
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_;
+ mutable std::array<WolfJet, RES> residuals_jets_;
+ mutable std::array<WolfJet, B0> jets_0_;
+ mutable std::array<WolfJet, B1> jets_1_;
+ mutable std::array<WolfJet, B2> jets_2_;
+ mutable std::array<WolfJet, B3> jets_3_;
+ mutable std::array<WolfJet, B4> jets_4_;
+ mutable std::array<WolfJet, B5> jets_5_;
public:
@@ -1722,14 +1590,7 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,0,0,0,0,0,0> : public FactorBase
StateBlockPtr _state4Ptr,
StateBlockPtr _state5Ptr) :
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}),
- 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>)
+ state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr,_state5Ptr})
{
// asserts for null states
assert(_state0Ptr != nullptr && "nullptr state block");
@@ -1742,29 +1603,22 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,0,0,0,0,0,0> : public FactorBase
// initialize jets
unsigned int last_jet_idx = 0;
for (unsigned int i = 0; i < B0; i++)
- (*jets_0_)[i] = WolfJet(0, last_jet_idx++);
+ jets_0_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B1; i++)
- (*jets_1_)[i] = WolfJet(0, last_jet_idx++);
+ jets_1_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B2; i++)
- (*jets_2_)[i] = WolfJet(0, last_jet_idx++);
+ jets_2_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B3; i++)
- (*jets_3_)[i] = WolfJet(0, last_jet_idx++);
+ jets_3_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B4; i++)
- (*jets_4_)[i] = WolfJet(0, last_jet_idx++);
+ jets_4_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B5; i++)
- (*jets_5_)[i] = WolfJet(0, last_jet_idx++);
+ jets_5_[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 residuals_jets_;
}
virtual JacobianMethod getJacobianMethod() const
@@ -1794,24 +1648,24 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,0,0,0,0,0,0> : public FactorBase
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(),
- residuals_jets_->data());
+ (*static_cast<FacT 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 (unsigned int i = 0; i < RES; i++)
- residuals[i] = (*residuals_jets_)[i].a;
+ 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),
+ 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;
@@ -1821,17 +1675,17 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,0,0,0,0,0,0> : public FactorBase
{
// update jets real part
for (unsigned int i = 0; i < B0; i++)
- (*jets_0_)[i].a = parameters[0][i];
+ jets_0_[i].a = parameters[0][i];
for (unsigned int i = 0; i < B1; i++)
- (*jets_1_)[i].a = parameters[1][i];
+ jets_1_[i].a = parameters[1][i];
for (unsigned int i = 0; i < B2; i++)
- (*jets_2_)[i].a = parameters[2][i];
+ jets_2_[i].a = parameters[2][i];
for (unsigned int i = 0; i < B3; i++)
- (*jets_3_)[i].a = parameters[3][i];
+ jets_3_[i].a = parameters[3][i];
for (unsigned int i = 0; i < B4; i++)
- (*jets_4_)[i].a = parameters[4][i];
+ jets_4_[i].a = parameters[4][i];
for (unsigned int i = 0; i < B5; i++)
- (*jets_5_)[i].a = parameters[5][i];
+ jets_5_[i].a = parameters[5][i];
}
virtual void evaluate(const std::vector<const double*>& _states_ptr, Eigen::VectorXd& residual_, std::vector<Eigen::MatrixXd>& jacobians_) const
@@ -1845,17 +1699,17 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,0,0,0,0,0,0> : public FactorBase
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(),
- residuals_jets_->data());
+ (*static_cast<FacT 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 (unsigned int i = 0; i < RES; i++)
- residual_(i) = (*residuals_jets_)[i].a;
+ residual_(i) = residuals_jets_[i].a;
// fill the jacobian matrices
for (unsigned int i = 0; i<n_blocks; i++)
@@ -1865,8 +1719,8 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,0,0,0,0,0,0> : public FactorBase
// 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),
+ 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);
@@ -1920,13 +1774,12 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,0,0,0,0,0,0,0> : public FactorBase
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_;
+ mutable std::array<WolfJet, RES> residuals_jets_;
+ mutable std::array<WolfJet, B0> jets_0_;
+ mutable std::array<WolfJet, B1> jets_1_;
+ mutable std::array<WolfJet, B2> jets_2_;
+ mutable std::array<WolfJet, B3> jets_3_;
+ mutable std::array<WolfJet, B4> jets_4_;
public:
@@ -1944,13 +1797,7 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,0,0,0,0,0,0,0> : public FactorBase
StateBlockPtr _state3Ptr,
StateBlockPtr _state4Ptr) :
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}),
- 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>)
+ state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr})
{
// asserts for null states
assert(_state0Ptr != nullptr && "nullptr state block");
@@ -1962,26 +1809,20 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,0,0,0,0,0,0,0> : public FactorBase
// initialize jets
unsigned int last_jet_idx = 0;
for (unsigned int i = 0; i < B0; i++)
- (*jets_0_)[i] = WolfJet(0, last_jet_idx++);
+ jets_0_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B1; i++)
- (*jets_1_)[i] = WolfJet(0, last_jet_idx++);
+ jets_1_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B2; i++)
- (*jets_2_)[i] = WolfJet(0, last_jet_idx++);
+ jets_2_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B3; i++)
- (*jets_3_)[i] = WolfJet(0, last_jet_idx++);
+ jets_3_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B4; i++)
- (*jets_4_)[i] = WolfJet(0, last_jet_idx++);
+ jets_4_[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 residuals_jets_;
}
virtual JacobianMethod getJacobianMethod() const
@@ -2010,23 +1851,23 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,0,0,0,0,0,0,0> : public FactorBase
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(),
- residuals_jets_->data());
+ (*static_cast<FacT 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 (unsigned int i = 0; i < RES; i++)
- residuals[i] = (*residuals_jets_)[i].a;
+ 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),
+ 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;
@@ -2036,15 +1877,15 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,0,0,0,0,0,0,0> : public FactorBase
{
// update jets real part
for (unsigned int i = 0; i < B0; i++)
- (*jets_0_)[i].a = parameters[0][i];
+ jets_0_[i].a = parameters[0][i];
for (unsigned int i = 0; i < B1; i++)
- (*jets_1_)[i].a = parameters[1][i];
+ jets_1_[i].a = parameters[1][i];
for (unsigned int i = 0; i < B2; i++)
- (*jets_2_)[i].a = parameters[2][i];
+ jets_2_[i].a = parameters[2][i];
for (unsigned int i = 0; i < B3; i++)
- (*jets_3_)[i].a = parameters[3][i];
+ jets_3_[i].a = parameters[3][i];
for (unsigned int i = 0; i < B4; i++)
- (*jets_4_)[i].a = parameters[4][i];
+ jets_4_[i].a = parameters[4][i];
}
virtual void evaluate(const std::vector<const double*>& _states_ptr, Eigen::VectorXd& residual_, std::vector<Eigen::MatrixXd>& jacobians_) const
@@ -2058,16 +1899,16 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,0,0,0,0,0,0,0> : public FactorBase
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(),
- residuals_jets_->data());
+ (*static_cast<FacT 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 (unsigned int i = 0; i < RES; i++)
- residual_(i) = (*residuals_jets_)[i].a;
+ residual_(i) = residuals_jets_[i].a;
// fill the jacobian matrices
for (unsigned int i = 0; i<n_blocks; i++)
@@ -2077,8 +1918,8 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,0,0,0,0,0,0,0> : public FactorBase
// 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),
+ 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);
@@ -2132,12 +1973,11 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,0,0,0,0,0,0,0,0> : public FactorBase
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_;
+ mutable std::array<WolfJet, RES> residuals_jets_;
+ mutable std::array<WolfJet, B0> jets_0_;
+ mutable std::array<WolfJet, B1> jets_1_;
+ mutable std::array<WolfJet, B2> jets_2_;
+ mutable std::array<WolfJet, B3> jets_3_;
public:
@@ -2154,12 +1994,7 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,0,0,0,0,0,0,0,0> : public FactorBase
StateBlockPtr _state2Ptr,
StateBlockPtr _state3Ptr) :
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}),
- 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>)
+ state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr})
{
// asserts for null states
assert(_state0Ptr != nullptr && "nullptr state block");
@@ -2170,23 +2005,18 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,0,0,0,0,0,0,0,0> : public FactorBase
// initialize jets
unsigned int last_jet_idx = 0;
for (unsigned int i = 0; i < B0; i++)
- (*jets_0_)[i] = WolfJet(0, last_jet_idx++);
+ jets_0_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B1; i++)
- (*jets_1_)[i] = WolfJet(0, last_jet_idx++);
+ jets_1_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B2; i++)
- (*jets_2_)[i] = WolfJet(0, last_jet_idx++);
+ jets_2_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B3; i++)
- (*jets_3_)[i] = WolfJet(0, last_jet_idx++);
+ jets_3_[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 residuals_jets_;
}
virtual JacobianMethod getJacobianMethod() const
@@ -2214,22 +2044,22 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,0,0,0,0,0,0,0,0> : public FactorBase
updateJetsRealPart(param_vec);
// call functor
- (*static_cast<FacT const*>(this))(jets_0_->data(),
- jets_1_->data(),
- jets_2_->data(),
- jets_3_->data(),
- residuals_jets_->data());
+ (*static_cast<FacT const*>(this))(jets_0_.data(),
+ jets_1_.data(),
+ jets_2_.data(),
+ jets_3_.data(),
+ residuals_jets_.data());
// fill the residual array
for (unsigned int i = 0; i < RES; i++)
- residuals[i] = (*residuals_jets_)[i].a;
+ 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),
+ 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;
@@ -2239,13 +2069,13 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,0,0,0,0,0,0,0,0> : public FactorBase
{
// update jets real part
for (unsigned int i = 0; i < B0; i++)
- (*jets_0_)[i].a = parameters[0][i];
+ jets_0_[i].a = parameters[0][i];
for (unsigned int i = 0; i < B1; i++)
- (*jets_1_)[i].a = parameters[1][i];
+ jets_1_[i].a = parameters[1][i];
for (unsigned int i = 0; i < B2; i++)
- (*jets_2_)[i].a = parameters[2][i];
+ jets_2_[i].a = parameters[2][i];
for (unsigned int i = 0; i < B3; i++)
- (*jets_3_)[i].a = parameters[3][i];
+ jets_3_[i].a = parameters[3][i];
}
virtual void evaluate(const std::vector<const double*>& _states_ptr, Eigen::VectorXd& residual_, std::vector<Eigen::MatrixXd>& jacobians_) const
@@ -2259,15 +2089,15 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,0,0,0,0,0,0,0,0> : public FactorBase
updateJetsRealPart(_states_ptr);
// call functor
- (*static_cast<FacT const*>(this))(jets_0_->data(),
- jets_1_->data(),
- jets_2_->data(),
- jets_3_->data(),
- residuals_jets_->data());
+ (*static_cast<FacT const*>(this))(jets_0_.data(),
+ jets_1_.data(),
+ jets_2_.data(),
+ jets_3_.data(),
+ residuals_jets_.data());
// fill the residual vector
for (unsigned int i = 0; i < RES; i++)
- residual_(i) = (*residuals_jets_)[i].a;
+ residual_(i) = residuals_jets_[i].a;
// fill the jacobian matrices
for (unsigned int i = 0; i<n_blocks; i++)
@@ -2277,8 +2107,8 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,0,0,0,0,0,0,0,0> : public FactorBase
// 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),
+ 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);
@@ -2336,11 +2166,10 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,0,0,0,0,0,0,0,0,0> : public FactorBase
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_;
+ mutable std::array<WolfJet, RES> residuals_jets_;
+ mutable std::array<WolfJet, B0> jets_0_;
+ mutable std::array<WolfJet, B1> jets_1_;
+ mutable std::array<WolfJet, B2> jets_2_;
public:
@@ -2356,11 +2185,7 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,0,0,0,0,0,0,0,0,0> : public FactorBase
StateBlockPtr _state1Ptr,
StateBlockPtr _state2Ptr) :
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}),
- 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>)
+ state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr})
{
// asserts for null states
assert(_state0Ptr != nullptr && "nullptr state block");
@@ -2370,20 +2195,16 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,0,0,0,0,0,0,0,0,0> : public FactorBase
// initialize jets
unsigned int last_jet_idx = 0;
for (unsigned int i = 0; i < B0; i++)
- (*jets_0_)[i] = WolfJet(0, last_jet_idx++);
+ jets_0_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B1; i++)
- (*jets_1_)[i] = WolfJet(0, last_jet_idx++);
+ jets_1_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B2; i++)
- (*jets_2_)[i] = WolfJet(0, last_jet_idx++);
+ jets_2_[i] = WolfJet(0, last_jet_idx++);
state_ptrs_.resize(n_blocks);
}
virtual ~FactorAutodiff()
{
- delete jets_0_;
- delete jets_1_;
- delete jets_2_;
- delete residuals_jets_;
}
virtual JacobianMethod getJacobianMethod() const
@@ -2410,21 +2231,21 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,0,0,0,0,0,0,0,0,0> : public FactorBase
updateJetsRealPart(param_vec);
// call functor
- (*static_cast<FacT const*>(this))(jets_0_->data(),
- jets_1_->data(),
- jets_2_->data(),
- residuals_jets_->data());
+ (*static_cast<FacT const*>(this))(jets_0_.data(),
+ jets_1_.data(),
+ jets_2_.data(),
+ residuals_jets_.data());
// fill the residual array
for (unsigned int i = 0; i < RES; i++)
- residuals[i] = (*residuals_jets_)[i].a;
+ 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),
+ 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;
@@ -2434,11 +2255,11 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,0,0,0,0,0,0,0,0,0> : public FactorBase
{
// update jets real part
for (unsigned int i = 0; i < B0; i++)
- (*jets_0_)[i].a = parameters[0][i];
+ jets_0_[i].a = parameters[0][i];
for (unsigned int i = 0; i < B1; i++)
- (*jets_1_)[i].a = parameters[1][i];
+ jets_1_[i].a = parameters[1][i];
for (unsigned int i = 0; i < B2; i++)
- (*jets_2_)[i].a = parameters[2][i];
+ jets_2_[i].a = parameters[2][i];
}
virtual void evaluate(const std::vector<const double*>& _states_ptr, Eigen::VectorXd& residual_, std::vector<Eigen::MatrixXd>& jacobians_) const
@@ -2452,14 +2273,14 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,0,0,0,0,0,0,0,0,0> : public FactorBase
updateJetsRealPart(_states_ptr);
// call functor
- (*static_cast<FacT const*>(this))(jets_0_->data(),
- jets_1_->data(),
- jets_2_->data(),
- residuals_jets_->data());
+ (*static_cast<FacT const*>(this))(jets_0_.data(),
+ jets_1_.data(),
+ jets_2_.data(),
+ residuals_jets_.data());
// fill the residual vector
for (unsigned int i = 0; i < RES; i++)
- residual_(i) = (*residuals_jets_)[i].a;
+ residual_(i) = residuals_jets_[i].a;
// fill the jacobian matrices
for (unsigned int i = 0; i<n_blocks; i++)
@@ -2469,8 +2290,8 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,0,0,0,0,0,0,0,0,0> : public FactorBase
// 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),
+ 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);
@@ -2528,10 +2349,9 @@ class FactorAutodiff<FacT,RES,B0,B1,0,0,0,0,0,0,0,0,0,0> : public FactorBase
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_;
+ mutable std::array<WolfJet, RES> residuals_jets_;
+ mutable std::array<WolfJet, B0> jets_0_;
+ mutable std::array<WolfJet, B1> jets_1_;
public:
@@ -2546,10 +2366,7 @@ class FactorAutodiff<FacT,RES,B0,B1,0,0,0,0,0,0,0,0,0,0> : public FactorBase
StateBlockPtr _state0Ptr,
StateBlockPtr _state1Ptr) :
FactorBase(_tp, _frame_other_ptr, _capture_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>)
+ state_ptrs_({_state0Ptr,_state1Ptr})
{
// asserts for null states
assert(_state0Ptr != nullptr && "nullptr state block");
@@ -2558,17 +2375,14 @@ class FactorAutodiff<FacT,RES,B0,B1,0,0,0,0,0,0,0,0,0,0> : public FactorBase
// initialize jets
unsigned int last_jet_idx = 0;
for (unsigned int i = 0; i < B0; i++)
- (*jets_0_)[i] = WolfJet(0, last_jet_idx++);
+ jets_0_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B1; i++)
- (*jets_1_)[i] = WolfJet(0, last_jet_idx++);
+ jets_1_[i] = WolfJet(0, last_jet_idx++);
state_ptrs_.resize(n_blocks);
}
virtual ~FactorAutodiff()
{
- delete jets_0_;
- delete jets_1_;
- delete residuals_jets_;
}
virtual JacobianMethod getJacobianMethod() const
@@ -2594,20 +2408,20 @@ class FactorAutodiff<FacT,RES,B0,B1,0,0,0,0,0,0,0,0,0,0> : public FactorBase
updateJetsRealPart(param_vec);
// call functor
- (*static_cast<FacT const*>(this))(jets_0_->data(),
- jets_1_->data(),
- residuals_jets_->data());
+ (*static_cast<FacT const*>(this))(jets_0_.data(),
+ jets_1_.data(),
+ residuals_jets_.data());
// fill the residual array
for (unsigned int i = 0; i < RES; i++)
- residuals[i] = (*residuals_jets_)[i].a;
+ 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),
+ 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;
@@ -2617,9 +2431,9 @@ class FactorAutodiff<FacT,RES,B0,B1,0,0,0,0,0,0,0,0,0,0> : public FactorBase
{
// update jets real part
for (unsigned int i = 0; i < B0; i++)
- (*jets_0_)[i].a = parameters[0][i];
+ jets_0_[i].a = parameters[0][i];
for (unsigned int i = 0; i < B1; i++)
- (*jets_1_)[i].a = parameters[1][i];
+ jets_1_[i].a = parameters[1][i];
}
virtual void evaluate(const std::vector<const double*>& _states_ptr, Eigen::VectorXd& residual_, std::vector<Eigen::MatrixXd>& jacobians_) const
@@ -2633,13 +2447,13 @@ class FactorAutodiff<FacT,RES,B0,B1,0,0,0,0,0,0,0,0,0,0> : public FactorBase
updateJetsRealPart(_states_ptr);
// call functor
- (*static_cast<FacT const*>(this))(jets_0_->data(),
- jets_1_->data(),
- residuals_jets_->data());
+ (*static_cast<FacT const*>(this))(jets_0_.data(),
+ jets_1_.data(),
+ residuals_jets_.data());
// fill the residual vector
for (unsigned int i = 0; i < RES; i++)
- residual_(i) = (*residuals_jets_)[i].a;
+ residual_(i) = residuals_jets_[i].a;
// fill the jacobian matrices
for (unsigned int i = 0; i<n_blocks; i++)
@@ -2649,8 +2463,8 @@ class FactorAutodiff<FacT,RES,B0,B1,0,0,0,0,0,0,0,0,0,0> : public FactorBase
// 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),
+ 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);
@@ -2708,9 +2522,8 @@ class FactorAutodiff<FacT,RES,B0,0,0,0,0,0,0,0,0,0,0,0> : public FactorBase
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_;
+ mutable std::array<WolfJet, RES> residuals_jets_;
+ mutable std::array<WolfJet, B0> jets_0_;
public:
@@ -2724,9 +2537,7 @@ class FactorAutodiff<FacT,RES,B0,0,0,0,0,0,0,0,0,0,0,0> : public FactorBase
FactorStatus _status,
StateBlockPtr _state0Ptr) :
FactorBase(_tp, _frame_other_ptr, _capture_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>)
+ state_ptrs_({_state0Ptr})
{
// asserts for null states
assert(_state0Ptr != nullptr && "nullptr state block");
@@ -2734,14 +2545,12 @@ class FactorAutodiff<FacT,RES,B0,0,0,0,0,0,0,0,0,0,0,0> : public FactorBase
// initialize jets
unsigned int last_jet_idx = 0;
for (unsigned int i = 0; i < B0; i++)
- (*jets_0_)[i] = WolfJet(0, last_jet_idx++);
+ jets_0_[i] = WolfJet(0, last_jet_idx++);
state_ptrs_.resize(n_blocks);
}
virtual ~FactorAutodiff()
{
- delete jets_0_;
- delete residuals_jets_;
}
virtual JacobianMethod getJacobianMethod() const
@@ -2766,19 +2575,19 @@ class FactorAutodiff<FacT,RES,B0,0,0,0,0,0,0,0,0,0,0,0> : public FactorBase
updateJetsRealPart(param_vec);
// call functor
- (*static_cast<FacT const*>(this))(jets_0_->data(),
- residuals_jets_->data());
+ (*static_cast<FacT const*>(this))(jets_0_.data(),
+ residuals_jets_.data());
// fill the residual array
for (unsigned int i = 0; i < RES; i++)
- residuals[i] = (*residuals_jets_)[i].a;
+ 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),
+ 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;
@@ -2788,7 +2597,7 @@ class FactorAutodiff<FacT,RES,B0,0,0,0,0,0,0,0,0,0,0,0> : public FactorBase
{
// update jets real part
for (unsigned int i = 0; i < B0; i++)
- (*jets_0_)[i].a = parameters[0][i];
+ jets_0_[i].a = parameters[0][i];
}
virtual void evaluate(const std::vector<const double*>& _states_ptr, Eigen::VectorXd& residual_, std::vector<Eigen::MatrixXd>& jacobians_) const
@@ -2802,12 +2611,12 @@ class FactorAutodiff<FacT,RES,B0,0,0,0,0,0,0,0,0,0,0,0> : public FactorBase
updateJetsRealPart(_states_ptr);
// call functor
- (*static_cast<FacT const*>(this))(jets_0_->data(),
- residuals_jets_->data());
+ (*static_cast<FacT const*>(this))(jets_0_.data(),
+ residuals_jets_.data());
// fill the residual vector
for (unsigned int i = 0; i < RES; i++)
- residual_(i) = (*residuals_jets_)[i].a;
+ residual_(i) = residuals_jets_[i].a;
// fill the jacobian matrices
for (unsigned int i = 0; i<n_blocks; i++)
@@ -2817,8 +2626,8 @@ class FactorAutodiff<FacT,RES,B0,0,0,0,0,0,0,0,0,0,0,0> : public FactorBase
// 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),
+ 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);