diff --git a/CMakeLists.txt b/CMakeLists.txt index 5f0bf77316d12028cc0aa6fe4e152a57c7691a03..184a42ee5a574e28f6d15e24b50b3a61b1fa1c67 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -358,6 +358,7 @@ SET(SRCS_LANDMARK src/landmark/landmark_base.cpp ) SET(SRCS_PROCESSOR + src/processor/is_motion.cpp src/processor/motion_buffer.cpp src/processor/processor_base.cpp src/processor/processor_diff_drive.cpp 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); diff --git a/include/core/problem/problem.h b/include/core/problem/problem.h index 0fe1cd5b6a18c007859440563eb7a3475eb432c3..69e2392494c8d1c8441feff04f112c05423b8911 100644 --- a/include/core/problem/problem.h +++ b/include/core/problem/problem.h @@ -50,6 +50,7 @@ class Problem : public std::enable_shared_from_this<Problem> friend SolverManager; // Enable SolverManager to acces protected functions (consumeXXXNotificationMap()) friend ProcessorBase; friend ProcessorMotion; + friend IsMotion; protected: diff --git a/include/core/processor/is_motion.h b/include/core/processor/is_motion.h index e586883ed2e199cc46ec68e94f2173a31cf631fa..1488e1a85d2f605949c966c17d0c322ea78882aa 100644 --- a/include/core/processor/is_motion.h +++ b/include/core/processor/is_motion.h @@ -22,7 +22,6 @@ class IsMotion { public: - virtual ~IsMotion(); // Queries to the processor: @@ -30,16 +29,17 @@ class IsMotion virtual void getTimeStamp ( TimeStamp& _ts) const = 0; virtual bool getState ( VectorComposite& _state) const = 0; virtual bool getState ( const TimeStamp& _ts, - VectorComposite& _state) const = 0; - + VectorComposite& _state) const = 0; + // implemented here TimeStamp getTimeStamp() const; - // Get composite states VectorComposite getState() const; VectorComposite getState(const TimeStamp& _ts) const; const StateStructure& getStateStructure() const {return state_structure_;}; void setStateStructure(const StateStructure& _state_structure){state_structure_ = _state_structure;}; + void addToProblem(ProblemPtr _prb_ptr, IsMotionPtr _motion_ptr); + protected: StateStructure state_structure_; ///< The structure of the state vector (to retrieve state blocks from frames) @@ -56,13 +56,6 @@ inline IsMotion::~IsMotion() { } -//inline Eigen::VectorXd IsMotion::getCurrentState() const -//{ -// Eigen::VectorXd x; -// getCurrentState(x); -// return x; -//} - inline TimeStamp IsMotion::getTimeStamp() const { TimeStamp ts; diff --git a/src/processor/is_motion.cpp b/src/processor/is_motion.cpp new file mode 100644 index 0000000000000000000000000000000000000000..1bfd06a5e9669f76ef48c49a559cade0dc7845f0 --- /dev/null +++ b/src/processor/is_motion.cpp @@ -0,0 +1,9 @@ +#include "core/processor/is_motion.h" +#include "core/problem/problem.h" + +using namespace wolf; + +void IsMotion::addToProblem(ProblemPtr _prb_ptr, IsMotionPtr _motion_ptr) +{ + _prb_ptr->addProcessorIsMotion(_motion_ptr); +} diff --git a/src/processor/processor_tracker.cpp b/src/processor/processor_tracker.cpp index 7483e4c7f6dcb8fed2f2621c16fcd2b26b604612..722ff1eaaf8056f6dbcbc510985adb8d5249d277 100644 --- a/src/processor/processor_tracker.cpp +++ b/src/processor/processor_tracker.cpp @@ -102,7 +102,8 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr) // No-break case only for debug. Next case will be executed too. PackKeyFramePtr pack = buffer_pack_kf_.selectPack( incoming_ptr_, params_tracker_->time_tolerance); WOLF_DEBUG( "PT ", getName(), " SECOND_TIME_WITH_PACK: KF" , pack->key_frame->id() , " callback unpacked with ts= " , pack->key_frame->getTimeStamp() ); - } // @suppress("No break at end of case") + } + // Fall through case SECOND_TIME_WITHOUT_PACK : { WOLF_DEBUG( "PT ", getName(), " SECOND_TIME_WITHOUT_PACK" ); diff --git a/test/gtest_pack_KF_buffer.cpp b/test/gtest_pack_KF_buffer.cpp index 3862e263cb747b443e77ef113e9d85efa529f3a5..b2fc65a6acea79636728737980f8d18818524ee8 100644 --- a/test/gtest_pack_KF_buffer.cpp +++ b/test/gtest_pack_KF_buffer.cpp @@ -137,7 +137,9 @@ TEST_F(BufferPackKeyFrameTest, selectPack) { PackKeyFramePtr packQ = pack_kf_buffer.selectPack(16, q[iq]); if (packQ!=nullptr) + { ASSERT_EQ(packQ->key_frame->getTimeStamp(),res(ip1*6+ip2*3+iq)); + } } pack_kf_buffer.clear(); } diff --git a/wolf_scripts/sed_rename.sh b/wolf_scripts/sed_rename.sh new file mode 100755 index 0000000000000000000000000000000000000000..53d20f2bba1f57a7c4569d1dd27b8d5ec09974ea --- /dev/null +++ b/wolf_scripts/sed_rename.sh @@ -0,0 +1,28 @@ +#!/bin/bash +#$1 flag telling whether we are in test mode or not + +modify=$1 +apply=false + +if [[ "$modify" = true || "$modify" = t ]]; then + read -p "Are you sure? " -n 1 -r + echo # (optional) move to a new line + if [[ $REPLY =~ ^[Yy]$ ]] + then + apply=true + echo "Modifying..." + fi +fi + +string="StringToBeReplaced" +replace="ReplacementString" + +for file in $(find include/ src/ test/ demos/ -type f); do + if [ "$apply" = true ]; then + # echo "APPLYING" + sed -Ei 's%'$string'%'$replace'%g' $file + else + # echo "NOT APPLYING" + sed -En 's%'$string'%'$replace'%gp' $file + fi +done