Skip to content
Snippets Groups Projects
Commit ab7dfc8c authored by Joan Vallvé Navarro's avatar Joan Vallvé Navarro
Browse files

Added getLastKeyFrame() to trajectory and problem used in ceres_manager

parent 7244521f
No related branches found
No related tags found
No related merge requests found
...@@ -52,7 +52,7 @@ ceres::Solver::Summary CeresManager::solve(const ceres::Solver::Options& _ceres_ ...@@ -52,7 +52,7 @@ ceres::Solver::Summary CeresManager::solve(const ceres::Solver::Options& _ceres_
void CeresManager::computeCovariances(CovarianceBlocksToBeComputed _blocks) void CeresManager::computeCovariances(CovarianceBlocksToBeComputed _blocks)
{ {
//std::cout << "computing covariances..." << std::endl; //std::cout << "CeresManager: computing covariances..." << std::endl;
// CLEAR STORED COVARIANCE BLOCKS IN WOLF PROBLEM // CLEAR STORED COVARIANCE BLOCKS IN WOLF PROBLEM
wolf_problem_->clearCovariance(); wolf_problem_->clearCovariance();
...@@ -128,30 +128,30 @@ void CeresManager::computeCovariances(CovarianceBlocksToBeComputed _blocks) ...@@ -128,30 +128,30 @@ void CeresManager::computeCovariances(CovarianceBlocksToBeComputed _blocks)
case ROBOT_LANDMARKS: case ROBOT_LANDMARKS:
{ {
//robot-robot //robot-robot
FrameBase* last_frame = wolf_problem_->getTrajectoryPtr()->getFrameListPtr()->back(); auto last_key_frame = wolf_problem_->getLastKeyFramePtr();
state_block_pairs.push_back(std::make_pair(last_frame->getPPtr(), last_frame->getPPtr())); state_block_pairs.push_back(std::make_pair(last_key_frame->getPPtr(), last_key_frame->getPPtr()));
state_block_pairs.push_back(std::make_pair(last_frame->getPPtr(), last_frame->getOPtr())); state_block_pairs.push_back(std::make_pair(last_key_frame->getPPtr(), last_key_frame->getOPtr()));
state_block_pairs.push_back(std::make_pair(last_frame->getOPtr(), last_frame->getOPtr())); state_block_pairs.push_back(std::make_pair(last_key_frame->getOPtr(), last_key_frame->getOPtr()));
double_pairs.push_back(std::make_pair(last_frame->getPPtr()->getPtr(), last_frame->getPPtr()->getPtr())); double_pairs.push_back(std::make_pair(last_key_frame->getPPtr()->getPtr(), last_key_frame->getPPtr()->getPtr()));
double_pairs.push_back(std::make_pair(last_frame->getPPtr()->getPtr(), last_frame->getOPtr()->getPtr())); double_pairs.push_back(std::make_pair(last_key_frame->getPPtr()->getPtr(), last_key_frame->getOPtr()->getPtr()));
double_pairs.push_back(std::make_pair(last_frame->getOPtr()->getPtr(), last_frame->getOPtr()->getPtr())); double_pairs.push_back(std::make_pair(last_key_frame->getOPtr()->getPtr(), last_key_frame->getOPtr()->getPtr()));
for(auto l_ptr : *(wolf_problem_->getMapPtr()->getLandmarkListPtr())) for(auto l_ptr : *(wolf_problem_->getMapPtr()->getLandmarkListPtr()))
{ {
state_block_pairs.push_back(std::make_pair(last_frame->getPPtr(), l_ptr->getPPtr())); state_block_pairs.push_back(std::make_pair(last_key_frame->getPPtr(), l_ptr->getPPtr()));
state_block_pairs.push_back(std::make_pair(last_frame->getPPtr(), l_ptr->getOPtr())); state_block_pairs.push_back(std::make_pair(last_key_frame->getPPtr(), l_ptr->getOPtr()));
state_block_pairs.push_back(std::make_pair(last_frame->getOPtr(), l_ptr->getPPtr())); state_block_pairs.push_back(std::make_pair(last_key_frame->getOPtr(), l_ptr->getPPtr()));
state_block_pairs.push_back(std::make_pair(last_frame->getOPtr(), l_ptr->getOPtr())); state_block_pairs.push_back(std::make_pair(last_key_frame->getOPtr(), l_ptr->getOPtr()));
state_block_pairs.push_back(std::make_pair(l_ptr->getPPtr(), l_ptr->getPPtr())); state_block_pairs.push_back(std::make_pair(l_ptr->getPPtr(), l_ptr->getPPtr()));
state_block_pairs.push_back(std::make_pair(l_ptr->getPPtr(), l_ptr->getOPtr())); state_block_pairs.push_back(std::make_pair(l_ptr->getPPtr(), l_ptr->getOPtr()));
state_block_pairs.push_back(std::make_pair(l_ptr->getOPtr(), l_ptr->getOPtr())); state_block_pairs.push_back(std::make_pair(l_ptr->getOPtr(), l_ptr->getOPtr()));
double_pairs.push_back(std::make_pair(last_frame->getPPtr()->getPtr(), l_ptr->getPPtr()->getPtr())); double_pairs.push_back(std::make_pair(last_key_frame->getPPtr()->getPtr(), l_ptr->getPPtr()->getPtr()));
double_pairs.push_back(std::make_pair(last_frame->getPPtr()->getPtr(), l_ptr->getOPtr()->getPtr())); double_pairs.push_back(std::make_pair(last_key_frame->getPPtr()->getPtr(), l_ptr->getOPtr()->getPtr()));
double_pairs.push_back(std::make_pair(last_frame->getOPtr()->getPtr(), l_ptr->getPPtr()->getPtr())); double_pairs.push_back(std::make_pair(last_key_frame->getOPtr()->getPtr(), l_ptr->getPPtr()->getPtr()));
double_pairs.push_back(std::make_pair(last_frame->getOPtr()->getPtr(), l_ptr->getOPtr()->getPtr())); double_pairs.push_back(std::make_pair(last_key_frame->getOPtr()->getPtr(), l_ptr->getOPtr()->getPtr()));
double_pairs.push_back(std::make_pair(l_ptr->getPPtr()->getPtr(), l_ptr->getPPtr()->getPtr())); double_pairs.push_back(std::make_pair(l_ptr->getPPtr()->getPtr(), l_ptr->getPPtr()->getPtr()));
double_pairs.push_back(std::make_pair(l_ptr->getPPtr()->getPtr(), l_ptr->getOPtr()->getPtr())); double_pairs.push_back(std::make_pair(l_ptr->getPPtr()->getPtr(), l_ptr->getOPtr()->getPtr()));
double_pairs.push_back(std::make_pair(l_ptr->getOPtr()->getPtr(), l_ptr->getOPtr()->getPtr())); double_pairs.push_back(std::make_pair(l_ptr->getOPtr()->getPtr(), l_ptr->getOPtr()->getPtr()));
...@@ -159,6 +159,7 @@ void CeresManager::computeCovariances(CovarianceBlocksToBeComputed _blocks) ...@@ -159,6 +159,7 @@ void CeresManager::computeCovariances(CovarianceBlocksToBeComputed _blocks)
break; break;
} }
} }
//std::cout << "pairs... " << double_pairs.size() << std::endl;
// COMPUTE DESIRED COVARIANCES // COMPUTE DESIRED COVARIANCES
if (covariance_->Compute(double_pairs, ceres_problem_)) if (covariance_->Compute(double_pairs, ceres_problem_))
...@@ -168,8 +169,8 @@ void CeresManager::computeCovariances(CovarianceBlocksToBeComputed _blocks) ...@@ -168,8 +169,8 @@ void CeresManager::computeCovariances(CovarianceBlocksToBeComputed _blocks)
{ {
Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> cov(state_block_pairs[i].first->getSize(),state_block_pairs[i].second->getSize()); 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()); covariance_->GetCovarianceBlock(double_pairs[i].first, double_pairs[i].second, cov.data());
//std::cout << "getted covariance " << std::endl << cov << std::endl;
wolf_problem_->addCovarianceBlock(state_block_pairs[i].first, state_block_pairs[i].second, cov); wolf_problem_->addCovarianceBlock(state_block_pairs[i].first, state_block_pairs[i].second, cov);
//std::cout << "getted covariance " << std::endl << cov << std::endl;
} }
} }
else else
...@@ -178,7 +179,9 @@ void CeresManager::computeCovariances(CovarianceBlocksToBeComputed _blocks) ...@@ -178,7 +179,9 @@ void CeresManager::computeCovariances(CovarianceBlocksToBeComputed _blocks)
void CeresManager::update(const bool _apply_loss_function) void CeresManager::update(const bool _apply_loss_function)
{ {
//std::cout << "CeresManager: updating... getConstraintRemoveList()->size()" << wolf_problem_->getConstraintRemoveList()->size() << std::endl; //std::cout << "CeresManager: updating... " << std::endl;
//std::cout << wolf_problem_->getStateBlockNotificationList().size() << " state block notifications" << std::endl;
//std::cout << wolf_problem_->getConstraintNotificationList().size() << " constraint notifications" << std::endl;
// UPDATE STATE BLOCKS // UPDATE STATE BLOCKS
while (!wolf_problem_->getStateBlockNotificationList().empty()) while (!wolf_problem_->getStateBlockNotificationList().empty())
......
...@@ -93,6 +93,9 @@ void FrameBase::setKey() ...@@ -93,6 +93,9 @@ void FrameBase::setKey()
{ {
type_id_ = KEY_FRAME; type_id_ = KEY_FRAME;
registerNewStateBlocks(); registerNewStateBlocks();
if (getTrajectoryPtr()->getLastKeyFramePtr() == nullptr || getTrajectoryPtr()->getLastKeyFramePtr()->getTimeStamp() < time_stamp_)
getTrajectoryPtr()->setLastKeyFramePtr(this);
} }
} }
......
...@@ -344,6 +344,11 @@ FrameBase* Problem::getLastFramePtr() ...@@ -344,6 +344,11 @@ FrameBase* Problem::getLastFramePtr()
return trajectory_ptr_->getLastFramePtr(); return trajectory_ptr_->getLastFramePtr();
} }
FrameBase* Problem::getLastKeyFramePtr()
{
return trajectory_ptr_->getLastKeyFramePtr();;
}
StateBlockList* Problem::getStateListPtr() StateBlockList* Problem::getStateListPtr()
{ {
return &state_block_ptr_list_; return &state_block_ptr_list_;
......
...@@ -218,10 +218,14 @@ class Problem : public NodeBase ...@@ -218,10 +218,14 @@ class Problem : public NodeBase
*/ */
HardwareBase* getHardwarePtr(); HardwareBase* getHardwarePtr();
/** \brief Returns a pointer to last Frame /** \brief Returns a pointer to last frame
**/ **/
FrameBase* getLastFramePtr(); FrameBase* getLastFramePtr();
/** \brief Returns a pointer to last key frame
*/
FrameBase* getLastKeyFramePtr();
/** \brief Gets a pointer to the state units list /** \brief Gets a pointer to the state units list
*/ */
StateBlockList* getStateListPtr(); StateBlockList* getStateListPtr();
......
...@@ -4,7 +4,7 @@ namespace wolf { ...@@ -4,7 +4,7 @@ namespace wolf {
TrajectoryBase::TrajectoryBase(FrameStructure _frame_structure) : TrajectoryBase::TrajectoryBase(FrameStructure _frame_structure) :
NodeLinked(MID, "TRAJECTORY"), NodeLinked(MID, "TRAJECTORY"),
frame_structure_(_frame_structure), fixed_size_(0) frame_structure_(_frame_structure), last_key_frame_ptr_(nullptr), fixed_size_(0)
{ {
// //
} }
...@@ -18,7 +18,11 @@ FrameBase* TrajectoryBase::addFrame(FrameBase* _frame_ptr) ...@@ -18,7 +18,11 @@ FrameBase* TrajectoryBase::addFrame(FrameBase* _frame_ptr)
{ {
addDownNode(_frame_ptr); addDownNode(_frame_ptr);
if (_frame_ptr->isKey()) if (_frame_ptr->isKey())
{
_frame_ptr->registerNewStateBlocks(); _frame_ptr->registerNewStateBlocks();
if (last_key_frame_ptr_ == nullptr || last_key_frame_ptr_->getTimeStamp() < _frame_ptr->getTimeStamp())
last_key_frame_ptr_ = _frame_ptr;
}
return _frame_ptr; return _frame_ptr;
} }
......
...@@ -22,6 +22,7 @@ class TrajectoryBase : public NodeLinked<Problem,FrameBase> ...@@ -22,6 +22,7 @@ class TrajectoryBase : public NodeLinked<Problem,FrameBase>
{ {
protected: protected:
FrameStructure frame_structure_; // Defines the structure of the Frames in the Trajectory. FrameStructure frame_structure_; // Defines the structure of the Frames in the Trajectory.
FrameBase* last_key_frame_ptr_; // keeps pointer to the last key frame
// TODO: JVN: No seria millor que aixo ho tingui el problem o el wolf_manager? JS: segurament. Pero fixed_size_ seria una de les opcions de moltes... // TODO: JVN: No seria millor que aixo ho tingui el problem o el wolf_manager? JS: segurament. Pero fixed_size_ seria una de les opcions de moltes...
unsigned int fixed_size_; // Limits the number of Frames forming the Trajectory unsigned int fixed_size_; // Limits the number of Frames forming the Trajectory
...@@ -43,14 +44,21 @@ class TrajectoryBase : public NodeLinked<Problem,FrameBase> ...@@ -43,14 +44,21 @@ class TrajectoryBase : public NodeLinked<Problem,FrameBase>
**/ **/
void removeFrame(const FrameBaseIter& _frame_iter); void removeFrame(const FrameBaseIter& _frame_iter);
/** \brief Returns a pointer to Frame list /** \brief Returns a pointer to frame list
**/ **/
FrameBaseList* getFrameListPtr(); FrameBaseList* getFrameListPtr();
/** \brief Returns a pointer to last Frame /** \brief Returns a pointer to last frame
**/ **/
FrameBase* getLastFramePtr(); FrameBase* getLastFramePtr();
/** \brief Returns a pointer to last key frame
*/
FrameBase* getLastKeyFramePtr();
/** \brief Sets the pointer to last key frame
*/
void setLastKeyFramePtr(FrameBase* _key_frame_ptr);
/** \brief Returns a list of all constraints in the trajectory thru reference /** \brief Returns a list of all constraints in the trajectory thru reference
**/ **/
...@@ -74,6 +82,16 @@ inline FrameBase* TrajectoryBase::getLastFramePtr() ...@@ -74,6 +82,16 @@ inline FrameBase* TrajectoryBase::getLastFramePtr()
return getDownNodeListPtr()->back(); return getDownNodeListPtr()->back();
} }
inline FrameBase* TrajectoryBase::getLastKeyFramePtr()
{
return last_key_frame_ptr_;
}
inline void TrajectoryBase::setLastKeyFramePtr(FrameBase* _key_frame_ptr)
{
last_key_frame_ptr_ = _key_frame_ptr;
}
inline FrameStructure TrajectoryBase::getFrameStructure() const inline FrameStructure TrajectoryBase::getFrameStructure() const
{ {
return frame_structure_; return frame_structure_;
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment