From aedba635b405c8013349145c68bf12bcfd8a0658 Mon Sep 17 00:00:00 2001 From: joanvallve <jvallve@iri.upc.edu> Date: Fri, 16 Oct 2020 10:21:49 +0200 Subject: [PATCH] added n_threads to ceres_solver parameters (solver and covariance) --- include/core/ceres_wrapper/solver_ceres.h | 5 +- src/ceres_wrapper/solver_ceres.cpp | 66 ++++--------------- ...ree_manager_sliding_window_dual_rate3.yaml | 1 + 3 files changed, 18 insertions(+), 54 deletions(-) diff --git a/include/core/ceres_wrapper/solver_ceres.h b/include/core/ceres_wrapper/solver_ceres.h index 9d473358a..62e2ab1a2 100644 --- a/include/core/ceres_wrapper/solver_ceres.h +++ b/include/core/ceres_wrapper/solver_ceres.h @@ -46,11 +46,15 @@ struct ParamsCeres : public ParamsSolver // ceres solver options solver_options.max_num_iterations = _server.getParam<int>(prefix + "max_num_iterations"); + solver_options.num_threads = _server.getParam<int>(prefix + "n_threads"); + covariance_options.num_threads = _server.getParam<int>(prefix + "n_threads"); } void loadHardcodedValues() { solver_options = ceres::Solver::Options(); + solver_options.minimizer_type = ceres::TRUST_REGION; //ceres::LINE_SEARCH; + solver_options.trust_region_strategy_type = ceres::LEVENBERG_MARQUARDT; //ceres::DOGLEG; problem_options = ceres::Problem::Options(); covariance_options = ceres::Covariance::Options(); problem_options.cost_function_ownership = ceres::DO_NOT_TAKE_OWNERSHIP; @@ -65,7 +69,6 @@ struct ParamsCeres : public ParamsSolver #else covariance_options.algorithm_type = ceres::SPARSE_QR;//ceres::DENSE_SVD; #endif - covariance_options.num_threads = 1; covariance_options.apply_loss_function = false; } diff --git a/src/ceres_wrapper/solver_ceres.cpp b/src/ceres_wrapper/solver_ceres.cpp index a01aa40f1..b2e04de32 100644 --- a/src/ceres_wrapper/solver_ceres.cpp +++ b/src/ceres_wrapper/solver_ceres.cpp @@ -53,7 +53,7 @@ std::string SolverCeres::solveDerived(const ReportVerbosity report_level) // run Ceres Solver ceres::Solve(getSolverOptions(), ceres_problem_.get(), &summary_); - // if termination by user (Iteration callback, update and solve again) + /*/ if termination by user (Iteration callback, update and solve again) // solve until max iterations reached if (params_ceres_->update_immediately) { @@ -70,7 +70,7 @@ std::string SolverCeres::solveDerived(const ReportVerbosity report_level) ceres::Solve(getSolverOptions(), ceres_problem_.get(), &summary_); } getSolverOptions().max_num_iterations = max_num_iterations; - } + }*/ std::string report; //return report @@ -230,77 +230,37 @@ bool SolverCeres::computeCovariances(const CovarianceBlocksToBeComputed _blocks) case CovarianceBlocksToBeComputed::GAUSS: { // State blocks: - // - Last KF: POV - // - CaptureGnss T - - std::vector<StateBlockPtr> gauss_state_blocks; + // - Last KF: PV // last KF FrameBasePtr last_frame = wolf_problem_->getLastFrame(); if (not last_frame) return false; - // get capture gnss - StateBlockPtr sb_gnss_T; - for (CaptureBasePtr cap : last_frame->getCaptureList()) - if (cap->getType() == "CaptureGnss" and cap->hasStateBlock('T')) - { - sb_gnss_T = cap->getStateBlock('T'); - break; - } - // Error if (not last_frame->hasStateBlock('P') or not isStateBlockRegisteredDerived(last_frame->getStateBlock('P')) or - not last_frame->hasStateBlock('O') or - not isStateBlockRegisteredDerived(last_frame->getStateBlock('O')) or not last_frame->hasStateBlock('V') or - not isStateBlockRegisteredDerived(last_frame->getStateBlock('V')) or - not sb_gnss_T or - not isStateBlockRegisteredDerived(sb_gnss_T)) + not isStateBlockRegisteredDerived(last_frame->getStateBlock('V'))) { WOLF_WARN("SolverCeres::computeCovariances: last KF have null or unregistered state blocks P O or T, returning..."); WOLF_WARN_COND(not last_frame->getStateBlock('P'), "SolverCeres::computeCovariances: KF state block 'P' not found"); WOLF_WARN_COND(last_frame->getStateBlock('P') and not isStateBlockRegisteredDerived(last_frame->getStateBlock('P')), "SolverCeres::computeCovariances: KF state block 'P' not registered ", last_frame->getStateBlock('P')); - WOLF_WARN_COND(not last_frame->getStateBlock('O'), "SolverCeres::computeCovariances: KF state block 'O' not found"); - WOLF_WARN_COND(last_frame->getStateBlock('O') and not isStateBlockRegisteredDerived(last_frame->getStateBlock('O')), "SolverCeres::computeCovariances: KF state block 'O' not registered ", last_frame->getStateBlock('O')); WOLF_WARN_COND(not last_frame->getStateBlock('V'), "SolverCeres::computeCovariances: KF state block 'V' not found"); WOLF_WARN_COND(last_frame->getStateBlock('V') and not isStateBlockRegisteredDerived(last_frame->getStateBlock('V')), "SolverCeres::computeCovariances: KF state block 'V' not registered ", last_frame->getStateBlock('V')); - WOLF_WARN_COND(not sb_gnss_T, "SolverCeres::computeCovariances: KF capture state block 'T' not found"); - WOLF_WARN_COND(sb_gnss_T and not isStateBlockRegisteredDerived(sb_gnss_T), "SolverCeres::computeCovariances: KF capture state block 'T' not registered ", sb_gnss_T); return false; } - // Fill vector of state blocks - gauss_state_blocks = std::vector<StateBlockPtr>{last_frame->getStateBlock('P'), - last_frame->getStateBlock('O'), - last_frame->getStateBlock('V'), - sb_gnss_T}; - - // only marginals - for (unsigned int i = 0; i < gauss_state_blocks.size(); i++) - { - assert(isStateBlockRegisteredDerived(gauss_state_blocks[i])); - - state_block_pairs.emplace_back(gauss_state_blocks[i],gauss_state_blocks[i]); - double_pairs.emplace_back(getAssociatedMemBlockPtr(gauss_state_blocks[i]), - getAssociatedMemBlockPtr(gauss_state_blocks[i])); - } - /*// double loop all against all (without repetitions) - for (unsigned int i = 0; i < gauss_state_blocks.size(); i++) - { - assert(isStateBlockRegisteredDerived(gauss_state_blocks[i])); - - for (unsigned int j = i; j < gauss_state_blocks.size(); j++) - { - assert(isStateBlockRegisteredDerived(gauss_state_blocks[j])); - - state_block_pairs.emplace_back(gauss_state_blocks[i],gauss_state_blocks[j]); - double_pairs.emplace_back(getAssociatedMemBlockPtr(gauss_state_blocks[i]), - getAssociatedMemBlockPtr(gauss_state_blocks[j])); - } - }*/ + // only marginals of P and V + state_block_pairs.emplace_back(last_frame->getStateBlock('P'), + last_frame->getStateBlock('P')); + double_pairs.emplace_back(getAssociatedMemBlockPtr(last_frame->getStateBlock('P')), + getAssociatedMemBlockPtr(last_frame->getStateBlock('P'))); + state_block_pairs.emplace_back(last_frame->getStateBlock('V'), + last_frame->getStateBlock('V')); + double_pairs.emplace_back(getAssociatedMemBlockPtr(last_frame->getStateBlock('V')), + getAssociatedMemBlockPtr(last_frame->getStateBlock('V'))); break; } diff --git a/test/yaml/params_tree_manager_sliding_window_dual_rate3.yaml b/test/yaml/params_tree_manager_sliding_window_dual_rate3.yaml index b327504aa..b59022518 100644 --- a/test/yaml/params_tree_manager_sliding_window_dual_rate3.yaml +++ b/test/yaml/params_tree_manager_sliding_window_dual_rate3.yaml @@ -4,6 +4,7 @@ config: verbose: 2 update_immediately: false max_num_iterations: 10 + n_threads: 2 problem: frame_structure: "PO" dimension: 3 -- GitLab