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
This commit is part of merge request !243. Comments created here will be created in the context of that merge request.
Hide whitespace changes
Inline Side-by-side
src/sensor_base.cpp
+ 7
− 47
View file @ 790806d3
......@@ -82,8 +82,6 @@ void SensorBase::removeStateBlocks()
}
}
void SensorBase::fix()
{
for( auto sbp : state_block_vec_)
......@@ -144,52 +142,14 @@ void SensorBase::unfixIntrinsics()
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(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);
}
assert(!isStateBlockDynamic(_i) && "SensorBase::addPriorParameter only allowed for static parameters");
assert(_i < state_block_vec_.size() && "State block not found");
// remove previous prior (if any)
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)
{
StateBlockPtr _sb = getStateBlockPtrStatic(_i);
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()) ||
(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()));
......@@ -207,8 +167,8 @@ void SensorBase::addPriorParameterDynamic(const StateBlockPtr& _sb, const Eigen:
}
// remove previous prior (if any)
if (params_prior_map_.find(_sb) != params_prior_map_.end())
params_prior_map_[_sb]->remove();
if (params_prior_map_.find(_i) != params_prior_map_.end())
params_prior_map_[_i]->remove();
// create feature
FeatureBasePtr ftr_prior = std::make_shared<FeatureBase>("ABSOLUTE",_x,_cov);
......@@ -223,7 +183,7 @@ void SensorBase::addPriorParameterDynamic(const StateBlockPtr& _sb, const Eigen:
ftr_prior->addConstraint(std::make_shared<ConstraintBlockAbsolute>(_sb, _start_idx, _size));
// store feature in params_prior_map_
params_prior_map_[_sb] = ftr_prior;
params_prior_map_[_i] = ftr_prior;
}
void SensorBase::registerNewStateBlocks()
......
src/sensor_base.h
+ 2
− 64
View file @ 790806d3
......@@ -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)
*
**/
void addParameterStaticPrior(const StateBlockPtr& _sb,
void addPriorParameter(const StateBlockPtr& _sb,
const Eigen::VectorXs& _x,
const Eigen::MatrixXs& _cov,
unsigned int _start_idx = 0,
int _size = -1);
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,
void addPriorParameter(const unsigned int _i,
const Eigen::VectorXs& _x,
const Eigen::MatrixXs& _cov,
unsigned int _start_idx = 0,
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,
const Eigen::MatrixXs& _cov,
unsigned int _start_idx = 0,
......@@ -321,41 +292,8 @@ inline void SensorBase::setHardwarePtr(const HardwareBasePtr _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,
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);
}
......
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