Add more tests for the kd tree