查了一下资料，发现SfM这个问题已经在Multiple view geometry这本书中讲的比较清楚了。SfM的最后一步，bundle adjustment网上也有现成的程序,
http://www.ics.forth.gr/~lourakis/sba/#Download。剩下得就是好好研究这些资料了。

今天在查SfM的时候想起来了Photo tourism这篇paper，就又看了一下。发现这个paper已经被微软做成产品了，photosynth，很cool。
http://labs.live.com/photosynth/default.html

Structure and Motion的matlab code
http://www.mathworks.com/matlabcentral/fileexchange/loadFile.do?objectId=4576&objectType=File
http://homepages.inf.ed.ac.uk/s0346435/projects/sfm/sfm_sift.html