Added TestConsensusThresholding.cc
Bug fixing in the consensus thresholding and kd tree classes