Fix: make sure point is finite before vicinity search
Fix for double free in KdTreeFLANN after copy operation #335.