Fix bugs in censure keypoint detector and improve code