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

add prior in dynamic parameters not allowed

parent eeda1f6a
No related branches found
No related tags found
1 merge request!243Constraint prior sensor params
...@@ -82,8 +82,6 @@ void SensorBase::removeStateBlocks() ...@@ -82,8 +82,6 @@ void SensorBase::removeStateBlocks()
} }
} }
void SensorBase::fix() void SensorBase::fix()
{ {
for( auto sbp : state_block_vec_) for( auto sbp : state_block_vec_)
...@@ -144,52 +142,14 @@ void SensorBase::unfixIntrinsics() ...@@ -144,52 +142,14 @@ void SensorBase::unfixIntrinsics()
updateCalibSize(); updateCalibSize();
} }
void SensorBase::addPriorParameterStatic(const StateBlockPtr& _sb, const Eigen::VectorXs& _x, const Eigen::MatrixXs& _cov, unsigned int _start_idx, int _size) void SensorBase::addPriorParameter(const unsigned int _i, const Eigen::VectorXs& _x, const Eigen::MatrixXs& _cov, unsigned int _start_idx, int _size)
{ {
bool is_quaternion = (std::dynamic_pointer_cast<StateQuaternion>(_sb) != nullptr); assert(!isStateBlockDynamic(_i) && "SensorBase::addPriorParameter only allowed for static parameters");
assert(_i < state_block_vec_.size() && "State block not found");
assert(std::find(state_block_vec_.begin(),state_block_vec_.end(),_sb) != state_block_vec_.end() && "adding prior to unknown state block");
assert(((!is_quaternion && _x.size() == _cov.rows() && _x.size() == _cov.cols()) ||
(is_quaternion && _x.size() == 4 &&_cov.rows() == 3 && _cov.cols() == 3)) && "bad prior/covariance dimensions");
assert((_size == -1 && _start_idx == 0) || (_size+_start_idx <= _sb->getSize()));
assert(_size == -1 || _size == _x.size());
assert(!(_size != -1 && is_quaternion) && "prior of a segment of state not available for quaternion");
// set StateBlock state
if (_size == -1)
_sb->setState(_x);
else
{
Eigen::VectorXs new_x = _sb->getState();
new_x.segment(_start_idx,_size) = _x;
_sb->setState(new_x);
}
// remove previous prior (if any) StateBlockPtr _sb = getStateBlockPtrStatic(_i);
if (params_prior_map_.find(_sb) != params_prior_map_.end())
params_prior_map_[_sb]->remove();
// create feature
FeatureBasePtr ftr_prior = std::make_shared<FeatureBase>("ABSOLUTE",_x,_cov);
// set feature problem
ftr_prior->setProblem(getProblem());
// create & add constraint absolute
if (is_quaternion)
ftr_prior->addConstraint(std::make_shared<ConstraintQuaternionAbsolute>(_sb));
else
ftr_prior->addConstraint(std::make_shared<ConstraintBlockAbsolute>(_sb, _start_idx, _size));
// store feature in params_prior_map_
params_prior_map_[_sb] = ftr_prior;
}
void SensorBase::addPriorParameterDynamic(const StateBlockPtr& _sb, const Eigen::VectorXs& _x, const Eigen::MatrixXs& _cov, unsigned int _start_idx, int _size)
{
bool is_quaternion = (std::dynamic_pointer_cast<StateQuaternion>(_sb) != nullptr); bool is_quaternion = (std::dynamic_pointer_cast<StateQuaternion>(_sb) != nullptr);
assert(std::find(state_block_vec_.begin(),state_block_vec_.end(),_sb) != state_block_vec_.end() && "adding prior to unknown state block");
assert(((!is_quaternion && _x.size() == _cov.rows() && _x.size() == _cov.cols()) || assert(((!is_quaternion && _x.size() == _cov.rows() && _x.size() == _cov.cols()) ||
(is_quaternion && _x.size() == 4 &&_cov.rows() == 3 && _cov.cols() == 3)) && "bad prior/covariance dimensions"); (is_quaternion && _x.size() == 4 &&_cov.rows() == 3 && _cov.cols() == 3)) && "bad prior/covariance dimensions");
assert((_size == -1 && _start_idx == 0) || (_size+_start_idx <= _sb->getSize())); assert((_size == -1 && _start_idx == 0) || (_size+_start_idx <= _sb->getSize()));
...@@ -207,8 +167,8 @@ void SensorBase::addPriorParameterDynamic(const StateBlockPtr& _sb, const Eigen: ...@@ -207,8 +167,8 @@ void SensorBase::addPriorParameterDynamic(const StateBlockPtr& _sb, const Eigen:
} }
// remove previous prior (if any) // remove previous prior (if any)
if (params_prior_map_.find(_sb) != params_prior_map_.end()) if (params_prior_map_.find(_i) != params_prior_map_.end())
params_prior_map_[_sb]->remove(); params_prior_map_[_i]->remove();
// create feature // create feature
FeatureBasePtr ftr_prior = std::make_shared<FeatureBase>("ABSOLUTE",_x,_cov); FeatureBasePtr ftr_prior = std::make_shared<FeatureBase>("ABSOLUTE",_x,_cov);
...@@ -223,7 +183,7 @@ void SensorBase::addPriorParameterDynamic(const StateBlockPtr& _sb, const Eigen: ...@@ -223,7 +183,7 @@ void SensorBase::addPriorParameterDynamic(const StateBlockPtr& _sb, const Eigen:
ftr_prior->addConstraint(std::make_shared<ConstraintBlockAbsolute>(_sb, _start_idx, _size)); ftr_prior->addConstraint(std::make_shared<ConstraintBlockAbsolute>(_sb, _start_idx, _size));
// store feature in params_prior_map_ // store feature in params_prior_map_
params_prior_map_[_sb] = ftr_prior; params_prior_map_[_i] = ftr_prior;
} }
void SensorBase::registerNewStateBlocks() void SensorBase::registerNewStateBlocks()
......
...@@ -150,46 +150,17 @@ class SensorBase : public NodeBase, public std::enable_shared_from_this<SensorBa ...@@ -150,46 +150,17 @@ class SensorBase : public NodeBase, public std::enable_shared_from_this<SensorBa
* \param _size state segment size (-1: whole state) (not used in quaternions) * \param _size state segment size (-1: whole state) (not used in quaternions)
* *
**/ **/
void addParameterStaticPrior(const StateBlockPtr& _sb, void addPriorParameter(const StateBlockPtr& _sb,
const Eigen::VectorXs& _x, const Eigen::VectorXs& _x,
const Eigen::MatrixXs& _cov, const Eigen::MatrixXs& _cov,
unsigned int _start_idx = 0, unsigned int _start_idx = 0,
int _size = -1); int _size = -1);
void addPriorParameter(const unsigned int _i,
void addParameterDynamicPrior(const CaptureBase& _cap,
const StateBlockPtr& _sb,
const Eigen::VectorXs& _x,
const Eigen::MatrixXs& _cov,
unsigned int _start_idx = 0,
int _size = -1);
void addParameterPrior(const unsigned int _i,
const Eigen::VectorXs& _x,
const Eigen::MatrixXs& _cov,
unsigned int _start_idx = 0,
int _size = -1);
void addParameterPrior(const unsigned int _i,
const TimeStamp _ts,
const Eigen::VectorXs& _x, const Eigen::VectorXs& _x,
const Eigen::MatrixXs& _cov, const Eigen::MatrixXs& _cov,
unsigned int _start_idx = 0, unsigned int _start_idx = 0,
int _size = -1); int _size = -1);
void addPPrior(const TimeStamp _ts,
const Eigen::VectorXs& _x,
const Eigen::MatrixXs& _cov,
unsigned int _start_idx = 0,
int _size = -1);
void addOPrior(const TimeStamp _ts,
const Eigen::VectorXs& _x,
const Eigen::MatrixXs& _cov);
void addIntrinsicsPrior(const TimeStamp _ts,
const Eigen::VectorXs& _x,
const Eigen::MatrixXs& _cov,
unsigned int _start_idx = 0,
int _size = -1);
void addPriorP(const Eigen::VectorXs& _x, void addPriorP(const Eigen::VectorXs& _x,
const Eigen::MatrixXs& _cov, const Eigen::MatrixXs& _cov,
unsigned int _start_idx = 0, unsigned int _start_idx = 0,
...@@ -321,41 +292,8 @@ inline void SensorBase::setHardwarePtr(const HardwareBasePtr _hw_ptr) ...@@ -321,41 +292,8 @@ inline void SensorBase::setHardwarePtr(const HardwareBasePtr _hw_ptr)
hardware_ptr_ = _hw_ptr; hardware_ptr_ = _hw_ptr;
} }
inline void SensorBase::addParameterPrior(const unsigned int _i, const TimeStamp _ts, const Eigen::VectorXs& _x,
const Eigen::MatrixXs& _cov, unsigned int _start_idx, int _size)
{
CaptureBasePtr cap;
// i is dynamic? //TODO
if (isStateBlockDynamic(_i, _ts, cap))
addParameterDynamicPrior(cap, cap->getStateBlockPtr(_i), _x, _cov, _start_idx, _size);
else
addParameterStaticPrior(getStateBlockPtrStatic(_i), _x, _cov, _start_idx, _size);
}
inline void SensorBase::addPriorParameter(const unsigned int _i, const Eigen::VectorXs& _x, const Eigen::MatrixXs& _cov, inline void SensorBase::addPriorParameter(const unsigned int _i, const Eigen::VectorXs& _x, const Eigen::MatrixXs& _cov,
unsigned int _start_idx, int _size) unsigned int _start_idx, int _size)
{
CaptureBasePtr cap;
// i is dynamic? //TODO
if (isStateBlockDynamic(_i, cap))
addParameterDynamicPrior(cap, cap->getStateBlockPtr(_i), _x, _cov, _start_idx, _size);
else
addParameterStaticPrior(getStateBlockPtrStatic(_i), _x, _cov, _start_idx, _size);
}
inline void SensorBase::addPPrior(const TimeStamp _ts, const Eigen::VectorXs& _x, const Eigen::MatrixXs& _cov, unsigned int _start_idx,
int _size)
{
addParameterPrior(0, _ts, _x, _cov, _start_idx, _size);
}
inline void SensorBase::addOPrior(const TimeStamp _ts, const Eigen::VectorXs& _x, const Eigen::MatrixXs& _cov)
{
addParameterPrior(1, _ts, _x, _cov);
}
inline void SensorBase::addIntrinsicsPrior(const TimeStamp _ts, const Eigen::VectorXs& _x, const Eigen::MatrixXs& _cov,
unsigned int _start_idx, int _size)
{ {
addPriorParameter(getStateBlockPtrStatic(_i), _x, _cov, _start_idx, _size); addPriorParameter(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