A 3d SLAM program, using novel plane ICP