Skip to content
Snippets Groups Projects

WIP: Resolve "Complete state vector new data structure?"

Closed Joan Solà Ortega requested to merge 287-tris into devel
Compare and
47 files
+ 3179
669
Compare changes
  • Side-by-side
  • Inline
Files
47
@@ -83,7 +83,7 @@ class CaptureBase : public NodeBase, public HasStateBlocks, public std::enable_s
// State blocks
const std::string& getStructure() const;
const StateStructure& getStructure() const;
StateBlockPtr getStateBlock(const std::string& _key) const;
StateBlockPtr getStateBlock(const char _key) const { return getStateBlock(std::string(1, _key)); }
StateBlockPtr getSensorP() const;
@@ -95,13 +95,16 @@ class CaptureBase : public NodeBase, public HasStateBlocks, public std::enable_s
bool hasCalibration() {return calib_size_ > 0;}
SizeEigen getCalibSize() const;
virtual Eigen::VectorXd getCalibration() const;
Eigen::VectorXd getCalibration() const;
void setCalibration(const Eigen::VectorXd& _calib);
void move(FrameBasePtr);
void link(FrameBasePtr);
template<typename classType, typename... T>
static std::shared_ptr<classType> emplace(FrameBasePtr _frm_ptr, T&&... all);
VectorComposite getCalibrationComposite() const;
void getCalibrationComposite(VectorComposite& _calib) const;
protected:
virtual SizeEigen computeCalibSize() const;
@@ -135,6 +138,26 @@ inline SizeEigen CaptureBase::getCalibSize() const
return calib_size_;
}
inline VectorComposite CaptureBase::getCalibrationComposite() const
{
VectorComposite calib;
getCalibrationComposite(calib);
return calib;
}
inline void CaptureBase::getCalibrationComposite(VectorComposite &_calib) const
{
for (const auto& ckey : getStructure())
{
std::string key(1,ckey);
const auto& sb = getStateBlock(key);
if (sb and not sb->isFixed())
_calib[key] = sb->getState();
else
_calib.erase(key);
}
}
inline void CaptureBase::updateCalibSize()
{
calib_size_ = computeCalibSize();
Loading