Switch to tangent space covariance

Right now, the state blocks covariances returned by Ceres match the size the implicit space parameterization, not that of the tangent space as it should (cov of a quaternion state block is 4x4 while it should be 3x3). This is due to the use of Ceres::Covariance::GetCovarianceBlock instead of Ceres::Covariance::GetCovarianceBlockInTangentSpace in ceres_manager.cpp, CeresManager::computeCovariances.

This should not influence the way Ceres works and I think right now we are only using getCovariance() methods for logs so this should be a painless change.

added To Do (release) enhancement help wanted labels

Are you proposing to switch to tangent space covariance or to allow both covariance sizes?

I think only tangent space covariances are meaningful as they are the one we want to use for covariance propagation (as in what we want to implement with issue #196).

Ok. Are we sure that we are not going to need the covariance in the quaternion space? If it is the case, the change is quite straigforward. What do you think @jsola, can we just get rid of the covariance in not minimal rep?

In my view, we will not need the 4x4 covariance of quaternion, just the 3x3 covariance.

I ignore if someone is currently using any 4x4 quat covariance. If we are using it, it's probably only in some tests to print them on screen. But no sensitive piece of algorithm is relying on these 4x4 covs as fr as I know.

Yes only printing. Have to remove a safeguard in Problem::getCovarianceBlock:

assert(_row + _state1->getSize() <= _cov.rows() && _col + _state2->getSize() <= _cov.cols() && "Problem::getCovarianceBlock: Bad matrix covariance size!");

Does not work with implicit parameterization since state.size() > _cov.cols() or _cov.rows()

Ok then! We keep the 3x3 only. Easy. The assert should be changed to:

assert(_row + _state1->getLocalSize() <= _cov.rows() && _col + _state2->getLocalSize() <= _cov.cols() && "Problem::getCovarianceBlock: Bad matrix covariance size!");

Ok I did not know the getLocalSize() method, thanks

Implemented and merged !269 (merged)

closed

removed To Do (release) label