Rtabmap Link.cpp: should rotVarirance & transVariance support 2D slam

[guoqingzh]

Current rotVariance() & transVariance() in Link.cpp don't support 2D slam. If ros odometry topic set small covariance to vz, vroll, vpitch, which are not used in 2D slam. The function will pick up the wrong covariance.

[matlabbe]

Set large variance for z, roll and pitch, then the function will work properly (at least for the minimum variation, the default). Note also that these functions are generally used only for statistics, not actual optimization, which the full covariance (information) matrix is used. To make sure graph is optimized in 2D (ignoring z, roll and pitch), parameter Reg/Force3DoF should be true.