diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index c0019cfcf05886f2350a29c8e6320001cdfa89e7..c8774da932baa28806790fc8b1d8743f5caa4a0a 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -194,6 +194,7 @@ SET(HDRS_BASE
local_parametrization_quaternion.h
local_parametrization_homogeneous.h
logging.h
+ make_unique.h
map_base.h
motion_buffer.h
node_base.h
@@ -202,6 +203,7 @@ SET(HDRS_BASE
processor_base.h
processor_capture_holder.h
processor_factory.h
+ processor_logging.h
processor_loopclosure_base.h
processor_motion.h
processor_tracker.h
@@ -354,11 +356,14 @@ SET(SRCS_DTASSC
data_association/association_nnls.cpp
)
+# Add the solver sub-directory
+add_subdirectory(solver)
+
#optional HDRS and SRCS
IF (Ceres_FOUND)
SET(HDRS_WRAPPER
ceres_wrapper/sparse_utils.h
- ceres_wrapper/solver_manager.h
+ #ceres_wrapper/solver_manager.h
ceres_wrapper/ceres_manager.h
#ceres_wrapper/qr_manager.h
ceres_wrapper/cost_function_wrapper.h
@@ -366,7 +371,7 @@ IF (Ceres_FOUND)
ceres_wrapper/local_parametrization_wrapper.h
)
SET(SRCS_WRAPPER
- ceres_wrapper/solver_manager.cpp
+ #ceres_wrapper/solver_manager.cpp
ceres_wrapper/ceres_manager.cpp
#ceres_wrapper/qr_manager.cpp
ceres_wrapper/local_parametrization_wrapper.cpp
@@ -447,9 +452,9 @@ IF (cereal_FOUND)
ADD_SUBDIRECTORY(serialization/cereal)
ENDIF(cereal_FOUND)
-#IF (Suitesparse_FOUND)
-# ADD_SUBDIRECTORY(solver)
-#ENDIF(Suitesparse_FOUND)
+IF (Suitesparse_FOUND)
+ ADD_SUBDIRECTORY(solver_suitesparse)
+ENDIF(Suitesparse_FOUND)
# LEAVE YAML FILES ALWAYS IN THE LAST POSITION !!
IF(YAMLCPP_FOUND)
@@ -490,6 +495,7 @@ ADD_LIBRARY(${PROJECT_NAME}
${SRCS_SENSOR}
${SRCS_PROCESSOR}
#${SRCS_DTASSC}
+ ${SRCS_SOLVER}
${SRCS_WRAPPER}
)
TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${CMAKE_THREAD_LIBS_INIT})
@@ -550,11 +556,12 @@ INSTALL(FILES ${HDRS_PROCESSOR}
DESTINATION include/iri-algorithms/wolf/processors)
INSTALL(FILES ${HDRS_WRAPPER}
DESTINATION include/iri-algorithms/wolf/ceres_wrapper)
-#INSTALL(FILES ${HDRS_SOLVER}
-# DESTINATION include/iri-algorithms/wolf/solver)
+#INSTALL(FILES ${HDRS_SOLVER_SUITESPARSE}
+# DESTINATION include/iri-algorithms/wolf/solver_suitesparse)
+INSTALL(FILES ${HDRS_SOLVER}
+ DESTINATION include/iri-algorithms/wolf/solver)
INSTALL(FILES ${HDRS_SERIALIZATION}
DESTINATION include/iri-algorithms/wolf/serialization)
-
INSTALL(FILES "${CMAKE_SOURCE_DIR}/cmake_modules/Findwolf.cmake"
DESTINATION "lib/cmake/${PROJECT_NAME}")
diff --git a/src/ceres_wrapper/ceres_manager.cpp b/src/ceres_wrapper/ceres_manager.cpp
index 556e1fa03d02c007ef0b0ed4b09c68e3aa444e42..f37b2377b391564d10a6163ca95d89a5823c480c 100644
--- a/src/ceres_wrapper/ceres_manager.cpp
+++ b/src/ceres_wrapper/ceres_manager.cpp
@@ -3,323 +3,321 @@
#include "../trajectory_base.h"
#include "../map_base.h"
#include "../landmark_base.h"
+#include "../make_unique.h"
namespace wolf {
-CeresManager::CeresManager(ProblemPtr _wolf_problem, const ceres::Solver::Options& _ceres_options) :
- SolverManager(_wolf_problem),
- ceres_options_(_ceres_options)
+CeresManager::CeresManager(ProblemPtr& _wolf_problem,
+ const ceres::Solver::Options& _ceres_options) :
+ SolverManager(_wolf_problem),
+ ceres_options_(_ceres_options)
{
- ceres::Covariance::Options covariance_options;
- #if CERES_VERSION_MINOR >= 13
- covariance_options.algorithm_type = ceres::SPARSE_QR;//ceres::DENSE_SVD;
- covariance_options.sparse_linear_algebra_library_type = ceres::SUITE_SPARSE;
- #elif CERES_VERSION_MINOR >= 10
- covariance_options.algorithm_type = ceres::SUITE_SPARSE_QR;//ceres::DENSE_SVD;
- #else
- covariance_options.algorithm_type = ceres::SPARSE_QR;//ceres::DENSE_SVD;
- #endif
- covariance_options.num_threads = 1;
- covariance_options.apply_loss_function = false;
- //covariance_options.null_space_rank = -1;
- covariance_ = new 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_ = new ceres::Problem(problem_options);
+ ceres::Covariance::Options covariance_options;
+#if CERES_VERSION_MINOR >= 13
+ covariance_options.algorithm_type = ceres::SPARSE_QR;//ceres::DENSE_SVD;
+ covariance_options.sparse_linear_algebra_library_type = ceres::SUITE_SPARSE;
+#elif CERES_VERSION_MINOR >= 10
+ covariance_options.algorithm_type = ceres::SUITE_SPARSE_QR;//ceres::DENSE_SVD;
+#else
+ covariance_options.algorithm_type = ceres::SPARSE_QR;//ceres::DENSE_SVD;
+#endif
+ covariance_options.num_threads = 1;
+ covariance_options.apply_loss_function = false;
+ //covariance_options.null_space_rank = -1;
+ 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()
{
-// std::cout << "ceres residual blocks: " << ceres_problem_->NumResidualBlocks() << std::endl;
-// std::cout << "ceres parameter blocks: " << ceres_problem_->NumParameterBlocks() << std::endl;
- while (!ctr_2_residual_idx_.empty())
- removeConstraint(ctr_2_residual_idx_.begin()->first);
-// std::cout << "all residuals removed! \n";
-
- delete covariance_;
- //std::cout << "covariance deleted! \n";
- delete ceres_problem_;
- //std::cout << "ceres problem deleted! \n";
+ while (!ctr_2_residual_idx_.empty())
+ removeConstraint(ctr_2_residual_idx_.begin()->first);
}
-std::string CeresManager::solve(const unsigned int& _report_level)
+std::string CeresManager::solveImpl(const ReportVerbosity report_level)
{
- //std::cout << "Residual blocks: " << ceres_problem_->NumResidualBlocks() << " Parameter blocks: " << ceres_problem_->NumParameterBlocks() << std::endl;
-
- // update problem
- update();
-
- //std::cout << "After Update: Residual blocks: " << ceres_problem_->NumResidualBlocks() << " Parameter blocks: " << ceres_problem_->NumParameterBlocks() << std::endl;
-
- // run Ceres Solver
- ceres::Solve(ceres_options_, ceres_problem_, &summary_);
- //std::cout << "solved" << std::endl;
-
- //return report
- if (_report_level == 0)
- return std::string();
- else if (_report_level == 1)
- return summary_.BriefReport();
- else if (_report_level == 2)
- return summary_.FullReport();
- else
- throw std::invalid_argument( "Report level should be 0, 1 or 2!" );
-}
-
-void CeresManager::computeCovariances(CovarianceBlocksToBeComputed _blocks)
-{
- //std::cout << "CeresManager: computing covariances..." << std::endl;
+ // run Ceres Solver
+ ceres::Solve(ceres_options_, ceres_problem_.get(), &summary_);
- // update problem
- update();
+ std::string report;
- // CLEAR STORED COVARIANCE BLOCKS IN WOLF PROBLEM
- wolf_problem_->clearCovariance();
+ //return report
+ if (report_level == ReportVerbosity::BRIEF)
+ report = summary_.BriefReport();
+ else if (report_level == ReportVerbosity::FULL)
+ report = summary_.FullReport();
- // CREATE DESIRED COVARIANCES LIST
- std::vector<std::pair<StateBlockPtr, StateBlockPtr>> state_block_pairs;
- std::vector<std::pair<const double*, const double*>> double_pairs;
+ return report;
+}
- switch (_blocks)
+void CeresManager::computeCovariances(const CovarianceBlocksToBeComputed _blocks)
+{
+ // update problem
+ update();
+
+ // CLEAR STORED COVARIANCE BLOCKS IN WOLF PROBLEM
+ wolf_problem_->clearCovariance();
+
+ // CREATE DESIRED COVARIANCES LIST
+ std::vector<std::pair<StateBlockPtr, StateBlockPtr>> state_block_pairs;
+ std::vector<std::pair<const double*, const double*>> double_pairs;
+
+ switch (_blocks)
+ {
+ case CovarianceBlocksToBeComputed::ALL:
+ {
+ // first create a vector containing all state blocks
+ std::vector<StateBlockPtr> all_state_blocks, landmark_state_blocks;
+ //frame state blocks
+ for(auto fr_ptr : wolf_problem_->getTrajectoryPtr()->getFrameList())
+ if (fr_ptr->isKey())
+ for (auto sb : fr_ptr->getStateBlockVec())
+ if (sb)
+ all_state_blocks.push_back(sb);
+
+ // landmark state blocks
+ for(auto l_ptr : wolf_problem_->getMapPtr()->getLandmarkList())
{
- case ALL:
- {
- // first create a vector containing all state blocks
- std::vector<StateBlockPtr> all_state_blocks, landmark_state_blocks, Frame_state_blocks;
- //frame state blocks
- for(auto fr_ptr : wolf_problem_->getTrajectoryPtr()->getFrameList())
- if (fr_ptr->isKey())
- for (auto sb : fr_ptr->getStateBlockVec())
- if (sb)
- all_state_blocks.push_back(sb);
-
- // landmark state blocks
- for(auto l_ptr : wolf_problem_->getMapPtr()->getLandmarkList())
- {
- landmark_state_blocks = l_ptr->getUsedStateBlockVec();
- all_state_blocks.insert(all_state_blocks.end(), landmark_state_blocks.begin(), landmark_state_blocks.end());
- }
- // double loop all against all (without repetitions)
- for (unsigned int i = 0; i < all_state_blocks.size(); i++)
- for (unsigned int j = i; j < all_state_blocks.size(); j++)
- {
- state_block_pairs.emplace_back(all_state_blocks[i],all_state_blocks[j]);
- double_pairs.emplace_back(all_state_blocks[i]->getPtr(),all_state_blocks[j]->getPtr());
- }
- break;
- }
- case ALL_MARGINALS:
+ landmark_state_blocks = l_ptr->getUsedStateBlockVec();
+ all_state_blocks.insert(all_state_blocks.end(), landmark_state_blocks.begin(), landmark_state_blocks.end());
+ }
+ // double loop all against all (without repetitions)
+ for (unsigned int i = 0; i < all_state_blocks.size(); i++)
+ for (unsigned int j = i; j < all_state_blocks.size(); j++)
+ {
+ state_block_pairs.emplace_back(all_state_blocks[i],all_state_blocks[j]);
+ double_pairs.emplace_back(getAssociatedMemBlockPtr(all_state_blocks[i]),
+ getAssociatedMemBlockPtr(all_state_blocks[j]));
+ }
+ break;
+ }
+ case CovarianceBlocksToBeComputed::ALL_MARGINALS:
+ {
+ // first create a vector containing all state blocks
+ for(auto fr_ptr : wolf_problem_->getTrajectoryPtr()->getFrameList())
+ if (fr_ptr->isKey())
+ for (auto sb : fr_ptr->getStateBlockVec())
+ if (sb)
+ for(auto sb2 : fr_ptr->getStateBlockVec())
+ if (sb)
+ {
+ state_block_pairs.emplace_back(sb, sb2);
+ double_pairs.emplace_back(getAssociatedMemBlockPtr(sb), getAssociatedMemBlockPtr(sb2));
+ if (sb == sb2) break;
+ }
+
+ // landmark state blocks
+ for(auto l_ptr : wolf_problem_->getMapPtr()->getLandmarkList())
+ for (auto sb : l_ptr->getUsedStateBlockVec())
+ for(auto sb2 : l_ptr->getUsedStateBlockVec())
{
- // first create a vector containing all state blocks
- for(auto fr_ptr : wolf_problem_->getTrajectoryPtr()->getFrameList())
- if (fr_ptr->isKey())
- for (auto sb : fr_ptr->getStateBlockVec())
- if (sb)
- for(auto sb2 : fr_ptr->getStateBlockVec())
- if (sb)
- {
- state_block_pairs.emplace_back(sb, sb2);
- double_pairs.emplace_back(sb->getPtr(), sb2->getPtr());
- if (sb == sb2) break;
- }
-
- // landmark state blocks
- for(auto l_ptr : wolf_problem_->getMapPtr()->getLandmarkList())
- for (auto sb : l_ptr->getUsedStateBlockVec())
- for(auto sb2 : l_ptr->getUsedStateBlockVec())
- {
- state_block_pairs.emplace_back(sb, sb2);
- double_pairs.emplace_back(sb->getPtr(), sb2->getPtr());
- if (sb == sb2) break;
- }
-// // loop all marginals (PO marginals)
-// for (unsigned int i = 0; 2*i+1 < all_state_blocks.size(); i++)
-// {
-// state_block_pairs.emplace_back(all_state_blocks[2*i],all_state_blocks[2*i]);
-// state_block_pairs.emplace_back(all_state_blocks[2*i],all_state_blocks[2*i+1]);
-// state_block_pairs.emplace_back(all_state_blocks[2*i+1],all_state_blocks[2*i+1]);
-//
-// double_pairs.emplace_back(all_state_blocks[2*i]->getPtr(),all_state_blocks[2*i]->getPtr());
-// double_pairs.emplace_back(all_state_blocks[2*i]->getPtr(),all_state_blocks[2*i+1]->getPtr());
-// double_pairs.emplace_back(all_state_blocks[2*i+1]->getPtr(),all_state_blocks[2*i+1]->getPtr());
-// }
- break;
+ state_block_pairs.emplace_back(sb, sb2);
+ double_pairs.emplace_back(getAssociatedMemBlockPtr(sb), getAssociatedMemBlockPtr(sb2));
+ if (sb == sb2) break;
}
- case ROBOT_LANDMARKS:
+ // // loop all marginals (PO marginals)
+ // for (unsigned int i = 0; 2*i+1 < all_state_blocks.size(); i++)
+ // {
+ // state_block_pairs.emplace_back(all_state_blocks[2*i],all_state_blocks[2*i]);
+ // state_block_pairs.emplace_back(all_state_blocks[2*i],all_state_blocks[2*i+1]);
+ // state_block_pairs.emplace_back(all_state_blocks[2*i+1],all_state_blocks[2*i+1]);
+ //
+ // double_pairs.emplace_back(all_state_blocks[2*i]->getPtr(),all_state_blocks[2*i]->getPtr());
+ // double_pairs.emplace_back(all_state_blocks[2*i]->getPtr(),all_state_blocks[2*i+1]->getPtr());
+ // double_pairs.emplace_back(all_state_blocks[2*i+1]->getPtr(),all_state_blocks[2*i+1]->getPtr());
+ // }
+ break;
+ }
+ case CovarianceBlocksToBeComputed::ROBOT_LANDMARKS:
+ {
+ //robot-robot
+ auto last_key_frame = wolf_problem_->getLastKeyFramePtr();
+
+ state_block_pairs.emplace_back(last_key_frame->getPPtr(), last_key_frame->getPPtr());
+ state_block_pairs.emplace_back(last_key_frame->getPPtr(), last_key_frame->getOPtr());
+ state_block_pairs.emplace_back(last_key_frame->getOPtr(), last_key_frame->getOPtr());
+
+ double_pairs.emplace_back(getAssociatedMemBlockPtr(last_key_frame->getPPtr()),
+ getAssociatedMemBlockPtr(last_key_frame->getPPtr()));
+ double_pairs.emplace_back(getAssociatedMemBlockPtr(last_key_frame->getPPtr()),
+ getAssociatedMemBlockPtr(last_key_frame->getOPtr()));
+ double_pairs.emplace_back(getAssociatedMemBlockPtr(last_key_frame->getOPtr()),
+ getAssociatedMemBlockPtr(last_key_frame->getOPtr()));
+
+ // landmarks
+ std::vector<StateBlockPtr> landmark_state_blocks;
+ for(auto l_ptr : wolf_problem_->getMapPtr()->getLandmarkList())
+ {
+ // load state blocks vector
+ landmark_state_blocks = l_ptr->getUsedStateBlockVec();
+
+ for (auto state_it = landmark_state_blocks.begin(); state_it != landmark_state_blocks.end(); state_it++)
+ {
+ // robot - landmark
+ state_block_pairs.emplace_back(last_key_frame->getPPtr(), *state_it);
+ state_block_pairs.emplace_back(last_key_frame->getOPtr(), *state_it);
+ double_pairs.emplace_back(getAssociatedMemBlockPtr(last_key_frame->getPPtr()),
+ getAssociatedMemBlockPtr((*state_it)));
+ double_pairs.emplace_back(getAssociatedMemBlockPtr(last_key_frame->getOPtr()),
+ getAssociatedMemBlockPtr((*state_it)));
+
+ // landmark marginal
+ for (auto next_state_it = state_it; next_state_it != landmark_state_blocks.end(); next_state_it++)
{
- //robot-robot
- auto last_key_frame = wolf_problem_->getLastKeyFramePtr();
-
- state_block_pairs.emplace_back(last_key_frame->getPPtr(), last_key_frame->getPPtr());
- state_block_pairs.emplace_back(last_key_frame->getPPtr(), last_key_frame->getOPtr());
- state_block_pairs.emplace_back(last_key_frame->getOPtr(), last_key_frame->getOPtr());
-
- double_pairs.emplace_back(last_key_frame->getPPtr()->getPtr(), last_key_frame->getPPtr()->getPtr());
- double_pairs.emplace_back(last_key_frame->getPPtr()->getPtr(), last_key_frame->getOPtr()->getPtr());
- double_pairs.emplace_back(last_key_frame->getOPtr()->getPtr(), last_key_frame->getOPtr()->getPtr());
-
- // landmarks
- std::vector<StateBlockPtr> landmark_state_blocks;
- for(auto l_ptr : wolf_problem_->getMapPtr()->getLandmarkList())
- {
- // load state blocks vector
- landmark_state_blocks = l_ptr->getUsedStateBlockVec();
-
- for (auto state_it = landmark_state_blocks.begin(); state_it != landmark_state_blocks.end(); state_it++)
- {
- // robot - landmark
- state_block_pairs.emplace_back(last_key_frame->getPPtr(), *state_it);
- state_block_pairs.emplace_back(last_key_frame->getOPtr(), *state_it);
- double_pairs.emplace_back(last_key_frame->getPPtr()->getPtr(), (*state_it)->getPtr());
- double_pairs.emplace_back(last_key_frame->getOPtr()->getPtr(), (*state_it)->getPtr());
-
- // landmark marginal
- for (auto next_state_it = state_it; next_state_it != landmark_state_blocks.end(); next_state_it++)
- {
- state_block_pairs.emplace_back(*state_it, *next_state_it);
- double_pairs.emplace_back((*state_it)->getPtr(), (*next_state_it)->getPtr());
- }
- }
- }
- break;
+ state_block_pairs.emplace_back(*state_it, *next_state_it);
+ double_pairs.emplace_back(getAssociatedMemBlockPtr((*state_it)),
+ getAssociatedMemBlockPtr((*next_state_it)));
}
+ }
}
- //std::cout << "pairs... " << double_pairs.size() << std::endl;
-
- // COMPUTE DESIRED COVARIANCES
- if (covariance_->Compute(double_pairs, ceres_problem_))
+ break;
+ }
+ }
+ //std::cout << "pairs... " << double_pairs.size() << std::endl;
+
+ // COMPUTE DESIRED COVARIANCES
+ if (covariance_->Compute(double_pairs, ceres_problem_.get()))
+ {
+ // STORE DESIRED COVARIANCES
+ for (unsigned int i = 0; i < double_pairs.size(); i++)
{
- // STORE DESIRED COVARIANCES
- for (unsigned int i = 0; i < double_pairs.size(); i++)
- {
- Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> cov(state_block_pairs[i].first->getSize(),state_block_pairs[i].second->getSize());
- covariance_->GetCovarianceBlock(double_pairs[i].first, double_pairs[i].second, cov.data());
- wolf_problem_->addCovarianceBlock(state_block_pairs[i].first, state_block_pairs[i].second, cov);
- //std::cout << "getted covariance " << std::endl << cov << std::endl;
- }
+ Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> cov(state_block_pairs[i].first->getSize(),state_block_pairs[i].second->getSize());
+ covariance_->GetCovarianceBlock(double_pairs[i].first, double_pairs[i].second, cov.data());
+ wolf_problem_->addCovarianceBlock(state_block_pairs[i].first, state_block_pairs[i].second, cov);
+ //std::cout << "getted covariance " << std::endl << cov << std::endl;
}
- else
- std::cout << "WARNING: Couldn't compute covariances!" << std::endl;
+ }
+ else
+ std::cout << "WARNING: Couldn't compute covariances!" << std::endl;
}
void CeresManager::computeCovariances(const StateBlockList& st_list)
{
- //std::cout << "CeresManager: computing covariances..." << std::endl;
+ //std::cout << "CeresManager: computing covariances..." << std::endl;
- // update problem
- update();
+ // update problem
+ update();
- // CLEAR STORED COVARIANCE BLOCKS IN WOLF PROBLEM
- wolf_problem_->clearCovariance();
+ // CLEAR STORED COVARIANCE BLOCKS IN WOLF PROBLEM
+ wolf_problem_->clearCovariance();
- // CREATE DESIRED COVARIANCES LIST
- std::vector<std::pair<StateBlockPtr, StateBlockPtr>> state_block_pairs;
- std::vector<std::pair<const double*, const double*>> double_pairs;
+ // CREATE DESIRED COVARIANCES LIST
+ std::vector<std::pair<StateBlockPtr, StateBlockPtr>> state_block_pairs;
+ std::vector<std::pair<const double*, const double*>> double_pairs;
- // double loop all against all (without repetitions)
- for (auto st_it1 = st_list.begin(); st_it1 != st_list.end(); st_it1++)
- for (auto st_it2 = st_it1; st_it2 != st_list.end(); st_it2++)
- {
- state_block_pairs.push_back(std::pair<StateBlockPtr, StateBlockPtr>(*st_it1,*st_it2));
- double_pairs.push_back(std::pair<const double*, const double*>((*st_it1)->getPtr(),(*st_it2)->getPtr()));
- }
+ // double loop all against all (without repetitions)
+ for (auto st_it1 = st_list.begin(); st_it1 != st_list.end(); st_it1++)
+ for (auto st_it2 = st_it1; st_it2 != st_list.end(); st_it2++)
+ {
+ state_block_pairs.emplace_back(*st_it1, *st_it2);
+ double_pairs.emplace_back(getAssociatedMemBlockPtr((*st_it1)),
+ getAssociatedMemBlockPtr((*st_it2)));
+ }
- //std::cout << "pairs... " << double_pairs.size() << std::endl;
+ //std::cout << "pairs... " << double_pairs.size() << std::endl;
- // COMPUTE DESIRED COVARIANCES
- if (covariance_->Compute(double_pairs, ceres_problem_))
- // STORE DESIRED COVARIANCES
- for (unsigned int i = 0; i < double_pairs.size(); i++)
- {
- Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> cov(state_block_pairs[i].first->getSize(),state_block_pairs[i].second->getSize());
- covariance_->GetCovarianceBlock(double_pairs[i].first, double_pairs[i].second, cov.data());
- wolf_problem_->addCovarianceBlock(state_block_pairs[i].first, state_block_pairs[i].second, cov);
- //std::cout << "getted covariance " << std::endl << cov << std::endl;
- }
- else
- std::cout << "WARNING: Couldn't compute covariances!" << std::endl;
+ // COMPUTE DESIRED COVARIANCES
+ if (covariance_->Compute(double_pairs, ceres_problem_.get()))
+ // STORE DESIRED COVARIANCES
+ for (unsigned int i = 0; i < double_pairs.size(); i++)
+ {
+ Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> cov(state_block_pairs[i].first->getSize(),state_block_pairs[i].second->getSize());
+ covariance_->GetCovarianceBlock(double_pairs[i].first, double_pairs[i].second, cov.data());
+ wolf_problem_->addCovarianceBlock(state_block_pairs[i].first, state_block_pairs[i].second, cov);
+ //std::cout << "getted covariance " << std::endl << cov << std::endl;
+ }
+ else
+ std::cout << "WARNING: Couldn't compute covariances!" << std::endl;
}
-void CeresManager::addConstraint(ConstraintBasePtr _ctr_ptr)
+void CeresManager::addConstraint(const ConstraintBasePtr& ctr_ptr)
{
- ctr_2_costfunction_[_ctr_ptr] = createCostFunction(_ctr_ptr);
+ auto cost_func_ptr = createCostFunction(ctr_ptr);
+
+ ctr_2_costfunction_[ctr_ptr] = cost_func_ptr;
-// std::cout << "adding constraint " << _ctr_ptr->id() << std::endl;
-// std::cout << "constraint pointer " << _ctr_ptr << std::endl;
+ std::vector<Scalar*> res_block_mem;
+ res_block_mem.reserve(ctr_ptr->getStateBlockPtrVector().size());
+ for (const StateBlockPtr& st : ctr_ptr->getStateBlockPtrVector())
+ {
+ res_block_mem.emplace_back( getAssociatedMemBlockPtr(st) );
+ }
- if (_ctr_ptr->getApplyLossFunction())
- ctr_2_residual_idx_[_ctr_ptr] = ceres_problem_->AddResidualBlock(ctr_2_costfunction_[_ctr_ptr].get(), new ceres::CauchyLoss(0.5), _ctr_ptr->getStateScalarPtrVector());
- else
- ctr_2_residual_idx_[_ctr_ptr] = ceres_problem_->AddResidualBlock(ctr_2_costfunction_[_ctr_ptr].get(), NULL, _ctr_ptr->getStateScalarPtrVector());
+ auto loss_func_ptr = (ctr_ptr->getApplyLossFunction())?
+ new ceres::CauchyLoss(0.5): nullptr;
- assert(ceres_problem_->NumResidualBlocks() == ctr_2_residual_idx_.size() && "ceres residuals different from wrapper residuals");
+ ctr_2_residual_idx_[ctr_ptr] =
+ ceres_problem_->AddResidualBlock(cost_func_ptr.get(),
+ loss_func_ptr, res_block_mem);
+
+ assert(ceres_problem_->NumResidualBlocks() == ctr_2_residual_idx_.size() && "ceres residuals different from wrapper residuals");
}
-void CeresManager::removeConstraint(ConstraintBasePtr _ctr_ptr)
+void CeresManager::removeConstraint(const ConstraintBasePtr& _ctr_ptr)
{
-// std::cout << "removing constraint " << _ctr_ptr->id() << std::endl;
+ assert(ctr_2_residual_idx_.find(_ctr_ptr) != ctr_2_residual_idx_.end());
- assert(ctr_2_residual_idx_.find(_ctr_ptr) != ctr_2_residual_idx_.end());
- ceres_problem_->RemoveResidualBlock(ctr_2_residual_idx_[_ctr_ptr]);
- ctr_2_residual_idx_.erase(_ctr_ptr);
- ctr_2_costfunction_.erase(_ctr_ptr);
+ ceres_problem_->RemoveResidualBlock(ctr_2_residual_idx_[_ctr_ptr]);
+ ctr_2_residual_idx_.erase(_ctr_ptr);
+ ctr_2_costfunction_.erase(_ctr_ptr);
-// std::cout << "removingremoved!" << std::endl;
- assert(ceres_problem_->NumResidualBlocks() == ctr_2_residual_idx_.size() && "ceres residuals different from wrapper residuals");
+ assert(ceres_problem_->NumResidualBlocks() == ctr_2_residual_idx_.size() && "ceres residuals different from wrapper residuals");
}
-void CeresManager::addStateBlock(StateBlockPtr _st_ptr)
+void CeresManager::addStateBlock(const StateBlockPtr& state_ptr)
{
-// std::cout << "Adding State Block " << _st_ptr->getPtr() << std::endl;
-// std::cout << " size: " << _st_ptr->getSize() << std::endl;
-// std::cout << " vector: " << _st_ptr->getVector().transpose() << std::endl;
+ ceres::LocalParameterization* local_parametrization_ptr = nullptr;
- if (_st_ptr->hasLocalParametrization())
- {
-// std::cout << "Local Parametrization to be added:" << _st_ptr->getLocalParametrizationPtr() << std::endl;
- ceres_problem_->AddParameterBlock(_st_ptr->getPtr(), _st_ptr->getSize(), new LocalParametrizationWrapper(_st_ptr->getLocalParametrizationPtr()));
- }
- else
- {
-// std::cout << "No Local Parametrization to be added" << std::endl;
- ceres_problem_->AddParameterBlock(_st_ptr->getPtr(), _st_ptr->getSize(), nullptr);
- }
+ if (state_ptr->hasLocalParametrization() &&
+ state_blocks_local_param_.find(state_ptr)==state_blocks_local_param_.end())
+ {
+ auto p = state_blocks_local_param_.emplace(
+ state_ptr,
+ std::make_shared<LocalParametrizationWrapper>(state_ptr->getLocalParametrizationPtr()));
+
+ local_parametrization_ptr = p.first->second.get();
+ }
+
+ ceres_problem_->AddParameterBlock(getAssociatedMemBlockPtr(state_ptr),
+ state_ptr->getSize(),
+ local_parametrization_ptr);
}
-void CeresManager::removeStateBlock(StateBlockPtr _st_ptr)
+void CeresManager::removeStateBlock(const StateBlockPtr& state_ptr)
{
- //std::cout << "Removing State Block " << _st_ptr << std::endl;
- assert(_st_ptr);
- ceres_problem_->RemoveParameterBlock(_st_ptr->getPtr());
+ assert(state_ptr);
+ ceres_problem_->RemoveParameterBlock(getAssociatedMemBlockPtr(state_ptr));
}
-void CeresManager::updateStateBlockStatus(StateBlockPtr _st_ptr)
+void CeresManager::updateStateBlockStatus(const StateBlockPtr& state_ptr)
{
- assert(_st_ptr != nullptr);
- if (_st_ptr->isFixed())
- ceres_problem_->SetParameterBlockConstant(_st_ptr->getPtr());
- else
- ceres_problem_->SetParameterBlockVariable(_st_ptr->getPtr());
+ assert(state_ptr != nullptr);
+ if (state_ptr->isFixed())
+ ceres_problem_->SetParameterBlockConstant(getAssociatedMemBlockPtr(state_ptr));
+ else
+ ceres_problem_->SetParameterBlockVariable(getAssociatedMemBlockPtr(state_ptr));
}
-ceres::CostFunctionPtr CeresManager::createCostFunction(ConstraintBasePtr _ctr_ptr)
+ceres::CostFunctionPtr CeresManager::createCostFunction(const ConstraintBasePtr& _ctr_ptr)
{
- assert(_ctr_ptr != nullptr);
- //std::cout << "creating cost function for constraint " << _ctr_ptr->id() << std::endl;
+ assert(_ctr_ptr != nullptr);
- // analitic & autodiff jacobian
- if (_ctr_ptr->getJacobianMethod() == JAC_ANALYTIC || _ctr_ptr->getJacobianMethod() == JAC_AUTO)
- return std::make_shared<CostFunctionWrapper>(_ctr_ptr);
+ // analitic & autodiff jacobian
+ if (_ctr_ptr->getJacobianMethod() == JAC_ANALYTIC || _ctr_ptr->getJacobianMethod() == JAC_AUTO)
+ return std::make_shared<CostFunctionWrapper>(_ctr_ptr);
- // numeric jacobian
- else if (_ctr_ptr->getJacobianMethod() == JAC_NUMERIC)
- return createNumericDiffCostFunction(_ctr_ptr);
+ // numeric jacobian
+ else if (_ctr_ptr->getJacobianMethod() == JAC_NUMERIC)
+ return createNumericDiffCostFunction(_ctr_ptr);
- else
- throw std::invalid_argument( "Bad Jacobian Method!" );
+ else
+ throw std::invalid_argument( "Wrong Jacobian Method!" );
}
} // namespace wolf
diff --git a/src/ceres_wrapper/ceres_manager.h b/src/ceres_wrapper/ceres_manager.h
index c36144c4ed60cb8f63e840475f3e90be138efd1e..2e1e0cb998df70e5ac6e42968edc62a6b326d151 100644
--- a/src/ceres_wrapper/ceres_manager.h
+++ b/src/ceres_wrapper/ceres_manager.h
@@ -7,13 +7,13 @@
#include "glog/logging.h"
//wolf includes
-#include "solver_manager.h"
+#include "../solver/solver_manager.h"
#include "cost_function_wrapper.h"
#include "local_parametrization_wrapper.h"
#include "create_numeric_diff_cost_function.h"
namespace ceres {
- typedef std::shared_ptr<CostFunction> CostFunctionPtr;
+typedef std::shared_ptr<CostFunction> CostFunctionPtr;
}
namespace wolf {
@@ -26,52 +26,60 @@ WOLF_PTR_TYPEDEFS(CeresManager);
class CeresManager : public SolverManager
{
- protected:
- std::map<ConstraintBasePtr, ceres::ResidualBlockId> ctr_2_residual_idx_;
- std::map<ConstraintBasePtr, ceres::CostFunctionPtr> ctr_2_costfunction_;
- ceres::Problem* ceres_problem_;
- ceres::Solver::Options ceres_options_;
- ceres::Covariance* covariance_;
- ceres::Solver::Summary summary_;
+protected:
- public:
- CeresManager(ProblemPtr _wolf_problem, const ceres::Solver::Options& _ceres_options = ceres::Solver::Options());
+ std::map<ConstraintBasePtr, ceres::ResidualBlockId> ctr_2_residual_idx_;
+ std::map<ConstraintBasePtr, ceres::CostFunctionPtr> ctr_2_costfunction_;
- ~CeresManager();
+ std::map<StateBlockPtr, LocalParametrizationWrapperPtr> state_blocks_local_param_;
- virtual std::string solve(const unsigned int& _report_level);
+ ceres::Solver::Options ceres_options_;
+ ceres::Solver::Summary summary_;
+ std::unique_ptr<ceres::Problem> ceres_problem_;
+ std::unique_ptr<ceres::Covariance> covariance_;
- ceres::Solver::Summary getSummary();
+public:
- virtual void computeCovariances(CovarianceBlocksToBeComputed _blocks = ROBOT_LANDMARKS);
+ CeresManager(ProblemPtr& _wolf_problem,
+ const ceres::Solver::Options& _ceres_options
+ = ceres::Solver::Options());
- virtual void computeCovariances(const StateBlockList& st_list);
+ ~CeresManager();
- ceres::Solver::Options& getSolverOptions();
+ ceres::Solver::Summary getSummary();
- private:
+ virtual void computeCovariances(CovarianceBlocksToBeComputed _blocks
+ = CovarianceBlocksToBeComputed::ROBOT_LANDMARKS);
- virtual void addConstraint(ConstraintBasePtr _ctr_ptr);
+ virtual void computeCovariances(const StateBlockList& st_list);
- virtual void removeConstraint(ConstraintBasePtr _ctr_ptr);
+ ceres::Solver::Options& getSolverOptions();
- virtual void addStateBlock(StateBlockPtr _st_ptr);
+private:
- virtual void removeStateBlock(StateBlockPtr _st_ptr);
+ std::string solveImpl(const ReportVerbosity report_level) override;
- virtual void updateStateBlockStatus(StateBlockPtr _st_ptr);
+ void addConstraint(const ConstraintBasePtr& ctr_ptr) override;
- ceres::CostFunctionPtr createCostFunction(ConstraintBasePtr _ctr_ptr);
+ void removeConstraint(const ConstraintBasePtr& ctr_ptr) override;
+
+ void addStateBlock(const StateBlockPtr& state_ptr) override;
+
+ void removeStateBlock(const StateBlockPtr& state_ptr) override;
+
+ void updateStateBlockStatus(const StateBlockPtr& state_ptr) override;
+
+ ceres::CostFunctionPtr createCostFunction(const ConstraintBasePtr& _ctr_ptr);
};
inline ceres::Solver::Summary CeresManager::getSummary()
{
- return summary_;
+ return summary_;
}
inline ceres::Solver::Options& CeresManager::getSolverOptions()
{
- return ceres_options_;
+ return ceres_options_;
}
} // namespace wolf
diff --git a/src/ceres_wrapper/local_parametrization_wrapper.h b/src/ceres_wrapper/local_parametrization_wrapper.h
index d81b2b3d4476bbd31c687d727f2eb89627b03692..1d81970a963c374cf91a5b283cf443774cb05729 100644
--- a/src/ceres_wrapper/local_parametrization_wrapper.h
+++ b/src/ceres_wrapper/local_parametrization_wrapper.h
@@ -21,7 +21,7 @@ class LocalParametrizationWrapper : public ceres::LocalParameterization
LocalParametrizationWrapper(LocalParametrizationBasePtr _local_parametrization_ptr);
- virtual ~LocalParametrizationWrapper();
+ virtual ~LocalParametrizationWrapper() = default;
virtual bool Plus(const double* x_raw, const double* delta_raw, double* x_plus_delta_raw) const;
@@ -32,6 +32,8 @@ class LocalParametrizationWrapper : public ceres::LocalParameterization
virtual int LocalSize() const;
};
+using LocalParametrizationWrapperPtr = std::shared_ptr<LocalParametrizationWrapper>;
+
} // namespace wolf
#include "../local_parametrization_base.h"
@@ -43,10 +45,6 @@ inline LocalParametrizationWrapper::LocalParametrizationWrapper(LocalParametriza
{
}
-inline LocalParametrizationWrapper::~LocalParametrizationWrapper()
-{
-}
-
inline int LocalParametrizationWrapper::GlobalSize() const
{
return local_parametrization_ptr_->getGlobalSize();
diff --git a/src/constraint_AHP.h b/src/constraint_AHP.h
index 73c84a9570db2bf4a7b2a5fe7e0742801f05006c..e7acfc0a354713bc2f408effb4907f6576a15d58 100644
--- a/src/constraint_AHP.h
+++ b/src/constraint_AHP.h
@@ -92,14 +92,15 @@ inline Eigen::VectorXs ConstraintAHP::expectation() const
FrameBasePtr frm_anchor = getFrameOtherPtr();
LandmarkBasePtr lmk = getLandmarkOtherPtr();
- const Scalar* const frame_current_pos = frm_current->getPPtr()->getPtr();
- const Scalar* const frame_current_ori = frm_current->getOPtr()->getPtr();
- const Scalar* const frame_anchor_pos = frm_anchor ->getPPtr()->getPtr();
- const Scalar* const frame_anchor_ori = frm_anchor ->getOPtr()->getPtr();
- const Scalar* const lmk_pos_hmg = lmk ->getPPtr()->getPtr();
+ const Eigen::MatrixXs frame_current_pos = frm_current->getPPtr()->getState();
+ const Eigen::MatrixXs frame_current_ori = frm_current->getOPtr()->getState();
+ const Eigen::MatrixXs frame_anchor_pos = frm_anchor ->getPPtr()->getState();
+ const Eigen::MatrixXs frame_anchor_ori = frm_anchor ->getOPtr()->getState();
+ const Eigen::MatrixXs lmk_pos_hmg = lmk ->getPPtr()->getState();
Eigen::Vector2s exp;
- expectation(frame_current_pos, frame_current_ori, frame_anchor_pos, frame_anchor_ori, lmk_pos_hmg, exp.data());
+ expectation(frame_current_pos.data(), frame_current_ori.data(), frame_anchor_pos.data(),
+ frame_anchor_ori.data(), lmk_pos_hmg.data(), exp.data());
return exp;
}
@@ -138,7 +139,7 @@ inline void ConstraintAHP::expectation(const T* const _current_frame_p,
// current robot to current camera transform
CaptureBasePtr current_capture = this->getFeaturePtr()->getCapturePtr();
Translation<T, 3> t_r1_c1 (current_capture->getSensorPPtr()->getState().cast<T>());
- Quaternions q_r1_c1_s(current_capture->getSensorOPtr()->getPtr());
+ Quaternions q_r1_c1_s(Eigen::Vector4s(current_capture->getSensorOPtr()->getState()));
Quaternion<T> q_r1_c1 = q_r1_c1_s.cast<T>();
TransformType T_R1_C1 = t_r1_c1 * q_r1_c1;
diff --git a/src/constraint_analytic.cpp b/src/constraint_analytic.cpp
index c2b0268e3f819162050a7404346a9a70ce388642..8b0422380bc4ad0d39f2e7f38955ea3b0bd948a8 100644
--- a/src/constraint_analytic.cpp
+++ b/src/constraint_analytic.cpp
@@ -29,20 +29,18 @@ ConstraintAnalytic::ConstraintAnalytic(const std::string& _tp,
{
resizeVectors();
}
-
-
-std::vector<Scalar*> ConstraintAnalytic::getStateScalarPtrVector() const
+/*
+ConstraintAnalytic::ConstraintAnalytic(ConstraintType _tp, const LandmarkBasePtr& _landmark_ptr, const ProcessorBasePtr& _processor_ptr,
+ 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, nullptr, nullptr, _landmark_ptr, _processor_ptr, _apply_loss_function, _status),
+ state_ptr_vector_({_state0Ptr, _state1Ptr, _state2Ptr, _state3Ptr, _state4Ptr,
+ _state5Ptr, _state6Ptr, _state7Ptr, _state8Ptr, _state9Ptr})
{
- assert(state_ptr_vector_.size() > 0 && state_ptr_vector_.size() <= 10 && "Wrong state vector size in constraint, it should be between 1 and 10");
-
- std::vector<Scalar*> state_scalar_ptr_vector(state_ptr_vector_.size());
-
- for (auto i = 0; i < state_scalar_ptr_vector.size(); i++)
- state_scalar_ptr_vector[i] = state_ptr_vector_[i]->getPtr();
-
- return state_scalar_ptr_vector;
+ resizeVectors();
}
-
+*/
std::vector<StateBlockPtr> ConstraintAnalytic::getStateBlockPtrVector() const
{
return state_ptr_vector_;
diff --git a/src/constraint_analytic.h b/src/constraint_analytic.h
index 9384afd71f23b46773dc59d1166830ce6a6d0272..5fb1495bfbb8963c8e1f0252aaac1a75719821d8 100644
--- a/src/constraint_analytic.h
+++ b/src/constraint_analytic.h
@@ -60,13 +60,6 @@ class ConstraintAnalytic: public ConstraintBase
virtual ~ConstraintAnalytic() = default;
- /** \brief Returns a vector of pointers to the state blocks
- *
- * Returns a vector of pointers to the state blocks in which this constraint depends
- *
- **/
- virtual std::vector<Scalar*> getStateScalarPtrVector() const override;
-
/** \brief Returns a vector of pointers to the states
*
* Returns a vector of pointers to the state in which this constraint depends
diff --git a/src/constraint_autodiff.h b/src/constraint_autodiff.h
index 7e6f227a44da539cb80a2d4a97f1e114372b93c5..09472aa90b31d8b1f000b6db7de1aaa65fcd442f 100644
--- a/src/constraint_autodiff.h
+++ b/src/constraint_autodiff.h
@@ -270,26 +270,6 @@ class ConstraintAutodiff : public ConstraintBase
// std::cout << jacobians_[i] << std::endl << std::endl;
}
- /** \brief Returns a vector of pointers to the state blocks
- *
- * Returns a vector of pointers to the state blocks in which this constraint depends
- *
- **/
- virtual std::vector<Scalar*> getStateScalarPtrVector() const
- {
- return std::vector<Scalar*>({state_ptrs_[0]->getPtr(),
- state_ptrs_[1]->getPtr(),
- state_ptrs_[2]->getPtr(),
- state_ptrs_[3]->getPtr(),
- state_ptrs_[4]->getPtr(),
- state_ptrs_[5]->getPtr(),
- state_ptrs_[6]->getPtr(),
- state_ptrs_[7]->getPtr(),
- state_ptrs_[8]->getPtr(),
- state_ptrs_[9]->getPtr()
- });
- }
-
/** \brief Returns a vector of pointers to the states
*
* Returns a vector of pointers to the state in which this constraint depends
@@ -556,20 +536,6 @@ class ConstraintAutodiff<CtrT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,0> : public Constra
// std::cout << jacobians_[i] << std::endl << std::endl;
}
- virtual std::vector<Scalar*> getStateScalarPtrVector() const
- {
- return std::vector<Scalar*>({state_ptrs_[0]->getPtr(),
- state_ptrs_[1]->getPtr(),
- state_ptrs_[2]->getPtr(),
- state_ptrs_[3]->getPtr(),
- state_ptrs_[4]->getPtr(),
- state_ptrs_[5]->getPtr(),
- state_ptrs_[6]->getPtr(),
- state_ptrs_[7]->getPtr(),
- state_ptrs_[8]->getPtr()
- });
- }
-
virtual std::vector<StateBlockPtr> getStateBlockPtrVector() const
{
return state_ptrs_;
@@ -811,19 +777,6 @@ class ConstraintAutodiff<CtrT,RES,B0,B1,B2,B3,B4,B5,B6,B7,0,0> : public Constrai
// std::cout << jacobians_[i] << std::endl << std::endl;
}
- virtual std::vector<Scalar*> getStateScalarPtrVector() const
- {
- return std::vector<Scalar*>({state_ptrs_[0]->getPtr(),
- state_ptrs_[1]->getPtr(),
- state_ptrs_[2]->getPtr(),
- state_ptrs_[3]->getPtr(),
- state_ptrs_[4]->getPtr(),
- state_ptrs_[5]->getPtr(),
- state_ptrs_[6]->getPtr(),
- state_ptrs_[7]->getPtr()
- });
- }
-
virtual std::vector<StateBlockPtr> getStateBlockPtrVector() const
{
return state_ptrs_;
@@ -1054,18 +1007,6 @@ class ConstraintAutodiff<CtrT,RES,B0,B1,B2,B3,B4,B5,B6,0,0,0> : public Constrain
// std::cout << jacobians_[i] << std::endl << std::endl;
}
- virtual std::vector<Scalar*> getStateScalarPtrVector() const
- {
- return std::vector<Scalar*>({state_ptrs_[0]->getPtr(),
- state_ptrs_[1]->getPtr(),
- state_ptrs_[2]->getPtr(),
- state_ptrs_[3]->getPtr(),
- state_ptrs_[4]->getPtr(),
- state_ptrs_[5]->getPtr(),
- state_ptrs_[6]->getPtr()
- });
- }
-
virtual std::vector<StateBlockPtr> getStateBlockPtrVector() const
{
return state_ptrs_;
@@ -1281,17 +1222,6 @@ class ConstraintAutodiff<CtrT,RES,B0,B1,B2,B3,B4,B5,0,0,0,0> : public Constraint
jacobians_[i].data() + row * state_block_sizes_.at(i));
}
- virtual std::vector<Scalar*> getStateScalarPtrVector() const
- {
- return std::vector<Scalar*>({state_ptrs_[0]->getPtr(),
- state_ptrs_[1]->getPtr(),
- state_ptrs_[2]->getPtr(),
- state_ptrs_[3]->getPtr(),
- state_ptrs_[4]->getPtr(),
- state_ptrs_[5]->getPtr()
- });
- }
-
virtual std::vector<StateBlockPtr> getStateBlockPtrVector() const
{
return state_ptrs_;
@@ -1495,16 +1425,6 @@ class ConstraintAutodiff<CtrT,RES,B0,B1,B2,B3,B4,0,0,0,0,0> : public ConstraintB
jacobians_[i].data() + row * state_block_sizes_.at(i));
}
- virtual std::vector<Scalar*> getStateScalarPtrVector() const
- {
- return std::vector<Scalar*>({state_ptrs_[0]->getPtr(),
- state_ptrs_[1]->getPtr(),
- state_ptrs_[2]->getPtr(),
- state_ptrs_[3]->getPtr(),
- state_ptrs_[4]->getPtr(),
- });
- }
-
virtual std::vector<StateBlockPtr> getStateBlockPtrVector() const
{
return state_ptrs_;
@@ -1701,15 +1621,6 @@ class ConstraintAutodiff<CtrT,RES,B0,B1,B2,B3,0,0,0,0,0,0> : public ConstraintBa
// std::cout << jacobians_[i] << std::endl << std::endl;
}
- virtual std::vector<Scalar*> getStateScalarPtrVector() const
- {
- return std::vector<Scalar*>({state_ptrs_[0]->getPtr(),
- state_ptrs_[1]->getPtr(),
- state_ptrs_[2]->getPtr(),
- state_ptrs_[3]->getPtr()
- });
- }
-
virtual std::vector<StateBlockPtr> getStateBlockPtrVector() const
{
return state_ptrs_;
@@ -1895,14 +1806,6 @@ class ConstraintAutodiff<CtrT,RES,B0,B1,B2,0,0,0,0,0,0,0> : public ConstraintBas
// std::cout << jacobians_[i] << std::endl << std::endl;
}
- virtual std::vector<Scalar*> getStateScalarPtrVector() const
- {
- return std::vector<Scalar*>({state_ptrs_[0]->getPtr(),
- state_ptrs_[1]->getPtr(),
- state_ptrs_[2]->getPtr()
- });
- }
-
virtual std::vector<StateBlockPtr> getStateBlockPtrVector() const
{
return state_ptrs_;
@@ -2077,13 +1980,6 @@ class ConstraintAutodiff<CtrT,RES,B0,B1,0,0,0,0,0,0,0,0> : public ConstraintBase
// std::cout << jacobians_[i] << std::endl << std::endl;
}
- virtual std::vector<Scalar*> getStateScalarPtrVector() const
- {
- return std::vector<Scalar*>({state_ptrs_[0]->getPtr(),
- state_ptrs_[1]->getPtr()
- });
- }
-
virtual std::vector<StateBlockPtr> getStateBlockPtrVector() const
{
return state_ptrs_;
@@ -2247,12 +2143,6 @@ class ConstraintAutodiff<CtrT,RES,B0,0,0,0,0,0,0,0,0,0> : public ConstraintBase
// std::cout << jacobians_[i] << std::endl << std::endl;
}
- virtual std::vector<Scalar*> getStateScalarPtrVector() const
- {
- return std::vector<Scalar*>({state_ptrs_[0]->getPtr()
- });
- }
-
virtual std::vector<StateBlockPtr> getStateBlockPtrVector() const
{
return state_ptrs_;
diff --git a/src/constraint_base.h b/src/constraint_base.h
index c5b2a756480a8f2a75ac3c55fcb49971d3a4ae9b..c90ade9153062c8e871ef450452208c50a8dcf72 100644
--- a/src/constraint_base.h
+++ b/src/constraint_base.h
@@ -91,10 +91,6 @@ class ConstraintBase : public NodeBase, public std::enable_shared_from_this<Cons
**/
virtual JacobianMethod getJacobianMethod() const = 0;
- /** \brief Returns a vector of scalar pointers to the first element of all state blocks involved in the constraint
- **/
- virtual std::vector<Scalar*> getStateScalarPtrVector() const = 0;
-
/** \brief Returns a vector of pointers to the states in which this constraint depends
**/
virtual std::vector<StateBlockPtr> getStateBlockPtrVector() const = 0;
diff --git a/src/constraint_epipolar.h b/src/constraint_epipolar.h
index 2d7d2a14214be26a6ee19fd858c86a72ea269a6e..262b3bc7858922e57e2e4f14cc453d6ad3c86fde 100644
--- a/src/constraint_epipolar.h
+++ b/src/constraint_epipolar.h
@@ -32,10 +32,6 @@ class ConstraintEpipolar : public ConstraintBase
**/
virtual JacobianMethod getJacobianMethod() const override {return JAC_ANALYTIC;}
- /** \brief Returns a vector of scalar pointers to the first element of all state blocks involved in the constraint
- **/
- virtual std::vector<Scalar*> getStateScalarPtrVector() const override {return std::vector<Scalar*>(0);}
-
/** \brief Returns a vector of pointers to the states in which this constraint depends
**/
virtual std::vector<StateBlockPtr> getStateBlockPtrVector() const override {return std::vector<StateBlockPtr>(0);}
diff --git a/src/constraint_odom_3D.h b/src/constraint_odom_3D.h
index a3d4b7af0ae3419d0a198591750aa174aeb26769..26811e4707fcc90f85a230a98903d3f77c6150cf 100644
--- a/src/constraint_odom_3D.h
+++ b/src/constraint_odom_3D.h
@@ -111,18 +111,32 @@ inline bool ConstraintOdom3D::expectation(const T* const _p_current, const T* co
Eigen::Map<Eigen::Matrix<T,3,1> > expectation_dp(_expectation_dp);
Eigen::Map<Eigen::Quaternion<T> > expectation_dq(_expectation_dq);
-
-// std::cout << "p_current: " << p_current(0) << std::endl << p_current(1) << std::endl << p_current(2) << std::endl;
-// std::cout << "q_current: " << q_current.x() << std::endl << q_current.y() << std::endl << q_current.z() << std::endl << q_current.w() << std::endl;
-// std::cout << "p_past: " << p_past(0) << std::endl << p_past(1) << std::endl << p_past(2) << std::endl;
-// std::cout << "q_past: " << q_past.x() << std::endl << q_past.y() << std::endl << q_past.z() << std::endl << q_past.w() << std::endl;
+// std::cout << "p_current: " << p_current(0) << " "
+// << p_current(1) << " "
+// << p_current(2) << "\n";
+// std::cout << "q_current: " << q_current.coeffs()(0) << " "
+// << q_current.coeffs()(1) << " "
+// << q_current.coeffs()(2) << " "
+// << q_current.coeffs()(3) << "\n";
+// std::cout << "p_past: " << p_past(0) << " "
+// << p_past(1) << " "
+// << p_past(2) << "\n";
+// std::cout << "q_past: " << q_past.coeffs()(0) << " "
+// << q_past.coeffs()(1) << " "
+// << q_past.coeffs()(2) << " "
+// << q_past.coeffs()(3) << "\n";
// estimate motion increment, dp, dq;
expectation_dp = q_past.conjugate() * (p_current - p_past);
- expectation_dq = q_past.conjugate() * q_current;
+ expectation_dq = q_past.conjugate() * q_current;
-// std::cout << "exp_p: " << expectation_dp(0) << std::endl << expectation_dp(1) << std::endl << expectation_dp(2) << std::endl;
-// std::cout << "exp_q: " << expectation(3) << std::endl << expectation(4) << std::endl << expectation(5) << std::endl << expectation(6) << std::endl;
+// std::cout << "exp_p: " << expectation_dp(0) << " "
+// << expectation_dp(1) << " "
+// << expectation_dp(2) << "\n";
+// std::cout << "exp_q: " << expectation_dq.coeffs()(0) << " "
+// << expectation_dq.coeffs()(1) << " "
+// << expectation_dq.coeffs()(2) << " "
+// << expectation_dq.coeffs()(3) << "\n";
return true;
}
@@ -132,18 +146,18 @@ inline Eigen::VectorXs ConstraintOdom3D::expectation() const
Eigen::VectorXs exp(7);
FrameBasePtr frm_current = getFeaturePtr()->getFramePtr();
FrameBasePtr frm_past = getFrameOtherPtr();
- const Scalar * const frame_current_pos = frm_current->getPPtr()->getState().data();
- const Scalar * const frame_current_ori = frm_current->getOPtr()->getState().data();
- const Scalar * const frame_past_pos = frm_past->getPPtr()->getState().data();
- const Scalar * const frame_past_ori = frm_past->getOPtr()->getState().data();
-
-// std::cout << "frame_current_pos: " << frm_current->getPPtr()->getVector().transpose() << std::endl;
-// std::cout << "frame_past_pos: " << frm_past->getPPtr()->getVector().transpose() << std::endl;
-
- expectation(frame_current_pos,
- frame_current_ori,
- frame_past_pos,
- frame_past_ori,
+ const Eigen::VectorXs frame_current_pos = frm_current->getPPtr()->getState();
+ const Eigen::VectorXs frame_current_ori = frm_current->getOPtr()->getState();
+ const Eigen::VectorXs frame_past_pos = frm_past->getPPtr()->getState();
+ const Eigen::VectorXs frame_past_ori = frm_past->getOPtr()->getState();
+
+// std::cout << "frame_current_pos: " << frm_current->getPPtr()->getState().transpose() << std::endl;
+// std::cout << "frame_past_pos: " << frm_past->getPPtr()->getState().transpose() << std::endl;
+
+ expectation(frame_current_pos.data(),
+ frame_current_ori.data(),
+ frame_past_pos.data(),
+ frame_past_ori.data(),
exp.data(),
exp.data()+3);
return exp;
diff --git a/src/examples/test_imuDock.cpp b/src/examples/test_imuDock.cpp
index 903e5c5201658143e776ba998180f7e5aaaffd2e..d68e8dd4391d9733c81c55c8f78d821f140fe700 100644
--- a/src/examples/test_imuDock.cpp
+++ b/src/examples/test_imuDock.cpp
@@ -240,8 +240,8 @@ int main(int argc, char** argv)
// ___Solve + compute covariances___
problem->print(4,0,1,0);
- std::string report = ceres_manager->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport
- ceres_manager->computeCovariances(ALL_MARGINALS);
+ std::string report = ceres_manager->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport
+ ceres_manager->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS);
problem->print(1,0,1,0);
//#################################################### RESULTS PART
diff --git a/src/examples/test_imuDock_autoKFs.cpp b/src/examples/test_imuDock_autoKFs.cpp
index 53f2fd9bdc893a90698293d383760ac9b5c9621d..03677027f7dc687d4e55a303ccbbefe2030a4c3a 100644
--- a/src/examples/test_imuDock_autoKFs.cpp
+++ b/src/examples/test_imuDock_autoKFs.cpp
@@ -249,8 +249,8 @@ int main(int argc, char** argv)
// ___Solve + compute covariances___
problem->print(4,0,1,0);
- std::string report = ceres_manager->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
- ceres_manager->computeCovariances(ALL_MARGINALS);
+ std::string report = ceres_manager->solve(SolverManager::ReportVerbosity::BRIEF);
+ ceres_manager->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS);
problem->print(1,0,1,0);
//#################################################### RESULTS PART
diff --git a/src/examples/test_imuPlateform_Offline.cpp b/src/examples/test_imuPlateform_Offline.cpp
index 644bf155a9d0efc19b7e2221933253a1feafd278..9d834d73e8408b1207a8f48d9225b4ec2f08d989 100644
--- a/src/examples/test_imuPlateform_Offline.cpp
+++ b/src/examples/test_imuPlateform_Offline.cpp
@@ -201,9 +201,9 @@ int main(int argc, char** argv)
origin_KF->getVPtr()->fix();
std::cout << "\t\t\t ______solving______" << std::endl;
- std::string report = ceres_manager_wolf_diff->solve(2);// 0: nothing, 1: BriefReport, 2: FullReport
+ std::string report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::FULL);// 0: nothing, 1: BriefReport, 2: FullReport
std::cout << report << std::endl;
- ceres_manager_wolf_diff->computeCovariances(ALL);
+ ceres_manager_wolf_diff->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL);
std::cout << "\t\t\t ______solved______" << std::endl;
wolf_problem_ptr_->print(4,1,1,1);
diff --git a/src/examples/test_imu_constrained0.cpp b/src/examples/test_imu_constrained0.cpp
index 2ee096b4e26cbab0aaa44ff31ec9f7ed972ae4a6..87330497c75beb1e0464382fd4ec1437bc848272 100644
--- a/src/examples/test_imu_constrained0.cpp
+++ b/src/examples/test_imu_constrained0.cpp
@@ -259,16 +259,16 @@ int main(int argc, char** argv)
std::cout << "\t\t\t ______solving______" << std::endl;
- std::string report = ceres_manager_wolf_diff->solve(2); //0: nothing, 1: BriefReport, 2: FullReport
+ std::string report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::FULL); //0: nothing, 1: BriefReport, 2: FullReport
std::cout << report << std::endl;
last_KF->getAccBiasPtr()->fix();
last_KF->getGyroBiasPtr()->fix();
std::cout << "\t\t\t solving after fixBias" << std::endl;
- report = ceres_manager_wolf_diff->solve(1); //0: nothing, 1: BriefReport, 2: FullReport
+ report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF); //0: nothing, 1: BriefReport, 2: FullReport
std::cout << report << std::endl;
- ceres_manager_wolf_diff->computeCovariances(ALL);
+ ceres_manager_wolf_diff->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL);
std::cout << "\t\t\t ______solved______" << std::endl;
wolf_problem_ptr_->print(4,1,1,1);
diff --git a/src/examples/test_processor_odom_3D.cpp b/src/examples/test_processor_odom_3D.cpp
index 6cb5e458dd92f40abfb37d26aa601c0242c8f510..8cfdb9d8822347057fe30751058f253b5ea340b9 100644
--- a/src/examples/test_processor_odom_3D.cpp
+++ b/src/examples/test_processor_odom_3D.cpp
@@ -93,7 +93,7 @@ int main (int argc, char** argv)
// frm->setState(problem->zeroState());
// }
// problem->print(1,0,1,0);
- std::string brief_report = ceres_manager.solve(1);// 0, 1 or 2
+ std::string brief_report = ceres_manager.solve(SolverManager::ReportVerbosity::FULL);
std::cout << brief_report << std::endl;
problem->print(1,0,1,0);
diff --git a/src/examples/test_processor_tracker_landmark_image.cpp b/src/examples/test_processor_tracker_landmark_image.cpp
index b9ea3772d7d93882e475263e48903a3d884ed1a2..4060656a2ac762d4bb2ac4904f047cf9ccccd163 100644
--- a/src/examples/test_processor_tracker_landmark_image.cpp
+++ b/src/examples/test_processor_tracker_landmark_image.cpp
@@ -229,7 +229,7 @@ int main(int argc, char** argv)
if (problem->getTrajectoryPtr()->getFrameList().size() > number_of_KFs)
{
number_of_KFs = problem->getTrajectoryPtr()->getFrameList().size();
- std::string summary = ceres_manager.solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
+ std::string summary = ceres_manager.solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport
std::cout << summary << std::endl;
}
diff --git a/src/examples/test_simple_AHP.cpp b/src/examples/test_simple_AHP.cpp
index bc2d27c54fff6bf8aede8e0d127664bde1a662b5..0f4d3f8eb8d7af228db27d3535c808909a780356 100644
--- a/src/examples/test_simple_AHP.cpp
+++ b/src/examples/test_simple_AHP.cpp
@@ -242,7 +242,7 @@ int main(int argc, char** argv)
CeresManager ceres_manager(problem, ceres_options);
- std::string summary = ceres_manager.solve(2);// 0: nothing, 1: BriefReport, 2: FullReport
+ std::string summary = ceres_manager.solve(SolverManager::ReportVerbosity::FULL);// 0: nothing, 1: BriefReport, 2: FullReport
std::cout << summary << std::endl;
// Test of convergence over the lmk params
diff --git a/src/examples/test_sparsification.cpp b/src/examples/test_sparsification.cpp
index bf797869098964ffe95683a7155e8657d6622b4f..ea8113d1bccfc185a9ea69fedbd90f0967969922 100644
--- a/src/examples/test_sparsification.cpp
+++ b/src/examples/test_sparsification.cpp
@@ -291,11 +291,11 @@ int main(int argc, char** argv)
// SOLVE
// solution
- bl_summary = bl_ceres_manager->solve(1);
+ bl_summary = bl_ceres_manager->solve(SolverManager::ReportVerbosity::FULL);
std::cout << bl_summary << std::endl;
// covariance
- bl_ceres_manager->computeCovariances(ALL);//ALL_MARGINALS
+ bl_ceres_manager->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL);//ALL_MARGINALS
// t1 = clock();
diff --git a/src/hello_wolf/hello_wolf.cpp b/src/hello_wolf/hello_wolf.cpp
index 27eb81987b9918e570c7a94420581f18b87b1651..653fdcc9ebd696d9774a97a60cb66d0016500227 100644
--- a/src/hello_wolf/hello_wolf.cpp
+++ b/src/hello_wolf/hello_wolf.cpp
@@ -214,7 +214,7 @@ int main()
// SOLVE with exact initial guess
WOLF_TRACE("======== SOLVE PROBLEM WITH EXACT PRIORS =======")
- std::string report = ceres->solve(2);
+ std::string report = ceres->solve(wolf::SolverManager::ReportVerbosity::FULL);
WOLF_TRACE(report); // should show a very low iteration number (possibly 1)
problem->print(4,1,1,1);
@@ -232,13 +232,13 @@ int main()
// SOLVE again
WOLF_TRACE("======== SOLVE PROBLEM WITH PERTURBED PRIORS =======")
- report = ceres->solve(2);
+ report = ceres->solve(wolf::SolverManager::ReportVerbosity::FULL);
WOLF_TRACE(report); // should show a very high iteration number (more than 10, or than 100!)
problem->print(4,1,1,1);
// GET COVARIANCES of all states
WOLF_TRACE("======== COVARIANCES OF SOLVED PROBLEM =======")
- ceres->computeCovariances(ALL_MARGINALS);
+ ceres->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS);
for (auto kf : problem->getTrajectoryPtr()->getFrameList())
if (kf->isKey())
WOLF_TRACE("KF", kf->id(), "_cov = \n", kf->getCovariance());
diff --git a/src/landmark_container.cpp b/src/landmark_container.cpp
index 0979387a6139c7afb2393d713da2722f04559385..a1ef7ed100e31b72619507bcb4d0663e1d50fe0c 100644
--- a/src/landmark_container.cpp
+++ b/src/landmark_container.cpp
@@ -30,16 +30,17 @@ LandmarkContainer::LandmarkContainer(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr,
M_PI / 4, 3 * M_PI / 4,-3 * M_PI / 4,-M_PI / 4;
// Computing center
- std::cout << "Container constructor: Computing center pose... " << std::endl;
- Eigen::Map<Eigen::Vector2s> container_position(getPPtr()->getPtr());
- Eigen::Map<Eigen::Vector1s> container_orientation(getOPtr()->getPtr());
+ WOLF_DEBUG( "Container constructor: Computing center pose... " );
+ Eigen::Vector2s container_position(getPPtr()->getState());
+ Eigen::Vector1s container_orientation(getOPtr()->getState());
- std::cout << "Container constructor: _corner_1_idx " << _corner_1_idx << "_corner_2_idx " << _corner_2_idx << std::endl;
+ WOLF_DEBUG( "Container constructor: _corner_1_idx ", _corner_1_idx,
+ "_corner_2_idx ", _corner_2_idx );
// Large side detected (A & B)
if ( (_corner_1_idx == 0 && _corner_2_idx == 1) || (_corner_1_idx == 1 && _corner_2_idx == 0) )
{
- std::cout << "Large side detected" << std::endl;
+ WOLF_DEBUG( "Large side detected" );
Eigen::Vector2s AB = (_corner_1_idx == 0 ? _corner_2_pose.head(2) - _corner_1_pose.head(2) : _corner_1_pose.head(2) - _corner_2_pose.head(2));
Eigen::Vector2s perpendicularAB;
perpendicularAB << -AB(1)/AB.norm(), AB(0)/AB.norm();
@@ -50,7 +51,7 @@ LandmarkContainer::LandmarkContainer(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr,
// Short side detected (B & C)
else if ( (_corner_1_idx == 1 && _corner_2_idx == 2) || (_corner_1_idx == 2 && _corner_2_idx == 1) )
{
- std::cout << "Short side detected" << std::endl;
+ WOLF_DEBUG( "Short side detected" );
Eigen::Vector2s BC = (_corner_1_idx == 1 ? _corner_2_pose.head(2) - _corner_1_pose.head(2) : _corner_1_pose.head(2) - _corner_2_pose.head(2));
Eigen::Vector2s perpendicularBC;
perpendicularBC << -BC(1)/BC.norm(), BC(0)/BC.norm();
@@ -62,7 +63,7 @@ LandmarkContainer::LandmarkContainer(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr,
// A & C
else if ( (_corner_1_idx == 0 && _corner_2_idx == 2) || (_corner_1_idx == 2 && _corner_2_idx == 0) )
{
- std::cout << "diagonal AC detected" << std::endl;
+ WOLF_DEBUG( "diagonal AC detected" );
Eigen::Vector2s AC = (_corner_1_idx == 0 ? _corner_2_pose.head(2) - _corner_1_pose.head(2) : _corner_1_pose.head(2) - _corner_2_pose.head(2));
container_position = (_corner_1_idx == 0 ? _corner_1_pose.head(2) : _corner_2_pose.head(2)) + AC / 2;
container_orientation(0) = atan2(AC(1),AC(0)) - atan2(_witdh,_length);
@@ -70,7 +71,7 @@ LandmarkContainer::LandmarkContainer(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr,
// B & D
else if ( (_corner_1_idx == 1 && _corner_2_idx == 3) || (_corner_1_idx == 3 && _corner_2_idx == 1) )
{
- std::cout << "diagonal BD detected" << std::endl;
+ WOLF_DEBUG( "diagonal BD detected" );
Eigen::Vector2s BD = (_corner_1_idx == 1 ? _corner_2_pose.head(2) - _corner_1_pose.head(2) : _corner_1_pose.head(2) - _corner_2_pose.head(2));
container_position = (_corner_1_idx == 1 ? _corner_1_pose.head(2) : _corner_2_pose.head(2)) + BD / 2;
container_orientation(0) = atan2(BD(1),BD(0)) + atan2(_witdh,_length);
@@ -78,9 +79,12 @@ LandmarkContainer::LandmarkContainer(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr,
else
assert(0 && "index corners combination not implemented!");
- std::cout << "_corner_1_pose... " << _corner_1_pose.transpose() << std::endl;
- std::cout << "_corner_2_pose... " << _corner_2_pose.transpose() << std::endl;
- std::cout << "Container center pose... " << container_position.transpose() << " " << container_orientation.transpose() << std::endl;
+ WOLF_DEBUG( "_corner_1_pose... ", _corner_1_pose.transpose() );
+ WOLF_DEBUG( "_corner_2_pose... ", _corner_2_pose.transpose() );
+ WOLF_DEBUG( "Container center pose... ", container_position.transpose(), " ", container_orientation.transpose() );
+
+ getPPtr()->setState(container_position);
+ getOPtr()->setState(container_orientation);
}
//LandmarkContainer::LandmarkContainer(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, LandmarkCorner2D* _corner_A_ptr, LandmarkCorner2D* _corner_B_ptr, LandmarkCorner2D* _corner_C_ptr, LandmarkCorner2D* _corner_D_ptr, const Scalar& _witdh, const Scalar& _length) :
diff --git a/src/landmark_polyline_2D.cpp b/src/landmark_polyline_2D.cpp
index bc1bcc71abedb342e2003498844593db8e538b36..d117754206d53c9a409e5e04e6e92548a0249d12 100644
--- a/src/landmark_polyline_2D.cpp
+++ b/src/landmark_polyline_2D.cpp
@@ -60,7 +60,7 @@ void LandmarkPolyline2D::setLast(const Eigen::VectorXs& _point, bool _defined)
defineExtreme(true);
}
-const Eigen::VectorXs& LandmarkPolyline2D::getPointVector(int _i) const
+const Eigen::VectorXs LandmarkPolyline2D::getPointVector(int _i) const
{
//std::cout << "LandmarkPolyline2D::getPointVector: " << _i << std::endl;
//std::cout << "First: " << first_id_ << " - size: " << point_state_ptr_vector_.size() << std::endl;
diff --git a/src/landmark_polyline_2D.h b/src/landmark_polyline_2D.h
index d2b8fb8f93e92896f85bcdd30b641346a9d1a754..a44a00b4cf5cf3aef2c560693e3232560e01a63a 100644
--- a/src/landmark_polyline_2D.h
+++ b/src/landmark_polyline_2D.h
@@ -72,7 +72,7 @@ class LandmarkPolyline2D : public LandmarkBase
int getFirstId() const;
int getLastId() const;
- const Eigen::VectorXs& getPointVector(int _i) const;
+ const Eigen::VectorXs getPointVector(int _i) const;
StateBlockPtr getPointStateBlockPtr(int _i);
diff --git a/src/make_unique.h b/src/make_unique.h
new file mode 100644
index 0000000000000000000000000000000000000000..f336eb4f7ff52eae0e7014505180bb060f48ee70
--- /dev/null
+++ b/src/make_unique.h
@@ -0,0 +1,24 @@
+/**
+ * \file make_unique.h
+ *
+ * Created on: Oct 11, 2017
+ * \author: Jeremie Deray
+ */
+
+#ifndef _WOLF_MAKE_UNIQUE_H_
+#define _WOLF_MAKE_UNIQUE_H_
+
+#include <memory>
+
+namespace wolf {
+
+/// @note this appears only in cpp14
+template <typename T, typename... Args>
+std::unique_ptr<T> make_unique(Args&&... args)
+{
+ return std::unique_ptr<T>(new T(std::forward<Args>(args)...));
+}
+
+}
+
+#endif /* _WOLF_MAKE_UNIQUE_H_ */
diff --git a/src/problem.cpp b/src/problem.cpp
index 50100be20ff50ec47858026cbbf5ec44f1bb20ad..2bf9ad01fa944fe8ae57dad5a325c550dff4285d 100644
--- a/src/problem.cpp
+++ b/src/problem.cpp
@@ -371,8 +371,13 @@ StateBlockPtr Problem::addStateBlock(StateBlockPtr _state_ptr)
// add the state unit to the list
state_block_list_.push_back(_state_ptr);
+
// queue for solver manager
- state_block_notification_list_.push_back(StateBlockNotification({ADD,_state_ptr}));
+ _state_ptr->addNotification(StateBlock::Notification::ADD);
+ notified_state_block_list_.push_back(_state_ptr);
+ notified_state_block_list_.sort();
+ notified_state_block_list_.unique();
+
return _state_ptr;
}
@@ -381,7 +386,10 @@ void Problem::updateStateBlockPtr(StateBlockPtr _state_ptr)
//std::cout << "Problem::updateStateBlockPtr " << _state_ptr.get() << std::endl;
// queue for solver manager
- state_block_notification_list_.push_back(StateBlockNotification({UPDATE,_state_ptr}));
+ _state_ptr->addNotification(StateBlock::Notification::FIX_UPDATE);
+ notified_state_block_list_.push_back(_state_ptr);
+ notified_state_block_list_.sort();
+ notified_state_block_list_.unique();
}
void Problem::removeStateBlockPtr(StateBlockPtr _state_ptr)
@@ -391,19 +399,11 @@ void Problem::removeStateBlockPtr(StateBlockPtr _state_ptr)
// add the state unit to the list
state_block_list_.remove(_state_ptr);
- // Check if the state addition is still as a notification
- auto state_notif_it = state_block_notification_list_.begin();
- for (; state_notif_it != state_block_notification_list_.end(); state_notif_it++)
- if (state_notif_it->notification_ == ADD && state_notif_it->state_block_ptr_ == _state_ptr)
- break;
-
- // Remove addition notification
- if (state_notif_it != state_block_notification_list_.end())
- state_block_notification_list_.erase(state_notif_it);
// Add remove notification
- else
- state_block_notification_list_.push_back(StateBlockNotification({REMOVE, _state_ptr}));
-
+ _state_ptr->addNotification(StateBlock::Notification::REMOVE);
+ notified_state_block_list_.push_back(_state_ptr);
+ notified_state_block_list_.sort();
+ notified_state_block_list_.unique();
}
ConstraintBasePtr Problem::addConstraintPtr(ConstraintBasePtr _constraint_ptr)
diff --git a/src/problem.h b/src/problem.h
index 53ee4c59b9b4be14213d3a2a3f75ee51ae29cb9e..ac69bd6c37a5d18401344bdc973bc827ae51f72b 100644
--- a/src/problem.h
+++ b/src/problem.h
@@ -29,11 +29,7 @@ enum Notification
REMOVE,
UPDATE
};
-struct StateBlockNotification
-{
- Notification notification_;
- StateBlockPtr state_block_ptr_;
-};
+
struct ConstraintNotification
{
Notification notification_;
@@ -53,10 +49,10 @@ class Problem : public std::enable_shared_from_this<Problem>
ProcessorMotionPtr processor_motion_ptr_;
StateBlockList state_block_list_;
std::map<std::pair<StateBlockPtr, StateBlockPtr>, Eigen::MatrixXs> covariances_;
- std::list<StateBlockNotification> state_block_notification_list_;
std::list<ConstraintNotification> constraint_notification_list_;
bool prior_is_set_;
Size state_size_, state_cov_size_;
+ StateBlockList notified_state_block_list_;
private: // CAUTION: THESE METHODS ARE PRIVATE, DO NOT MAKE THEM PUBLIC !!
Problem(const std::string& _frame_structure); // USE create() below !!
@@ -278,10 +274,9 @@ class Problem : public std::enable_shared_from_this<Problem>
*/
void removeStateBlockPtr(StateBlockPtr _state_ptr);
- /** \brief Gets a queue of state blocks notification to be handled by the solver
+ /** \brief Gets a list of state blocks which state has been changed to be handled by the solver
*/
- std::list<StateBlockNotification>& getStateBlockNotificationList();
-
+ StateBlockList& getNotifiedStateBlockList();
/** \brief Gets a queue of constraint notification to be handled by the solver
*/
@@ -331,16 +326,15 @@ inline ProcessorMotionPtr& Problem::getProcessorMotionPtr()
return processor_motion_ptr_;
}
-inline std::list<StateBlockNotification>& Problem::getStateBlockNotificationList()
-{
- return state_block_notification_list_;
-}
-
inline std::list<ConstraintNotification>& Problem::getConstraintNotificationList()
{
return constraint_notification_list_;
}
+inline StateBlockList& Problem::getNotifiedStateBlockList()
+{
+ return notified_state_block_list_;
+}
} // namespace wolf
diff --git a/src/processor_base.h b/src/processor_base.h
index 73fdc721ebbadc8630f36772282c44db9b45afd6..d63edd93cdf79cb5381531f669fc6e5635bcfb39 100644
--- a/src/processor_base.h
+++ b/src/processor_base.h
@@ -109,10 +109,31 @@ class KFPackBuffer
*/
struct ProcessorParamsBase
{
+ ProcessorParamsBase() = default;
+
+ ProcessorParamsBase(bool _voting_active,
+ Scalar _time_tolerance,
+ const std::string& _type,
+ const std::string& _name)
+ : voting_active(_voting_active)
+ , time_tolerance(_time_tolerance)
+ , type(_type)
+ , name(_name)
+ {
+ //
+ }
+
+ virtual ~ProcessorParamsBase() = default;
+
+ bool voting_active = false;
+
+ ///< maximum time difference between a Keyframe time stamp and
+ /// a particular Capture of this processor to allow assigning
+ /// this Capture to the Keyframe.
+ Scalar time_tolerance = Scalar(0);
+
std::string type;
std::string name;
- Scalar time_tolerance; ///< maximum time difference between a Keyframe time stamp and a particular Capture of this processor to allow assigning this Capture to the Keyframe.
- bool voting_active;
};
//class ProcessorBase
diff --git a/src/solver/CMakeLists.txt b/src/solver/CMakeLists.txt
index 60df425ecf4f7c061b022515ffa4456b457604e7..7263fb5e8ffab31a2b7dce2857fc840f2604b2b7 100644
--- a/src/solver/CMakeLists.txt
+++ b/src/solver/CMakeLists.txt
@@ -1,12 +1,9 @@
-SET(HDRS_SOLVER
- ccolamd_ordering.h
- cost_function_base.h
- cost_function_sparse_base.h
- cost_function_sparse.h
- qr_solver.h
- solver_manager.h
- solver_QR.h
- sparse_utils.h)
-SET(SRCS_SOLVER
- solver_manager.cpp
- )
\ No newline at end of file
+INCLUDE_DIRECTORIES(${CMAKE_CURRENT_SOURCE_DIR})
+
+# Forward var to parent scope
+
+SET(HDRS_SOLVER ${HDRS_SOLVER}
+ ${CMAKE_CURRENT_SOURCE_DIR}/solver_manager.h PARENT_SCOPE)
+
+SET(SRCS_SOLVER ${SRCS_SOLVER}
+ ${CMAKE_CURRENT_SOURCE_DIR}/solver_manager.cpp PARENT_SCOPE)
diff --git a/src/solver/solver_manager.cpp b/src/solver/solver_manager.cpp
index cb39335b0705f8edf6222d78db36a0e18c21d504..b26b977a519983f5d0e06b6cd177682dd78272a4 100644
--- a/src/solver/solver_manager.cpp
+++ b/src/solver/solver_manager.cpp
@@ -1,245 +1,181 @@
-#include "ceres_manager.h"
+#include "solver_manager.h"
+#include "../trajectory_base.h"
+#include "../map_base.h"
+#include "../landmark_base.h"
-SolverManager::SolverManager()
-{
-
-}
+namespace wolf {
-SolverManager::~SolverManager()
+SolverManager::SolverManager(const ProblemPtr& _wolf_problem) :
+ wolf_problem_(_wolf_problem)
{
- removeAllStateUnits();
+ assert(_wolf_problem != nullptr && "Passed a nullptr ProblemPtr.");
}
-void SolverManager::solve()
+void SolverManager::update()
{
-
+ // REMOVE CONSTRAINTS
+ auto ctr_notification_it = wolf_problem_->getConstraintNotificationList().begin();
+ while ( ctr_notification_it != wolf_problem_->getConstraintNotificationList().end() )
+ {
+ if (ctr_notification_it->notification_ == REMOVE)
+ {
+ removeConstraint(ctr_notification_it->constraint_ptr_);
+ ctr_notification_it = wolf_problem_->getConstraintNotificationList().erase(ctr_notification_it);
+ }
+ else
+ ctr_notification_it++;
+ }
+
+ StateBlockList& states = wolf_problem_->getNotifiedStateBlockList();
+
+ for (StateBlockPtr& state : states)
+ {
+ const auto notifications = state->consumeNotifications();
+
+ for (const auto notif : notifications)
+ {
+ switch (notif)
+ {
+ case StateBlock::Notification::ADD:
+ {
+ const bool registered = state_blocks_.find(state)!=state_blocks_.end();
+
+// const auto p = state_blocks_.emplace(state, state->getState());
+
+ // call addStateBlock only if first time added.
+ if (!registered)
+ {
+ state_blocks_.emplace(state, state->getState());
+ addStateBlock(state);
+ }
+
+ WOLF_DEBUG_COND(registered, "Tried adding an already registered StateBlock.");
+
+ break;
+ }
+ case StateBlock::Notification::STATE_UPDATE:
+ {
+ const bool registered = state_blocks_.find(state)!=state_blocks_.end();
+
+ WOLF_DEBUG_COND(!registered,
+ "Updating the state of an unregistered StateBlock !");
+
+ // This will throw if StateBlock wasn't registered
+// state_blocks_.at(state) = state->getState();
+
+// state_blocks_[state] = state->getState();
+
+// if (!registered)
+// {
+// addStateBlock(state);
+// }
+
+// assert(state_blocks_.find(state)!=state_blocks_.end() &&
+// "Updating the state of an unregistered StateBlock !");
+
+ break;
+ }
+ case StateBlock::Notification::FIX_UPDATE:
+ {
+ WOLF_DEBUG_COND(state_blocks_.find(state)==state_blocks_.end(),
+ "Updating the fix state of an unregistered StateBlock !");
+
+ assert(state_blocks_.find(state)!=state_blocks_.end() &&
+ "Updating the fix state of an unregistered StateBlock !");
+
+ updateStateBlockStatus(state);
+
+ break;
+ }
+ case StateBlock::Notification::REMOVE:
+ {
+ WOLF_DEBUG_COND(state_blocks_.find(state)==state_blocks_.end(),
+ "Tried to remove a StateBlock that was not added !");
+
+ if (state_blocks_.erase(state) > 0)
+ removeStateBlock(state);
+
+ break;
+ }
+ default:
+ throw std::runtime_error("SolverManager::update: State Block notification "
+ "must be ADD, STATE_UPDATE, FIX_UPDATE or REMOVE.");
+ }
+ }
+ }
+
+ states.clear();
+
+ // ADD CONSTRAINTS
+ while (!wolf_problem_->getConstraintNotificationList().empty())
+ {
+ switch (wolf_problem_->getConstraintNotificationList().front().notification_)
+ {
+ case Notification::ADD:
+ {
+ addConstraint(wolf_problem_->getConstraintNotificationList().front().constraint_ptr_);
+
+ break;
+ }
+ default:
+ throw std::runtime_error("SolverManager::update:"
+ " Constraint notification must be ADD or REMOVE.");
+ }
+
+ wolf_problem_->getConstraintNotificationList().pop_front();
+ }
+
+ assert(wolf_problem_->getConstraintNotificationList().empty() &&
+ "wolf problem's constraints notification list not empty after update");
+ assert(wolf_problem_->getNotifiedStateBlockList().empty() &&
+ "wolf problem's state_blocks notification list not empty after update");
}
-//void SolverManager::computeCovariances(WolfProblemPtr _problem_ptr)
-//{
-//}
-
-void SolverManager::update(const WolfProblemPtr _problem_ptr)
+wolf::ProblemPtr SolverManager::getProblemPtr()
{
- // IF REALLOCATION OF STATE, REMOVE EVERYTHING AND BUILD THE PROBLEM AGAIN
- if (_problem_ptr->isReallocated())
- {
- // todo: reallocate x
- }
- else
- {
- // ADD/UPDATE STATE UNITS
- for(auto state_unit_it = _problem_ptr->getStateList().begin(); state_unit_it!=_problem_ptr->getStateList().end(); state_unit_it++)
- {
- if ((*state_unit_it)->getPendingStatus() == ADD_PENDING)
- addStateUnit(*state_unit_it);
-
- else if((*state_unit_it)->getPendingStatus() == UPDATE_PENDING)
- updateStateUnitStatus(*state_unit_it);
- }
- //std::cout << "state units updated!" << std::endl;
-
- // REMOVE STATE UNITS
- while (!_problem_ptr->getRemovedStateList().empty())
- {
- // TODO: remove state unit
- //_problem_ptr->getRemovedStateList().pop_front();
- }
- //std::cout << "state units removed!" << std::endl;
-
- // ADD CONSTRAINTS
- ConstraintBaseList ctr_list;
- _problem_ptr->getTrajectoryPtr()->getConstraintList(ctr_list);
- //std::cout << "ctr_list.size() = " << ctr_list.size() << std::endl;
- for(auto ctr_it = ctr_list.begin(); ctr_it!=ctr_list.end(); ctr_it++)
- if ((*ctr_it)->getPendingStatus() == ADD_PENDING)
- addConstraint(*ctr_it);
-
- //std::cout << "constraints updated!" << std::endl;
- }
+ return wolf_problem_;
}
-void SolverManager::addConstraint(ConstraintBasePtr _corr_ptr)
+std::string SolverManager::solve(const ReportVerbosity report_level)
{
- //TODO MatrixXs J; Vector e;
- // getResidualsAndJacobian(_corr_ptr, J, e);
- // solverQR->addConstraint(_corr_ptr, J, e);
+ // update problem
+ update();
-// constraint_map_[_corr_ptr->id()] = blockIdx;
- _corr_ptr->setPendingStatus(NOT_PENDING);
-}
+ std::string report = solveImpl(report_level);
-void SolverManager::removeConstraint(const unsigned int& _corr_idx)
-{
- // TODO
-}
+ // update StateBlocks with optimized state value.
+ /// @todo whatif someone has changed the state notification during opti ??
-void SolverManager::addStateUnit(StateBlockPtr _st_ptr)
-{
- //std::cout << "Adding State Unit " << _st_ptr->id() << std::endl;
- //_st_ptr->print();
-
- switch (_st_ptr->getStateType())
- {
- case ST_COMPLEX_ANGLE:
- {
- // TODO
- //std::cout << "Adding Complex angle Local Parametrization to the List... " << std::endl;
- //ceres_problem_->AddParameterBlock(_st_ptr->getPtr(), ((StateComplexAngle*)_st_ptr)->BLOCK_SIZE, new ComplexAngleParameterization);
- break;
- }
- case ST_THETA:
- {
- //std::cout << "No Local Parametrization to be added" << std::endl;
- ceres_problem_->AddParameterBlock(_st_ptr->getPtr(), ((StateBlockPtr)_st_ptr)->BLOCK_SIZE, nullptr);
- break;
- }
- case ST_POINT_1D:
- {
- //std::cout << "No Local Parametrization to be added" << std::endl;
- ceres_problem_->AddParameterBlock(_st_ptr->getPtr(), ((StatePoint1D*)_st_ptr)->BLOCK_SIZE, nullptr);
- break;
- }
- case ST_VECTOR:
- {
- //std::cout << "No Local Parametrization to be added" << std::endl;
- ceres_problem_->AddParameterBlock(_st_ptr->getPtr(), ((StateBlockPtr)_st_ptr)->BLOCK_SIZE, nullptr);
- break;
- }
- case ST_POINT_3D:
- {
- //std::cout << "No Local Parametrization to be added" << std::endl;
- ceres_problem_->AddParameterBlock(_st_ptr->getPtr(), ((StateBlockPtr)_st_ptr)->BLOCK_SIZE, nullptr);
- break;
- }
- default:
- std::cout << "Unknown Local Parametrization type!" << std::endl;
- }
- if (_st_ptr->isFixed())
- updateStateUnitStatus(_st_ptr);
-
- _st_ptr->setPendingStatus(NOT_PENDING);
+ std::map<StateBlockPtr, Eigen::VectorXs>::iterator it = state_blocks_.begin(),
+ it_end = state_blocks_.end();
+ for (; it != it_end; ++it)
+ {
+ // Avoid usuless copies
+ if (!it->first->isFixed())
+ it->first->setState(it->second);
+ }
+
+ return report;
}
-void SolverManager::removeAllStateUnits()
+Eigen::VectorXs& SolverManager::getAssociatedMemBlock(const StateBlockPtr& state_ptr)
{
- std::vector<double*> parameter_blocks;
+ auto it = state_blocks_.find(state_ptr);
- ceres_problem_->GetParameterBlocks(¶meter_blocks);
+ if (it == state_blocks_.end())
+ throw std::runtime_error("Tried to retrieve the memory block of an unregistered StateBlock !");
- for (unsigned int i = 0; i< parameter_blocks.size(); i++)
- ceres_problem_->RemoveParameterBlock(parameter_blocks[i]);
+ return it->second;
}
-void SolverManager::updateStateUnitStatus(StateBlockPtr _st_ptr)
+Scalar* SolverManager::getAssociatedMemBlockPtr(const StateBlockPtr& state_ptr)
{
- // TODO
-
-// if (!_st_ptr->isFixed())
-// ceres_problem_->SetParameterBlockVariable(_st_ptr->getPtr());
-// else if (_st_ptr->isFixed())
-// ceres_problem_->SetParameterBlockConstant(_st_ptr->getPtr());
-// else
-// printf("\nERROR: Update state unit status with unknown status");
-//
-// _st_ptr->setPendingStatus(NOT_PENDING);
-}
+ auto it = state_blocks_.find(state_ptr);
-ceres::CostFunction* SolverManager::createCostFunction(ConstraintBasePtr _corrPtr)
-{
- //std::cout << "adding ctr " << _corrPtr->id() << std::endl;
- //_corrPtr->print();
-
- switch (_corrPtr->getConstraintType())
- {
- case CTR_GPS_FIX_2D:
- {
- ConstraintGPS2D* specific_ptr = (ConstraintGPS2D*)(_corrPtr);
- return new ceres::AutoDiffCostFunction<ConstraintGPS2D,
- specific_ptr->residualSize,
- specific_ptr->block0Size,
- specific_ptr->block1Size,
- specific_ptr->block2Size,
- specific_ptr->block3Size,
- specific_ptr->block4Size,
- specific_ptr->block5Size,
- specific_ptr->block6Size,
- specific_ptr->block7Size,
- specific_ptr->block8Size,
- specific_ptr->block9Size>(specific_ptr);
- break;
- }
- case CTR_ODOM_2D_COMPLEX_ANGLE:
- {
- ConstraintOdom2DComplexAngle* specific_ptr = (ConstraintOdom2DComplexAngle*)(_corrPtr);
- return new ceres::AutoDiffCostFunction<ConstraintOdom2DComplexAngle,
- specific_ptr->residualSize,
- specific_ptr->block0Size,
- specific_ptr->block1Size,
- specific_ptr->block2Size,
- specific_ptr->block3Size,
- specific_ptr->block4Size,
- specific_ptr->block5Size,
- specific_ptr->block6Size,
- specific_ptr->block7Size,
- specific_ptr->block8Size,
- specific_ptr->block9Size>(specific_ptr);
- break;
- }
- case CTR_ODOM_2D:
- {
- ConstraintOdom2D* specific_ptr = (ConstraintOdom2D*)(_corrPtr);
- return new ceres::AutoDiffCostFunction<ConstraintOdom2D,
- specific_ptr->residualSize,
- specific_ptr->block0Size,
- specific_ptr->block1Size,
- specific_ptr->block2Size,
- specific_ptr->block3Size,
- specific_ptr->block4Size,
- specific_ptr->block5Size,
- specific_ptr->block6Size,
- specific_ptr->block7Size,
- specific_ptr->block8Size,
- specific_ptr->block9Size>(specific_ptr);
- break;
- }
- case CTR_CORNER_2D:
- {
- ConstraintCorner2D* specific_ptr = (ConstraintCorner2D*)(_corrPtr);
- return new ceres::AutoDiffCostFunction<ConstraintCorner2D,
- specific_ptr->residualSize,
- specific_ptr->block0Size,
- specific_ptr->block1Size,
- specific_ptr->block2Size,
- specific_ptr->block3Size,
- specific_ptr->block4Size,
- specific_ptr->block5Size,
- specific_ptr->block6Size,
- specific_ptr->block7Size,
- specific_ptr->block8Size,
- specific_ptr->block9Size>(specific_ptr);
- break;
- }
- case CTR_IMU:
- {
- ConstraintIMU* specific_ptr = (ConstraintIMU*)(_corrPtr);
- return new ceres::AutoDiffCostFunction<ConstraintIMU,
- specific_ptr->residualSize,
- specific_ptr->block0Size,
- specific_ptr->block1Size,
- specific_ptr->block2Size,
- specific_ptr->block3Size,
- specific_ptr->block4Size,
- specific_ptr->block5Size,
- specific_ptr->block6Size,
- specific_ptr->block7Size,
- specific_ptr->block8Size,
- specific_ptr->block9Size>(specific_ptr);
- break;
- }
- default:
- std::cout << "Unknown constraint type! Please add it in the CeresWrapper::createCostFunction()" << std::endl;
-
- return nullptr;
- }
+ if (it == state_blocks_.end())
+ throw std::runtime_error("Tried to retrieve the memory block of an unregistered StateBlock !");
+
+ return it->second.data();
}
+
+} // namespace wolf
diff --git a/src/solver/solver_manager.h b/src/solver/solver_manager.h
index 81a8e083eb30587543382814bae24f9662196335..51325380a31c9d10cba2a3039c62f2216a5b0706 100644
--- a/src/solver/solver_manager.h
+++ b/src/solver/solver_manager.h
@@ -1,49 +1,84 @@
-#ifndef CERES_MANAGER_H_
-#define CERES_MANAGER_H_
+#ifndef _WOLF_SOLVER_MANAGER_H_
+#define _WOLF_SOLVER_MANAGER_H_
//wolf includes
-#include <constraint_GPS_2D.h>
#include "../wolf.h"
#include "../state_block.h"
-#include "../state_point.h"
-#include "../state_complex_angle.h"
-#include "../state_theta.h"
-#include "../constraint_sparse.h"
-#include "../constraint_odom_2D_theta.h"
-#include "../constraint_odom_2D_complex_angle.h"
-#include "../constraint_corner_2D_theta.h"
-
-/** \brief solver manager for WOLF
- *
- */
+#include "../constraint_base.h"
+
+namespace wolf {
+WOLF_PTR_TYPEDEFS(SolverManager)
+
+/**
+ * \brief Solver manager for WOLF
+ */
class SolverManager
{
- protected:
+public:
+
+ /** \brief Enumeration of covariance blocks to be computed
+ *
+ * Enumeration of covariance blocks to be computed
+ *
+ */
+ enum class CovarianceBlocksToBeComputed : std::size_t
+ {
+ ALL, ///< All blocks and all cross-covariances
+ ALL_MARGINALS, ///< All marginals
+ ROBOT_LANDMARKS ///< marginals of landmarks and current robot pose plus cross covariances of current robot and all landmarks
+ };
+
+ /**
+ * \brief Enumeration for the verbosity of the solver report.
+ */
+ enum class ReportVerbosity : std::size_t
+ {
+ QUIET = 0,
+ BRIEF,
+ FULL
+ };
+
+protected:
+ ProblemPtr wolf_problem_;
- public:
- SolverManager(ceres::Problem::Options _options);
+public:
- ~SolverManager();
+ SolverManager(const ProblemPtr& wolf_problem);
- ceres::Solver::Summary solve(const ceres::Solver::Options& _ceres_options);
+ virtual ~SolverManager() = default;
- //void computeCovariances(WolfProblemPtr _problem_ptr);
+ std::string solve(const ReportVerbosity report_level = ReportVerbosity::QUIET);
- void update(const WolfProblemPtr _problem_ptr);
+ virtual void computeCovariances(const CovarianceBlocksToBeComputed blocks) = 0;
- void addConstraint(ConstraintBasePtr _corr_ptr);
+ virtual void computeCovariances(const StateBlockList& st_list) = 0;
- void removeConstraint(const unsigned int& _corr_idx);
+ virtual void update();
- void addStateUnit(StateBlockPtr _st_ptr);
+ ProblemPtr getProblemPtr();
- void removeAllStateUnits();
+protected:
- void updateStateUnitStatus(StateBlockPtr _st_ptr);
+ std::map<StateBlockPtr, Eigen::VectorXs> state_blocks_;
- ceres::CostFunction* createCostFunction(ConstraintBasePtr _corrPtr);
+ virtual Eigen::VectorXs& getAssociatedMemBlock(const StateBlockPtr& state_ptr);
+ virtual Scalar* getAssociatedMemBlockPtr(const StateBlockPtr& state_ptr);
+
+ virtual std::string solveImpl(const ReportVerbosity report_level) = 0;
+
+ virtual void addConstraint(const ConstraintBasePtr& ctr_ptr) = 0;
+
+ virtual void removeConstraint(const ConstraintBasePtr& ctr_ptr) = 0;
+
+ virtual void addStateBlock(const StateBlockPtr& state_ptr) = 0;
+
+ virtual void removeStateBlock(const StateBlockPtr& state_ptr) = 0;
+
+ virtual void updateStateBlockStatus(const StateBlockPtr& state_ptr) = 0;
};
-#endif
+} // namespace wolf
+
+#endif /* _WOLF_SOLVER_MANAGER_H_ */
diff --git a/src/solver_suitesparse/CMakeLists.txt b/src/solver_suitesparse/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..62ccd94a3e2395a59cd417458317bc1d80040747
--- /dev/null
+++ b/src/solver_suitesparse/CMakeLists.txt
@@ -0,0 +1,12 @@
+SET(HDRS_SOLVER_SUITESPARSE
+ ccolamd_ordering.h
+ cost_function_base.h
+ cost_function_sparse_base.h
+ cost_function_sparse.h
+ qr_solver.h
+ solver_manager.h
+ solver_QR.h
+ sparse_utils.h)
+SET(SRCS_SOLVER_SUITESPARSE
+ solver_manager.cpp
+ )
diff --git a/src/solver/ccolamd_ordering.h b/src/solver_suitesparse/ccolamd_ordering.h
similarity index 100%
rename from src/solver/ccolamd_ordering.h
rename to src/solver_suitesparse/ccolamd_ordering.h
diff --git a/src/solver/cost_function_base.h b/src/solver_suitesparse/cost_function_base.h
similarity index 100%
rename from src/solver/cost_function_base.h
rename to src/solver_suitesparse/cost_function_base.h
diff --git a/src/solver/cost_function_sparse.h b/src/solver_suitesparse/cost_function_sparse.h
similarity index 100%
rename from src/solver/cost_function_sparse.h
rename to src/solver_suitesparse/cost_function_sparse.h
diff --git a/src/solver/cost_function_sparse_base.h b/src/solver_suitesparse/cost_function_sparse_base.h
similarity index 100%
rename from src/solver/cost_function_sparse_base.h
rename to src/solver_suitesparse/cost_function_sparse_base.h
diff --git a/src/solver/qr_solver.h b/src/solver_suitesparse/qr_solver.h
similarity index 100%
rename from src/solver/qr_solver.h
rename to src/solver_suitesparse/qr_solver.h
diff --git a/src/solver/solver_QR.h b/src/solver_suitesparse/solver_QR.h
similarity index 100%
rename from src/solver/solver_QR.h
rename to src/solver_suitesparse/solver_QR.h
diff --git a/src/solver_suitesparse/solver_manager.cpp b/src/solver_suitesparse/solver_manager.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..cb39335b0705f8edf6222d78db36a0e18c21d504
--- /dev/null
+++ b/src/solver_suitesparse/solver_manager.cpp
@@ -0,0 +1,245 @@
+#include "ceres_manager.h"
+
+SolverManager::SolverManager()
+{
+
+}
+
+SolverManager::~SolverManager()
+{
+ removeAllStateUnits();
+}
+
+void SolverManager::solve()
+{
+
+}
+
+//void SolverManager::computeCovariances(WolfProblemPtr _problem_ptr)
+//{
+//}
+
+void SolverManager::update(const WolfProblemPtr _problem_ptr)
+{
+ // IF REALLOCATION OF STATE, REMOVE EVERYTHING AND BUILD THE PROBLEM AGAIN
+ if (_problem_ptr->isReallocated())
+ {
+ // todo: reallocate x
+ }
+ else
+ {
+ // ADD/UPDATE STATE UNITS
+ for(auto state_unit_it = _problem_ptr->getStateList().begin(); state_unit_it!=_problem_ptr->getStateList().end(); state_unit_it++)
+ {
+ if ((*state_unit_it)->getPendingStatus() == ADD_PENDING)
+ addStateUnit(*state_unit_it);
+
+ else if((*state_unit_it)->getPendingStatus() == UPDATE_PENDING)
+ updateStateUnitStatus(*state_unit_it);
+ }
+ //std::cout << "state units updated!" << std::endl;
+
+ // REMOVE STATE UNITS
+ while (!_problem_ptr->getRemovedStateList().empty())
+ {
+ // TODO: remove state unit
+ //_problem_ptr->getRemovedStateList().pop_front();
+ }
+ //std::cout << "state units removed!" << std::endl;
+
+ // ADD CONSTRAINTS
+ ConstraintBaseList ctr_list;
+ _problem_ptr->getTrajectoryPtr()->getConstraintList(ctr_list);
+ //std::cout << "ctr_list.size() = " << ctr_list.size() << std::endl;
+ for(auto ctr_it = ctr_list.begin(); ctr_it!=ctr_list.end(); ctr_it++)
+ if ((*ctr_it)->getPendingStatus() == ADD_PENDING)
+ addConstraint(*ctr_it);
+
+ //std::cout << "constraints updated!" << std::endl;
+ }
+}
+
+void SolverManager::addConstraint(ConstraintBasePtr _corr_ptr)
+{
+ //TODO MatrixXs J; Vector e;
+ // getResidualsAndJacobian(_corr_ptr, J, e);
+ // solverQR->addConstraint(_corr_ptr, J, e);
+
+// constraint_map_[_corr_ptr->id()] = blockIdx;
+ _corr_ptr->setPendingStatus(NOT_PENDING);
+}
+
+void SolverManager::removeConstraint(const unsigned int& _corr_idx)
+{
+ // TODO
+}
+
+void SolverManager::addStateUnit(StateBlockPtr _st_ptr)
+{
+ //std::cout << "Adding State Unit " << _st_ptr->id() << std::endl;
+ //_st_ptr->print();
+
+ switch (_st_ptr->getStateType())
+ {
+ case ST_COMPLEX_ANGLE:
+ {
+ // TODO
+ //std::cout << "Adding Complex angle Local Parametrization to the List... " << std::endl;
+ //ceres_problem_->AddParameterBlock(_st_ptr->getPtr(), ((StateComplexAngle*)_st_ptr)->BLOCK_SIZE, new ComplexAngleParameterization);
+ break;
+ }
+ case ST_THETA:
+ {
+ //std::cout << "No Local Parametrization to be added" << std::endl;
+ ceres_problem_->AddParameterBlock(_st_ptr->getPtr(), ((StateBlockPtr)_st_ptr)->BLOCK_SIZE, nullptr);
+ break;
+ }
+ case ST_POINT_1D:
+ {
+ //std::cout << "No Local Parametrization to be added" << std::endl;
+ ceres_problem_->AddParameterBlock(_st_ptr->getPtr(), ((StatePoint1D*)_st_ptr)->BLOCK_SIZE, nullptr);
+ break;
+ }
+ case ST_VECTOR:
+ {
+ //std::cout << "No Local Parametrization to be added" << std::endl;
+ ceres_problem_->AddParameterBlock(_st_ptr->getPtr(), ((StateBlockPtr)_st_ptr)->BLOCK_SIZE, nullptr);
+ break;
+ }
+ case ST_POINT_3D:
+ {
+ //std::cout << "No Local Parametrization to be added" << std::endl;
+ ceres_problem_->AddParameterBlock(_st_ptr->getPtr(), ((StateBlockPtr)_st_ptr)->BLOCK_SIZE, nullptr);
+ break;
+ }
+ default:
+ std::cout << "Unknown Local Parametrization type!" << std::endl;
+ }
+ if (_st_ptr->isFixed())
+ updateStateUnitStatus(_st_ptr);
+
+ _st_ptr->setPendingStatus(NOT_PENDING);
+}
+
+void SolverManager::removeAllStateUnits()
+{
+ std::vector<double*> parameter_blocks;
+
+ ceres_problem_->GetParameterBlocks(¶meter_blocks);
+
+ for (unsigned int i = 0; i< parameter_blocks.size(); i++)
+ ceres_problem_->RemoveParameterBlock(parameter_blocks[i]);
+}
+
+void SolverManager::updateStateUnitStatus(StateBlockPtr _st_ptr)
+{
+ // TODO
+
+// if (!_st_ptr->isFixed())
+// ceres_problem_->SetParameterBlockVariable(_st_ptr->getPtr());
+// else if (_st_ptr->isFixed())
+// ceres_problem_->SetParameterBlockConstant(_st_ptr->getPtr());
+// else
+// printf("\nERROR: Update state unit status with unknown status");
+//
+// _st_ptr->setPendingStatus(NOT_PENDING);
+}
+
+ceres::CostFunction* SolverManager::createCostFunction(ConstraintBasePtr _corrPtr)
+{
+ //std::cout << "adding ctr " << _corrPtr->id() << std::endl;
+ //_corrPtr->print();
+
+ switch (_corrPtr->getConstraintType())
+ {
+ case CTR_GPS_FIX_2D:
+ {
+ ConstraintGPS2D* specific_ptr = (ConstraintGPS2D*)(_corrPtr);
+ return new ceres::AutoDiffCostFunction<ConstraintGPS2D,
+ specific_ptr->residualSize,
+ specific_ptr->block0Size,
+ specific_ptr->block1Size,
+ specific_ptr->block2Size,
+ specific_ptr->block3Size,
+ specific_ptr->block4Size,
+ specific_ptr->block5Size,
+ specific_ptr->block6Size,
+ specific_ptr->block7Size,
+ specific_ptr->block8Size,
+ specific_ptr->block9Size>(specific_ptr);
+ break;
+ }
+ case CTR_ODOM_2D_COMPLEX_ANGLE:
+ {
+ ConstraintOdom2DComplexAngle* specific_ptr = (ConstraintOdom2DComplexAngle*)(_corrPtr);
+ return new ceres::AutoDiffCostFunction<ConstraintOdom2DComplexAngle,
+ specific_ptr->residualSize,
+ specific_ptr->block0Size,
+ specific_ptr->block1Size,
+ specific_ptr->block2Size,
+ specific_ptr->block3Size,
+ specific_ptr->block4Size,
+ specific_ptr->block5Size,
+ specific_ptr->block6Size,
+ specific_ptr->block7Size,
+ specific_ptr->block8Size,
+ specific_ptr->block9Size>(specific_ptr);
+ break;
+ }
+ case CTR_ODOM_2D:
+ {
+ ConstraintOdom2D* specific_ptr = (ConstraintOdom2D*)(_corrPtr);
+ return new ceres::AutoDiffCostFunction<ConstraintOdom2D,
+ specific_ptr->residualSize,
+ specific_ptr->block0Size,
+ specific_ptr->block1Size,
+ specific_ptr->block2Size,
+ specific_ptr->block3Size,
+ specific_ptr->block4Size,
+ specific_ptr->block5Size,
+ specific_ptr->block6Size,
+ specific_ptr->block7Size,
+ specific_ptr->block8Size,
+ specific_ptr->block9Size>(specific_ptr);
+ break;
+ }
+ case CTR_CORNER_2D:
+ {
+ ConstraintCorner2D* specific_ptr = (ConstraintCorner2D*)(_corrPtr);
+ return new ceres::AutoDiffCostFunction<ConstraintCorner2D,
+ specific_ptr->residualSize,
+ specific_ptr->block0Size,
+ specific_ptr->block1Size,
+ specific_ptr->block2Size,
+ specific_ptr->block3Size,
+ specific_ptr->block4Size,
+ specific_ptr->block5Size,
+ specific_ptr->block6Size,
+ specific_ptr->block7Size,
+ specific_ptr->block8Size,
+ specific_ptr->block9Size>(specific_ptr);
+ break;
+ }
+ case CTR_IMU:
+ {
+ ConstraintIMU* specific_ptr = (ConstraintIMU*)(_corrPtr);
+ return new ceres::AutoDiffCostFunction<ConstraintIMU,
+ specific_ptr->residualSize,
+ specific_ptr->block0Size,
+ specific_ptr->block1Size,
+ specific_ptr->block2Size,
+ specific_ptr->block3Size,
+ specific_ptr->block4Size,
+ specific_ptr->block5Size,
+ specific_ptr->block6Size,
+ specific_ptr->block7Size,
+ specific_ptr->block8Size,
+ specific_ptr->block9Size>(specific_ptr);
+ break;
+ }
+ default:
+ std::cout << "Unknown constraint type! Please add it in the CeresWrapper::createCostFunction()" << std::endl;
+
+ return nullptr;
+ }
+}
diff --git a/src/solver_suitesparse/solver_manager.h b/src/solver_suitesparse/solver_manager.h
new file mode 100644
index 0000000000000000000000000000000000000000..81a8e083eb30587543382814bae24f9662196335
--- /dev/null
+++ b/src/solver_suitesparse/solver_manager.h
@@ -0,0 +1,49 @@
+#ifndef CERES_MANAGER_H_
+#define CERES_MANAGER_H_
+
+//wolf includes
+#include <constraint_GPS_2D.h>
+#include "../wolf.h"
+#include "../state_block.h"
+#include "../state_point.h"
+#include "../state_complex_angle.h"
+#include "../state_theta.h"
+#include "../constraint_sparse.h"
+#include "../constraint_odom_2D_theta.h"
+#include "../constraint_odom_2D_complex_angle.h"
+#include "../constraint_corner_2D_theta.h"
+
+/** \brief solver manager for WOLF
+ *
+ */
+
+class SolverManager
+{
+ protected:
+
+
+ public:
+ SolverManager(ceres::Problem::Options _options);
+
+ ~SolverManager();
+
+ ceres::Solver::Summary solve(const ceres::Solver::Options& _ceres_options);
+
+ //void computeCovariances(WolfProblemPtr _problem_ptr);
+
+ void update(const WolfProblemPtr _problem_ptr);
+
+ void addConstraint(ConstraintBasePtr _corr_ptr);
+
+ void removeConstraint(const unsigned int& _corr_idx);
+
+ void addStateUnit(StateBlockPtr _st_ptr);
+
+ void removeAllStateUnits();
+
+ void updateStateUnitStatus(StateBlockPtr _st_ptr);
+
+ ceres::CostFunction* createCostFunction(ConstraintBasePtr _corrPtr);
+};
+
+#endif
diff --git a/src/solver/sparse_utils.h b/src/solver_suitesparse/sparse_utils.h
similarity index 100%
rename from src/solver/sparse_utils.h
rename to src/solver_suitesparse/sparse_utils.h
diff --git a/src/state_block.h b/src/state_block.h
index 7903ceb748e1700495c83f6bae02efc869dfe2e1..780b0b8bf107eb0897f033f183e85b19d7724cba 100644
--- a/src/state_block.h
+++ b/src/state_block.h
@@ -29,14 +29,30 @@ namespace wolf {
*/
class StateBlock
{
+public:
+
+ enum class Notification : std::size_t
+ {
+ ADD = 0,
+ REMOVE,
+ STATE_UPDATE,
+ FIX_UPDATE
+ };
+
+ using Notifications = std::list<Notification>;
+
protected:
+
+ mutable Notifications notifications_;
+ mutable std::mutex notifictions_mut_;
+
NodeBaseWPtr node_ptr_; //< pointer to the wolf Node owning this StateBlock
bool fixed_; ///< Key to indicate whether the state is fixed or not
Eigen::VectorXs state_; ///< State vector storing the state values
LocalParametrizationBasePtr local_param_ptr_; ///< Local parametrization useful for optimizing in the tangent space to the manifold
-
+
public:
/** \brief Constructor from size
*
@@ -53,18 +69,14 @@ class StateBlock
* \param _local_param_ptr pointer to the local parametrization for the block
**/
StateBlock(const Eigen::VectorXs& _state, bool _fixed = false, LocalParametrizationBasePtr _local_param_ptr = nullptr);
-
+
/** \brief Destructor
**/
virtual ~StateBlock();
- /** \brief Returns the pointer to the first element of the state
- **/
- Scalar* getPtr();
-
/** \brief Returns the state vector
**/
- const Eigen::VectorXs& getState() const;
+ Eigen::VectorXs getState() const;
/** \brief Sets the state vector
**/
@@ -94,14 +106,19 @@ class StateBlock
void setFixed(bool _fixed);
- bool hasLocalParametrization();
+ bool hasLocalParametrization() const;
- LocalParametrizationBasePtr getLocalParametrizationPtr();
+ LocalParametrizationBasePtr getLocalParametrizationPtr() const;
void setLocalParametrizationPtr(LocalParametrizationBasePtr _local_param);
void removeLocalParametrization();
+ void addNotification(const StateBlock::Notification _new_notification);
+
+ StateBlock::Notifications consumeNotifications() const;
+
+ bool hasNotifications() const;
};
} // namespace wolf
@@ -112,6 +129,7 @@ class StateBlock
namespace wolf {
inline StateBlock::StateBlock(const Eigen::VectorXs& _state, bool _fixed, LocalParametrizationBasePtr _local_param_ptr) :
+// notifications_{Notification::ADD},
node_ptr_(), // nullptr
fixed_(_fixed),
state_(_state),
@@ -121,6 +139,7 @@ inline StateBlock::StateBlock(const Eigen::VectorXs& _state, bool _fixed, LocalP
}
inline StateBlock::StateBlock(const unsigned int _size, bool _fixed, LocalParametrizationBasePtr _local_param_ptr) :
+// notifications_{Notification::ADD},
node_ptr_(), // nullptr
fixed_(_fixed),
state_(Eigen::VectorXs::Zero(_size)),
@@ -135,12 +154,7 @@ inline StateBlock::~StateBlock()
// std::cout << "destructed -sb" << std::endl;
}
-inline Scalar* StateBlock::getPtr()
-{
- return state_.data();
-}
-
-inline const Eigen::VectorXs& StateBlock::getState() const
+inline Eigen::VectorXs StateBlock::getState() const
{
return state_;
}
@@ -149,6 +163,7 @@ inline void StateBlock::setState(const Eigen::VectorXs& _state)
{
assert(_state.size() == state_.size());
state_ = _state;
+ addNotification(Notification::STATE_UPDATE);
}
inline unsigned int StateBlock::getSize() const
@@ -170,25 +185,26 @@ inline bool StateBlock::isFixed() const
inline void StateBlock::fix()
{
- fixed_ = true;
+ setFixed(true);
}
inline void StateBlock::unfix()
{
- fixed_ = false;
+ setFixed(false);
}
inline void StateBlock::setFixed(bool _fixed)
{
fixed_ = _fixed;
+ addNotification(Notification::FIX_UPDATE);
}
-inline bool StateBlock::hasLocalParametrization()
+inline bool StateBlock::hasLocalParametrization() const
{
return (local_param_ptr_ != nullptr);
}
-inline LocalParametrizationBasePtr StateBlock::getLocalParametrizationPtr()
+inline LocalParametrizationBasePtr StateBlock::getLocalParametrizationPtr() const
{
return local_param_ptr_;
}
@@ -205,6 +221,24 @@ inline void StateBlock::setLocalParametrizationPtr(LocalParametrizationBasePtr _
local_param_ptr_ = _local_param;
}
+inline void StateBlock::addNotification(const StateBlock::Notification _new_notification)
+{
+ std::lock_guard<std::mutex> lock(notifictions_mut_);
+ notifications_.emplace_back(_new_notification);
+}
+
+inline StateBlock::Notifications StateBlock::consumeNotifications() const
+{
+ std::lock_guard<std::mutex> lock(notifictions_mut_);
+ return std::move(notifications_);
+}
+
+inline bool StateBlock::hasNotifications() const
+{
+ std::lock_guard<std::mutex> lock(notifictions_mut_);
+ return !notifications_.empty();
+}
+
} // namespace wolf
#endif
diff --git a/src/test/gtest_IMU.cpp b/src/test/gtest_IMU.cpp
index 0a23dff8cd4b85df626891719b345532dffc515d..38dfbc223a2e3137c503eef65ff343e379265365 100644
--- a/src/test/gtest_IMU.cpp
+++ b/src/test/gtest_IMU.cpp
@@ -521,7 +521,7 @@ class Process_Constraint_IMU : public testing::Test
- string solveProblem(int verbose = 1)
+ string solveProblem(SolverManager::ReportVerbosity verbose = SolverManager::ReportVerbosity::BRIEF)
{
string report = ceres_manager->solve(verbose);
@@ -544,7 +544,7 @@ class Process_Constraint_IMU : public testing::Test
- string runAll(int verbose)
+ string runAll(SolverManager::ReportVerbosity verbose)
{
configureAll();
integrateAll();
@@ -729,7 +729,7 @@ TEST_F(Process_Constraint_IMU, MotionConstant_PQV_b__PQV_b) // F_ixed___e_stimat
// ===================================== RUN ALL
- string report = runAll(1);
+ string report = runAll(SolverManager::ReportVerbosity::BRIEF);
// printAll(report);
@@ -822,7 +822,7 @@ TEST_F(Process_Constraint_IMU, MotionConstant_pqv_b__PQV_b) // F_ixed___e_stimat
// ===================================== RUN ALL
- string report = runAll(1);
+ string report = runAll(SolverManager::ReportVerbosity::BRIEF);
// printAll(report);
@@ -868,7 +868,7 @@ TEST_F(Process_Constraint_IMU, MotionConstant_pqV_b__PQv_b) // F_ixed___e_stimat
// ===================================== RUN ALL
- string report = runAll(1);
+ string report = runAll(SolverManager::ReportVerbosity::BRIEF);
// printAll(report);
@@ -914,7 +914,7 @@ TEST_F(Process_Constraint_IMU, MotionRandom_PQV_b__PQV_b) // F_ixed___e_stimated
// ===================================== RUN ALL
- string report = runAll(1);
+ string report = runAll(SolverManager::ReportVerbosity::BRIEF);
// printAll(report);
@@ -960,7 +960,7 @@ TEST_F(Process_Constraint_IMU, MotionRandom_pqV_b__PQv_b) // F_ixed___e_stimated
// ===================================== RUN ALL
- string report = runAll(1);
+ string report = runAll(SolverManager::ReportVerbosity::BRIEF);
// printAll(report);
@@ -1005,7 +1005,7 @@ TEST_F(Process_Constraint_IMU, MotionRandom_pqV_b__pQV_b) // F_ixed___e_stimated
// ===================================== RUN ALL
- string report = runAll(1);
+ string report = runAll(SolverManager::ReportVerbosity::BRIEF);
// printAll(report);
@@ -1050,7 +1050,7 @@ TEST_F(Process_Constraint_IMU, MotionConstant_NonNullState_PQV_b__PQV_b) // F_ix
// ===================================== RUN ALL
- string report = runAll(1);
+ string report = runAll(SolverManager::ReportVerbosity::BRIEF);
// printAll(report);
@@ -1095,7 +1095,7 @@ TEST_F(Process_Constraint_IMU_ODO, MotionConstantRotation_PQV_b__PQV_b) // F_ixe
// ===================================== RUN ALL
- string report = runAll(1);
+ string report = runAll(SolverManager::ReportVerbosity::BRIEF);
// printAll(report);
@@ -1140,7 +1140,7 @@ TEST_F(Process_Constraint_IMU_ODO, MotionConstantRotation_PQV_b__PQv_b) // F_ixe
// ===================================== RUN ALL
- string report = runAll(1);
+ string report = runAll(SolverManager::ReportVerbosity::BRIEF);
// printAll(report);
@@ -1185,7 +1185,7 @@ TEST_F(Process_Constraint_IMU_ODO, MotionConstantRotation_PQV_b__Pqv_b) // F_ixe
// ===================================== RUN ALL
- string report = runAll(1);
+ string report = runAll(SolverManager::ReportVerbosity::BRIEF);
// printAll(report);
@@ -1230,7 +1230,7 @@ TEST_F(Process_Constraint_IMU_ODO, MotionConstantRotation_PQV_b__pQv_b) // F_ixe
// ===================================== RUN ALL
- string report = runAll(1);
+ string report = runAll(SolverManager::ReportVerbosity::BRIEF);
// printAll(report);
@@ -1275,7 +1275,7 @@ TEST_F(Process_Constraint_IMU_ODO, MotionConstantRotation_PQV_b__pqv_b) // F_ixe
// ===================================== RUN ALL
- string report = runAll(1);
+ string report = runAll(SolverManager::ReportVerbosity::BRIEF);
// printAll(report);
@@ -1320,7 +1320,7 @@ TEST_F(Process_Constraint_IMU_ODO, MotionConstant_pqv_b__pqV_b) // F_ixed___e_st
// ===================================== RUN ALL
- string report = runAll(1);
+ string report = runAll(SolverManager::ReportVerbosity::BRIEF);
// printAll(report);
@@ -1365,7 +1365,7 @@ TEST_F(Process_Constraint_IMU_ODO, MotionConstant_pqV_b__pqv_b) // F_ixed___e_st
// ===================================== RUN ALL
- string report = runAll(1);
+ string report = runAll(SolverManager::ReportVerbosity::BRIEF);
// printAll(report);
@@ -1410,7 +1410,7 @@ TEST_F(Process_Constraint_IMU_ODO, MotionRandom_PQV_b__pqv_b) // F_ixed___e_stim
// ===================================== RUN ALL
- string report = runAll(1);
+ string report = runAll(SolverManager::ReportVerbosity::BRIEF);
// printAll(report);
@@ -1455,7 +1455,7 @@ TEST_F(Process_Constraint_IMU_ODO, MotionRandom_PqV_b__pqV_b) // F_ixed___e_stim
// ===================================== RUN ALL
- string report = runAll(1);
+ string report = runAll(SolverManager::ReportVerbosity::BRIEF);
// printAll(report);
@@ -1503,7 +1503,7 @@ TEST_F(Process_Constraint_IMU_ODO, RecoverTrajectory_MotionRandom_PqV_b__pqV_b)
configureAll();
integrateAllTrajectories();
buildProblem();
- string report = solveProblem(1);
+ string report = solveProblem(SolverManager::ReportVerbosity::BRIEF);
assertAll();
diff --git a/src/test/gtest_constraint_absolute.cpp b/src/test/gtest_constraint_absolute.cpp
index 52adcd099403218395990b69ed857da62259d507..80a6579ae459d7932e7c0e4df9b95e48a76ce326 100644
--- a/src/test/gtest_constraint_absolute.cpp
+++ b/src/test/gtest_constraint_absolute.cpp
@@ -70,7 +70,7 @@ TEST(ConstraintBlockAbs, ctr_block_abs_p_solve)
frm0->setState(x0);
// solve for frm0
- std::string brief_report = ceres_mgr.solve(1);
+ std::string brief_report = ceres_mgr.solve(wolf::SolverManager::ReportVerbosity::BRIEF);
//only orientation is constrained
ASSERT_MATRIX_APPROX(frm0->getState().head<3>(), pose10.head<3>(), 1e-6);
@@ -97,7 +97,7 @@ TEST(ConstraintBlockAbs, ctr_block_abs_v_solve)
frm0->setState(x0);
// solve for frm0
- std::string brief_report = ceres_mgr.solve(1);
+ std::string brief_report = ceres_mgr.solve(wolf::SolverManager::ReportVerbosity::BRIEF);
//only velocity is constrained
ASSERT_MATRIX_APPROX(frm0->getState().tail<3>(), pose10.tail<3>(), 1e-6);
@@ -124,7 +124,7 @@ TEST(ConstraintQuatAbs, ctr_block_abs_o_solve)
frm0->setState(x0);
// solve for frm0
- std::string brief_report = ceres_mgr.solve(1);
+ std::string brief_report = ceres_mgr.solve(wolf::SolverManager::ReportVerbosity::BRIEF);
//only velocity is constrained
ASSERT_MATRIX_APPROX(frm0->getState().segment<4>(3), pose10.segment<4>(3), 1e-6);
diff --git a/src/test/gtest_constraint_autodiff.cpp b/src/test/gtest_constraint_autodiff.cpp
index 52b5d1c121987779fb20661319f5b8f1a27fd8ad..c5820537f54d0aaf8e251cc4867a88b82f1d0e33 100644
--- a/src/test/gtest_constraint_autodiff.cpp
+++ b/src/test/gtest_constraint_autodiff.cpp
@@ -74,7 +74,14 @@ TEST(CaptureAutodiff, ResidualOdom2d)
fr1_ptr->addConstrainedBy(constraint_ptr);
// EVALUATE
- std::vector<Scalar*> states_ptr({fr1_ptr->getPPtr()->getPtr(), fr1_ptr->getOPtr()->getPtr(), fr2_ptr->getPPtr()->getPtr(), fr2_ptr->getOPtr()->getPtr()});
+
+ Eigen::VectorXs fr1_pstate = fr1_ptr->getPPtr()->getState();
+ Eigen::VectorXs fr1_ostate = fr1_ptr->getOPtr()->getState();
+ Eigen::VectorXs fr2_pstate = fr2_ptr->getPPtr()->getState();
+ Eigen::VectorXs fr2_ostate = fr2_ptr->getOPtr()->getState();
+
+ std::vector<Scalar*> states_ptr({fr1_pstate.data(), fr1_ostate.data(), fr2_pstate.data(), fr2_ostate.data()});
+
double const* const* parameters = states_ptr.data();
Eigen::VectorXs residuals(constraint_ptr->getSize());
constraint_ptr->evaluate(parameters, residuals.data(), nullptr);
@@ -111,7 +118,14 @@ TEST(CaptureAutodiff, JacobianOdom2d)
fr1_ptr->addConstrainedBy(constraint_ptr);
// COMPUTE JACOBIANS
- std::vector<const Scalar*> states_ptr({fr1_ptr->getPPtr()->getPtr(), fr1_ptr->getOPtr()->getPtr(), fr2_ptr->getPPtr()->getPtr(), fr2_ptr->getOPtr()->getPtr()});
+
+ const Eigen::VectorXs fr1_pstate = fr1_ptr->getPPtr()->getState();
+ const Eigen::VectorXs fr1_ostate = fr1_ptr->getOPtr()->getState();
+ const Eigen::VectorXs fr2_pstate = fr2_ptr->getPPtr()->getState();
+ const Eigen::VectorXs fr2_ostate = fr2_ptr->getOPtr()->getState();
+
+ std::vector<const Scalar*> states_ptr({fr1_pstate.data(), fr1_ostate.data(), fr2_pstate.data(), fr2_ostate.data()});
+
std::vector<Eigen::MatrixXs> Jauto;
Eigen::VectorXs residuals(constraint_ptr->getSize());
constraint_ptr->evaluate(states_ptr, residuals, Jauto);
@@ -186,7 +200,14 @@ TEST(CaptureAutodiff, AutodiffVsAnalytic)
fr1_ptr->addConstrainedBy(ctr_analytic_ptr);
// COMPUTE JACOBIANS
- std::vector<const Scalar*> states_ptr({fr1_ptr->getPPtr()->getPtr(), fr1_ptr->getOPtr()->getPtr(), fr2_ptr->getPPtr()->getPtr(), fr2_ptr->getOPtr()->getPtr()});
+
+ const Eigen::VectorXs fr1_pstate = fr1_ptr->getPPtr()->getState();
+ const Eigen::VectorXs fr1_ostate = fr1_ptr->getOPtr()->getState();
+ const Eigen::VectorXs fr2_pstate = fr2_ptr->getPPtr()->getState();
+ const Eigen::VectorXs fr2_ostate = fr2_ptr->getOPtr()->getState();
+
+ std::vector<const Scalar*> states_ptr({fr1_pstate.data(), fr1_ostate.data(), fr2_pstate.data(), fr2_ostate.data()});
+
std::vector<Eigen::MatrixXs> Jautodiff, Janalytic;
Eigen::VectorXs residuals(ctr_autodiff_ptr->getSize());
clock_t t = clock();
diff --git a/src/test/gtest_constraint_autodiff_distance_3D.cpp b/src/test/gtest_constraint_autodiff_distance_3D.cpp
index 768be80e93b39146ebc42a91b63f7b3011e42066..bc6e402e423c2f6531a94591a1add61733872607 100644
--- a/src/test/gtest_constraint_autodiff_distance_3D.cpp
+++ b/src/test/gtest_constraint_autodiff_distance_3D.cpp
@@ -98,7 +98,7 @@ TEST_F(ConstraintAutodiffDistance3D_Test, solve)
Scalar measurement = 1.400;
f2->setMeasurement(Vector1s(measurement));
- std::string report = ceres_manager->solve(2);
+ std::string report = ceres_manager->solve(SolverManager::ReportVerbosity::QUIET);
// Check distance between F1 and F2 positions -- must match the measurement
ASSERT_NEAR( (F1->getPPtr()->getState() - F2->getPPtr()->getState()).norm(), measurement, 1e-10);
diff --git a/src/test/gtest_constraint_odom_3D.cpp b/src/test/gtest_constraint_odom_3D.cpp
index 1fba7cd1dd83b88824adcfb7c623be64ce34589c..f3b9e0cca1770a6b06d6f2a51c408d54d982a69b 100644
--- a/src/test/gtest_constraint_odom_3D.cpp
+++ b/src/test/gtest_constraint_odom_3D.cpp
@@ -69,7 +69,7 @@ TEST(ConstraintOdom3D, fix_0_solve)
frm1->setState(x1);
// solve for frm1
- std::string report = ceres_mgr.solve(1);
+ std::string report = ceres_mgr.solve(SolverManager::ReportVerbosity::BRIEF);
ASSERT_MATRIX_APPROX(frm1->getState(), delta, 1e-6);
@@ -83,7 +83,7 @@ TEST(ConstraintOdom3D, fix_1_solve)
frm0->setState(x0);
// solve for frm0
- std::string brief_report = ceres_mgr.solve(1);
+ std::string brief_report = ceres_mgr.solve(SolverManager::ReportVerbosity::BRIEF);
ASSERT_MATRIX_APPROX(frm0->getState(), problem->zeroState(), 1e-6);
}
diff --git a/src/test/gtest_constraint_pose_2D.cpp b/src/test/gtest_constraint_pose_2D.cpp
index 220ead9cfdd33baddcc525966890ff82b0b73754..c98de12b0f2dd05628ff905b8877e40daa80ba16 100644
--- a/src/test/gtest_constraint_pose_2D.cpp
+++ b/src/test/gtest_constraint_pose_2D.cpp
@@ -59,7 +59,7 @@ TEST(ConstraintPose2D, solve)
frm0->setState(x0);
// solve for frm0
- std::string report = ceres_mgr.solve(0); // 0: nothing, 1: BriefReport, 2: FullReport
+ std::string report = ceres_mgr.solve(SolverManager::ReportVerbosity::QUIET); // 0: nothing, 1: BriefReport, 2: FullReport
ASSERT_MATRIX_APPROX(frm0->getState(), pose, 1e-6);
diff --git a/src/test/gtest_constraint_pose_3D.cpp b/src/test/gtest_constraint_pose_3D.cpp
index a22d361c842b21e80c2a63dcae5d04f621fc744d..77604701ad57fc9851169a599822d10e6beb7c77 100644
--- a/src/test/gtest_constraint_pose_3D.cpp
+++ b/src/test/gtest_constraint_pose_3D.cpp
@@ -67,7 +67,7 @@ TEST(ConstraintPose3D, solve)
frm0->setState(x0);
// solve for frm0
- std::string brief_report = ceres_mgr.solve(1);
+ std::string brief_report = ceres_mgr.solve(SolverManager::ReportVerbosity::FULL);
ASSERT_MATRIX_APPROX(frm0->getState(), pose7, 1e-6);
diff --git a/src/test/gtest_odom_2D.cpp b/src/test/gtest_odom_2D.cpp
index bd101f24074cf8fc426a8551bcce85e1bd94e97a..b8bb7388cad12f319a37e053d747df66ce0b63a5 100644
--- a/src/test/gtest_odom_2D.cpp
+++ b/src/test/gtest_odom_2D.cpp
@@ -153,10 +153,10 @@ TEST(Odom2D, ConstraintFix_and_ConstraintOdom2D)
F0->setState(Vector3s(1,2,3));
F1->setState(Vector3s(2,3,1));
F2->setState(Vector3s(3,1,2));
- std::string report = ceres_manager.solve(1);
+ std::string report = ceres_manager.solve(SolverManager::ReportVerbosity::FULL);
// std::cout << report << std::endl;
- ceres_manager.computeCovariances(ALL_MARGINALS);
+ ceres_manager.computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS);
// show(Pr);
Matrix3s P1, P2;
@@ -211,8 +211,8 @@ TEST(Odom2D, VoteForKfAndSolve)
// Origin Key Frame
FrameBasePtr origin_frame = problem->setPrior(x0, P0, t0, dt/2);
- ceres_manager.solve(0);
- ceres_manager.computeCovariances(ALL_MARGINALS);
+ ceres_manager.solve(SolverManager::ReportVerbosity::BRIEF);
+ ceres_manager.computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS);
// std::cout << "Initial pose : " << problem->getCurrentState().transpose() << std::endl;
// std::cout << "Initial covariance : " << std::endl << problem->getLastKeyFrameCovariance() << std::endl;
@@ -280,9 +280,9 @@ TEST(Odom2D, VoteForKfAndSolve)
}
// Solve
- std::string report = ceres_manager.solve(1);
+ std::string report = ceres_manager.solve(SolverManager::ReportVerbosity::BRIEF);
// std::cout << report << std::endl;
- ceres_manager.computeCovariances(ALL_MARGINALS);
+ ceres_manager.computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS);
ASSERT_MATRIX_APPROX(problem->getLastKeyFrameCovariance() , integrated_cov_vector[5], 1e-6);
}
@@ -402,9 +402,9 @@ TEST(Odom2D, KF_callback)
MotionBuffer key_buffer_n = key_capture_n->getBuffer();
- std::string report = ceres_manager.solve(1);
+ std::string report = ceres_manager.solve(SolverManager::ReportVerbosity::BRIEF);
// std::cout << report << std::endl;
- ceres_manager.computeCovariances(ALL_MARGINALS);
+ ceres_manager.computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS);
ASSERT_POSE2D_APPROX(problem->getLastKeyFramePtr()->getState() , integrated_pose_vector[n_split], 1e-6);
ASSERT_MATRIX_APPROX(problem->getLastKeyFrameCovariance() , integrated_cov_vector [n_split], 1e-6);
@@ -437,8 +437,8 @@ TEST(Odom2D, KF_callback)
keyframe_1->setState(Vector3s(2,3,1));
keyframe_2->setState(Vector3s(3,1,2));
- report = ceres_manager.solve(1);
- ceres_manager.computeCovariances(ALL_MARGINALS);
+ report = ceres_manager.solve(SolverManager::ReportVerbosity::BRIEF);
+ ceres_manager.computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS);
// check the split KF
ASSERT_POSE2D_APPROX(keyframe_1->getState() , integrated_pose_vector[m_split], 1e-6);
@@ -455,7 +455,7 @@ TEST(Odom2D, KF_callback)
t += dt;
// WOLF_DEBUG(" estimated(", t, ") = ", problem->getState(t).transpose());
// WOLF_DEBUG("ground truth(", t, ") = ", integrated_pose_vector[n].transpose());
- ASSERT_MATRIX_APPROX(problem->getState(t), integrated_pose_vector[n], 1e-6);
+ EXPECT_POSE2D_APPROX(problem->getState(t), integrated_pose_vector[n], 1e-6);
}
}
diff --git a/src/test/gtest_problem.cpp b/src/test/gtest_problem.cpp
index 1d0a26ae8e027befecc08dfc62e46a36abe516e6..420e9b271a16e70c41bd72df083f9ae6137cf5eb 100644
--- a/src/test/gtest_problem.cpp
+++ b/src/test/gtest_problem.cpp
@@ -79,7 +79,7 @@ TEST(Problem, Installers)
SensorBasePtr S = P->installSensor ("ODOM 3D", "odometer", xs, wolf_root + "/src/examples/sensor_odom_3D.yaml");
// install processor tracker (dummy installation under an Odometry sensor -- it's OK for this test)
- ProcessorBasePtr pt = std::make_shared<ProcessorTrackerFeatureDummy>(ProcessorTrackerFeatureDummy(0.1, 5, 10));
+ ProcessorBasePtr pt = std::make_shared<ProcessorTrackerFeatureDummy>(0.1, 5, 10);
S->addProcessor(pt);
// check motion processor IS NOT set
@@ -210,21 +210,21 @@ TEST(Problem, StateBlocks)
// 2 state blocks, fixed
SensorBasePtr Sm = P->installSensor ("ODOM 3D", "odometer",xs, wolf_root + "/src/examples/sensor_odom_3D.yaml");
ASSERT_EQ(P->getStateBlockList().size(), 2);
- ASSERT_EQ(P->getStateBlockNotificationList().size(), 2);
+ ASSERT_EQ(P->getNotifiedStateBlockList().size(), 2);
// 3 state blocks, fixed
SensorBasePtr St = P->installSensor ("CAMERA", "camera", xs, wolf_root + "/src/examples/camera_params_ueye_sim.yaml");
ASSERT_EQ(P->getStateBlockList().size(), 2 + 3);
- ASSERT_EQ(P->getStateBlockNotificationList().size(), 2 + 3);
+ ASSERT_EQ(P->getNotifiedStateBlockList().size(), 2 + 3);
- ProcessorBasePtr pt = std::make_shared<ProcessorTrackerFeatureDummy>(ProcessorTrackerFeatureDummy(0.1, 5, 10));
+ ProcessorBasePtr pt = std::make_shared<ProcessorTrackerFeatureDummy>(0.1, 5, 10);
St->addProcessor(pt);
ProcessorBasePtr pm = P->installProcessor("ODOM 3D", "odom integrator", "odometer", wolf_root + "/src/examples/processor_odom_3D.yaml");
// 2 state blocks, estimated
P->emplaceFrame("PO 3D", KEY_FRAME, xs, 0);
ASSERT_EQ(P->getStateBlockList().size(), 2 + 3 + 2);
- ASSERT_EQ(P->getStateBlockNotificationList().size(), 2 + 3 + 2);
+ ASSERT_EQ(P->getNotifiedStateBlockList().size(), 2 + 3 + 2);
// P->print(4,1,1,1);
@@ -232,7 +232,7 @@ TEST(Problem, StateBlocks)
// change some SB properties
St->unfixExtrinsics();
ASSERT_EQ(P->getStateBlockList().size(), 2 + 3 + 2);
- ASSERT_EQ(P->getStateBlockNotificationList().size(), 2 + 3 + 2 + 2); // XXX: 2 more notifications on the same SB!
+ ASSERT_EQ(P->getNotifiedStateBlockList().size(), 2 + 3 + 2 /*+ 2*/); // XXX: 2 more notifications on the same SB!
// P->print(4,1,1,1);
}
@@ -246,7 +246,7 @@ TEST(Problem, Covariances)
SensorBasePtr Sm = P->installSensor ("ODOM 3D", "odometer",xs, wolf_root + "/src/examples/sensor_odom_3D.yaml");
SensorBasePtr St = P->installSensor ("CAMERA", "camera", xs, wolf_root + "/src/examples/camera_params_ueye_sim.yaml");
- ProcessorBasePtr pt = std::make_shared<ProcessorTrackerFeatureDummy>(ProcessorTrackerFeatureDummy(0.1, 5, 10));
+ ProcessorBasePtr pt = std::make_shared<ProcessorTrackerFeatureDummy>(0.1, 5, 10);
St->addProcessor(pt);
ProcessorBasePtr pm = P->installProcessor("ODOM 3D", "odom integrator", "odometer", wolf_root + "/src/examples/processor_odom_3D.yaml");
diff --git a/src/test/gtest_trajectory.cpp b/src/test/gtest_trajectory.cpp
index f6835809e5074b2bcf30c789c8e38aaff0e268bb..0c1b2cdf7597f22c042c3c09ad61a77943cfdcaf 100644
--- a/src/test/gtest_trajectory.cpp
+++ b/src/test/gtest_trajectory.cpp
@@ -17,6 +17,63 @@
using namespace wolf;
+struct DummyNotificationProcessor
+{
+ DummyNotificationProcessor(ProblemPtr _problem)
+ : problem_(_problem)
+ {
+ //
+ }
+
+ void update()
+ {
+ if (problem_ == nullptr)
+ {
+ FAIL() << "problem_ is nullptr !";
+ }
+
+ StateBlockList& states = problem_->getNotifiedStateBlockList();
+
+ for (StateBlockPtr& state : states)
+ {
+ const auto notifications = state->consumeNotifications();
+
+ for (const auto notif : notifications)
+ {
+ switch (notif)
+ {
+ case StateBlock::Notification::ADD:
+ {
+ break;
+ }
+ case StateBlock::Notification::STATE_UPDATE:
+ {
+ break;
+ }
+ case StateBlock::Notification::FIX_UPDATE:
+ {
+ break;
+ }
+ case StateBlock::Notification::REMOVE:
+ {
+ break;
+ }
+ default:
+ throw std::runtime_error("SolverManager::update: State Block notification "
+ "must be ADD, STATE_UPDATE, FIX_UPDATE, REMOVE or ENABLED.");
+ }
+ }
+
+ ASSERT_FALSE(state->hasNotifications());
+ }
+
+ states.clear();
+ }
+
+ ProblemPtr problem_;
+};
+
+
/// Set to true if you want debug info
bool debug = false;
@@ -63,6 +120,8 @@ TEST(TrajectoryBase, Add_Remove_Frame)
ProblemPtr P = Problem::create("PO 2D");
TrajectoryBasePtr T = P->getTrajectoryPtr();
+ DummyNotificationProcessor N(P);
+
// Trajectory status:
// kf1 kf2 f3 frames
// 1 2 3 time stamps
@@ -77,29 +136,31 @@ TEST(TrajectoryBase, Add_Remove_Frame)
if (debug) P->print(2,0,0,0);
ASSERT_EQ(T->getFrameList(). size(), 1);
ASSERT_EQ(P->getStateBlockList(). size(), 2);
- ASSERT_EQ(P->getStateBlockNotificationList().size(), 2);
+ ASSERT_EQ(P->getNotifiedStateBlockList().size(), 2);
T->addFrame(f2); // KF
if (debug) P->print(2,0,0,0);
ASSERT_EQ(T->getFrameList(). size(), 2);
ASSERT_EQ(P->getStateBlockList(). size(), 4);
- ASSERT_EQ(P->getStateBlockNotificationList().size(), 4);
+ ASSERT_EQ(P->getNotifiedStateBlockList().size(), 4);
T->addFrame(f3); // F
if (debug) P->print(2,0,0,0);
ASSERT_EQ(T->getFrameList(). size(), 3);
ASSERT_EQ(P->getStateBlockList(). size(), 4);
- ASSERT_EQ(P->getStateBlockNotificationList().size(), 4);
+ ASSERT_EQ(P->getNotifiedStateBlockList().size(), 4);
ASSERT_EQ(T->getLastFramePtr()->id(), f3->id());
ASSERT_EQ(T->getLastKeyFramePtr()->id(), f2->id());
+ N.update();
+
// remove frames and keyframes
f2->remove(); // KF
if (debug) P->print(2,0,0,0);
ASSERT_EQ(T->getFrameList(). size(), 2);
ASSERT_EQ(P->getStateBlockList(). size(), 2);
- ASSERT_EQ(P->getStateBlockNotificationList().size(), 2);
+ ASSERT_EQ(P->getNotifiedStateBlockList().size(), 2);
ASSERT_EQ(T->getLastFramePtr()->id(), f3->id());
ASSERT_EQ(T->getLastKeyFramePtr()->id(), f1->id());
@@ -108,7 +169,7 @@ TEST(TrajectoryBase, Add_Remove_Frame)
if (debug) P->print(2,0,0,0);
ASSERT_EQ(T->getFrameList(). size(), 1);
ASSERT_EQ(P->getStateBlockList(). size(), 2);
- ASSERT_EQ(P->getStateBlockNotificationList().size(), 2);
+ ASSERT_EQ(P->getNotifiedStateBlockList().size(), 2);
ASSERT_EQ(T->getLastKeyFramePtr()->id(), f1->id());
@@ -116,8 +177,11 @@ TEST(TrajectoryBase, Add_Remove_Frame)
if (debug) P->print(2,0,0,0);
ASSERT_EQ(T->getFrameList(). size(), 0);
ASSERT_EQ(P->getStateBlockList(). size(), 0);
- ASSERT_EQ(P->getStateBlockNotificationList().size(), 0);
+ ASSERT_EQ(P->getNotifiedStateBlockList().size(), 4);
+
+ N.update();
+ ASSERT_EQ(P->getNotifiedStateBlockList().size(), 0);
}
TEST(TrajectoryBase, KeyFramesAreSorted)