diff --git a/CMakeLists.txt b/CMakeLists.txt index 9112af5f16d8a84404e203361c568563ccb88dee..271db2988212108d5e8baf42e206601f92a98eaf 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -392,7 +392,7 @@ SET(SRCS_YAML IF (Ceres_FOUND) SET(HDRS_WRAPPER #ceres_wrapper/qr_manager.h - include/core/ceres_wrapper/ceres_manager.h + include/core/ceres_wrapper/solver_ceres.h include/core/ceres_wrapper/cost_function_wrapper.h include/core/ceres_wrapper/create_numeric_diff_cost_function.h include/core/ceres_wrapper/local_parametrization_wrapper.h @@ -401,7 +401,7 @@ IF (Ceres_FOUND) ) SET(SRCS_WRAPPER #ceres_wrapper/qr_manager.cpp - src/ceres_wrapper/ceres_manager.cpp + src/ceres_wrapper/solver_ceres.cpp src/ceres_wrapper/local_parametrization_wrapper.cpp src/solver/solver_manager.cpp ) diff --git a/demos/demo_analytic_autodiff_factor.cpp b/demos/demo_analytic_autodiff_factor.cpp index 6325f785bd2cbfab046f31b73e419c01adf3d7ff..6a9a0b67a97707185be3ef3dd6f39063952bf916 100644 --- a/demos/demo_analytic_autodiff_factor.cpp +++ b/demos/demo_analytic_autodiff_factor.cpp @@ -10,10 +10,10 @@ #include <cmath> #include <queue> +#include "../include/core/ceres_wrapper/solver_ceres.h" //Wolf includes #include "wolf_manager.h" #include "core/capture/capture_void.h" -#include "core/ceres_wrapper/ceres_manager.h" // EIGEN //#include <Eigen/CholmodSupport> @@ -66,8 +66,8 @@ int main(int argc, char** argv) ceres_options.minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;LINE_SEARCH ceres_options.max_line_search_step_contraction = 1e-3; ceres_options.max_num_iterations = 1e4; - CeresManager* ceres_manager_autodiff = new CeresManager(wolf_problem_autodiff, ceres_options); - CeresManager* ceres_manager_analytic = new CeresManager(wolf_problem_analytic, ceres_options); + SolverCeres* ceres_manager_autodiff = new SolverCeres(wolf_problem_autodiff, ceres_options); + SolverCeres* ceres_manager_analytic = new SolverCeres(wolf_problem_analytic, ceres_options); // load graph from .txt offLineFile_.open(file_path_.c_str(), std::ifstream::in); diff --git a/demos/demo_wolf_imported_graph.cpp b/demos/demo_wolf_imported_graph.cpp index f19095d3400f9c08b9633e6486398a0d059bc707..d3be8bc31e3a1821e2ff7442f56e33e6456b2e89 100644 --- a/demos/demo_wolf_imported_graph.cpp +++ b/demos/demo_wolf_imported_graph.cpp @@ -10,10 +10,10 @@ #include <cmath> #include <queue> +#include "../include/core/ceres_wrapper/solver_ceres.h" //Wolf includes #include "wolf_manager.h" #include "core/capture/capture_void.h" -#include "core/ceres_wrapper/ceres_manager.h" // EIGEN //#include <Eigen/CholmodSupport> @@ -72,8 +72,8 @@ int main(int argc, char** argv) ceres_options.minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;LINE_SEARCH ceres_options.max_line_search_step_contraction = 1e-3; ceres_options.max_num_iterations = 1e4; - CeresManager* ceres_manager_full = new CeresManager(wolf_problem_full, ceres_options); - CeresManager* ceres_manager_prun = new CeresManager(wolf_problem_prun, ceres_options); + SolverCeres* ceres_manager_full = new SolverCeres(wolf_problem_full, ceres_options); + SolverCeres* ceres_manager_prun = new SolverCeres(wolf_problem_prun, ceres_options); // load graph from .txt offLineFile_.open(file_path_.c_str(), std::ifstream::in); diff --git a/demos/hello_wolf/hello_wolf.cpp b/demos/hello_wolf/hello_wolf.cpp index e51f7a33658bf1c3afb9f2d20e8118825886ebf6..65de0c5807cf768c462f0c9a2f0bb7ee69ee00f5 100644 --- a/demos/hello_wolf/hello_wolf.cpp +++ b/demos/hello_wolf/hello_wolf.cpp @@ -14,13 +14,11 @@ // wolf core includes +#include "../../include/core/ceres_wrapper/solver_ceres.h" #include "core/common/wolf.h" #include "core/sensor/sensor_odom_2d.h" #include "core/processor/processor_odom_2d.h" #include "core/capture/capture_odom_2d.h" -#include "core/ceres_wrapper/ceres_manager.h" - -// hello wolf local includes #include "sensor_range_bearing.h" #include "processor_range_bearing.h" #include "capture_range_bearing.h" @@ -108,7 +106,7 @@ int main() ProblemPtr problem = Problem::create("PO", 2); ceres::Solver::Options options; options.max_num_iterations = 1000; // We depart far from solution, need a lot of iterations - CeresManagerPtr ceres = std::make_shared<CeresManager>(problem, options); + SolverCeresPtr ceres = std::make_shared<SolverCeres>(problem, options); // sensor odometer 2d ParamsSensorOdom2dPtr intrinsics_odo = std::make_shared<ParamsSensorOdom2d>(); diff --git a/demos/hello_wolf/hello_wolf_autoconf.cpp b/demos/hello_wolf/hello_wolf_autoconf.cpp index fbe33c3afd885a4ccafdb72a10bad23d19cf507a..4a12a89073ecc0b53c24f87a273fcb5621433e09 100644 --- a/demos/hello_wolf/hello_wolf_autoconf.cpp +++ b/demos/hello_wolf/hello_wolf_autoconf.cpp @@ -7,13 +7,11 @@ // wolf core includes +#include "../../include/core/ceres_wrapper/solver_ceres.h" #include "core/common/wolf.h" #include "core/capture/capture_odom_2d.h" #include "core/processor/processor_motion.h" #include "core/yaml/parser_yaml.h" -#include "core/ceres_wrapper/ceres_manager.h" - -// hello wolf local includes #include "capture_range_bearing.h" @@ -114,7 +112,7 @@ int main() // Solver. Configure a Ceres solver ceres::Solver::Options options; options.max_num_iterations = 1000; // We depart far from solution, need a lot of iterations - CeresManagerPtr ceres = std::make_shared<CeresManager>(problem, options); + SolverCeresPtr ceres = std::make_shared<SolverCeres>(problem, options); // recover sensor pointers and other stuff for later use (access by sensor name) SensorBasePtr sensor_odo = problem->getSensor("sen odom"); diff --git a/demos/solver/test_iQR_wolf2.cpp b/demos/solver/test_iQR_wolf2.cpp index 8084a74093ba94602ce19dd413deb12c24cfa047..de9f6f157cc58ef2b5a542056cae7d7e32b16afd 100644 --- a/demos/solver/test_iQR_wolf2.cpp +++ b/demos/solver/test_iQR_wolf2.cpp @@ -35,9 +35,8 @@ //Ceres includes #include "glog/logging.h" -#include "core/ceres_wrapper/ceres_manager.h" -//laser_scan_utils +#include "../../include/core/ceres_wrapper/solver_ceres.h" #include "iri-algorithms/laser_scan_utils/corner_detector.h" #include "iri-algorithms/laser_scan_utils/entities.h" @@ -181,7 +180,7 @@ int main(int argc, char *argv[]) // ceres_options.minimizer_progress_to_stdout = false; // ceres_options.line_search_direction_type = ceres::LBFGS; // ceres_options.max_num_iterations = 100; - CeresManager* ceres_manager = new CeresManager(wolf_manager_ceres->getProblem(), ceres_options); + SolverCeres* ceres_manager = new SolverCeres(wolf_manager_ceres->getProblem(), ceres_options); std::ofstream log_file, landmark_file; //output file // Own solver diff --git a/include/core/ceres_wrapper/ceres_manager.h b/include/core/ceres_wrapper/solver_ceres.h similarity index 84% rename from include/core/ceres_wrapper/ceres_manager.h rename to include/core/ceres_wrapper/solver_ceres.h index 9408ab7ad594b73e5ec75aa689a646fe68a24380..0b42b47f2887a82f377cd278a55fe25d72254418 100644 --- a/include/core/ceres_wrapper/ceres_manager.h +++ b/include/core/ceres_wrapper/solver_ceres.h @@ -19,7 +19,7 @@ typedef std::shared_ptr<CostFunction> CostFunctionPtr; namespace wolf { -WOLF_PTR_TYPEDEFS(CeresManager); +WOLF_PTR_TYPEDEFS(SolverCeres); WOLF_PTR_TYPEDEFS(ParamsCeres); struct ParamsCeres : public ParamsSolver @@ -63,11 +63,8 @@ struct ParamsCeres : public ParamsSolver ~ParamsCeres() override = default; }; -/** \brief Ceres manager for WOLF - * - */ -class CeresManager : public SolverManager +class SolverCeres : public SolverManager { protected: @@ -84,17 +81,14 @@ class CeresManager : public SolverManager public: -// CeresManager(const ProblemPtr& _wolf_problem, -// const ceres::Solver::Options& _ceres_options -// = ceres::Solver::Options()); - CeresManager(const ProblemPtr& _wolf_problem); + SolverCeres(const ProblemPtr& _wolf_problem); - CeresManager(const ProblemPtr& _wolf_problem, + SolverCeres(const ProblemPtr& _wolf_problem, const ParamsCeresPtr& _params); - WOLF_SOLVER_CREATE(CeresManager, ParamsCeres); + WOLF_SOLVER_CREATE(SolverCeres, ParamsCeres); - ~CeresManager() override; + ~SolverCeres() override; ceres::Solver::Summary getSummary(); @@ -143,28 +137,28 @@ class CeresManager : public SolverManager bool isStateBlockRegisteredDerived(const StateBlockPtr& state_ptr) override; }; -inline ceres::Solver::Summary CeresManager::getSummary() +inline ceres::Solver::Summary SolverCeres::getSummary() { return summary_; } -inline std::unique_ptr<ceres::Problem>& CeresManager::getCeresProblem() +inline std::unique_ptr<ceres::Problem>& SolverCeres::getCeresProblem() { return ceres_problem_; } -inline ceres::Solver::Options& CeresManager::getSolverOptions() +inline ceres::Solver::Options& SolverCeres::getSolverOptions() { return params_ceres_->solver_options; } -inline bool CeresManager::isFactorRegisteredDerived(const FactorBasePtr& fac_ptr) const +inline bool SolverCeres::isFactorRegisteredDerived(const FactorBasePtr& fac_ptr) const { return fac_2_residual_idx_.find(fac_ptr) != fac_2_residual_idx_.end() && fac_2_costfunction_.find(fac_ptr) != fac_2_costfunction_.end(); } -inline bool CeresManager::isStateBlockRegisteredDerived(const StateBlockPtr& state_ptr) +inline bool SolverCeres::isStateBlockRegisteredDerived(const StateBlockPtr& state_ptr) { return state_blocks_local_param_.find(state_ptr) != state_blocks_local_param_.end() && ceres_problem_->HasParameterBlock(getAssociatedMemBlockPtr(state_ptr)); diff --git a/src/ceres_wrapper/ceres_manager.cpp b/src/ceres_wrapper/solver_ceres.cpp similarity index 85% rename from src/ceres_wrapper/ceres_manager.cpp rename to src/ceres_wrapper/solver_ceres.cpp index d78128ae960e7be8b9c957cd36f9812d53f14249..e499173a8d9c2f2551393beea506b3d47d099273 100644 --- a/src/ceres_wrapper/ceres_manager.cpp +++ b/src/ceres_wrapper/solver_ceres.cpp @@ -1,4 +1,4 @@ -#include "core/ceres_wrapper/ceres_manager.h" +#include "../../include/core/ceres_wrapper/solver_ceres.h" #include "core/ceres_wrapper/create_numeric_diff_cost_function.h" #include "core/trajectory/trajectory_base.h" #include "core/map/map_base.h" @@ -8,13 +8,13 @@ namespace wolf { -CeresManager::CeresManager(const ProblemPtr& _wolf_problem) : - CeresManager(_wolf_problem, std::make_shared<ParamsCeres>()) +SolverCeres::SolverCeres(const ProblemPtr& _wolf_problem) : + SolverCeres(_wolf_problem, std::make_shared<ParamsCeres>()) { } -CeresManager::CeresManager(const ProblemPtr& _wolf_problem, +SolverCeres::SolverCeres(const ProblemPtr& _wolf_problem, const ParamsCeresPtr& _params) : SolverManager(_wolf_problem), params_ceres_(_params) @@ -23,23 +23,7 @@ CeresManager::CeresManager(const ProblemPtr& _wolf_problem, ceres_problem_ = wolf::make_unique<ceres::Problem>(params_ceres_->problem_options); } -//CeresManager::CeresManager(const ProblemPtr& _wolf_problem, -// const ceres::Solver::Options& _ceres_options) : -// SolverManager(_wolf_problem), -// params_ceres_->solver_options(_ceres_options) -//{ -// -// covariance_ = wolf::make_unique<ceres::Covariance>(covariance_options); -// -// ceres::Problem::Options problem_options; -// problem_options.cost_function_ownership = ceres::DO_NOT_TAKE_OWNERSHIP; -// problem_options.loss_function_ownership = ceres::TAKE_OWNERSHIP; -// problem_options.local_parameterization_ownership = ceres::DO_NOT_TAKE_OWNERSHIP; -// -// ceres_problem_ = wolf::make_unique<ceres::Problem>(problem_options); -//} - -CeresManager::~CeresManager() +SolverCeres::~SolverCeres() { while (!fac_2_residual_idx_.empty()) removeFactorDerived(fac_2_residual_idx_.begin()->first); @@ -51,7 +35,7 @@ CeresManager::~CeresManager() } } -std::string CeresManager::solveDerived(const ReportVerbosity report_level) +std::string SolverCeres::solveDerived(const ReportVerbosity report_level) { // run Ceres Solver ceres::Solve(params_ceres_->solver_options, ceres_problem_.get(), &summary_); @@ -67,7 +51,7 @@ std::string CeresManager::solveDerived(const ReportVerbosity report_level) return report; } -void CeresManager::computeCovariances(const CovarianceBlocksToBeComputed _blocks) +void SolverCeres::computeCovariances(const CovarianceBlocksToBeComputed _blocks) { // update problem update(); @@ -211,9 +195,9 @@ void CeresManager::computeCovariances(const CovarianceBlocksToBeComputed _blocks else std::cout << "WARNING: Couldn't compute covariances!" << std::endl; } -void CeresManager::computeCovariances(const std::vector<StateBlockPtr>& st_list) +void SolverCeres::computeCovariances(const std::vector<StateBlockPtr>& st_list) { - //std::cout << "CeresManager: computing covariances..." << std::endl; + //std::cout << "SolverCeres: computing covariances..." << std::endl; // update problem update(); @@ -257,7 +241,7 @@ void CeresManager::computeCovariances(const std::vector<StateBlockPtr>& st_list) std::cout << "WARNING: Couldn't compute covariances!" << std::endl; } -void CeresManager::addFactorDerived(const FactorBasePtr& fac_ptr) +void SolverCeres::addFactorDerived(const FactorBasePtr& fac_ptr) { assert(fac_2_costfunction_.find(fac_ptr) == fac_2_costfunction_.end() && "adding a factor that is already in the fac_2_costfunction_ map"); @@ -285,7 +269,7 @@ void CeresManager::addFactorDerived(const FactorBasePtr& fac_ptr) assert((unsigned int)(ceres_problem_->NumResidualBlocks()) == fac_2_residual_idx_.size() && "ceres residuals different from wrapper residuals"); } -void CeresManager::removeFactorDerived(const FactorBasePtr& _fac_ptr) +void SolverCeres::removeFactorDerived(const FactorBasePtr& _fac_ptr) { assert(fac_2_residual_idx_.find(_fac_ptr) != fac_2_residual_idx_.end() && "removing a factor that is not in the fac_2_residual map"); @@ -296,7 +280,7 @@ void CeresManager::removeFactorDerived(const FactorBasePtr& _fac_ptr) assert((unsigned int)(ceres_problem_->NumResidualBlocks()) == fac_2_residual_idx_.size() && "ceres residuals different from wrapper residuals"); } -void CeresManager::addStateBlockDerived(const StateBlockPtr& state_ptr) +void SolverCeres::addStateBlockDerived(const StateBlockPtr& state_ptr) { assert(state_ptr); assert(!ceres_problem_->HasParameterBlock(getAssociatedMemBlockPtr(state_ptr)) && "adding a state block already added"); @@ -322,16 +306,16 @@ void CeresManager::addStateBlockDerived(const StateBlockPtr& state_ptr) updateStateBlockStatusDerived(state_ptr); } -void CeresManager::removeStateBlockDerived(const StateBlockPtr& state_ptr) +void SolverCeres::removeStateBlockDerived(const StateBlockPtr& state_ptr) { - //std::cout << "CeresManager::removeStateBlock " << state_ptr.get() << " - " << getAssociatedMemBlockPtr(state_ptr) << std::endl; + //std::cout << "SolverCeres::removeStateBlock " << state_ptr.get() << " - " << getAssociatedMemBlockPtr(state_ptr) << std::endl; assert(state_ptr); assert(ceres_problem_->HasParameterBlock(getAssociatedMemBlockPtr(state_ptr)) && "removing a state block that is not in ceres"); ceres_problem_->RemoveParameterBlock(getAssociatedMemBlockPtr(state_ptr)); state_blocks_local_param_.erase(state_ptr); } -void CeresManager::updateStateBlockStatusDerived(const StateBlockPtr& state_ptr) +void SolverCeres::updateStateBlockStatusDerived(const StateBlockPtr& state_ptr) { assert(state_ptr != nullptr); if (state_ptr->isFixed()) @@ -340,7 +324,7 @@ void CeresManager::updateStateBlockStatusDerived(const StateBlockPtr& state_ptr) ceres_problem_->SetParameterBlockVariable(getAssociatedMemBlockPtr(state_ptr)); } -void CeresManager::updateStateBlockLocalParametrizationDerived(const StateBlockPtr& state_ptr) +void SolverCeres::updateStateBlockLocalParametrizationDerived(const StateBlockPtr& state_ptr) { assert(state_ptr != nullptr); @@ -376,27 +360,27 @@ void CeresManager::updateStateBlockLocalParametrizationDerived(const StateBlockP addFactorDerived(fac); } -bool CeresManager::hasConverged() +bool SolverCeres::hasConverged() { return summary_.termination_type == ceres::CONVERGENCE; } -SizeStd CeresManager::iterations() +SizeStd SolverCeres::iterations() { return summary_.iterations.size(); } -double CeresManager::initialCost() +double SolverCeres::initialCost() { return double(summary_.initial_cost); } -double CeresManager::finalCost() +double SolverCeres::finalCost() { return double(summary_.final_cost); } -ceres::CostFunctionPtr CeresManager::createCostFunction(const FactorBasePtr& _fac_ptr) +ceres::CostFunctionPtr SolverCeres::createCostFunction(const FactorBasePtr& _fac_ptr) { assert(_fac_ptr != nullptr); @@ -412,7 +396,7 @@ ceres::CostFunctionPtr CeresManager::createCostFunction(const FactorBasePtr& _fa throw std::invalid_argument( "Wrong Jacobian Method!" ); } -bool CeresManager::checkDerived(std::string prefix) const +bool SolverCeres::checkDerived(std::string prefix) const { bool ok = true; @@ -421,12 +405,12 @@ bool CeresManager::checkDerived(std::string prefix) const ceres_problem_->NumResidualBlocks() != fac_2_costfunction_.size() or ceres_problem_->NumResidualBlocks() != fac_2_residual_idx_.size()) { - WOLF_ERROR("CeresManager::check: number of residuals mismatch - in ", prefix, ":\n\tceres_problem_->NumResidualBlocks(): ", ceres_problem_->NumResidualBlocks(), "\n\tfactors_.size(): ", factors_.size(), "\n\tfac_2_costfunction_.size(): ", fac_2_costfunction_.size(), "\n\tfac_2_residual_idx_.size(): ", fac_2_residual_idx_.size()); + WOLF_ERROR("SolverCeres::check: number of residuals mismatch - in ", prefix, ":\n\tceres_problem_->NumResidualBlocks(): ", ceres_problem_->NumResidualBlocks(), "\n\tfactors_.size(): ", factors_.size(), "\n\tfac_2_costfunction_.size(): ", fac_2_costfunction_.size(), "\n\tfac_2_residual_idx_.size(): ", fac_2_residual_idx_.size()); ok = false; } if (ceres_problem_->NumParameterBlocks() != state_blocks_.size()) { - WOLF_ERROR("CeresManager::check: number of parameters mismatch ((ceres_problem_->NumParameterBlocks(): ", ceres_problem_->NumParameterBlocks(), " != state_blocks_.size(): ", state_blocks_.size(), ") - in ", prefix); + WOLF_ERROR("SolverCeres::check: number of parameters mismatch ((ceres_problem_->NumParameterBlocks(): ", ceres_problem_->NumParameterBlocks(), " != state_blocks_.size(): ", state_blocks_.size(), ") - in ", prefix); ok = false; } @@ -436,7 +420,7 @@ bool CeresManager::checkDerived(std::string prefix) const { if (!ceres_problem_->HasParameterBlock(state_block_pair.second.data())) { - WOLF_ERROR("CeresManager::check: any state block is missing in ceres problem - in ", prefix); + WOLF_ERROR("SolverCeres::check: any state block is missing in ceres problem - in ", prefix); ok = false; } } @@ -447,27 +431,27 @@ bool CeresManager::checkDerived(std::string prefix) const // SolverManager::factors_ if (factors_.count(fac_res_pair.first) == 0) { - WOLF_ERROR("CeresManager::check: factor ", fac_res_pair.first->id(), " (in fac_2_residual_idx_) not in factors_ - in ", prefix); + WOLF_ERROR("SolverCeres::check: factor ", fac_res_pair.first->id(), " (in fac_2_residual_idx_) not in factors_ - in ", prefix); ok = false; } // costfunction - residual if (fac_2_costfunction_.count(fac_res_pair.first) == 0) { - WOLF_ERROR("CeresManager::check: any factor in fac_2_residual_idx_ is not in fac_2_costfunction_ - in ", prefix); + WOLF_ERROR("SolverCeres::check: any factor in fac_2_residual_idx_ is not in fac_2_costfunction_ - in ", prefix); ok = false; } //if (fac_2_costfunction_[fac_res_pair.first].get() != ceres_problem_->GetCostFunctionForResidualBlock(fac_res_pair.second)) if (fac_2_costfunction_.at(fac_res_pair.first).get() != ceres_problem_->GetCostFunctionForResidualBlock(fac_res_pair.second)) { - WOLF_ERROR("CeresManager::check: fac_2_costfunction_ and ceres mismatch - in ", prefix); + WOLF_ERROR("SolverCeres::check: fac_2_costfunction_ and ceres mismatch - in ", prefix); ok = false; } // factor - residual if (fac_res_pair.first != static_cast<const CostFunctionWrapper*>(ceres_problem_->GetCostFunctionForResidualBlock(fac_res_pair.second))->getFactor()) { - WOLF_ERROR("CeresManager::check: fac_2_residual_idx_ and ceres mismatch - in ", prefix); + WOLF_ERROR("SolverCeres::check: fac_2_residual_idx_ and ceres mismatch - in ", prefix); ok = false; } @@ -476,7 +460,7 @@ bool CeresManager::checkDerived(std::string prefix) const ceres_problem_->GetParameterBlocksForResidualBlock(fac_res_pair.second, ¶m_blocks); if (param_blocks.size() != fac_res_pair.first->getStateBlockPtrVector().size()) { - WOLF_ERROR("CeresManager::check: number parameter blocks in ceres residual", param_blocks.size(), " different from number of state blocks in factor ", fac_res_pair.first->getStateBlockPtrVector().size(), " - in ", prefix); + WOLF_ERROR("SolverCeres::check: number parameter blocks in ceres residual", param_blocks.size(), " different from number of state blocks in factor ", fac_res_pair.first->getStateBlockPtrVector().size(), " - in ", prefix); ok = false; } else @@ -486,7 +470,7 @@ bool CeresManager::checkDerived(std::string prefix) const { if (getAssociatedMemBlockPtr(st) != param_blocks[i]) { - WOLF_ERROR("CeresManager::check: parameter", i, " mismatch - in ", prefix); + WOLF_ERROR("SolverCeres::check: parameter", i, " mismatch - in ", prefix); ok = false; } i++; @@ -505,7 +489,7 @@ void insertSparseBlock(const Eigen::MatrixXd& ins, Eigen::SparseMatrixd& origina original.makeCompressed(); } -const Eigen::SparseMatrixd CeresManager::computeHessian() const +const Eigen::SparseMatrixd SolverCeres::computeHessian() const { Eigen::SparseMatrixd H; Eigen::SparseMatrixd A; @@ -570,6 +554,6 @@ const Eigen::SparseMatrixd CeresManager::computeHessian() const } // namespace wolf #include "core/solver/factory_solver.h" namespace wolf { - WOLF_REGISTER_SOLVER(CeresManager) + WOLF_REGISTER_SOLVER(SolverCeres) } // namespace wolf diff --git a/src/solver_suitesparse/solver_manager.cpp b/src/solver_suitesparse/solver_manager.cpp index 908a2f4aabcddc90248ab99261c7d5824c54c826..f94353d93dff9a0425e984372dde89b0116c8b4d 100644 --- a/src/solver_suitesparse/solver_manager.cpp +++ b/src/solver_suitesparse/solver_manager.cpp @@ -1,4 +1,4 @@ -#include "core/ceres_wrapper/ceres_manager.h" +#include "../../include/core/ceres_wrapper/solver_ceres.h" SolverManager::SolverManager() { diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 05717bad197c33e241146a8cf4d08fc337d99ea2..96656415449da2d8b229f67d11e76529c34764d3 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -165,12 +165,6 @@ target_link_libraries(gtest_tree_manager ${PROJECT_NAME}) # ------- Now Derived classes ---------- -IF (Ceres_FOUND) -# CeresManager test -wolf_add_gtest(gtest_ceres_manager gtest_ceres_manager.cpp) -target_link_libraries(gtest_ceres_manager ${PROJECT_NAME}) -ENDIF(Ceres_FOUND) - # FactorAbs(P/O/V) classes test wolf_add_gtest(gtest_factor_absolute gtest_factor_absolute.cpp) target_link_libraries(gtest_factor_absolute ${PROJECT_NAME}) @@ -255,6 +249,12 @@ target_link_libraries(gtest_processor_tracker_landmark_dummy ${PROJECT_NAME} dum wolf_add_gtest(gtest_sensor_diff_drive gtest_sensor_diff_drive.cpp) target_link_libraries(gtest_sensor_diff_drive ${PROJECT_NAME}) +IF (Ceres_FOUND) +# SolverCeres test +wolf_add_gtest(gtest_solver_ceres gtest_solver_ceres.cpp) +target_link_libraries(gtest_solver_ceres ${PROJECT_NAME}) +ENDIF(Ceres_FOUND) + # TreeManagerSlidingWindow class test wolf_add_gtest(gtest_tree_manager_sliding_window gtest_tree_manager_sliding_window.cpp) target_link_libraries(gtest_tree_manager_sliding_window ${PROJECT_NAME}) diff --git a/test/gtest_factor_absolute.cpp b/test/gtest_factor_absolute.cpp index 82b56da523c70dff134908ba3e63adf97493d914..9e19dce8b5fad3d950461a74f13d4226036c704f 100644 --- a/test/gtest_factor_absolute.cpp +++ b/test/gtest_factor_absolute.cpp @@ -5,12 +5,12 @@ * \author: datchuth */ +#include "../include/core/ceres_wrapper/solver_ceres.h" #include "core/utils/utils_gtest.h" #include "core/factor/factor_block_absolute.h" #include "core/factor/factor_quaternion_absolute.h" #include "core/capture/capture_motion.h" -#include "core/ceres_wrapper/ceres_manager.h" using namespace Eigen; using namespace wolf; @@ -32,7 +32,7 @@ Vector10d x0 = pose9toPose10(pose + Vector9d::Random()*0.25); // Problem and solver ProblemPtr problem_ptr = Problem::create("POV", 3); -CeresManager ceres_mgr(problem_ptr); +SolverCeres solver(problem_ptr); // Two frames FrameBasePtr frm0 = problem_ptr->emplaceFrame(KEY, 0, problem_ptr->stateZero() ); @@ -59,7 +59,7 @@ TEST(FactorBlockAbs, fac_block_abs_p) frm0->setState(x0); // solve for frm0 - std::string brief_report = ceres_mgr.solve(wolf::SolverManager::ReportVerbosity::BRIEF); + std::string brief_report = solver.solve(wolf::SolverManager::ReportVerbosity::BRIEF); //only orientation is constrained ASSERT_MATRIX_APPROX(frm0->getP()->getState(), pose10.head<3>(), 1e-6); @@ -75,7 +75,7 @@ TEST(FactorBlockAbs, fac_block_abs_p_tail2) frm0->setState(x0); // solve for frm0 - std::string brief_report = ceres_mgr.solve(wolf::SolverManager::ReportVerbosity::BRIEF); + std::string brief_report = solver.solve(wolf::SolverManager::ReportVerbosity::BRIEF); //only orientation is constrained ASSERT_MATRIX_APPROX(frm0->getP()->getState().tail<2>(), pose10.segment<2>(1), 1e-6); @@ -93,7 +93,7 @@ TEST(FactorBlockAbs, fac_block_abs_v) frm0->setState(x0); // solve for frm0 - std::string brief_report = ceres_mgr.solve(wolf::SolverManager::ReportVerbosity::BRIEF); + std::string brief_report = solver.solve(wolf::SolverManager::ReportVerbosity::BRIEF); //only velocity is constrained ASSERT_MATRIX_APPROX(frm0->getV()->getState(), pose10.tail<3>(), 1e-6); @@ -111,7 +111,7 @@ TEST(FactorQuatAbs, fac_block_abs_o) frm0->setState(x0); // solve for frm0 - std::string brief_report = ceres_mgr.solve(wolf::SolverManager::ReportVerbosity::BRIEF); + std::string brief_report = solver.solve(wolf::SolverManager::ReportVerbosity::BRIEF); //only velocity is constrained ASSERT_MATRIX_APPROX(frm0->getO()->getState(), pose10.segment<4>(3), 1e-6); diff --git a/test/gtest_factor_autodiff_distance_3d.cpp b/test/gtest_factor_autodiff_distance_3d.cpp index 1a3f492384290e2bb14e8f01af153aa61abc1193..c8f33528b9320d01e281add3bcaf0a74420ec35d 100644 --- a/test/gtest_factor_autodiff_distance_3d.cpp +++ b/test/gtest_factor_autodiff_distance_3d.cpp @@ -5,10 +5,10 @@ * \author: jsola */ +#include "../include/core/ceres_wrapper/solver_ceres.h" #include "core/factor/factor_autodiff_distance_3d.h" #include "core/problem/problem.h" -#include "core/ceres_wrapper/ceres_manager.h" #include "core/math/rotations.h" #include "core/utils/utils_gtest.h" @@ -34,7 +34,7 @@ class FactorAutodiffDistance3d_Test : public testing::Test Matrix1d dist_cov; ProblemPtr problem; - CeresManagerPtr ceres_manager; + SolverCeresPtr solver; void SetUp() override { @@ -53,7 +53,7 @@ class FactorAutodiffDistance3d_Test : public testing::Test dist_cov = Matrix1d(0.01); problem = Problem::create("PO", 3); - ceres_manager = std::make_shared<CeresManager>(problem); + solver = std::make_shared<SolverCeres>(problem); F1 = problem->emplaceFrame (KEY, 1.0, pose1); @@ -96,7 +96,7 @@ TEST_F(FactorAutodiffDistance3d_Test, solve) f2 = FeatureBase::emplace<FeatureBase>(C2, "Dist", Vector1d(measurement), dist_cov); c2 = FactorBase::emplace<FactorAutodiffDistance3d>(f2, f2, F1, nullptr, false, FAC_ACTIVE); - std::string report = ceres_manager->solve(SolverManager::ReportVerbosity::QUIET); + std::string report = solver->solve(SolverManager::ReportVerbosity::QUIET); // Check distance between F1 and F2 positions -- must match the measurement ASSERT_NEAR( (F1->getP()->getState() - F2->getP()->getState()).norm(), measurement, 1e-10); diff --git a/test/gtest_factor_block_difference.cpp b/test/gtest_factor_block_difference.cpp index 738ec794e1d1d7ffc1ed96588207d66488933ffb..2b7c9cb0c3ff6deb8ae9ec0b2279963d46a6a090 100644 --- a/test/gtest_factor_block_difference.cpp +++ b/test/gtest_factor_block_difference.cpp @@ -8,9 +8,9 @@ #include "core/utils/utils_gtest.h" #include <Eigen/Dense> -#include "core/problem/problem.h" -#include <core/ceres_wrapper/ceres_manager.h> +#include "../include/core/ceres_wrapper/solver_ceres.h" +#include "core/problem/problem.h" #include "core/frame/frame_base.h" #include "core/capture/capture_base.h" #include "core/feature/feature_base.h" @@ -30,7 +30,7 @@ class FixtureFactorBlockDifference : public testing::Test { public: ProblemPtr problem_; - CeresManagerPtr ceres_manager_; + SolverCeresPtr solver_; FrameBasePtr KF0_; FrameBasePtr KF1_; CaptureBasePtr Cap_; @@ -43,7 +43,7 @@ class FixtureFactorBlockDifference : public testing::Test { // Problem and solver problem_ = Problem::create("POV", 3); - ceres_manager_ = std::make_shared<CeresManager>(problem_); + solver_ = std::make_shared<SolverCeres>(problem_); TimeStamp t0(0); TimeStamp t1(1); @@ -83,7 +83,7 @@ TEST_F(FixtureFactorBlockDifference, EqualP) KF0_->getP()->setState(Vector3d::Random()); KF1_->getP()->setState(Vector3d::Random()); - std::string report = ceres_manager_->solve(SolverManager::ReportVerbosity::BRIEF); + std::string report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); ASSERT_MATRIX_APPROX(KF1_->getP()->getState() - KF0_->getP()->getState(), zero3, 1e-8); } @@ -97,7 +97,7 @@ TEST_F(FixtureFactorBlockDifference, EqualV) KF0_->getV()->setState(Vector3d::Random()); KF1_->getV()->setState(Vector3d::Random()); - std::string report = ceres_manager_->solve(SolverManager::ReportVerbosity::BRIEF); + std::string report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); ASSERT_MATRIX_APPROX(KF1_->getV()->getState() - KF0_->getV()->getState(), zero3, 1e-8); } @@ -113,7 +113,7 @@ TEST_F(FixtureFactorBlockDifference, DiffP) KF0_->getP()->setState(Vector3d::Random()); KF1_->getP()->setState(Vector3d::Random()); - std::string report = ceres_manager_->solve(SolverManager::ReportVerbosity::BRIEF); + std::string report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); ASSERT_MATRIX_APPROX(KF1_->getP()->getState() - KF0_->getP()->getState(), diff, 1e-8); } @@ -129,7 +129,7 @@ TEST_F(FixtureFactorBlockDifference, DiffV) KF0_->getV()->setState(Vector3d::Random()); KF1_->getV()->setState(Vector3d::Random()); - std::string report = ceres_manager_->solve(SolverManager::ReportVerbosity::BRIEF); + std::string report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); ASSERT_MATRIX_APPROX(KF1_->getV()->getState() - KF0_->getV()->getState(), diff, 1e-8); } @@ -150,7 +150,7 @@ TEST_F(FixtureFactorBlockDifference, DiffPx) KF0_->getP()->setState(Vector3d::Random()); KF1_->getP()->setState(Vector3d::Random()); - std::string report = ceres_manager_->solve(SolverManager::ReportVerbosity::BRIEF); + std::string report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); ASSERT_MATRIX_APPROX(KF1_->getP()->getState().head<1>() - KF0_->getP()->getState().head<1>(), diff.head<1>(), 1e-8); } @@ -170,7 +170,7 @@ TEST_F(FixtureFactorBlockDifference, DiffPxy) KF0_->getP()->setState(Vector3d::Random()); KF1_->getP()->setState(Vector3d::Random()); - std::string report = ceres_manager_->solve(SolverManager::ReportVerbosity::BRIEF); + std::string report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); ASSERT_MATRIX_APPROX(KF1_->getP()->getState().head<2>() - KF0_->getP()->getState().head<2>(), diff, 1e-8); } @@ -191,7 +191,7 @@ TEST_F(FixtureFactorBlockDifference, DiffPyz) KF1_->getP()->setState(Vector3d::Random()); problem_->print(4,true,true,true); - std::string report = ceres_manager_->solve(SolverManager::ReportVerbosity::BRIEF); + std::string report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); std::cout << report << std::endl; problem_->print(4,true,true,true); diff --git a/test/gtest_factor_diff_drive.cpp b/test/gtest_factor_diff_drive.cpp index f2e8e5f6014811ea3b99c2843fa4b0e8e94c5b72..c8338be45a4388c282da32037a27e5b4530e3f02 100644 --- a/test/gtest_factor_diff_drive.cpp +++ b/test/gtest_factor_diff_drive.cpp @@ -5,13 +5,13 @@ * \author: jsola */ +#include "../include/core/ceres_wrapper/solver_ceres.h" #include "core/processor/processor_diff_drive.h" #include "core/sensor/sensor_diff_drive.h" #include "core/utils/utils_gtest.h" #include "core/factor/factor_diff_drive.h" -#include "core/ceres_wrapper/ceres_manager.h" #include "core/frame/frame_base.h" #include "core/capture/capture_motion.h" #include "core/feature/feature_motion.h" @@ -114,7 +114,7 @@ class FactorDiffDriveTest : public testing::Test { public: ProblemPtr problem; - CeresManagerPtr solver; + SolverCeresPtr solver; TrajectoryBasePtr trajectory; ParamsSensorDiffDrivePtr intr; SensorDiffDrivePtr sensor; @@ -137,7 +137,7 @@ class FactorDiffDriveTest : public testing::Test problem = Problem::create("PO", 2); trajectory = problem->getTrajectory(); - solver = std::make_shared<CeresManager>(problem); + solver = std::make_shared<SolverCeres>(problem); // make a sensor first // DO NOT MODIFY THESE VALUES !!! intr = std::make_shared<ParamsSensorDiffDrive>(); @@ -436,7 +436,7 @@ TEST(FactorDiffDrive, preintegrate_and_solve_sensor_intrinsics_gt) */ ProblemPtr problem = Problem::create("PO", 2); - CeresManagerPtr solver = std::make_shared<CeresManager>(problem); + SolverCeresPtr solver = std::make_shared<SolverCeres>(problem); // make a sensor first // DO NOT MODIFY THESE VALUES !!! ParamsSensorDiffDrivePtr intr = std::make_shared<ParamsSensorDiffDrive>(); @@ -569,7 +569,7 @@ TEST(FactorDiffDrive, preintegrate_and_solve_sensor_intrinsics) ProblemPtr problem = Problem::create("PO", 2); - CeresManagerPtr solver = std::make_shared<CeresManager>(problem); + SolverCeresPtr solver = std::make_shared<SolverCeres>(problem); // make a sensor first // DO NOT MODIFY THESE VALUES !!! ParamsSensorDiffDrivePtr intr = std::make_shared<ParamsSensorDiffDrive>(); diff --git a/test/gtest_factor_odom_2d.cpp b/test/gtest_factor_odom_2d.cpp index 8c0b11e3b428cfbfa99c91c29bffe53ceeeb1d92..6230881af471e41fc187ddab8757845471311b50 100644 --- a/test/gtest_factor_odom_2d.cpp +++ b/test/gtest_factor_odom_2d.cpp @@ -1,10 +1,10 @@ +#include "../include/core/ceres_wrapper/solver_ceres.h" #include "core/utils/utils_gtest.h" #include "core/factor/factor_odom_2d.h" #include "core/capture/capture_odom_2d.h" #include "core/math/rotations.h" -#include "core/ceres_wrapper/ceres_manager.h" using namespace Eigen; using namespace wolf; @@ -16,7 +16,7 @@ Matrix3d data_cov = 0.1*Matrix3d::Identity(); // Problem and solver ProblemPtr problem_ptr = Problem::create("PO", 2); -CeresManager ceres_mgr(problem_ptr); +SolverCeres solver(problem_ptr); // Two frames FrameBasePtr frm0 = problem_ptr->emplaceFrame(KEY, TimeStamp(0), Vector3d::Zero()); @@ -62,7 +62,7 @@ TEST(FactorOdom2d, fix_0_solve) frm1->perturb(5); // solve for frm1 - std::string report = ceres_mgr.solve(SolverManager::ReportVerbosity::BRIEF); + std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF); ASSERT_POSE2d_APPROX(frm1->getStateVector(), x1, 1e-6); @@ -103,7 +103,7 @@ TEST(FactorOdom2d, fix_1_solve) frm0->perturb(5); // solve for frm0 - std::string report = ceres_mgr.solve(SolverManager::ReportVerbosity::BRIEF); + std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF); ASSERT_POSE2d_APPROX(frm0->getStateVector(), x0, 1e-6); diff --git a/test/gtest_factor_odom_3d.cpp b/test/gtest_factor_odom_3d.cpp index 8001bbe70f25f774a55cf0c834950529934bdb36..3ac18c88700d5d4169b6b9c6f59b607780b897a1 100644 --- a/test/gtest_factor_odom_3d.cpp +++ b/test/gtest_factor_odom_3d.cpp @@ -5,12 +5,12 @@ * \author: jsola */ +#include "../include/core/ceres_wrapper/solver_ceres.h" #include "core/utils/utils_gtest.h" #include "core/factor/factor_odom_3d.h" #include "core/capture/capture_motion.h" -#include "core/ceres_wrapper/ceres_manager.h" using namespace Eigen; using namespace wolf; @@ -34,7 +34,7 @@ Vector7d x1 = data2delta(data + Vector6d::Random()*0.25); // Problem and solver ProblemPtr problem_ptr = Problem::create("PO", 3); -CeresManager ceres_mgr(problem_ptr); +SolverCeres solver(problem_ptr); // Two frames FrameBasePtr frm0 = problem_ptr->emplaceFrame(KEY, TimeStamp(0), problem_ptr->stateZero()); @@ -66,7 +66,7 @@ TEST(FactorOdom3d, fix_0_solve) frm1->setState(x1); // solve for frm1 - std::string report = ceres_mgr.solve(SolverManager::ReportVerbosity::BRIEF); + std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF); ASSERT_MATRIX_APPROX(frm1->getStateVector(), delta, 1e-6); @@ -80,7 +80,7 @@ TEST(FactorOdom3d, fix_1_solve) frm0->setState(x0); // solve for frm0 - std::string brief_report = ceres_mgr.solve(SolverManager::ReportVerbosity::BRIEF); + std::string brief_report = solver.solve(SolverManager::ReportVerbosity::BRIEF); ASSERT_MATRIX_APPROX(frm0->getStateVector(), problem_ptr->stateZero().vector("PO"), 1e-6); } diff --git a/test/gtest_factor_pose_2d.cpp b/test/gtest_factor_pose_2d.cpp index e3107b7a5f976f5626f22016f64f60695e1fc982..6f9804f9d16518d1eaa349a1359a1f4da153cf3f 100644 --- a/test/gtest_factor_pose_2d.cpp +++ b/test/gtest_factor_pose_2d.cpp @@ -5,12 +5,12 @@ * \author: jsola */ +#include "../include/core/ceres_wrapper/solver_ceres.h" #include "core/factor/factor_pose_2d.h" #include "core/utils/utils_gtest.h" #include "core/capture/capture_motion.h" -#include "core/ceres_wrapper/ceres_manager.h" using namespace Eigen; using namespace wolf; @@ -27,7 +27,7 @@ Vector3d x0 = pose + Vector3d::Random()*0.25; // Problem and solver ProblemPtr problem = Problem::create("PO", 2); -CeresManager ceres_mgr(problem); +SolverCeres solver(problem); // Two frames FrameBasePtr frm0 = problem->emplaceFrame(KEY, TimeStamp(0), problem->stateZero()); @@ -57,7 +57,7 @@ TEST(FactorPose2d, solve) frm0->setState(x0); // solve for frm0 - std::string report = ceres_mgr.solve(SolverManager::ReportVerbosity::QUIET); // 0: nothing, 1: BriefReport, 2: FullReport + std::string report = solver.solve(SolverManager::ReportVerbosity::QUIET); // 0: nothing, 1: BriefReport, 2: FullReport ASSERT_MATRIX_APPROX(frm0->getStateVector(), pose, 1e-6); diff --git a/test/gtest_factor_pose_3d.cpp b/test/gtest_factor_pose_3d.cpp index d57aee85c27b47e41f9610040c3b8bcceb57ad35..d50660e2f0515aa5589d8fb0146e7ca75f8cfa84 100644 --- a/test/gtest_factor_pose_3d.cpp +++ b/test/gtest_factor_pose_3d.cpp @@ -5,12 +5,12 @@ * \author: jsola */ +#include "../include/core/ceres_wrapper/solver_ceres.h" #include "core/factor/factor_pose_3d.h" #include "core/utils/utils_gtest.h" #include "core/capture/capture_motion.h" -#include "core/ceres_wrapper/ceres_manager.h" using namespace Eigen; using namespace wolf; @@ -33,7 +33,7 @@ Vector7d x0 = pose6toPose7(pose + Vector6d::Random()*0.25); // Problem and solver ProblemPtr problem = Problem::create("PO", 3); -CeresManager ceres_mgr(problem); +SolverCeres solver(problem); // Two frames FrameBasePtr frm0 = problem->emplaceFrame(KEY, 0, problem->stateZero() ); @@ -63,7 +63,7 @@ TEST(FactorPose3d, solve) frm0->setState(x0); // solve for frm0 - std::string brief_report = ceres_mgr.solve(SolverManager::ReportVerbosity::FULL); + std::string brief_report = solver.solve(SolverManager::ReportVerbosity::FULL); ASSERT_MATRIX_APPROX(frm0->getStateVector(), pose7, 1e-6); diff --git a/test/gtest_factor_relative_2d_analytic.cpp b/test/gtest_factor_relative_2d_analytic.cpp index c461bbb94743c9ee8b3665650fb00686d979fc20..5650f90618f6e556cd80a0f5cd423e7ac3b722b2 100644 --- a/test/gtest_factor_relative_2d_analytic.cpp +++ b/test/gtest_factor_relative_2d_analytic.cpp @@ -1,10 +1,10 @@ +#include "../include/core/ceres_wrapper/solver_ceres.h" #include "core/utils/utils_gtest.h" #include "core/factor/factor_relative_2d_analytic.h" #include "core/capture/capture_odom_2d.h" #include "core/math/rotations.h" -#include "core/ceres_wrapper/ceres_manager.h" using namespace Eigen; using namespace wolf; @@ -16,7 +16,7 @@ Matrix3d data_cov = 0.1*Matrix3d::Identity(); // Problem and solver ProblemPtr problem_ptr = Problem::create("PO", 2); -CeresManager ceres_mgr(problem_ptr); +SolverCeres solver(problem_ptr); // Two frames FrameBasePtr frm0 = problem_ptr->emplaceFrame(KEY, 0.0, Vector3d::Zero()); @@ -62,7 +62,7 @@ TEST(FactorRelative2dAnalytic, fix_0_solve) frm1->perturb(5); // solve for frm1 - std::string report = ceres_mgr.solve(SolverManager::ReportVerbosity::BRIEF); + std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF); ASSERT_POSE2d_APPROX(frm1->getStateVector(), x1, 1e-6); @@ -103,7 +103,7 @@ TEST(FactorRelative2dAnalytic, fix_1_solve) frm0->perturb(5); // solve for frm0 - std::string report = ceres_mgr.solve(SolverManager::ReportVerbosity::BRIEF); + std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF); ASSERT_POSE2d_APPROX(frm0->getStateVector(), x0, 1e-6); diff --git a/test/gtest_factor_relative_pose_2d_with_extrinsics.cpp b/test/gtest_factor_relative_pose_2d_with_extrinsics.cpp index b9ad5bbe208d981a4ca69aa3eab8eac806f62e14..feee02d63ca843c9f89fd8efc3443fedc40567b1 100644 --- a/test/gtest_factor_relative_pose_2d_with_extrinsics.cpp +++ b/test/gtest_factor_relative_pose_2d_with_extrinsics.cpp @@ -1,3 +1,4 @@ +#include "../include/core/ceres_wrapper/solver_ceres.h" #include "core/utils/utils_gtest.h" #include "core/factor/factor_relative_pose_2d_with_extrinsics.h" @@ -6,7 +7,6 @@ #include "core/processor/processor_odom_2d.h" #include "core/math/rotations.h" -#include "core/ceres_wrapper/ceres_manager.h" using namespace Eigen; using namespace wolf; @@ -20,7 +20,7 @@ Matrix3d data_cov = 0.1*Matrix3d::Identity(); // Problem and solver ProblemPtr problem_ptr = Problem::create("PO", 2); -CeresManager ceres_mgr(problem_ptr); +SolverCeres solver(problem_ptr); // Sensor auto sensor_odom2d = problem_ptr->installSensor("SensorOdom2d", "odom", Vector3d(0,0,0), wolf_root + "/test/yaml/sensor_odom_2d.yaml"); @@ -77,7 +77,7 @@ TEST(FactorRelativePose2dWithExtrinsics, fix_0_solve) frm1->perturb(5); // solve for frm1 - std::string report = ceres_mgr.solve(SolverManager::ReportVerbosity::BRIEF); + std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF); ASSERT_POSE2d_APPROX(frm1->getStateVector(), x1, 1e-6); @@ -125,7 +125,7 @@ TEST(FactorRelativePose2dWithExtrinsics, fix_1_solve) frm0->perturb(5); // solve for frm0 - std::string report = ceres_mgr.solve(SolverManager::ReportVerbosity::BRIEF); + std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF); ASSERT_POSE2d_APPROX(frm0->getStateVector(), x0, 1e-6); @@ -173,7 +173,7 @@ TEST(FactorRelativePose2dWithExtrinsics, extrinsics_p_solve) sensor_odom2d->getP()->perturb(1); // solve for frm0 - std::string report = ceres_mgr.solve(SolverManager::ReportVerbosity::BRIEF); + std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF); ASSERT_POSE2d_APPROX(sensor_odom2d->getStateVector(), xs, 1e-6); @@ -223,7 +223,7 @@ TEST(FactorRelativePose2dWithExtrinsics, extrinsics_o_solve) //std::cout << "Sensor O (perturbed): " << sensor_odom2d->getO()->getState().transpose() << std::endl; // solve for frm0 - std::string report = ceres_mgr.solve(SolverManager::ReportVerbosity::BRIEF); + std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF); ASSERT_POSE2d_APPROX(sensor_odom2d->getStateVector(), xs, 1e-6); diff --git a/test/gtest_has_state_blocks.cpp b/test/gtest_has_state_blocks.cpp index 1369fd5a87dc31f4d0a7af14a929f0d23637b041..334aa59be910d88c2994c37dd324bad360b265b8 100644 --- a/test/gtest_has_state_blocks.cpp +++ b/test/gtest_has_state_blocks.cpp @@ -6,13 +6,13 @@ */ +#include "../include/core/ceres_wrapper/solver_ceres.h" #include "core/utils/utils_gtest.h" #include "core/frame/frame_base.h" #include "core/sensor/sensor_base.h" #include "core/landmark/landmark_base.h" #include "core/state_block/state_quaternion.h" -#include "core/ceres_wrapper/ceres_manager.h" using namespace wolf; @@ -121,7 +121,7 @@ TEST_F(HasStateBlocksTest, Notifications_makeKF_add) TEST_F(HasStateBlocksTest, Add_solve_notify_solve_add) { - CeresManagerPtr solver = std::make_shared<CeresManager>(problem); + SolverCeresPtr solver = std::make_shared<SolverCeres>(problem); Notification n; diff --git a/test/gtest_odom_2d.cpp b/test/gtest_odom_2d.cpp index 8d97fc8d063ed1a08c36b25f259353cc74852f76..62109f6323593cada613758a05e762618d33bd84 100644 --- a/test/gtest_odom_2d.cpp +++ b/test/gtest_odom_2d.cpp @@ -16,7 +16,6 @@ // Wolf includes #include "core/state_block/state_block.h" #include "core/common/wolf.h" -#include "core/ceres_wrapper/ceres_manager.h" #include "core/capture/capture_pose.h" // STL includes @@ -28,6 +27,7 @@ // General includes #include <iostream> #include <iomanip> // std::setprecision +#include "../include/core/ceres_wrapper/solver_ceres.h" using namespace wolf; using namespace Eigen; @@ -125,7 +125,7 @@ TEST(Odom2d, FactorFix_and_FactorOdom2d) Matrix3d delta_cov; delta_cov << .02, 0, 0, 0, .025, .02, 0, .02, .02; ProblemPtr Pr = Problem::create("PO", 2); - CeresManager ceres_manager(Pr); + SolverCeres solver(Pr); // KF0 and absolute prior FrameBasePtr F0 = Pr->setPriorFactor(x0, s0,t0, dt/2); @@ -150,10 +150,10 @@ TEST(Odom2d, FactorFix_and_FactorOdom2d) F0->setState(Vector3d(1,2,3)); F1->setState(Vector3d(2,3,1)); F2->setState(Vector3d(3,1,2)); - std::string report = ceres_manager.solve(SolverManager::ReportVerbosity::FULL); + std::string report = solver.solve(SolverManager::ReportVerbosity::FULL); // std::cout << report << std::endl; - ceres_manager.computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS); + solver.computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS); // show(Pr); Matrix3d P1, P2; @@ -218,14 +218,14 @@ TEST(Odom2d, VoteForKfAndSolve) // KF - * -- * -- KF - * -- * -- KF - * // Ceres wrapper - CeresManager ceres_manager(problem); + SolverCeres solver(problem); // Origin Key Frame auto KF = problem->setPriorFactor(x0, s0, t, dt/2); processor_odom2d->setOrigin(KF); - ceres_manager.solve(SolverManager::ReportVerbosity::BRIEF); - ceres_manager.computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS); + solver.solve(SolverManager::ReportVerbosity::BRIEF); + solver.computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS); // std::cout << "Initial pose : " << problem->getCurrentState().transpose() << std::endl; // std::cout << "Initial covariance : " << std::endl << problem->getLastKeyFrameCovariance() << std::endl; @@ -297,8 +297,8 @@ TEST(Odom2d, VoteForKfAndSolve) } // Solve - std::string report = ceres_manager.solve(SolverManager::ReportVerbosity::BRIEF); - ceres_manager.computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS); + std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF); + solver.computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS); problem->print(4,1,1,1); @@ -358,7 +358,7 @@ TEST(Odom2d, KF_callback) processor_odom2d->setTimeTolerance(dt/2); // Ceres wrapper - CeresManager ceres_manager(problem); + SolverCeres solver(problem); // Origin Key Frame FrameBasePtr keyframe_0 = problem->setPriorFactor(x0, x0_cov, t0, dt/2); @@ -442,9 +442,9 @@ TEST(Odom2d, KF_callback) MotionBuffer key_buffer_n = key_capture_n->getBuffer(); - std::string report = ceres_manager.solve(SolverManager::ReportVerbosity::BRIEF); + std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF); // std::cout << report << std::endl; - ceres_manager.computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS); + solver.computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS); ASSERT_POSE2d_APPROX(problem->getLastKeyFrame()->getStateVector() , integrated_pose_vector[n_split], 1e-6); MatrixXd computed_cov; @@ -479,8 +479,8 @@ TEST(Odom2d, KF_callback) keyframe_1->setState(Vector3d(2,3,1)); keyframe_2->setState(Vector3d(3,1,2)); - report = ceres_manager.solve(SolverManager::ReportVerbosity::BRIEF); - ceres_manager.computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS); + report = solver.solve(SolverManager::ReportVerbosity::BRIEF); + solver.computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS); problem->print(4,1,1,1); diff --git a/test/gtest_param_prior.cpp b/test/gtest_param_prior.cpp index a512601e2b75ed7035f3b70745294ccffe1a9596..5d0d2cbfb209e6acf4396cbf5dad70fddac11285 100644 --- a/test/gtest_param_prior.cpp +++ b/test/gtest_param_prior.cpp @@ -9,15 +9,15 @@ #include "core/problem/problem.h" -#include "core/ceres_wrapper/ceres_manager.h" #include "core/sensor/sensor_odom_3d.h" #include <iostream> +#include "../include/core/ceres_wrapper/solver_ceres.h" using namespace wolf; ProblemPtr problem_ptr = Problem::create("PO", 3); -CeresManagerPtr ceres_mgr_ptr = std::make_shared<CeresManager>(problem_ptr); +SolverCeresPtr solver_ceres = std::make_shared<SolverCeres>(problem_ptr); Eigen::Vector7d initial_extrinsics((Eigen::Vector7d() << 1, 2, 3, 1, 0, 0, 0).finished()); Eigen::Vector7d prior_extrinsics((Eigen::Vector7d() << 10, 20, 30, 0, 0, 0, 1).finished()); Eigen::Vector7d prior2_extrinsics((Eigen::Vector7d() << 100, 200, 300, 0, 0, 0, 1).finished()); @@ -37,7 +37,7 @@ TEST(ParameterPrior, prior_p) { odom_sensor_ptr_->addPriorP(prior_extrinsics.head(3),Eigen::Matrix3d::Identity()); - std::string report = ceres_mgr_ptr->solve(SolverManager::ReportVerbosity::BRIEF); + std::string report = solver_ceres->solve(SolverManager::ReportVerbosity::BRIEF); ASSERT_MATRIX_APPROX(odom_sensor_ptr_->getP()->getState(),prior_extrinsics.head(3),1e-6); } @@ -46,7 +46,7 @@ TEST(ParameterPrior, prior_o) { odom_sensor_ptr_->addPriorO(prior_extrinsics.tail(4),Eigen::Matrix3d::Identity()); - std::string report = ceres_mgr_ptr->solve(SolverManager::ReportVerbosity::BRIEF); + std::string report = solver_ceres->solve(SolverManager::ReportVerbosity::BRIEF); ASSERT_MATRIX_APPROX(odom_sensor_ptr_->getO()->getState(),prior_extrinsics.tail(4),1e-6); } @@ -55,7 +55,7 @@ TEST(ParameterPrior, prior_p_overwrite) { odom_sensor_ptr_->addPriorP(prior2_extrinsics.head(3),Eigen::Matrix3d::Identity()); - std::string report = ceres_mgr_ptr->solve(SolverManager::ReportVerbosity::BRIEF); + std::string report = solver_ceres->solve(SolverManager::ReportVerbosity::BRIEF); ASSERT_MATRIX_APPROX(odom_sensor_ptr_->getP()->getState(),prior2_extrinsics.head(3),1e-6); } @@ -64,7 +64,7 @@ TEST(ParameterPrior, prior_p_segment) { odom_sensor_ptr_->addPriorP(prior_extrinsics.segment(1,2),Eigen::Matrix2d::Identity(),1,2); - std::string report = ceres_mgr_ptr->solve(SolverManager::ReportVerbosity::BRIEF); + std::string report = solver_ceres->solve(SolverManager::ReportVerbosity::BRIEF); ASSERT_MATRIX_APPROX(odom_sensor_ptr_->getP()->getState().segment(1,2),prior_extrinsics.segment(1,2),1e-6); } diff --git a/test/gtest_processor_motion.cpp b/test/gtest_processor_motion.cpp index 410ebdf1faea8175c8e73435670fdb46de85fd9e..85332d277caff7667ac27e76d18be4c1b06ab6a2 100644 --- a/test/gtest_processor_motion.cpp +++ b/test/gtest_processor_motion.cpp @@ -12,7 +12,6 @@ #include "core/sensor/sensor_odom_2d.h" #include "core/processor/processor_odom_2d.h" -#include "core/ceres_wrapper/ceres_manager.h" using namespace Eigen; using namespace wolf; diff --git a/test/gtest_ceres_manager.cpp b/test/gtest_solver_ceres.cpp similarity index 59% rename from test/gtest_ceres_manager.cpp rename to test/gtest_solver_ceres.cpp index 5586332e6eb0470e8ffef1c9b0eb2d9d2fd1b313..7b811e7b970416180132252b1b25462572f19557 100644 --- a/test/gtest_ceres_manager.cpp +++ b/test/gtest_solver_ceres.cpp @@ -1,5 +1,5 @@ /* - * gtest_ceres_manager.cpp + * gtest_solver_ceres.cpp * * Created on: Jun, 2018 * Author: jvallve @@ -15,27 +15,27 @@ #include "core/factor/factor_pose_2d.h" #include "core/factor/factor_quaternion_absolute.h" #include "core/solver/solver_manager.h" -#include "core/ceres_wrapper/ceres_manager.h" #include "core/state_block/local_parametrization_angle.h" #include "core/state_block/local_parametrization_quaternion.h" #include "ceres/ceres.h" #include <iostream> +#include "../include/core/ceres_wrapper/solver_ceres.h" using namespace wolf; using namespace Eigen; -WOLF_PTR_TYPEDEFS(CeresManagerWrapper); -class CeresManagerWrapper : public CeresManager +WOLF_PTR_TYPEDEFS(SolverCeresWrapper); +class SolverCeresWrapper : public SolverCeres { public: - CeresManagerWrapper(const ProblemPtr& wolf_problem) : - CeresManager(wolf_problem) + SolverCeresWrapper(const ProblemPtr& wolf_problem) : + SolverCeres(wolf_problem) { }; - bool isStateBlockRegisteredCeresManager(const StateBlockPtr& st) + bool isStateBlockRegisteredSolverCeres(const StateBlockPtr& st) { return ceres_problem_->HasParameterBlock(SolverManager::getAssociatedMemBlockPtr(st)); }; @@ -79,22 +79,22 @@ class CeresManagerWrapper : public CeresManager }; -TEST(CeresManager, Create) +TEST(SolverCeres, Create) { ProblemPtr P = Problem::create("PO", 2); - CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P); + SolverCeresWrapperPtr solver_ceres = std::make_shared<SolverCeresWrapper>(P); // check double ointers to branches - ASSERT_EQ(P, ceres_manager_ptr->getProblem()); + ASSERT_EQ(P, solver_ceres->getProblem()); // run ceres manager check - ASSERT_TRUE(ceres_manager_ptr->check()); + ASSERT_TRUE(solver_ceres->check()); } -TEST(CeresManager, AddStateBlock) +TEST(SolverCeres, AddStateBlock) { ProblemPtr P = Problem::create("PO", 2); - CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P); + SolverCeresWrapperPtr solver_ceres = std::make_shared<SolverCeresWrapper>(P); // Create State block Vector2d state; state << 1, 2; @@ -104,20 +104,20 @@ TEST(CeresManager, AddStateBlock) P->notifyStateBlock(sb_ptr,ADD); // update solver - ceres_manager_ptr->update(); + solver_ceres->update(); // check stateblock - ASSERT_TRUE(ceres_manager_ptr->isStateBlockRegisteredSolverManager(sb_ptr)); - ASSERT_TRUE(ceres_manager_ptr->isStateBlockRegisteredCeresManager(sb_ptr)); + ASSERT_TRUE(solver_ceres->isStateBlockRegisteredSolverManager(sb_ptr)); + ASSERT_TRUE(solver_ceres->isStateBlockRegisteredSolverCeres(sb_ptr)); // run ceres manager check - ASSERT_TRUE(ceres_manager_ptr->check()); + ASSERT_TRUE(solver_ceres->check()); } -TEST(CeresManager, DoubleAddStateBlock) +TEST(SolverCeres, DoubleAddStateBlock) { ProblemPtr P = Problem::create("PO", 2); - CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P); + SolverCeresWrapperPtr solver_ceres = std::make_shared<SolverCeresWrapper>(P); // Create State block Vector2d state; state << 1, 2; @@ -127,26 +127,26 @@ TEST(CeresManager, DoubleAddStateBlock) P->notifyStateBlock(sb_ptr,ADD); // update solver - ceres_manager_ptr->update(); + solver_ceres->update(); // add stateblock again P->notifyStateBlock(sb_ptr,ADD); // update solver - ceres_manager_ptr->update(); + solver_ceres->update(); // check stateblock - ASSERT_TRUE(ceres_manager_ptr->isStateBlockRegisteredSolverManager(sb_ptr)); - ASSERT_TRUE(ceres_manager_ptr->isStateBlockRegisteredCeresManager(sb_ptr)); + ASSERT_TRUE(solver_ceres->isStateBlockRegisteredSolverManager(sb_ptr)); + ASSERT_TRUE(solver_ceres->isStateBlockRegisteredSolverCeres(sb_ptr)); // run ceres manager check - ASSERT_TRUE(ceres_manager_ptr->check()); + ASSERT_TRUE(solver_ceres->check()); } -TEST(CeresManager, UpdateStateBlock) +TEST(SolverCeres, UpdateStateBlock) { ProblemPtr P = Problem::create("PO", 2); - CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P); + SolverCeresWrapperPtr solver_ceres = std::make_shared<SolverCeresWrapper>(P); // Create State block Vector2d state; state << 1, 2; @@ -156,28 +156,28 @@ TEST(CeresManager, UpdateStateBlock) P->notifyStateBlock(sb_ptr,ADD); // update solver - ceres_manager_ptr->update(); + solver_ceres->update(); // check stateblock unfixed - ASSERT_FALSE(ceres_manager_ptr->isStateBlockFixed(sb_ptr)); + ASSERT_FALSE(solver_ceres->isStateBlockFixed(sb_ptr)); // Fix frame sb_ptr->fix(); // update solver manager - ceres_manager_ptr->update(); + solver_ceres->update(); // check stateblock fixed - ASSERT_TRUE(ceres_manager_ptr->isStateBlockFixed(sb_ptr)); + ASSERT_TRUE(solver_ceres->isStateBlockFixed(sb_ptr)); // run ceres manager check - ASSERT_TRUE(ceres_manager_ptr->check()); + ASSERT_TRUE(solver_ceres->check()); } -TEST(CeresManager, AddUpdateStateBlock) +TEST(SolverCeres, AddUpdateStateBlock) { ProblemPtr P = Problem::create("PO", 2); - CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P); + SolverCeresWrapperPtr solver_ceres = std::make_shared<SolverCeresWrapper>(P); // Create State block Vector2d state; state << 1, 2; @@ -190,21 +190,21 @@ TEST(CeresManager, AddUpdateStateBlock) sb_ptr->fix(); // update solver manager - ceres_manager_ptr->update(); + solver_ceres->update(); // check stateblock fixed - ASSERT_TRUE(ceres_manager_ptr->isStateBlockRegisteredSolverManager(sb_ptr)); - ASSERT_TRUE(ceres_manager_ptr->isStateBlockRegisteredCeresManager(sb_ptr)); - ASSERT_TRUE(ceres_manager_ptr->isStateBlockFixed(sb_ptr)); + ASSERT_TRUE(solver_ceres->isStateBlockRegisteredSolverManager(sb_ptr)); + ASSERT_TRUE(solver_ceres->isStateBlockRegisteredSolverCeres(sb_ptr)); + ASSERT_TRUE(solver_ceres->isStateBlockFixed(sb_ptr)); // run ceres manager check - ASSERT_TRUE(ceres_manager_ptr->check()); + ASSERT_TRUE(solver_ceres->check()); } -TEST(CeresManager, RemoveStateBlock) +TEST(SolverCeres, RemoveStateBlock) { ProblemPtr P = Problem::create("PO", 2); - CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P); + SolverCeresWrapperPtr solver_ceres = std::make_shared<SolverCeresWrapper>(P); // Create State block Vector2d state; state << 1, 2; @@ -214,29 +214,29 @@ TEST(CeresManager, RemoveStateBlock) P->notifyStateBlock(sb_ptr,ADD); // update solver - ceres_manager_ptr->update(); + solver_ceres->update(); - ASSERT_TRUE(ceres_manager_ptr->isStateBlockRegisteredSolverManager(sb_ptr)); - ASSERT_TRUE(ceres_manager_ptr->isStateBlockRegisteredCeresManager(sb_ptr)); + ASSERT_TRUE(solver_ceres->isStateBlockRegisteredSolverManager(sb_ptr)); + ASSERT_TRUE(solver_ceres->isStateBlockRegisteredSolverCeres(sb_ptr)); // remove state_block P->notifyStateBlock(sb_ptr,REMOVE); // update solver - ceres_manager_ptr->update(); + solver_ceres->update(); // check stateblock - ASSERT_FALSE(ceres_manager_ptr->isStateBlockRegisteredSolverManager(sb_ptr)); - ASSERT_EQ(ceres_manager_ptr->numStateBlocks(), 0); + ASSERT_FALSE(solver_ceres->isStateBlockRegisteredSolverManager(sb_ptr)); + ASSERT_EQ(solver_ceres->numStateBlocks(), 0); // run ceres manager check - ASSERT_TRUE(ceres_manager_ptr->check()); + ASSERT_TRUE(solver_ceres->check()); } -TEST(CeresManager, AddRemoveStateBlock) +TEST(SolverCeres, AddRemoveStateBlock) { ProblemPtr P = Problem::create("PO", 2); - CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P); + SolverCeresWrapperPtr solver_ceres = std::make_shared<SolverCeresWrapper>(P); // Create State block Vector2d state; state << 1, 2; @@ -249,20 +249,20 @@ TEST(CeresManager, AddRemoveStateBlock) P->notifyStateBlock(sb_ptr,REMOVE); // update solver - ceres_manager_ptr->update(); + solver_ceres->update(); // check no stateblocks - ASSERT_FALSE(ceres_manager_ptr->isStateBlockRegisteredSolverManager(sb_ptr)); - ASSERT_EQ(ceres_manager_ptr->numStateBlocks(), 0); + ASSERT_FALSE(solver_ceres->isStateBlockRegisteredSolverManager(sb_ptr)); + ASSERT_EQ(solver_ceres->numStateBlocks(), 0); // run ceres manager check - ASSERT_TRUE(ceres_manager_ptr->check()); + ASSERT_TRUE(solver_ceres->check()); } -TEST(CeresManager, RemoveUpdateStateBlock) +TEST(SolverCeres, RemoveUpdateStateBlock) { ProblemPtr P = Problem::create("PO", 2); - CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P); + SolverCeresWrapperPtr solver_ceres = std::make_shared<SolverCeresWrapper>(P); // Create State block Vector2d state; state << 1, 2; @@ -272,22 +272,22 @@ TEST(CeresManager, RemoveUpdateStateBlock) P->notifyStateBlock(sb_ptr,ADD); // update solver - ceres_manager_ptr->update(); + solver_ceres->update(); // remove state_block P->notifyStateBlock(sb_ptr,REMOVE); // update solver - ceres_manager_ptr->update(); + solver_ceres->update(); // run ceres manager check - ASSERT_TRUE(ceres_manager_ptr->check()); + ASSERT_TRUE(solver_ceres->check()); } -TEST(CeresManager, DoubleRemoveStateBlock) +TEST(SolverCeres, DoubleRemoveStateBlock) { ProblemPtr P = Problem::create("PO", 2); - CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P); + SolverCeresWrapperPtr solver_ceres = std::make_shared<SolverCeresWrapper>(P); // Create State block Vector2d state; state << 1, 2; @@ -300,22 +300,22 @@ TEST(CeresManager, DoubleRemoveStateBlock) P->notifyStateBlock(sb_ptr,REMOVE); // update solver - ceres_manager_ptr->update(); + solver_ceres->update(); // remove state_block P->notifyStateBlock(sb_ptr,REMOVE); // update solver manager - ceres_manager_ptr->update(); + solver_ceres->update(); // run ceres manager check - ASSERT_TRUE(ceres_manager_ptr->check()); + ASSERT_TRUE(solver_ceres->check()); } -TEST(CeresManager, AddFactor) +TEST(SolverCeres, AddFactor) { ProblemPtr P = Problem::create("PO", 2); - CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P); + SolverCeresWrapperPtr solver_ceres = std::make_shared<SolverCeresWrapper>(P); // Create (and add) factor point 2d FrameBasePtr F = P->emplaceFrame(KEY, 0, P->stateZero() ); @@ -324,20 +324,20 @@ TEST(CeresManager, AddFactor) FactorPose2dPtr c = FactorBase::emplace<FactorPose2d>(f,f,nullptr,false); // update solver - ceres_manager_ptr->update(); + solver_ceres->update(); // check factor - ASSERT_TRUE(ceres_manager_ptr->isFactorRegistered(c)); - ASSERT_EQ(ceres_manager_ptr->numFactors(), 1); + ASSERT_TRUE(solver_ceres->isFactorRegistered(c)); + ASSERT_EQ(solver_ceres->numFactors(), 1); // run ceres manager check - ASSERT_TRUE(ceres_manager_ptr->check()); + ASSERT_TRUE(solver_ceres->check()); } -TEST(CeresManager, DoubleAddFactor) +TEST(SolverCeres, DoubleAddFactor) { ProblemPtr P = Problem::create("PO", 2); - CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P); + SolverCeresWrapperPtr solver_ceres = std::make_shared<SolverCeresWrapper>(P); // Create (and add) factor point 2d FrameBasePtr F = P->emplaceFrame(KEY, 0, P->stateZero() ); @@ -349,20 +349,20 @@ TEST(CeresManager, DoubleAddFactor) P->notifyFactor(c,ADD); // update solver - ceres_manager_ptr->update(); + solver_ceres->update(); // check factor - ASSERT_TRUE(ceres_manager_ptr->isFactorRegistered(c)); - ASSERT_EQ(ceres_manager_ptr->numFactors(), 1); + ASSERT_TRUE(solver_ceres->isFactorRegistered(c)); + ASSERT_EQ(solver_ceres->numFactors(), 1); // run ceres manager check - ASSERT_TRUE(ceres_manager_ptr->check()); + ASSERT_TRUE(solver_ceres->check()); } -TEST(CeresManager, RemoveFactor) +TEST(SolverCeres, RemoveFactor) { ProblemPtr P = Problem::create("PO", 2); - CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P); + SolverCeresWrapperPtr solver_ceres = std::make_shared<SolverCeresWrapper>(P); // Create (and add) factor point 2d FrameBasePtr F = P->emplaceFrame(KEY, 0, P->stateZero() ); @@ -371,26 +371,26 @@ TEST(CeresManager, RemoveFactor) FactorPose2dPtr c = FactorBase::emplace<FactorPose2d>(f,f,nullptr,false); // update solver - ceres_manager_ptr->update(); + solver_ceres->update(); // remove factor P->notifyFactor(c,REMOVE); // update solver - ceres_manager_ptr->update(); + solver_ceres->update(); // check factor - ASSERT_FALSE(ceres_manager_ptr->isFactorRegistered(c)); - ASSERT_EQ(ceres_manager_ptr->numFactors(), 0); + ASSERT_FALSE(solver_ceres->isFactorRegistered(c)); + ASSERT_EQ(solver_ceres->numFactors(), 0); // run ceres manager check - ASSERT_TRUE(ceres_manager_ptr->check()); + ASSERT_TRUE(solver_ceres->check()); } -TEST(CeresManager, AddRemoveFactor) +TEST(SolverCeres, AddRemoveFactor) { ProblemPtr P = Problem::create("PO", 2); - CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P); + SolverCeresWrapperPtr solver_ceres = std::make_shared<SolverCeresWrapper>(P); // Create (and add) factor point 2d FrameBasePtr F = P->emplaceFrame(KEY, 0, P->stateZero() ); @@ -404,20 +404,20 @@ TEST(CeresManager, AddRemoveFactor) ASSERT_TRUE(P->getFactorNotificationMapSize() == 0); // add+remove = empty // update solver - ceres_manager_ptr->update(); + solver_ceres->update(); // check factor - ASSERT_FALSE(ceres_manager_ptr->isFactorRegistered(c)); - ASSERT_EQ(ceres_manager_ptr->numFactors(), 0); + ASSERT_FALSE(solver_ceres->isFactorRegistered(c)); + ASSERT_EQ(solver_ceres->numFactors(), 0); // run ceres manager check - ASSERT_TRUE(ceres_manager_ptr->check()); + ASSERT_TRUE(solver_ceres->check()); } -TEST(CeresManager, DoubleRemoveFactor) +TEST(SolverCeres, DoubleRemoveFactor) { ProblemPtr P = Problem::create("PO", 2); - CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P); + SolverCeresWrapperPtr solver_ceres = std::make_shared<SolverCeresWrapper>(P); // Create (and add) factor point 2d FrameBasePtr F = P->emplaceFrame(KEY, 0, P->stateZero() ); @@ -426,32 +426,32 @@ TEST(CeresManager, DoubleRemoveFactor) FactorPose2dPtr c = FactorBase::emplace<FactorPose2d>(f,f,nullptr,false); // update solver - ceres_manager_ptr->update(); + solver_ceres->update(); // remove factor P->notifyFactor(c,REMOVE); // update solver - ceres_manager_ptr->update(); + solver_ceres->update(); // remove factor P->notifyFactor(c,REMOVE); // update solver - ceres_manager_ptr->update(); + solver_ceres->update(); // check factor - ASSERT_FALSE(ceres_manager_ptr->isFactorRegistered(c)); - ASSERT_EQ(ceres_manager_ptr->numFactors(), 0); + ASSERT_FALSE(solver_ceres->isFactorRegistered(c)); + ASSERT_EQ(solver_ceres->numFactors(), 0); // run ceres manager check - ASSERT_TRUE(ceres_manager_ptr->check()); + ASSERT_TRUE(solver_ceres->check()); } -TEST(CeresManager, AddStateBlockLocalParam) +TEST(SolverCeres, AddStateBlockLocalParam) { ProblemPtr P = Problem::create("PO", 2); - CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P); + SolverCeresWrapperPtr solver_ceres = std::make_shared<SolverCeresWrapper>(P); // Create State block Vector1d state; state << 1; @@ -465,20 +465,20 @@ TEST(CeresManager, AddStateBlockLocalParam) P->notifyStateBlock(sb_ptr,ADD); // update solver - ceres_manager_ptr->update(); + solver_ceres->update(); // check stateblock - ASSERT_TRUE(ceres_manager_ptr->hasLocalParametrization(sb_ptr)); - ASSERT_TRUE(ceres_manager_ptr->hasThisLocalParametrization(sb_ptr,local_param_ptr)); + ASSERT_TRUE(solver_ceres->hasLocalParametrization(sb_ptr)); + ASSERT_TRUE(solver_ceres->hasThisLocalParametrization(sb_ptr,local_param_ptr)); // run ceres manager check - ASSERT_TRUE(ceres_manager_ptr->check()); + ASSERT_TRUE(solver_ceres->check()); } -TEST(CeresManager, RemoveLocalParam) +TEST(SolverCeres, RemoveLocalParam) { ProblemPtr P = Problem::create("PO", 2); - CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P); + SolverCeresWrapperPtr solver_ceres = std::make_shared<SolverCeresWrapper>(P); // Create State block Vector1d state; state << 1; @@ -492,25 +492,25 @@ TEST(CeresManager, RemoveLocalParam) P->notifyStateBlock(sb_ptr,ADD); // update solver - ceres_manager_ptr->update(); + solver_ceres->update(); // Remove local param sb_ptr->removeLocalParametrization(); // update solver - ceres_manager_ptr->update(); + solver_ceres->update(); // check stateblock - ASSERT_FALSE(ceres_manager_ptr->hasLocalParametrization(sb_ptr)); + ASSERT_FALSE(solver_ceres->hasLocalParametrization(sb_ptr)); // run ceres manager check - ASSERT_TRUE(ceres_manager_ptr->check()); + ASSERT_TRUE(solver_ceres->check()); } -TEST(CeresManager, AddLocalParam) +TEST(SolverCeres, AddLocalParam) { ProblemPtr P = Problem::create("PO", 2); - CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P); + SolverCeresWrapperPtr solver_ceres = std::make_shared<SolverCeresWrapper>(P); // Create State block Vector1d state; state << 1; @@ -520,30 +520,30 @@ TEST(CeresManager, AddLocalParam) P->notifyStateBlock(sb_ptr,ADD); // update solver - ceres_manager_ptr->update(); + solver_ceres->update(); // check stateblock - ASSERT_FALSE(ceres_manager_ptr->hasLocalParametrization(sb_ptr)); + ASSERT_FALSE(solver_ceres->hasLocalParametrization(sb_ptr)); // Local param LocalParametrizationBasePtr local_param_ptr = std::make_shared<LocalParametrizationAngle>(); sb_ptr->setLocalParametrization(local_param_ptr); // update solver - ceres_manager_ptr->update(); + solver_ceres->update(); // check stateblock - ASSERT_TRUE(ceres_manager_ptr->hasLocalParametrization(sb_ptr)); - ASSERT_TRUE(ceres_manager_ptr->hasThisLocalParametrization(sb_ptr,local_param_ptr)); + ASSERT_TRUE(solver_ceres->hasLocalParametrization(sb_ptr)); + ASSERT_TRUE(solver_ceres->hasThisLocalParametrization(sb_ptr,local_param_ptr)); // run ceres manager check - ASSERT_TRUE(ceres_manager_ptr->check()); + ASSERT_TRUE(solver_ceres->check()); } -TEST(CeresManager, FactorsRemoveLocalParam) +TEST(SolverCeres, FactorsRemoveLocalParam) { ProblemPtr P = Problem::create("PO", 3); - CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P); + SolverCeresWrapperPtr solver_ceres = std::make_shared<SolverCeresWrapper>(P); // Create (and add) 2 factors quaternion FrameBasePtr F = P->emplaceFrame(KEY, 0, P->stateZero() ); @@ -553,39 +553,39 @@ TEST(CeresManager, FactorsRemoveLocalParam) FactorQuaternionAbsolutePtr c2 = FactorBase::emplace<FactorQuaternionAbsolute>(f, f, F->getO(),nullptr,false); // update solver - ceres_manager_ptr->update(); + solver_ceres->update(); // check local param - ASSERT_TRUE(ceres_manager_ptr->hasLocalParametrization(F->getO())); - ASSERT_TRUE(ceres_manager_ptr->hasThisLocalParametrization(F->getO(),F->getO()->getLocalParametrization())); + ASSERT_TRUE(solver_ceres->hasLocalParametrization(F->getO())); + ASSERT_TRUE(solver_ceres->hasThisLocalParametrization(F->getO(),F->getO()->getLocalParametrization())); // check factor - ASSERT_TRUE(ceres_manager_ptr->isFactorRegistered(c1)); - ASSERT_TRUE(ceres_manager_ptr->isFactorRegistered(c2)); - ASSERT_EQ(ceres_manager_ptr->numFactors(), 2); + ASSERT_TRUE(solver_ceres->isFactorRegistered(c1)); + ASSERT_TRUE(solver_ceres->isFactorRegistered(c2)); + ASSERT_EQ(solver_ceres->numFactors(), 2); // remove local param F->getO()->removeLocalParametrization(); // update solver - ceres_manager_ptr->update(); + solver_ceres->update(); // check local param - ASSERT_FALSE(ceres_manager_ptr->hasLocalParametrization(F->getO())); + ASSERT_FALSE(solver_ceres->hasLocalParametrization(F->getO())); // check factor - ASSERT_TRUE(ceres_manager_ptr->isFactorRegistered(c1)); - ASSERT_TRUE(ceres_manager_ptr->isFactorRegistered(c2)); - ASSERT_EQ(ceres_manager_ptr->numFactors(), 2); + ASSERT_TRUE(solver_ceres->isFactorRegistered(c1)); + ASSERT_TRUE(solver_ceres->isFactorRegistered(c2)); + ASSERT_EQ(solver_ceres->numFactors(), 2); // run ceres manager check - ASSERT_TRUE(ceres_manager_ptr->check()); + ASSERT_TRUE(solver_ceres->check()); } -TEST(CeresManager, FactorsUpdateLocalParam) +TEST(SolverCeres, FactorsUpdateLocalParam) { ProblemPtr P = Problem::create("PO", 3); - CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P); + SolverCeresWrapperPtr solver_ceres = std::make_shared<SolverCeresWrapper>(P); // Create (and add) 2 factors quaternion FrameBasePtr F = P->emplaceFrame(KEY, 0, P->stateZero() ); @@ -595,35 +595,35 @@ TEST(CeresManager, FactorsUpdateLocalParam) FactorQuaternionAbsolutePtr c2 = FactorBase::emplace<FactorQuaternionAbsolute>(f, f, F->getO(),nullptr,false); // update solver - ceres_manager_ptr->update(); + solver_ceres->update(); // check local param - ASSERT_TRUE(ceres_manager_ptr->hasLocalParametrization(F->getO())); - ASSERT_TRUE(ceres_manager_ptr->hasThisLocalParametrization(F->getO(),F->getO()->getLocalParametrization())); + ASSERT_TRUE(solver_ceres->hasLocalParametrization(F->getO())); + ASSERT_TRUE(solver_ceres->hasThisLocalParametrization(F->getO(),F->getO()->getLocalParametrization())); // check factor - ASSERT_TRUE(ceres_manager_ptr->isFactorRegistered(c1)); - ASSERT_TRUE(ceres_manager_ptr->isFactorRegistered(c2)); - ASSERT_EQ(ceres_manager_ptr->numFactors(), 2); + ASSERT_TRUE(solver_ceres->isFactorRegistered(c1)); + ASSERT_TRUE(solver_ceres->isFactorRegistered(c2)); + ASSERT_EQ(solver_ceres->numFactors(), 2); // remove local param LocalParametrizationBasePtr local_param_ptr = std::make_shared<LocalParametrizationQuaternionGlobal>(); F->getO()->setLocalParametrization(local_param_ptr); // update solver - ceres_manager_ptr->update(); + solver_ceres->update(); // check local param - ASSERT_TRUE(ceres_manager_ptr->hasLocalParametrization(F->getO())); - ASSERT_TRUE(ceres_manager_ptr->hasThisLocalParametrization(F->getO(),local_param_ptr)); + ASSERT_TRUE(solver_ceres->hasLocalParametrization(F->getO())); + ASSERT_TRUE(solver_ceres->hasThisLocalParametrization(F->getO(),local_param_ptr)); // check factor - ASSERT_TRUE(ceres_manager_ptr->isFactorRegistered(c1)); - ASSERT_TRUE(ceres_manager_ptr->isFactorRegistered(c2)); - ASSERT_EQ(ceres_manager_ptr->numFactors(), 2); + ASSERT_TRUE(solver_ceres->isFactorRegistered(c1)); + ASSERT_TRUE(solver_ceres->isFactorRegistered(c2)); + ASSERT_EQ(solver_ceres->numFactors(), 2); // run ceres manager check - ASSERT_TRUE(ceres_manager_ptr->check()); + ASSERT_TRUE(solver_ceres->check()); } int main(int argc, char **argv)