Added a toVector() method to Particle to ease consistency checking in Hybrid SLAM.
Implemented resamplingHybrid in a non-safe probabilistic way.
Wrote an utility function to sample normaly inside a box
Little confusion arout intervalVectorToEigenVector, corrected
DrawSample in box tested. Generates 3D vectors only, hum hum..
Added a utility function to get a specific landmark estimate from BESE.
Also wrote a method to get the covariance matrix from a box and a confidence value.
boxToCovarianceMatrix is working now, I had to use box[i].diam() instead of box[i].mid() of course.
Filling BESEFastSLAM methods. I could definitively have taken advantage of inheritance... Could be a future redesign.
Fixed: tring to access element [3] of a Vector3d, but indices start at 0... Weird that it didn't cause any problem with FastSLAM !
As expected, the hybrid resampling is not safe and crashes!
Map wasn't updated. & going to fix the sample while loop.
Now when a particle is inconsistent, we shake its map following a uniform distribution in the map box. Could cause problem when the position is inconsistent, not the map.
FastSLAM is working way better than the hybrid implementation. Probably due to the fact there is no selection for now.
Preparing to push.