Added the functionality of adding an absolute constraint in sensor parameters (extrinsics or intrinsics). The class SensorBase has now two new members:
addParameterPrior(...)
std::map<StateBlockPtr,FeatureBasePtr> params_prior_map_
The new function addParameterPrior(...) adds the corresponding feature and constraint (ConstraintBlockAbsolute or ConstraintQuaternionAbsolute).
This MR also reimplemented ConstraintBlockAbsolute generalizing to any state block size and allowing constraining just a segment of the state. Now inheriting from ConstraintAnalytic. Accordingly, SensorBase::addParameterPrior(...) API also allows specifying segment constrains (not allowed in case of StateQuaternion):
void addParameterPrior(const StateBlockPtr& _sb, const Eigen::VectorXs& _x, const Eigen::MatrixXs& _cov, unsigned int _start_idx = 0, int size = -1)
_sb is the constrained state block (must be in SensorBase::state_block_vec_)._x and _cov are the prior value and covariance (in case of StateQuaternion, the covariance is 3x3)._start_idx and _size specifies the segment of the state that is constrained. Default values allows simple calling without considering it.The current implementation allows only one constraint per each state block, if SensorBase::addParameterPrior(...) is called two times, the second one is the one that remains.
All tests passed, devel was merged just now.