Integral Histograms

In order to get good performance from the particle filter, I need at least 200 particles. Calculating so many histograms slows things down. So I will try to implement an integral histogram solution that will allow me to do fast calculations of histograms and thus speeding the whole thing.

After performing the transformation it is possible to extract a histogram with only 4 array lookups.

The paper I'm following is Integral Histogram: A Fast Way to Extract Histograms in Cartesian Spaces.