diff --git a/src/ceres_wrapper/ceres_manager.cpp b/src/ceres_wrapper/ceres_manager.cpp index 068e8825556c2c882883ad8794126f43b9297e48..4b1daee8127371190509ff872d4e1cec5617290d 100644 --- a/src/ceres_wrapper/ceres_manager.cpp +++ b/src/ceres_wrapper/ceres_manager.cpp @@ -23,14 +23,14 @@ CeresManager::CeresManager(Problem* _wolf_problem, const ceres::Solver::Options& CeresManager::~CeresManager() { + std::cout << "ceres residual blocks: " << ceres_problem_->NumResidualBlocks() << std::endl; + std::cout << "ceres parameter blocks: " << ceres_problem_->NumParameterBlocks() << std::endl; while (!id_2_residual_idx_.empty()) removeConstraint(id_2_residual_idx_.begin()->first); - + std::cout << "all residuals removed! \n"; removeAllStateBlocks(); + std::cout << "all parameter blocks removed! \n"; - //std::cout << "all state units removed! \n"; - //std::cout << "residual blocks: " << ceres_problem_->NumResidualBlocks() << "\n"; - //std::cout << "parameter blocks: " << ceres_problem_->NumParameterBlocks() << "\n"; delete covariance_; //std::cout << "covariance deleted! \n"; delete ceres_problem_; @@ -201,19 +201,6 @@ void CeresManager::update() //std::cout << wolf_problem_->getStateBlockNotificationList().size() << " state block notifications" << std::endl; //std::cout << wolf_problem_->getConstraintNotificationList().size() << " constraint notifications" << std::endl; - // REMOVE STATE BLOCKS - auto state_notification_it = wolf_problem_->getStateBlockNotificationList().begin(); - while ( state_notification_it != wolf_problem_->getStateBlockNotificationList().end() ) - { - if (state_notification_it->notification_ == REMOVE) - { - removeStateBlock((double *)(state_notification_it->scalar_ptr_)); - state_notification_it = wolf_problem_->getStateBlockNotificationList().erase(state_notification_it); - } - else - state_notification_it++; - } - // REMOVE CONSTRAINTS auto ctr_notification_it = wolf_problem_->getConstraintNotificationList().begin(); while ( ctr_notification_it != wolf_problem_->getConstraintNotificationList().end() ) @@ -227,6 +214,19 @@ void CeresManager::update() ctr_notification_it++; } + // REMOVE STATE BLOCKS + auto state_notification_it = wolf_problem_->getStateBlockNotificationList().begin(); + while ( state_notification_it != wolf_problem_->getStateBlockNotificationList().end() ) + { + if (state_notification_it->notification_ == REMOVE) + { + removeStateBlock((double *)(state_notification_it->scalar_ptr_)); + state_notification_it = wolf_problem_->getStateBlockNotificationList().erase(state_notification_it); + } + else + state_notification_it++; + } + // ADD/UPDATE STATE BLOCKS while (!wolf_problem_->getStateBlockNotificationList().empty()) { @@ -265,13 +265,18 @@ void CeresManager::update() wolf_problem_->getConstraintNotificationList().pop_front(); } //std::cout << "all constraints added" << std::endl; + //std::cout << "ceres residual blocks: " << ceres_problem_->NumResidualBlocks() << std::endl; + //std::cout << "wrapper residual blocks: " << id_2_residual_idx_.size() << std::endl; + //std::cout << "parameter blocks: " << ceres_problem_->NumParameterBlocks() << std::endl; + + assert(ceres_problem_->NumResidualBlocks() == id_2_residual_idx_.size() && "ceres residuals different from wrapper residuals"); } void CeresManager::addConstraint(ConstraintBase* _ctr_ptr, unsigned int _id) { id_2_costfunction_[_id] = createCostFunction(_ctr_ptr); - //std::cout << "adding residual" << std::endl; + //std::cout << "adding residual " << _ctr_ptr->id() << std::endl; if (_ctr_ptr->getApplyLossFunction()) id_2_residual_idx_[_id] = ceres_problem_->AddResidualBlock(id_2_costfunction_[_id], new ceres::CauchyLoss(0.5), _ctr_ptr->getStateBlockPtrVector()); @@ -291,7 +296,7 @@ void CeresManager::removeConstraint(const unsigned int& _corr_id) void CeresManager::addStateBlock(StateBlock* _st_ptr) { - //std::cout << "Adding State Block" << std::endl; + //std::cout << "Adding State Block " << _st_ptr->getPtr() << std::endl; //std::cout << " size: " << _st_ptr->getSize() << std::endl; //std::cout << " vector: " << _st_ptr->getVector() << std::endl; @@ -311,6 +316,7 @@ void CeresManager::addStateBlock(StateBlock* _st_ptr) void CeresManager::removeStateBlock(double* _st_ptr) { + //std::cout << "Removing State Block " << _st_ptr << std::endl; assert(_st_ptr != nullptr); ceres_problem_->RemoveParameterBlock(_st_ptr); }