added SfM code

code by Christopher Zach