Skip to content
Snippets Groups Projects
Commit fce48793 authored by Joan Solà Ortega's avatar Joan Solà Ortega
Browse files

Add getCovariance() in Frames and Lmks

parent a6c0d7f4
No related branches found
No related tags found
No related merge requests found
...@@ -224,6 +224,18 @@ unsigned int FrameBase::getSize() const ...@@ -224,6 +224,18 @@ unsigned int FrameBase::getSize() const
return size; return size;
} }
bool FrameBase::getCovariance(Eigen::MatrixXs& _cov) const
{
return getProblem()->getFrameCovariance(shared_from_this(), _cov);
}
Eigen::MatrixXs FrameBase::getCovariance() const
{
return getProblem()->getFrameCovariance(shared_from_this());
}
FrameBasePtr FrameBase::getPreviousFrame() const FrameBasePtr FrameBase::getPreviousFrame() const
{ {
//std::cout << "finding previous frame of " << this->frame_id_ << std::endl; //std::cout << "finding previous frame of " << this->frame_id_ << std::endl;
......
...@@ -109,6 +109,8 @@ class FrameBase : public NodeBase, public std::enable_shared_from_this<FrameBase ...@@ -109,6 +109,8 @@ class FrameBase : public NodeBase, public std::enable_shared_from_this<FrameBase
Eigen::VectorXs getState() const; Eigen::VectorXs getState() const;
void getState(Eigen::VectorXs& _state) const; void getState(Eigen::VectorXs& _state) const;
unsigned int getSize() const; unsigned int getSize() const;
bool getCovariance(Eigen::MatrixXs& _cov) const;
Eigen::MatrixXs getCovariance() const;
// Wolf tree access --------------------------------------------------- // Wolf tree access ---------------------------------------------------
public: public:
......
...@@ -68,13 +68,14 @@ class LandmarkBase : public NodeBase, public std::enable_shared_from_this<Landma ...@@ -68,13 +68,14 @@ class LandmarkBase : public NodeBase, public std::enable_shared_from_this<Landma
void setStateBlockPtr(unsigned int _i, StateBlockPtr _sb_ptr); void setStateBlockPtr(unsigned int _i, StateBlockPtr _sb_ptr);
StateBlockPtr getPPtr() const; StateBlockPtr getPPtr() const;
StateBlockPtr getOPtr() const; StateBlockPtr getOPtr() const;
StateBlockPtr getVPtr() const;
void setPPtr(const StateBlockPtr _p_ptr); void setPPtr(const StateBlockPtr _p_ptr);
void setOPtr(const StateBlockPtr _o_ptr); void setOPtr(const StateBlockPtr _o_ptr);
void setVPtr(const StateBlockPtr _v_ptr);
virtual void registerNewStateBlocks(); virtual void registerNewStateBlocks();
Eigen::VectorXs getState() const; Eigen::VectorXs getState() const;
void getState(Eigen::VectorXs& _state) const; void getState(Eigen::VectorXs& _state) const;
bool getCovariance(Eigen::MatrixXs& _cov) const;
Eigen::MatrixXs getCovariance() const;
protected: protected:
virtual void removeStateBlocks(); virtual void removeStateBlocks();
...@@ -104,6 +105,16 @@ class LandmarkBase : public NodeBase, public std::enable_shared_from_this<Landma ...@@ -104,6 +105,16 @@ class LandmarkBase : public NodeBase, public std::enable_shared_from_this<Landma
namespace wolf{ namespace wolf{
inline bool LandmarkBase::getCovariance(Eigen::MatrixXs& _cov) const
{
return getProblem()->getLandmarkCovariance(shared_from_this(), _cov);
}
inline Eigen::MatrixXs LandmarkBase::getCovariance() const
{
return getProblem()->getLandmarkCovariance(shared_from_this());
}
inline MapBasePtr LandmarkBase::getMapPtr() inline MapBasePtr LandmarkBase::getMapPtr()
{ {
return map_ptr_.lock(); return map_ptr_.lock();
......
...@@ -512,13 +512,8 @@ bool Problem::getCovarianceBlock(StateBlockPtr _state, Eigen::MatrixXs& _cov, co ...@@ -512,13 +512,8 @@ bool Problem::getCovarianceBlock(StateBlockPtr _state, Eigen::MatrixXs& _cov, co
return getCovarianceBlock(_state, _state, _cov, _row_and_col, _row_and_col); return getCovarianceBlock(_state, _state, _cov, _row_and_col, _row_and_col);
} }
bool Problem::getFrameCovariance(FrameBasePtr _frame_ptr, Eigen::MatrixXs& _covariance) bool Problem::getFrameCovariance(FrameBaseConstPtr _frame_ptr, Eigen::MatrixXs& _covariance)
{ {
// return getCovarianceBlock(_frame_ptr->getPPtr(), _frame_ptr->getPPtr(), _covariance, 0, 0 ) &&
// getCovarianceBlock(_frame_ptr->getPPtr(), _frame_ptr->getOPtr(), _covariance, 0, _frame_ptr->getPPtr()->getSize()) &&
// getCovarianceBlock(_frame_ptr->getOPtr(), _frame_ptr->getPPtr(), _covariance, _frame_ptr->getPPtr()->getSize(), 0 ) &&
// getCovarianceBlock(_frame_ptr->getOPtr(), _frame_ptr->getOPtr(), _covariance, _frame_ptr->getPPtr()->getSize(), _frame_ptr->getPPtr()->getSize());
bool success(true); bool success(true);
int i = 0, j = 0; int i = 0, j = 0;
...@@ -543,7 +538,7 @@ bool Problem::getFrameCovariance(FrameBasePtr _frame_ptr, Eigen::MatrixXs& _cova ...@@ -543,7 +538,7 @@ bool Problem::getFrameCovariance(FrameBasePtr _frame_ptr, Eigen::MatrixXs& _cova
return success; return success;
} }
Eigen::MatrixXs Problem::getFrameCovariance(FrameBasePtr _frame_ptr) Eigen::MatrixXs Problem::getFrameCovariance(FrameBaseConstPtr _frame_ptr)
{ {
Size sz = 0; Size sz = 0;
for (const auto& sb : _frame_ptr->getStateBlockVec()) for (const auto& sb : _frame_ptr->getStateBlockVec())
...@@ -562,7 +557,7 @@ Eigen::MatrixXs Problem::getLastKeyFrameCovariance() ...@@ -562,7 +557,7 @@ Eigen::MatrixXs Problem::getLastKeyFrameCovariance()
return getFrameCovariance(frm); return getFrameCovariance(frm);
} }
bool Problem::getLandmarkCovariance(LandmarkBasePtr _landmark_ptr, Eigen::MatrixXs& _covariance) bool Problem::getLandmarkCovariance(LandmarkBaseConstPtr _landmark_ptr, Eigen::MatrixXs& _covariance)
{ {
bool success(true); bool success(true);
int i = 0, j = 0; int i = 0, j = 0;
...@@ -588,7 +583,7 @@ bool Problem::getLandmarkCovariance(LandmarkBasePtr _landmark_ptr, Eigen::Matrix ...@@ -588,7 +583,7 @@ bool Problem::getLandmarkCovariance(LandmarkBasePtr _landmark_ptr, Eigen::Matrix
return success; return success;
} }
Eigen::MatrixXs Problem::getLandmarkCovariance(LandmarkBasePtr _landmark_ptr) Eigen::MatrixXs Problem::getLandmarkCovariance(LandmarkBaseConstPtr _landmark_ptr)
{ {
Size sz = 0; Size sz = 0;
for (const auto& sb : _landmark_ptr->getStateBlockVec()) for (const auto& sb : _landmark_ptr->getStateBlockVec())
......
...@@ -252,11 +252,11 @@ class Problem : public std::enable_shared_from_this<Problem> ...@@ -252,11 +252,11 @@ class Problem : public std::enable_shared_from_this<Problem>
bool getCovarianceBlock(StateBlockPtr _state1, StateBlockPtr _state2, Eigen::MatrixXs& _cov, const int _row = 0, const int _col=0); bool getCovarianceBlock(StateBlockPtr _state1, StateBlockPtr _state2, Eigen::MatrixXs& _cov, const int _row = 0, const int _col=0);
bool getCovarianceBlock(std::map<StateBlockPtr, unsigned int> _sb_2_idx, Eigen::MatrixXs& _cov); bool getCovarianceBlock(std::map<StateBlockPtr, unsigned int> _sb_2_idx, Eigen::MatrixXs& _cov);
bool getCovarianceBlock(StateBlockPtr _state, Eigen::MatrixXs& _cov, const int _row_and_col = 0); bool getCovarianceBlock(StateBlockPtr _state, Eigen::MatrixXs& _cov, const int _row_and_col = 0);
bool getFrameCovariance(FrameBasePtr _frame_ptr, Eigen::MatrixXs& _covariance); bool getFrameCovariance(FrameBaseConstPtr _frame_ptr, Eigen::MatrixXs& _covariance);
Eigen::MatrixXs getFrameCovariance(FrameBasePtr _frame_ptr); Eigen::MatrixXs getFrameCovariance(FrameBaseConstPtr _frame_ptr);
Eigen::MatrixXs getLastKeyFrameCovariance(); Eigen::MatrixXs getLastKeyFrameCovariance();
bool getLandmarkCovariance(LandmarkBasePtr _landmark_ptr, Eigen::MatrixXs& _covariance); bool getLandmarkCovariance(LandmarkBaseConstPtr _landmark_ptr, Eigen::MatrixXs& _covariance);
Eigen::MatrixXs getLandmarkCovariance(LandmarkBasePtr _landmark_ptr); Eigen::MatrixXs getLandmarkCovariance(LandmarkBaseConstPtr _landmark_ptr);
// Solver management ---------------------------------------- // Solver management ----------------------------------------
......
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