diff --git a/include/core/ceres_wrapper/solver_ceres.h b/include/core/ceres_wrapper/solver_ceres.h
index 9d473358abdec1c18f2b7239deabaa6399106825..62e2ab1a2da15ecec5ea372150a3c83c8b6b3bea 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 a01aa40f1a8faee05b5f3ccfcbfe727278c5ab55..b2e04de327a97cb76dd955b7506437693f63f4ae 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 b327504aa414e4b36e6163175b20b0ba4a4f79b9..b59022518aaf931890368185fb9b489611cda257 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