diff --git a/include/core/capture/capture_motion.h b/include/core/capture/capture_motion.h index 8621a0eac7a781bb08abe419bc71fc517e886bce..72ed561c556275804b89b8bb9a54c876506d1926 100644 --- a/include/core/capture/capture_motion.h +++ b/include/core/capture/capture_motion.h @@ -152,7 +152,7 @@ inline MotionBuffer& CaptureMotion::getBuffer() inline Eigen::MatrixXd CaptureMotion::getJacobianCalib() const { - return getBuffer().get().back().jacobian_calib_; + return getBuffer().back().jacobian_calib_; } inline Eigen::MatrixXd CaptureMotion::getJacobianCalib(const TimeStamp& _ts) const @@ -193,7 +193,7 @@ inline void CaptureMotion::setCalibrationPreint(const VectorXd& _calib_preint) inline VectorXd CaptureMotion::getDeltaPreint() const { - return getBuffer().get().back().delta_integr_; + return getBuffer().back().delta_integr_; } inline VectorXd CaptureMotion::getDeltaPreint(const TimeStamp& _ts) const @@ -203,7 +203,7 @@ inline VectorXd CaptureMotion::getDeltaPreint(const TimeStamp& _ts) const inline MatrixXd CaptureMotion::getDeltaPreintCov() const { - return getBuffer().get().back().delta_integr_cov_; + return getBuffer().back().delta_integr_cov_; } inline MatrixXd CaptureMotion::getDeltaPreintCov(const TimeStamp& _ts) const 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/processor/motion_buffer.h b/include/core/processor/motion_buffer.h index a5b41ebfe3ebf77f269148d42781b7dd96cd36a2..548fcf6e1e4f99b9d020784b226f4f1df75e4b98 100644 --- a/include/core/processor/motion_buffer.h +++ b/include/core/processor/motion_buffer.h @@ -72,30 +72,16 @@ struct Motion * - If the query time stamp does not match any time stamp in the buffer, * the returned motion-integral or delta-integral is the one immediately before the query time stamp. */ -class MotionBuffer{ +class MotionBuffer : public std::list<Motion> +{ public: MotionBuffer() ; - std::list<Motion>& get(); - const std::list<Motion>& get() const; const Motion& getMotion(const TimeStamp& _ts) const; void getMotion(const TimeStamp& _ts, Motion& _motion) const; void split(const TimeStamp& _ts, MotionBuffer& _oldest_buffer); void print(bool show_data = 0, bool show_delta = 0, bool show_delta_int = 0, bool show_jacs = 0); - - private: - std::list<Motion> container_; }; -inline std::list<Motion>& MotionBuffer::get() -{ - return container_; -} - -inline const std::list<Motion>& MotionBuffer::get() const -{ - return container_; -} - } // namespace wolf #endif /* SRC_MOTIONBUFFER_H_ */ diff --git a/include/core/processor/processor_base.h b/include/core/processor/processor_base.h index 00d5698016447ede93473c94fc36b2a57179beeb..b12d92f10a430810150040fe9277346d2f63c620 100644 --- a/include/core/processor/processor_base.h +++ b/include/core/processor/processor_base.h @@ -326,6 +326,8 @@ class ProcessorBase : public NodeBase, public std::enable_shared_from_this<Proce virtual bool permittedAuxFrame() final; + virtual void setProblem(ProblemPtr) override; + public: /**\brief notify a new keyframe made by another processor * diff --git a/include/core/processor/processor_motion.h b/include/core/processor/processor_motion.h index 74d834693858cfb2fb86d9b30e3755ccc198a6be..d55c279fc3fd9ce5d083529b41d3448191f1f395 100644 --- a/include/core/processor/processor_motion.h +++ b/include/core/processor/processor_motion.h @@ -147,7 +147,6 @@ class ProcessorMotion : public ProcessorBase, public IsMotion protected: ParamsProcessorMotionPtr params_motion_; ProcessingStep processing_step_; ///< State machine controlling the processing step - virtual void setProblem(ProblemPtr) override; // This is the main public interface public: @@ -171,7 +170,7 @@ class ProcessorMotion : public ProcessorBase, public IsMotion * \param _x the returned state vector */ virtual void getCurrentState(Eigen::VectorXd& _x) const override; - virtual void getCurrentTimeStamp(TimeStamp& _ts) const override { _ts = getBuffer().get().back().ts_; } + virtual void getCurrentTimeStamp(TimeStamp& _ts) const override { _ts = getBuffer().back().ts_; } using IsMotion::getCurrentState; using IsMotion::getCurrentTimeStamp; @@ -530,12 +529,12 @@ inline void ProcessorMotion::getCurrentState(Eigen::VectorXd& _x) const inline const Eigen::MatrixXd ProcessorMotion::getCurrentDeltaPreintCov() const { - return getBuffer().get().back().delta_integr_cov_; + return getBuffer().back().delta_integr_cov_; } inline Motion ProcessorMotion::getMotion() const { - return getBuffer().get().back(); + return getBuffer().back(); } inline Motion ProcessorMotion::getMotion(const TimeStamp& _ts) const @@ -548,7 +547,7 @@ inline Motion ProcessorMotion::getMotion(const TimeStamp& _ts) const inline double ProcessorMotion::updateDt() { - return dt_ = incoming_ptr_->getTimeStamp() - getBuffer().get().back().ts_; + return dt_ = incoming_ptr_->getTimeStamp() - getBuffer().back().ts_; } inline const MotionBuffer& ProcessorMotion::getBuffer() const diff --git a/src/capture/capture_base.cpp b/src/capture/capture_base.cpp index 9d4b96c3b9a096089b9ad051b4cf8a48510860c1..65a78728260a755e3b9ddbc370d7d8bc623a3160 100644 --- a/src/capture/capture_base.cpp +++ b/src/capture/capture_base.cpp @@ -228,15 +228,12 @@ void CaptureBase::move(FrameBasePtr _frm_ptr) WOLF_WARN_COND(this->getFrame() == nullptr, "moving a capture not linked to any frame"); WOLF_WARN_COND(_frm_ptr == nullptr, "moving a capture to a null FrameBasePtr"); + assert((this->getFrame() == nullptr || this->getFrame()->isKey()) && "Forbidden: moving a capture already linked to a KF"); + assert((_frm_ptr == nullptr || _frm_ptr->isKey()) && "Forbidden: moving a capture to a non-estimated frame"); + // Unlink if (this->getFrame()) { - if (this->getFrame()->isKey()) - { - WOLF_ERROR("moving a capture linked to a KF"); - return; - } - // unlink from previous non-key frame this->getFrame()->removeCapture(shared_from_this()); this->setFrame(nullptr); diff --git a/src/capture/capture_motion.cpp b/src/capture/capture_motion.cpp index f9cda1f9ad32920b7c66c435f8fdffee1277d279..aa7d4f23a633a01090c9a0d042e46ff153613bd6 100644 --- a/src/capture/capture_motion.cpp +++ b/src/capture/capture_motion.cpp @@ -54,8 +54,8 @@ CaptureMotion::~CaptureMotion() Eigen::VectorXd CaptureMotion::getDeltaCorrected(const VectorXd& _calib_current) const { VectorXd calib_preint = getCalibrationPreint(); - VectorXd delta_preint = getBuffer().get().back().delta_integr_; - MatrixXd jac_calib = getBuffer().get().back().jacobian_calib_; + VectorXd delta_preint = getBuffer().back().delta_integr_; + MatrixXd jac_calib = getBuffer().back().jacobian_calib_; VectorXd delta_error = jac_calib * (_calib_current - calib_preint); VectorXd delta_corrected = correctDelta(delta_preint, delta_error); return delta_corrected; @@ -110,8 +110,8 @@ void CaptureMotion::printHeader(int _depth, bool _constr_by, bool _metric, bool } } - _stream << _tabs << " " << "buffer size : " << getBuffer().get().size() << std::endl; - if ( _metric && ! getBuffer().get().empty()) + _stream << _tabs << " " << "buffer size : " << getBuffer().size() << std::endl; + if ( _metric && ! getBuffer().empty()) { _stream << _tabs << " " << "delta preint : (" << getDeltaPreint().transpose() << ")" << std::endl; if (hasCalibration()) diff --git a/src/factor/factor_base.cpp b/src/factor/factor_base.cpp index 0f65ddb4dcd0587fff4cc9cae0d0f6491c10f437..ee45aa68570db221c37b2d686c26aab948d6f40e 100644 --- a/src/factor/factor_base.cpp +++ b/src/factor/factor_base.cpp @@ -256,12 +256,17 @@ void FactorBase::link(FeatureBasePtr _ftr_ptr) { WOLF_WARN("Linking with nullptr"); return; + + // if frame, should not be not-key + if (getCapture() and getFrame()) + assert(getFrame()->isKey() && "Forbidden: linking a factor with a non-key frame."); } // link with feature _ftr_ptr->addFactor(shared_from_this()); this->setFeature(_ftr_ptr); + // set problem ( and register factor ) WOLF_WARN_COND(_ftr_ptr->getProblem() == nullptr, "ADDING FACTOR ", this->id(), " TO FEATURE ", _ftr_ptr->id(), " THAT IS NOT CONNECTED WITH PROBLEM."); this->setProblem(_ftr_ptr->getProblem()); @@ -270,7 +275,11 @@ void FactorBase::link(FeatureBasePtr _ftr_ptr) for (auto& frm_ow : frame_other_list_) { auto frame_other = frm_ow.lock(); - if(frame_other != nullptr) frame_other->addConstrainedBy(shared_from_this()); + if(frame_other != nullptr) + { + assert(frame_other->isKey() && "Forbidden: linking a factor with a non-key frame_other."); + frame_other->addConstrainedBy(shared_from_this()); + } } for (auto& cap_ow : capture_other_list_) { diff --git a/src/processor/motion_buffer.cpp b/src/processor/motion_buffer.cpp index 5d0a595673da23a73efd69d6b1c12d0734f1172b..52383b14cbf0cb7c355f416ef96a20e726bd711b 100644 --- a/src/processor/motion_buffer.cpp +++ b/src/processor/motion_buffer.cpp @@ -64,17 +64,17 @@ void Motion::resize(SizeEigen _data_s, SizeEigen _delta_s, SizeEigen _delta_cov_ MotionBuffer::MotionBuffer() { - container_.clear(); + this->clear(); } const Motion& MotionBuffer::getMotion(const TimeStamp& _ts) const { - //assert((container_.front().ts_ <= _ts) && "Query time stamp out of buffer bounds"); - auto previous = std::find_if(container_.rbegin(), container_.rend(), [&](const Motion& m) + //assert((this->front().ts_ <= _ts) && "Query time stamp out of buffer bounds"); + auto previous = std::find_if(this->rbegin(), this->rend(), [&](const Motion& m) { return m.ts_ <= _ts; }); - if (previous == container_.rend()) + if (previous == this->rend()) // The time stamp is older than the buffer's oldest data. // We could do something here, and throw an error or something, but by now we'll return the first valid data previous--; @@ -84,12 +84,12 @@ const Motion& MotionBuffer::getMotion(const TimeStamp& _ts) const void MotionBuffer::getMotion(const TimeStamp& _ts, Motion& _motion) const { - //assert((container_.front().ts_ <= _ts) && "Query time stamp out of buffer bounds"); - auto previous = std::find_if(container_.rbegin(), container_.rend(), [&](const Motion& m) + //assert((this->front().ts_ <= _ts) && "Query time stamp out of buffer bounds"); + auto previous = std::find_if(this->rbegin(), this->rend(), [&](const Motion& m) { return m.ts_ <= _ts; }); - if (previous == container_.rend()) + if (previous == this->rend()) // The time stamp is older than the buffer's oldest data. // We could do something here, but by now we'll return the last valid data previous--; @@ -99,25 +99,25 @@ void MotionBuffer::getMotion(const TimeStamp& _ts, Motion& _motion) const void MotionBuffer::split(const TimeStamp& _ts, MotionBuffer& _buffer_part_before_ts) { - assert((container_.front().ts_ <= _ts) && "Error: Query time stamp is smaller than the buffer's first tick"); - assert((container_.back().ts_ >= _ts) && "Error: Query time stamp is greater than the buffer's last tick"); + assert((this->front().ts_ <= _ts) && "Error: Query time stamp is smaller than the buffer's first tick"); + assert((this->back().ts_ >= _ts) && "Error: Query time stamp is greater than the buffer's last tick"); - auto previous = std::find_if(container_.rbegin(), container_.rend(), [&](const Motion& m) + auto previous = std::find_if(this->rbegin(), this->rend(), [&](const Motion& m) { return m.ts_ <= _ts; }); - if (previous == container_.rend()) + if (previous == this->rend()) { // The time stamp is more recent than the buffer's most recent data: // return an empty buffer as the _oldest_buffer - _buffer_part_before_ts.get().clear(); + _buffer_part_before_ts.clear(); } else { // Transfer part of the buffer - _buffer_part_before_ts.container_.splice(_buffer_part_before_ts.container_.begin(), - container_, - container_.begin(), + _buffer_part_before_ts.splice(_buffer_part_before_ts.begin(), + *this, + this->begin(), (previous--).base()); } } @@ -130,15 +130,15 @@ void MotionBuffer::print(bool show_data, bool show_delta, bool show_delta_int, b if (!show_data && !show_delta && !show_delta_int && !show_jacs) { - cout << "Buffer state [" << container_.size() << "] : <"; - for (Motion mot : container_) + cout << "Buffer state [" << this->size() << "] : <"; + for (Motion mot : *this) cout << " " << mot.ts_; cout << " >" << endl; } else { print(0,0,0,0); - for (Motion mot : container_) + for (Motion mot : *this) { cout << "-- Motion (" << mot.ts_ << ")" << endl; if (show_data) diff --git a/src/processor/processor_base.cpp b/src/processor/processor_base.cpp index 101bd5abb500ff95feec86e3c8969ab4f82301d5..6f28aef4d3393d1baf4a652fa4a06052ba9abb82 100644 --- a/src/processor/processor_base.cpp +++ b/src/processor/processor_base.cpp @@ -104,6 +104,23 @@ void ProcessorBase::link(SensorBasePtr _sen_ptr) WOLF_WARN("Linking with a nullptr"); } } + +void ProcessorBase::setProblem(ProblemPtr _problem) +{ + std::string str = "Processor works with " + std::to_string(dim_) + "D but problem is " + std::to_string(_problem->getDim()) + "D"; + assert((dim_ == 0 or dim_ == _problem->getDim()) && str.c_str()); + + if (_problem == nullptr or _problem == this->getProblem()) + return; + + NodeBase::setProblem(_problem); + + // adding processor is motion to the processor is motion vector + auto is_motion_ptr = std::dynamic_pointer_cast<IsMotion>(shared_from_this()); + if (is_motion_ptr) + getProblem()->addProcessorIsMotion(is_motion_ptr); +} + ///////////////////////////////////////////////////////////////////////////////////////// void BufferPackKeyFrame::add(const FrameBasePtr& _key_frame, const double& _time_tolerance) diff --git a/src/processor/processor_diff_drive.cpp b/src/processor/processor_diff_drive.cpp index 6993332c3e23dec79c7697abe6f56a1feb531ce7..55841ede92a1bb5dbf4d49bc190b53ab3392cb64 100644 --- a/src/processor/processor_diff_drive.cpp +++ b/src/processor/processor_diff_drive.cpp @@ -148,10 +148,10 @@ FeatureBasePtr ProcessorDiffDrive::emplaceFeature(CaptureMotionPtr _capture_moti { auto key_feature_ptr = FeatureBase::emplace<FeatureMotion>(_capture_motion, "ProcessorDiffDrive", - _capture_motion->getBuffer().get().back().delta_integr_, - _capture_motion->getBuffer().get().back().delta_integr_cov_, + _capture_motion->getBuffer().back().delta_integr_, + _capture_motion->getBuffer().back().delta_integr_cov_, _capture_motion->getCalibrationPreint(), - _capture_motion->getBuffer().get().back().jacobian_calib_); + _capture_motion->getBuffer().back().jacobian_calib_); return key_feature_ptr; } diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp index 080f54afce83a7fe4f8c7b1e5cd02f0e69dbb581..817dfb367f6aacee7d8be66231f5d2e7e4d56c29 100644 --- a/src/processor/processor_motion.cpp +++ b/src/processor/processor_motion.cpp @@ -66,8 +66,8 @@ void ProcessorMotion::splitBuffer(const wolf::CaptureMotionPtr& _capture_source, _capture_source->getBuffer().split(_ts_split, _capture_target->getBuffer()); // start with empty motion - TimeStamp t_zero = _capture_target->getBuffer().get().back().ts_; // last tick of previous buffer is zero tick of current buffer - _capture_source->getBuffer().get().push_front(motionZero(t_zero)); + TimeStamp t_zero = _capture_target->getBuffer().back().ts_; // last tick of previous buffer is zero tick of current buffer + _capture_source->getBuffer().push_front(motionZero(t_zero)); // Update the existing capture _capture_source->setOriginCapture(_capture_target); @@ -207,8 +207,8 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr) auto feature_existing = capture_existing->getFeatureList().back(); // there is only one feature! // Modify existing feature -------- - feature_existing->setMeasurement (capture_existing->getBuffer().get().back().delta_integr_); - feature_existing->setMeasurementCovariance(capture_existing->getBuffer().get().back().delta_integr_cov_); + feature_existing->setMeasurement (capture_existing->getBuffer().back().delta_integr_); + feature_existing->setMeasurementCovariance(capture_existing->getBuffer().back().delta_integr_cov_); // Modify existing factor -------- // Instead of modifying, we remove one ctr, and create a new one. @@ -367,7 +367,7 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr) last_ptr_->getCalibration(), last_ptr_); // reset the new buffer - capture_new->getBuffer().get().push_back( motionZero(key_frame->getTimeStamp()) ) ; + capture_new->getBuffer().push_back( motionZero(key_frame->getTimeStamp()) ) ; // reset derived things resetDerived(); @@ -422,7 +422,7 @@ bool ProcessorMotion::getState(const TimeStamp& _ts, Eigen::VectorXd& _x) const _x.resize( state_0.size() ); // Compose on top of origin state using the buffered time stamp, not the query time stamp - double dt = motion.ts_ - capture_motion->getBuffer().get().front().ts_; + double dt = motion.ts_ - capture_motion->getBuffer().front().ts_; statePlusDelta(state_0, delta, dt, _x); } else @@ -482,7 +482,7 @@ void ProcessorMotion::setOrigin(FrameBasePtr _origin_frame) origin_ptr_); last_frame_ptr_ = new_frame_ptr; // clear and reset buffer - getBuffer().get().push_back(motionZero(_origin_frame->getTimeStamp())); + getBuffer().push_back(motionZero(_origin_frame->getTimeStamp())); // Reset derived things resetDerived(); @@ -507,7 +507,7 @@ void ProcessorMotion::integrateOneStep() jacobian_delta_calib_); // integrate the current delta to pre-integrated measurements, and get Jacobians - deltaPlusDelta(getBuffer().get().back().delta_integr_, + deltaPlusDelta(getBuffer().back().delta_integr_, delta_, dt_, delta_integrated_, @@ -518,17 +518,17 @@ void ProcessorMotion::integrateOneStep() if ( hasCalibration() ) { jacobian_calib_.noalias() - = jacobian_delta_preint_ * getBuffer().get().back().jacobian_calib_ + = jacobian_delta_preint_ * getBuffer().back().jacobian_calib_ + jacobian_delta_ * jacobian_delta_calib_; } // Integrate covariance delta_integrated_cov_.noalias() - = jacobian_delta_preint_ * getBuffer().get().back().delta_integr_cov_ * jacobian_delta_preint_.transpose() + = jacobian_delta_preint_ * getBuffer().back().delta_integr_cov_ * jacobian_delta_preint_.transpose() + jacobian_delta_ * delta_cov_ * jacobian_delta_.transpose(); // push all into buffer - getBuffer().get().emplace_back(incoming_ptr_->getTimeStamp(), + getBuffer().emplace_back(incoming_ptr_->getTimeStamp(), incoming_ptr_->getData(), incoming_ptr_->getDataCovariance(), delta_, @@ -550,15 +550,15 @@ void ProcessorMotion::reintegrateBuffer(CaptureMotionPtr _capture_ptr) jacobian_calib_ .setZero(); // check that first motion in buffer is zero! - assert(_capture_ptr->getBuffer().get().front().delta_integr_ == delta_integrated_ && "Buffer's first motion is not zero!"); - assert(_capture_ptr->getBuffer().get().front().delta_integr_cov_ == delta_integrated_cov_ && "Buffer's first motion is not zero!"); - assert(_capture_ptr->getBuffer().get().front().jacobian_calib_ == jacobian_calib_ && "Buffer's first motion is not zero!"); + assert(_capture_ptr->getBuffer().front().delta_integr_ == delta_integrated_ && "Buffer's first motion is not zero!"); + assert(_capture_ptr->getBuffer().front().delta_integr_cov_ == delta_integrated_cov_ && "Buffer's first motion is not zero!"); + assert(_capture_ptr->getBuffer().front().jacobian_calib_ == jacobian_calib_ && "Buffer's first motion is not zero!"); // Iterate all the buffer - auto motion_it = _capture_ptr->getBuffer().get().begin(); + auto motion_it = _capture_ptr->getBuffer().begin(); auto prev_motion_it = motion_it; motion_it++; - while (motion_it != _capture_ptr->getBuffer().get().end()) + while (motion_it != _capture_ptr->getBuffer().end()) { // get dt const double dt = motion_it->ts_ - prev_motion_it->ts_; @@ -621,7 +621,7 @@ CaptureMotionPtr ProcessorMotion::findCaptureContainingTimeStamp(const TimeStamp { // Rule 1 satisfied! We found a Capture belonging to this processor's Sensor ==> it is a CaptureMotion capture_motion = std::static_pointer_cast<CaptureMotion>(capture); - if (_ts < frame->getTimeStamp() and !capture_motion->getBuffer().get().empty() and _ts >= capture_motion->getBuffer().get().front().ts_) + if (_ts < frame->getTimeStamp() and !capture_motion->getBuffer().empty() and _ts >= capture_motion->getBuffer().front().ts_) { // Found time stamp satisfying rule 3 above !! ==> break for loop break; @@ -671,7 +671,7 @@ PackKeyFramePtr ProcessorMotion::computeProcessingStep() PackKeyFramePtr pack = buffer_pack_kf_.selectFirstPackBefore(last_ptr_, params_motion_->time_tolerance); // ignore "future" KF to avoid MotionBuffer::split() error - if (pack && pack->key_frame->getTimeStamp() > last_ptr_->getBuffer().get().back().ts_) + if (pack && pack->key_frame->getTimeStamp() > last_ptr_->getBuffer().back().ts_) pack = nullptr; if (pack) @@ -701,24 +701,6 @@ PackKeyFramePtr ProcessorMotion::computeProcessingStep() return nullptr; } -void ProcessorMotion::setProblem(ProblemPtr _problem) -{ - std::string str = "Processor works with " + std::to_string(dim_) + "D but problem is " + std::to_string(_problem->getDim()) + "D"; - assert((dim_ == 0 or dim_ == _problem->getDim()) && str.c_str()); - - if (_problem == nullptr or _problem == this->getProblem()) - return; - - NodeBase::setProblem(_problem); - - // set the origin - if (origin_ptr_ == nullptr && this->getProblem()->getLastKeyFrame() != nullptr) - this->setOrigin(this->getProblem()->getLastKeyFrame()); - - // adding processor is motion to the processor is motion vector - getProblem()->addProcessorIsMotion(std::dynamic_pointer_cast<IsMotion>(shared_from_this())); -} - bool ProcessorMotion::storeKeyFrame(FrameBasePtr _frame_ptr) { return true; diff --git a/src/processor/processor_odom_2d.cpp b/src/processor/processor_odom_2d.cpp index 04265e964c385bdbe591e84c650f8c7d99c36eeb..931efc2e3ea522f4afde3a1647e329d26507859d 100644 --- a/src/processor/processor_odom_2d.cpp +++ b/src/processor/processor_odom_2d.cpp @@ -98,24 +98,24 @@ void ProcessorOdom2d::statePlusDelta(const Eigen::VectorXd& _x, const Eigen::Vec bool ProcessorOdom2d::voteForKeyFrame() const { // Distance criterion - if (getBuffer().get().back().delta_integr_.head<2>().norm() > params_odom_2d_->dist_traveled) + if (getBuffer().back().delta_integr_.head<2>().norm() > params_odom_2d_->dist_traveled) { WOLF_DEBUG("PM", id(), " ", getType(), " votes per distance"); return true; } - if (getBuffer().get().back().delta_integr_.tail<1>().norm() > params_odom_2d_->angle_turned) + if (getBuffer().back().delta_integr_.tail<1>().norm() > params_odom_2d_->angle_turned) { WOLF_DEBUG("PM", id(), " ", getType(), " votes per angle"); return true; } // Uncertainty criterion - if (getBuffer().get().back().delta_integr_cov_.determinant() > params_odom_2d_->cov_det) + if (getBuffer().back().delta_integr_cov_.determinant() > params_odom_2d_->cov_det) { WOLF_DEBUG("PM", id(), " ", getType(), " votes per state covariance"); return true; } // Time criterion - if (getBuffer().get().back().ts_.get() - origin_ptr_->getFrame()->getTimeStamp().get() > params_odom_2d_->max_time_span) + if (getBuffer().back().ts_.get() - origin_ptr_->getFrame()->getTimeStamp().get() > params_odom_2d_->max_time_span) { WOLF_DEBUG("PM", id(), " ", getType(), " votes per time"); return true; @@ -151,12 +151,12 @@ FactorBasePtr ProcessorOdom2d::emplaceFactor(FeatureBasePtr _feature, CaptureBas FeatureBasePtr ProcessorOdom2d::emplaceFeature(CaptureMotionPtr _capture_motion) { - Eigen::MatrixXd covariance = _capture_motion->getBuffer().get().back().delta_integr_cov_; + Eigen::MatrixXd covariance = _capture_motion->getBuffer().back().delta_integr_cov_; makePosDef(covariance); FeatureBasePtr key_feature_ptr = FeatureBase::emplace<FeatureBase>(_capture_motion, "ProcessorOdom2d", - _capture_motion->getBuffer().get().back().delta_integr_, + _capture_motion->getBuffer().back().delta_integr_, covariance); return key_feature_ptr; } diff --git a/src/processor/processor_odom_3d.cpp b/src/processor/processor_odom_3d.cpp index 457394c9213b39a69da4a01f74f2804db723a573..d8f2781f4667989b155ad65e5640a01e4aa52db3 100644 --- a/src/processor/processor_odom_3d.cpp +++ b/src/processor/processor_odom_3d.cpp @@ -161,20 +161,20 @@ void ProcessorOdom3d::statePlusDelta(const Eigen::VectorXd& _x, const Eigen::Vec bool ProcessorOdom3d::voteForKeyFrame() const { - //WOLF_DEBUG( "Time span : " , getBuffer().get().back().ts_ - getBuffer().get().front().ts_ ); - //WOLF_DEBUG( " last ts : ", getBuffer().get().back().ts_); - //WOLF_DEBUG( " first ts : ", getBuffer().get().front().ts_); - //WOLF_DEBUG( "BufferLength: " , getBuffer().get().size() ); + //WOLF_DEBUG( "Time span : " , getBuffer().back().ts_ - getBuffer().front().ts_ ); + //WOLF_DEBUG( " last ts : ", getBuffer().back().ts_); + //WOLF_DEBUG( " first ts : ", getBuffer().front().ts_); + //WOLF_DEBUG( "BufferLength: " , getBuffer().size() ); //WOLF_DEBUG( "DistTraveled: " , delta_integrated_.head(3).norm() ); //WOLF_DEBUG( "AngleTurned : " , 2.0 * acos(delta_integrated_(6)) ); // time span - if (getBuffer().get().back().ts_ - getBuffer().get().front().ts_ > params_odom_3d_->max_time_span) + if (getBuffer().back().ts_ - getBuffer().front().ts_ > params_odom_3d_->max_time_span) { WOLF_DEBUG( "PM: vote: time span" ); return true; } // buffer length - if (getBuffer().get().size() > params_odom_3d_->max_buff_length) + if (getBuffer().size() > params_odom_3d_->max_buff_length) { WOLF_DEBUG( "PM: vote: buffer size" ); return true; @@ -217,8 +217,8 @@ FeatureBasePtr ProcessorOdom3d::emplaceFeature(CaptureMotionPtr _capture_motion) { FeatureBasePtr key_feature_ptr = FeatureBase::emplace<FeatureBase>(_capture_motion, "ProcessorOdom3d", - _capture_motion->getBuffer().get().back().delta_integr_, - _capture_motion->getBuffer().get().back().delta_integr_cov_); + _capture_motion->getBuffer().back().delta_integr_, + _capture_motion->getBuffer().back().delta_integr_cov_); return key_feature_ptr; } diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index b0309585c0a4a1a16b2ce0582669e2e75cf41d68..cb9421c5ed18c64af24816a7ba14c13d92ea3471 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -35,7 +35,8 @@ set(SRC_DUMMY dummy/processor_tracker_feature_dummy.cpp dummy/processor_tracker_landmark_dummy.cpp ) -add_library(dummy ${SRC_DUMMY}) +add_library(dummy SHARED ${SRC_DUMMY}) +target_link_libraries(dummy ${PROJECT_NAME}) ################# ADD YOUR TESTS BELOW #################### # # # ==== IN ALPHABETICAL ORDER! ==== # diff --git a/test/gtest_factor_autodiff.cpp b/test/gtest_factor_autodiff.cpp index 8374c1d2475d86e1c4fa08b046541c80226770f2..68712f969a298f5986b93378480f1a66e03cafe1 100644 --- a/test/gtest_factor_autodiff.cpp +++ b/test/gtest_factor_autodiff.cpp @@ -348,8 +348,8 @@ TEST(FactorAutodiff, AutodiffDummy12) TEST(FactorAutodiff, EmplaceOdom2d) { - FrameBasePtr fr1_ptr(std::make_shared<FrameBase>(NON_ESTIMATED, TimeStamp(0), std::make_shared<StateBlock>(2), std::make_shared<StateBlock>(1))); - FrameBasePtr fr2_ptr(std::make_shared<FrameBase>(NON_ESTIMATED, TimeStamp(0), std::make_shared<StateBlock>(2), std::make_shared<StateBlock>(1))); + FrameBasePtr fr1_ptr(std::make_shared<FrameBase>(KEY, TimeStamp(0), std::make_shared<StateBlock>(2), std::make_shared<StateBlock>(1))); + FrameBasePtr fr2_ptr(std::make_shared<FrameBase>(KEY, TimeStamp(0), std::make_shared<StateBlock>(2), std::make_shared<StateBlock>(1))); // SENSOR ParamsSensorOdom2d intrinsics_odo; @@ -379,8 +379,8 @@ TEST(FactorAutodiff, ResidualOdom2d) f1_pose << 2,1,M_PI; f2_pose << 3,5,3*M_PI/2; - FrameBasePtr fr1_ptr(std::make_shared<FrameBase>(NON_ESTIMATED, TimeStamp(0), std::make_shared<StateBlock>(f1_pose.head<2>()), std::make_shared<StateBlock>(f1_pose.tail<1>()))); - FrameBasePtr fr2_ptr(std::make_shared<FrameBase>(NON_ESTIMATED, TimeStamp(0), std::make_shared<StateBlock>(f2_pose.head<2>()), std::make_shared<StateBlock>(f2_pose.tail<1>()))); + FrameBasePtr fr1_ptr(std::make_shared<FrameBase>(KEY, TimeStamp(0), std::make_shared<StateBlock>(f1_pose.head<2>()), std::make_shared<StateBlock>(f1_pose.tail<1>()))); + FrameBasePtr fr2_ptr(std::make_shared<FrameBase>(KEY, TimeStamp(0), std::make_shared<StateBlock>(f2_pose.head<2>()), std::make_shared<StateBlock>(f2_pose.tail<1>()))); // SENSOR ParamsSensorOdom2d intrinsics_odo; @@ -423,8 +423,8 @@ TEST(FactorAutodiff, JacobianOdom2d) f1_pose << 2,1,M_PI; f2_pose << 3,5,3*M_PI/2; - FrameBasePtr fr1_ptr(std::make_shared<FrameBase>(NON_ESTIMATED, TimeStamp(0), std::make_shared<StateBlock>(f1_pose.head<2>()), std::make_shared<StateBlock>(f1_pose.tail<1>()))); - FrameBasePtr fr2_ptr(std::make_shared<FrameBase>(NON_ESTIMATED, TimeStamp(0), std::make_shared<StateBlock>(f2_pose.head<2>()), std::make_shared<StateBlock>(f2_pose.tail<1>()))); + FrameBasePtr fr1_ptr(std::make_shared<FrameBase>(KEY, TimeStamp(0), std::make_shared<StateBlock>(f1_pose.head<2>()), std::make_shared<StateBlock>(f1_pose.tail<1>()))); + FrameBasePtr fr2_ptr(std::make_shared<FrameBase>(KEY, TimeStamp(0), std::make_shared<StateBlock>(f2_pose.head<2>()), std::make_shared<StateBlock>(f2_pose.tail<1>()))); // SENSOR ParamsSensorOdom2d intrinsics_odo; @@ -501,8 +501,8 @@ TEST(FactorAutodiff, AutodiffVsAnalytic) f1_pose << 2,1,M_PI; f2_pose << 3,5,3*M_PI/2; - FrameBasePtr fr1_ptr(std::make_shared<FrameBase>(NON_ESTIMATED, TimeStamp(0), std::make_shared<StateBlock>(f1_pose.head<2>()), std::make_shared<StateBlock>(f1_pose.tail<1>()))); - FrameBasePtr fr2_ptr(std::make_shared<FrameBase>(NON_ESTIMATED, TimeStamp(0), std::make_shared<StateBlock>(f2_pose.head<2>()), std::make_shared<StateBlock>(f2_pose.tail<1>()))); + FrameBasePtr fr1_ptr(std::make_shared<FrameBase>(KEY, TimeStamp(0), std::make_shared<StateBlock>(f1_pose.head<2>()), std::make_shared<StateBlock>(f1_pose.tail<1>()))); + FrameBasePtr fr2_ptr(std::make_shared<FrameBase>(KEY, TimeStamp(0), std::make_shared<StateBlock>(f2_pose.head<2>()), std::make_shared<StateBlock>(f2_pose.tail<1>()))); // SENSOR ParamsSensorOdom2d intrinsics_odo; diff --git a/test/gtest_frame_base.cpp b/test/gtest_frame_base.cpp index 57b817eb8e93bbb3093975bbbd9d4390e9f54180..2accaa608175f73ffe6d056e54180013d98c3a07 100644 --- a/test/gtest_frame_base.cpp +++ b/test/gtest_frame_base.cpp @@ -70,9 +70,14 @@ TEST(FrameBase, LinksToTree) intrinsics_odo.k_rot_to_rot = 1; auto S = SensorBase::emplace<SensorOdom2d>(P->getHardware(), Vector3d::Zero(), intrinsics_odo); auto F1 = FrameBase::emplaceKeyFrame<FrameBase>(T, 1, make_shared<StateBlock>(2), make_shared<StateBlock>(1)); - auto F2 = FrameBase::emplaceKeyFrame<FrameBase>(T, 2, make_shared<StateBlock>(2), make_shared<StateBlock>(1)); + auto F2 = FrameBase::emplaceKeyFrame<FrameBase>(T, 1, make_shared<StateBlock>(2), make_shared<StateBlock>(1)); auto C = CaptureBase::emplace<CaptureMotion>(F1, "CaptureMotion", 1, S, Vector3d::Zero(), 3, 3, nullptr); - auto p = ProcessorBase::emplace<ProcessorOdom2d>(S, std::make_shared<ParamsProcessorOdom2d>()); + WOLF_INFO("F2->getCaptureList().size() ", F2->getCaptureList().size()); + auto p = std::make_shared<ProcessorOdom2d>(std::make_shared<ParamsProcessorOdom2d>()); + WOLF_INFO("F2->getCaptureList().size() ", F2->getCaptureList().size()); + p->link(S); + //auto p = ProcessorBase::emplace<ProcessorOdom2d>(S, std::make_shared<ParamsProcessorOdom2d>()); + WOLF_INFO("F2->getCaptureList().size() ", F2->getCaptureList().size()); auto f = FeatureBase::emplace<FeatureBase>(C, "f", Vector1d(1), Matrix<double,1,1>::Identity()*.01); auto c = FactorBase::emplace<FactorOdom2d>(f, f, F2, p, false); diff --git a/test/gtest_motion_buffer.cpp b/test/gtest_motion_buffer.cpp index 987c713a245df58dba2c32e9edde9505bcd15d56..3c74ede52ce7ec57a153cdcf416b9c84597cadd9 100644 --- a/test/gtest_motion_buffer.cpp +++ b/test/gtest_motion_buffer.cpp @@ -42,8 +42,8 @@ TEST(MotionBuffer, QueryTimeStamps) { MotionBuffer MB; - MB.get().push_back(m0); - MB.get().push_back(m1); + MB.push_back(m0); + MB.push_back(m1); TimeStamp t; t.set(-1); // t is older than m0.ts_ -> return m0 @@ -66,10 +66,10 @@ TEST(MotionBuffer, getMotion) { MotionBuffer MB; - MB.get().push_back(m0); + MB.push_back(m0); ASSERT_EQ(MB.getMotion(t0).delta_, m0.delta_); - MB.get().push_back(m1); + MB.push_back(m1); ASSERT_EQ(MB.getMotion(t0).delta_, m0.delta_); ASSERT_EQ(MB.getMotion(t1).delta_, m1.delta_); ASSERT_EQ(MB.getMotion(t0).delta_integr_, m0.delta_integr_); @@ -80,11 +80,11 @@ TEST(MotionBuffer, getDelta) { MotionBuffer MB; - MB.get().push_back(m0); + MB.push_back(m0); ASSERT_EQ(MB.getMotion(t0).delta_integr_, m0.delta_integr_); - MB.get().push_back(m1); + MB.push_back(m1); ASSERT_EQ(MB.getMotion(t0).delta_integr_, m0.delta_integr_); ASSERT_EQ(MB.getMotion(t1).delta_integr_, m1.delta_integr_); @@ -94,19 +94,19 @@ TEST(MotionBuffer, Split) { MotionBuffer MB; - MB.get().push_back(m0); - MB.get().push_back(m1); - MB.get().push_back(m2); - MB.get().push_back(m3); - MB.get().push_back(m4); // put 5 motions + MB.push_back(m0); + MB.push_back(m1); + MB.push_back(m2); + MB.push_back(m3); + MB.push_back(m4); // put 5 motions MotionBuffer MB_old; TimeStamp t = 1.5; // between m1 and m2 MB.split(t, MB_old); - ASSERT_EQ(MB_old.get().size(), (unsigned int) 2); - ASSERT_EQ(MB .get().size(), (unsigned int) 3); + ASSERT_EQ(MB_old.size(), (unsigned int) 2); + ASSERT_EQ(MB .size(), (unsigned int) 3); ASSERT_EQ(MB_old.getMotion(t1).ts_, t1); ASSERT_EQ(MB_old.getMotion(t2).ts_, t1); // last ts is t1 @@ -118,11 +118,11 @@ TEST(MotionBuffer, Split) // { // MotionBuffer MB; // -// MB.get().push_back(m0); -// MB.get().push_back(m1); -// MB.get().push_back(m2); -// MB.get().push_back(m3); -// MB.get().push_back(m4); // put 5 motions +// MB.push_back(m0); +// MB.push_back(m1); +// MB.push_back(m2); +// MB.push_back(m3); +// MB.push_back(m4); // put 5 motions // // Eigen::MatrixXd cov = MB.integrateCovariance(); // ASSERT_NEAR(cov(0), 0.04, 1e-8); @@ -133,11 +133,11 @@ TEST(MotionBuffer, Split) // { // MotionBuffer MB; // -// MB.get().push_back(m0); -// MB.get().push_back(m1); -// MB.get().push_back(m2); -// MB.get().push_back(m3); -// MB.get().push_back(m4); // put 5 motions +// MB.push_back(m0); +// MB.push_back(m1); +// MB.push_back(m2); +// MB.push_back(m3); +// MB.push_back(m4); // put 5 motions // // Eigen::MatrixXd cov = MB.integrateCovariance(t2); // ASSERT_NEAR(cov(0), 0.02, 1e-8); @@ -147,11 +147,11 @@ TEST(MotionBuffer, Split) // { // MotionBuffer MB; // -// MB.get().push_back(m0); -// MB.get().push_back(m1); -// MB.get().push_back(m2); -// MB.get().push_back(m3); -// MB.get().push_back(m4); // put 5 motions +// MB.push_back(m0); +// MB.push_back(m1); +// MB.push_back(m2); +// MB.push_back(m3); +// MB.push_back(m4); // put 5 motions // // Eigen::MatrixXd cov = MB.integrateCovariance(t1, t3); // ASSERT_NEAR(cov(0), 0.03, 1e-8); @@ -164,9 +164,9 @@ TEST(MotionBuffer, print) { MotionBuffer MB; - MB.get().push_back(m0); - MB.get().push_back(m1); - MB.get().push_back(m2); + MB.push_back(m0); + MB.push_back(m1); + MB.push_back(m2); MB.print(); MB.print(0,0,0,0); 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