Add internal kdtree implementation with custom queries compatibles with current pipeline
Add missing include directive
Fix bug in iterator-based initialization function
Add convenience functions
Switch from ANN to internal kd-tree
Use Scalar instead of float type
Remove usused variables
Add timing tools (disabled by default)
Add more points to get more stable test (bunny)