HI
I would like to help you out with your project. I done a similar project before, but I use 2 digital camera's to create a stero pair, then using image processing to extract key points from the camera frames I was able to extract 3D data.
The software was written in C for the Linux environment, but the concpets could be easily transferred to any operating environment, or programming language!
Please let me know if you are interested in my bid. I have to tell you that I do not have a copy of Matlabs on me, and Its been a few years since I've touched the package. But I'm a quick learner and can adapt to create any solution very quickly.