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

Derive CaptureBase from HasStateBlocks

parent 24156ce3
No related branches found
No related tags found
1 merge request!323Resolve "New data structure for storing stateblocks"
Hide whitespace changes
Inline Side-by-side
include/core/capture/capture_base.h
+ 24
− 23
View file @ ab9f5fdc
......@@ -11,13 +11,14 @@ class FeatureBase;
#include "core/common/wolf.h"
#include "core/common/node_base.h"
#include "core/common/time_stamp.h"
#include "core/state_block/has_state_blocks.h"
//std includes
namespace wolf{
//class CaptureBase
class CaptureBase : public NodeBase, public std::enable_shared_from_this<CaptureBase>
class CaptureBase : public NodeBase, public HasStateBlocks, public std::enable_shared_from_this<CaptureBase>
{
friend FeatureBase;
friend FactorBase;
......@@ -29,7 +30,7 @@ class CaptureBase : public NodeBase, public std::enable_shared_from_this<Capture
FactorBasePtrList constrained_by_list_;
SensorBaseWPtr sensor_ptr_; ///< Pointer to sensor
// Deal with sensors with dynamic extrinsics (check dynamic_extrinsic_ in SensorBase)
std::vector<StateBlockPtr> state_block_vec_; ///< vector of state blocks, in the order P, O, intrinsic.
// std::vector<StateBlockPtr> state_block_vec_; ///< vector of state blocks, in the order P, O, intrinsic.
SizeEigen calib_size_; ///< size of the calibration parameters (dynamic or static sensor params that are not fixed)
static unsigned int capture_id_count_;
......@@ -82,10 +83,10 @@ class CaptureBase : public NodeBase, public std::enable_shared_from_this<Capture
const FactorBasePtrList& getConstrainedByList() const;
// State blocks
const std::vector<StateBlockPtr>& getStateBlockVec() const;
std::vector<StateBlockPtr>& getStateBlockVec();
StateBlockPtr getStateBlock(unsigned int _i) const;
void setStateBlock(unsigned int _i, const StateBlockPtr _sb_ptr);
// const std::vector<StateBlockPtr>& getStateBlockVec() const;
// std::vector<StateBlockPtr>& getStateBlockVec();
StateBlockPtr getStateBlock(const std::string& key) const;
// void setStateBlock(unsigned int _i, const StateBlockPtr _sb_ptr);
StateBlockPtr getSensorP() const;
StateBlockPtr getSensorO() const;
......@@ -147,34 +148,34 @@ inline void CaptureBase::updateCalibSize()
calib_size_ = computeCalibSize();
}
inline const std::vector<StateBlockPtr>& CaptureBase::getStateBlockVec() const
{
return state_block_vec_;
}
inline std::vector<StateBlockPtr>& CaptureBase::getStateBlockVec()
{
return state_block_vec_;
}
inline void CaptureBase::setStateBlock(unsigned int _i, const StateBlockPtr _sb_ptr)
{
state_block_vec_[_i] = _sb_ptr;
}
//inline const std::vector<StateBlockPtr>& CaptureBase::getStateBlockVec() const
//{
// return state_block_vec_;
//}
//
//inline std::vector<StateBlockPtr>& CaptureBase::getStateBlockVec()
//{
// return state_block_vec_;
//}
//
//inline void CaptureBase::setStateBlock(unsigned int _i, const StateBlockPtr _sb_ptr)
//{
// state_block_vec_[_i] = _sb_ptr;
//}
inline StateBlockPtr CaptureBase::getSensorP() const
{
return getStateBlock(0);
return getStateBlock("P");
}
inline StateBlockPtr CaptureBase::getSensorO() const
{
return getStateBlock(1);
return getStateBlock("O");
}
inline StateBlockPtr CaptureBase::getSensorIntrinsic() const
{
return getStateBlock(2);
return getStateBlock("I");
}
inline unsigned int CaptureBase::id() const
......
src/capture/capture_base.cpp
+ 135
− 52
View file @ ab9f5fdc
......@@ -14,9 +14,10 @@ CaptureBase::CaptureBase(const std::string& _type,
StateBlockPtr _o_ptr,
StateBlockPtr _intr_ptr) :
NodeBase("CAPTURE", _type),
HasStateBlocks(""),
frame_ptr_(), // nullptr
sensor_ptr_(_sensor_ptr),
state_block_vec_(3),
// state_block_vec_(3),
calib_size_(0),
capture_id_(++capture_id_count_),
time_stamp_(_ts)
......@@ -27,10 +28,12 @@ CaptureBase::CaptureBase(const std::string& _type,
if (_sensor_ptr->isExtrinsicDynamic())
{
assert(_p_ptr && "Pointer to dynamic position params is null!");
setStateBlock("P", _p_ptr);
appendToStructure("P");
assert(_o_ptr && "Pointer to dynamic orientation params is null!");
// assign to Capture's members
state_block_vec_[0] = _p_ptr;
state_block_vec_[1] = _o_ptr;
setStateBlock("O", _o_ptr);
appendToStructure("O");
}
else if (_p_ptr || _o_ptr)
{
......@@ -40,8 +43,8 @@ CaptureBase::CaptureBase(const std::string& _type,
if (_sensor_ptr->isIntrinsicDynamic())
{
assert(_intr_ptr && "Pointer to dynamic intrinsic params is null!");
// assign to Capture's member
state_block_vec_[2] = _intr_ptr;
setStateBlock("I", _intr_ptr);
appendToStructure("I");
}
else if (_intr_ptr)
{
......@@ -124,56 +127,107 @@ void CaptureBase::removeConstrainedBy(FactorBasePtr _fac_ptr)
constrained_by_list_.remove(_fac_ptr);
}
StateBlockPtr CaptureBase::getStateBlock(unsigned int _i) const
//StateBlockPtr CaptureBase::getStateBlock(unsigned int _i) const
//{
// if (getSensor())
// {
// if (_i < 2) // _i == 0 is position, 1 is orientation, 2 and onwards are intrinsics
// if (getSensor()->extrinsicsInCaptures())
// {
// assert(_i < state_block_vec_.size() && "Requested a state block pointer out of the vector range!");
// return state_block_vec_[_i];
// }
// else
// return getSensor()->getStateBlockPtrStatic(_i);
//
// else // 2 and onwards are intrinsics
// if (getSensor()->intrinsicsInCaptures())
// {
// assert(_i < state_block_vec_.size() && "Requested a state block pointer out of the vector range!");
// return state_block_vec_[_i];
// }
// else
// return getSensor()->getStateBlockPtrStatic(_i);
// }
// else // No sensor associated: assume sensor params are here
// {
// assert(_i < state_block_vec_.size() && "Requested a state block pointer out of the vector range!");
// return state_block_vec_[_i];
// }
//}
StateBlockPtr CaptureBase::getStateBlock(const std::string& key) const
{
if (getSensor())
{
if (_i < 2) // _i == 0 is position, 1 is orientation, 2 and onwards are intrinsics
if (key == "P") // extrinsics
{
if (getSensor()->extrinsicsInCaptures())
{
assert(_i < state_block_vec_.size() && "Requested a state block pointer out of the vector range!");
return state_block_vec_[_i];
}
return HasStateBlocks::getStateBlock(key);
else
return getSensor()->getStateBlockPtrStatic(0);
}
if (key == "O") // extrinsics
{
if (getSensor()->extrinsicsInCaptures())
return HasStateBlocks::getStateBlock(key);
else
return getSensor()->getStateBlockPtrStatic(_i);
return getSensor()->getStateBlockPtrStatic(1);
}
else // 2 and onwards are intrinsics
if (getSensor()->intrinsicsInCaptures())
else // intrinsics
{
assert(_i < state_block_vec_.size() && "Requested a state block pointer out of the vector range!");
return state_block_vec_[_i];
if (getSensor()->intrinsicsInCaptures())
return HasStateBlocks::getStateBlock(key);
else
return getSensor()->getStateBlockPtrStatic(2);
}
else
return getSensor()->getStateBlockPtrStatic(_i);
}
else // No sensor associated: assume sensor params are here
{
assert(_i < state_block_vec_.size() && "Requested a state block pointer out of the vector range!");
return state_block_vec_[_i];
}
return HasStateBlocks::getStateBlock(key);
}
//void CaptureBase::removeStateBlocks()
//{
// for (unsigned int i = 0; i < state_block_vec_.size(); i++)
// {
// auto sbp = state_block_vec_[i];
// if (sbp != nullptr)
// {
// if (getProblem() != nullptr)
// {
// getProblem()->notifyStateBlock(sbp, REMOVE);
// }
// setStateBlock(i, nullptr);
// }
// }
//}
void CaptureBase::removeStateBlocks()
{
for (unsigned int i = 0; i < state_block_vec_.size(); i++)
{
auto sbp = state_block_vec_[i];
for (const char& key : getStructure()) // note: key is a char
{
auto sbp = HasStateBlocks::getStateBlock(key);
if (sbp != nullptr)
{
if (getProblem() != nullptr)
{
getProblem()->notifyStateBlock(sbp, REMOVE);
}
setStateBlock(i, nullptr);
}
removeStateBlock(key);
}
}
void CaptureBase::fix()
{
for (unsigned int i = 0; i<getStateBlockVec().size(); i++)
// for (unsigned int i = 0; i<getStateBlockVec().size(); i++)
// {
// auto sbp = getStateBlock(i);
for (auto pair_key_sbp : getStateBlockMap())
{
auto sbp = getStateBlock(i);
auto sbp = pair_key_sbp.second;
if (sbp != nullptr)
sbp->fix();
}
......@@ -182,9 +236,12 @@ void CaptureBase::fix()
void CaptureBase::unfix()
{
for (unsigned int i = 0; i<getStateBlockVec().size(); i++)
// for (unsigned int i = 0; i<getStateBlockVec().size(); i++)
// {
// auto sbp = getStateBlock(i);
for (auto pair_key_sbp : getStateBlockMap())
{
auto sbp = getStateBlock(i);
auto sbp = pair_key_sbp.second;
if (sbp != nullptr)
sbp->unfix();
}
......@@ -193,9 +250,12 @@ void CaptureBase::unfix()
void CaptureBase::fixExtrinsics()
{
for (unsigned int i = 0; i < 2; i++)
// for (unsigned int i = 0; i < 2; i++)
// {
// auto sbp = getStateBlock(i);
for (auto key : "PO")
{
auto sbp = getStateBlock(i);
auto sbp = HasStateBlocks::getStateBlock(key);
if (sbp != nullptr)
sbp->fix();
}
......@@ -204,9 +264,12 @@ void CaptureBase::fixExtrinsics()
void CaptureBase::unfixExtrinsics()
{
for (unsigned int i = 0; i < 2; i++)
// for (unsigned int i = 0; i < 2; i++)
// {
// auto sbp = getStateBlock(i);
for (auto key : "PO")
{
auto sbp = getStateBlock(i);
auto sbp = HasStateBlocks::getStateBlock(key);
if (sbp != nullptr)
sbp->unfix();
}
......@@ -215,22 +278,30 @@ void CaptureBase::unfixExtrinsics()
void CaptureBase::fixIntrinsics()
{
for (unsigned int i = 2; i < getStateBlockVec().size(); i++)
// for (unsigned int i = 2; i < getStateBlockVec().size(); i++)
for (auto& key : getStructure())
{
auto sbp = getStateBlock(i);
if (sbp != nullptr)
sbp->fix();
if (key != 'P' and key != 'O')
{
auto sbp = HasStateBlocks::getStateBlock(key);
if (sbp != nullptr)
sbp->fix();
}
}
updateCalibSize();
}
void CaptureBase::unfixIntrinsics()
{
for (unsigned int i = 2; i < getStateBlockVec().size(); i++)
// for (unsigned int i = 2; i < getStateBlockVec().size(); i++)
for (auto& key : getStructure())
{
auto sbp = getStateBlock(i);
if (sbp != nullptr)
sbp->unfix();
if (key != 'P' and key != 'O')
{
auto sbp = HasStateBlocks::getStateBlock(key);
if (sbp != nullptr)
sbp->unfix();
}
}
updateCalibSize();
}
......@@ -239,18 +310,26 @@ void CaptureBase::registerNewStateBlocks()
{
if (getProblem() != nullptr)
{
for (auto sbp : getStateBlockVec())
if (sbp != nullptr)
getProblem()->notifyStateBlock(sbp,ADD);
for (auto pair_key_sbp : getStateBlockMap())
if (pair_key_sbp.second != nullptr)
getProblem()->notifyStateBlock(pair_key_sbp.second,ADD);
}
// if (getProblem() != nullptr)
// {
// for (auto sbp : getStateBlockVec())
// if (sbp != nullptr)
// getProblem()->notifyStateBlock(sbp,ADD);
// }
}
SizeEigen CaptureBase::computeCalibSize() const
{
SizeEigen sz = 0;
for (unsigned int i = 0; i < state_block_vec_.size(); i++)
for (auto& pair_key_sb : getStateBlockMap())
// for (unsigned int i = 0; i < state_block_vec_.size(); i++)
{
auto sb = getStateBlock(i);
// auto sb = getStateBlock(i);
auto sb = pair_key_sb.second;
if (sb && !sb->isFixed())
sz += sb->getSize();
}
......@@ -261,9 +340,11 @@ Eigen::VectorXs CaptureBase::getCalibration() const
{
Eigen::VectorXs calib(calib_size_);
SizeEigen index = 0;
for (unsigned int i = 0; i < getStateBlockVec().size(); i++)
for (auto& pair_key_sb : getStateBlockMap())
// for (unsigned int i = 0; i < getStateBlockVec().size(); i++)
{
auto sb = getStateBlock(i);
// auto sb = getStateBlock(i);
auto sb = pair_key_sb.second;
if (sb && !sb->isFixed())
{
calib.segment(index, sb->getSize()) = sb->getState();
......@@ -278,9 +359,11 @@ void CaptureBase::setCalibration(const VectorXs& _calib)
updateCalibSize();
assert(_calib.size() == calib_size_ && "Wrong size of calibration vector");
SizeEigen index = 0;
for (unsigned int i = 0; i < getStateBlockVec().size(); i++)
for (auto& pair_key_sb : getStateBlockMap())
// for (unsigned int i = 0; i < getStateBlockVec().size(); i++)
{
auto sb = getStateBlock(i);
// auto sb = getStateBlock(i);
auto sb = pair_key_sb.second;
if (sb && !sb->isFixed())
{
sb->setState(_calib.segment(index, sb->getSize()));
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment