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

changed isDynamic for isStateBlockDynamic

parent f117b641
No related branches found
No related tags found
1 merge request!243Constraint prior sensor params
......@@ -342,33 +342,25 @@ ProcessorBasePtr SensorBase::addProcessor(ProcessorBasePtr _proc_ptr)
StateBlockPtr SensorBase::getStateBlockPtrDynamic(unsigned int _i)
{
if ((_i<2 && this->extrinsicsInCaptures()) || (_i>=2 && intrinsicsInCaptures()))
{
CaptureBasePtr cap = lastKeyCapture();
if (cap)
return cap->getStateBlockPtr(_i);
else
return getStateBlockPtrStatic(_i);
}
else
return getStateBlockPtrStatic(_i);
CaptureBasePtr cap;
if (isStateBlockDynamic(_i, cap))
return cap->getStateBlockPtr(_i);
return getStateBlockPtrStatic(_i);
}
StateBlockPtr SensorBase::getStateBlockPtrDynamic(unsigned int _i, const TimeStamp& _ts)
{
if ((_i<2 && this->extrinsicsInCaptures()) || (_i>=2 && intrinsicsInCaptures()))
{
CaptureBasePtr cap = lastCapture(_ts);
if (cap)
return cap->getStateBlockPtr(_i);
else
return getStateBlockPtrStatic(_i);
}
else
return getStateBlockPtrStatic(_i);
CaptureBasePtr cap;
if (isStateBlockDynamic(_i, _ts, cap))
return cap->getStateBlockPtr(_i);
return getStateBlockPtrStatic(_i);
}
bool SensorBase::isDynamic(unsigned int _i, CaptureBasePtr& cap) const
bool SensorBase::isStateBlockDynamic(unsigned int _i, CaptureBasePtr& cap) const
{
if ((_i<2 && this->extrinsicsInCaptures()) || (_i>=2 && intrinsicsInCaptures()))
{
......@@ -380,7 +372,7 @@ bool SensorBase::isDynamic(unsigned int _i, CaptureBasePtr& cap) const
return false;
}
bool SensorBase::isDynamic(unsigned int _i, const TimeStamp& _ts, CaptureBasePtr& cap) const
bool SensorBase::isStateBlockDynamic(unsigned int _i, const TimeStamp& _ts, CaptureBasePtr& cap) const
{
if ((_i<2 && this->extrinsicsInCaptures()) || (_i>=2 && intrinsicsInCaptures()))
{
......@@ -392,18 +384,18 @@ bool SensorBase::isDynamic(unsigned int _i, const TimeStamp& _ts, CaptureBasePtr
return false;
}
bool SensorBase::isDynamic(unsigned int _i) const
bool SensorBase::isStateBlockDynamic(unsigned int _i) const
{
CaptureBasePtr& cap;
return isDynamic(_i,cap);
return isStateBlockDynamic(_i,cap);
}
bool SensorBase::isDynamic(unsigned int _i, const TimeStamp& _ts) const
bool SensorBase::isStateBlockDynamic(unsigned int _i, const TimeStamp& _ts) const
{
CaptureBasePtr& cap;
return isDynamic(_i,_ts,cap);
return isStateBlockDynamic(_i,_ts,cap);
}
void SensorBase::setProblem(ProblemPtr _problem)
......
......@@ -117,10 +117,11 @@ class SensorBase : public NodeBase, public std::enable_shared_from_this<SensorBa
void setStateBlockPtrStatic(unsigned int _i, const StateBlockPtr _sb_ptr);
void resizeStateBlockVec(unsigned int _size);
bool isDynamic(unsigned int _i, const TimeStamp& _ts, CaptureBasePtr& cap) const;
bool isDynamic(unsigned int _i, CaptureBasePtr& cap) const;
bool isDynamic(unsigned int _i, const TimeStamp& _ts) const;
bool isDynamic(unsigned int _i) const;
bool isStateBlockDynamic(unsigned int _i, const TimeStamp& _ts, CaptureBasePtr& cap) const;
bool isStateBlockDynamic(unsigned int _i, CaptureBasePtr& cap) const;
bool isStateBlockDynamic(unsigned int _i, const TimeStamp& _ts) const;
bool isStateBlockDynamic(unsigned int _i) const;
StateBlockPtr getPPtr(const TimeStamp _ts);
StateBlockPtr getOPtr(const TimeStamp _ts);
StateBlockPtr getIntrinsicPtr(const TimeStamp _ts);
......@@ -138,6 +139,7 @@ class SensorBase : public NodeBase, public std::enable_shared_from_this<SensorBa
void unfixExtrinsics();
void fixIntrinsics();
void unfixIntrinsics();
/** \brief Add an absolute constraint to a parameter
*
* Add an absolute constraint to a parameter
......@@ -198,6 +200,7 @@ class SensorBase : public NodeBase, public std::enable_shared_from_this<SensorBa
const Eigen::MatrixXs& _cov,
unsigned int _start_idx = 0,
int _size = -1);
SizeEigen getCalibSize() const;
Eigen::VectorXs getCalibration() const;
......@@ -323,7 +326,7 @@ inline void SensorBase::addParameterPrior(const unsigned int _i, const TimeStamp
{
CaptureBasePtr cap;
// i is dynamic? //TODO
if (isDynamic(_i, _ts, cap))
if (isStateBlockDynamic(_i, _ts, cap))
addParameterDynamicPrior(cap, cap->getStateBlockPtr(_i), _x, _cov, _start_idx, _size);
else
addParameterStaticPrior(getStateBlockPtrStatic(_i), _x, _cov, _start_idx, _size);
......@@ -334,7 +337,7 @@ inline void SensorBase::addParameterPrior(const unsigned int _i, const Eigen::Ve
{
CaptureBasePtr cap;
// i is dynamic? //TODO
if (isDynamic(_i, cap))
if (isStateBlockDynamic(_i, cap))
addParameterDynamicPrior(cap, cap->getStateBlockPtr(_i), _x, _cov, _start_idx, _size);
else
addParameterStaticPrior(getStateBlockPtrStatic(_i), _x, _cov, _start_idx, _size);
......
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