add code from https://bitbucket.org/williammc/ptam_plus
remove old code