Fixed compilation of kdtree
Added operator [] to 2d vectors
Miscelaneous fixes to kdtree