diff --git a/src/constraint_analytic.cpp b/src/constraint_analytic.cpp index 8b0422380bc4ad0d39f2e7f38955ea3b0bd948a8..1d10e7a06c81a0370eed2df600e9ab0242398264 100644 --- a/src/constraint_analytic.cpp +++ b/src/constraint_analytic.cpp @@ -7,9 +7,19 @@ ConstraintAnalytic::ConstraintAnalytic(const std::string& _tp, bool _apply_loss_function, ConstraintStatus _status, StateBlockPtr _state0Ptr, StateBlockPtr _state1Ptr, StateBlockPtr _state2Ptr, StateBlockPtr _state3Ptr, StateBlockPtr _state4Ptr, StateBlockPtr _state5Ptr, StateBlockPtr _state6Ptr, StateBlockPtr _state7Ptr, StateBlockPtr _state8Ptr, StateBlockPtr _state9Ptr ) : - ConstraintBase(_tp, _apply_loss_function, _status), - state_ptr_vector_({_state0Ptr, _state1Ptr, _state2Ptr, _state3Ptr, _state4Ptr, - _state5Ptr, _state6Ptr, _state7Ptr, _state8Ptr, _state9Ptr}) + ConstraintBase(_tp, _apply_loss_function, _status), + state_ptr_vector_({_state0Ptr, _state1Ptr, _state2Ptr, _state3Ptr, _state4Ptr, + _state5Ptr, _state6Ptr, _state7Ptr, _state8Ptr, _state9Ptr}), + state_block_sizes_vector_({_state0Ptr ? (unsigned int) _state0Ptr->getSize() : 0, + _state1Ptr ? (unsigned int) _state1Ptr->getSize() : 0, + _state2Ptr ? (unsigned int) _state2Ptr->getSize() : 0, + _state3Ptr ? (unsigned int) _state3Ptr->getSize() : 0, + _state4Ptr ? (unsigned int) _state4Ptr->getSize() : 0, + _state5Ptr ? (unsigned int) _state5Ptr->getSize() : 0, + _state6Ptr ? (unsigned int) _state6Ptr->getSize() : 0, + _state7Ptr ? (unsigned int) _state7Ptr->getSize() : 0, + _state8Ptr ? (unsigned int) _state8Ptr->getSize() : 0, + _state9Ptr ? (unsigned int) _state9Ptr->getSize() : 0}) { resizeVectors(); } @@ -23,9 +33,19 @@ ConstraintAnalytic::ConstraintAnalytic(const std::string& _tp, bool _apply_loss_function, ConstraintStatus _status, StateBlockPtr _state0Ptr, StateBlockPtr _state1Ptr, StateBlockPtr _state2Ptr, StateBlockPtr _state3Ptr, StateBlockPtr _state4Ptr, StateBlockPtr _state5Ptr, StateBlockPtr _state6Ptr, StateBlockPtr _state7Ptr, StateBlockPtr _state8Ptr, StateBlockPtr _state9Ptr ) : - ConstraintBase(_tp, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status), - state_ptr_vector_({_state0Ptr, _state1Ptr, _state2Ptr, _state3Ptr, _state4Ptr, - _state5Ptr, _state6Ptr, _state7Ptr, _state8Ptr, _state9Ptr}) + ConstraintBase(_tp, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status), + state_ptr_vector_({_state0Ptr, _state1Ptr, _state2Ptr, _state3Ptr, _state4Ptr, + _state5Ptr, _state6Ptr, _state7Ptr, _state8Ptr, _state9Ptr}), + state_block_sizes_vector_({_state0Ptr ? (unsigned int) _state0Ptr->getSize() : 0, + _state1Ptr ? (unsigned int) _state1Ptr->getSize() : 0, + _state2Ptr ? (unsigned int) _state2Ptr->getSize() : 0, + _state3Ptr ? (unsigned int) _state3Ptr->getSize() : 0, + _state4Ptr ? (unsigned int) _state4Ptr->getSize() : 0, + _state5Ptr ? (unsigned int) _state5Ptr->getSize() : 0, + _state6Ptr ? (unsigned int) _state6Ptr->getSize() : 0, + _state7Ptr ? (unsigned int) _state7Ptr->getSize() : 0, + _state8Ptr ? (unsigned int) _state8Ptr->getSize() : 0, + _state9Ptr ? (unsigned int) _state9Ptr->getSize() : 0}) { resizeVectors(); } @@ -58,17 +78,15 @@ JacobianMethod ConstraintAnalytic::getJacobianMethod() const void ConstraintAnalytic::resizeVectors() { - for (unsigned int ii = 1; ii<state_ptr_vector_.size(); ii++) - { - if (state_ptr_vector_.at(ii) != nullptr) - state_block_sizes_vector_.push_back(state_ptr_vector_.at(ii)->getSize()); + assert(state_ptr_vector_[0] != nullptr && "at least one not null state block pointer required"); - else + for (unsigned int ii = 1; ii<state_ptr_vector_.size(); ii++) + if (state_ptr_vector_.at(ii) == nullptr) { state_ptr_vector_.resize(ii); + state_block_sizes_vector_.resize(ii); break; } - } } } // namespace wolf