diff --git a/src/sensor_base.cpp b/src/sensor_base.cpp
index fa7e4273fd524d46655263df68270a54beb39e7e..4bd789f671a8571105c6df5c69e95c39973c88e3 100644
--- a/src/sensor_base.cpp
+++ b/src/sensor_base.cpp
@@ -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()
diff --git a/src/sensor_base.h b/src/sensor_base.h
index f4d4563f379e737b541cfd71aee10727bf603c55..d65567b94a6338cacb5e681d0e8e229c8afe8b73 100644
--- a/src/sensor_base.h
+++ b/src/sensor_base.h
@@ -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);
}