StateBlock::isValid
Currently, a StateQuaternion
with a not normalized state can be provided to the solver. It will crash when the local parametrization is called (since there is an assertion).
This shouldn't be allowed and should be checked (assert) in SolverManager::addStateBlock()
.
To do so, it would be useful a method for checking that a state block (that has a local parametrization) has a valid state. I propose to implement it both in LocalParametrization
to be derived:
virtual bool LocalParametrizationBase::isValid(const Eigen::VectorXd& state) = 0
Also, in StateBlock
we could add it as well:
StateBlock::isValid()
{
return local_param_ptr_ ? local_param_ptr_->isValid(state_) : true;
}