Bug fix and refactoring

- phi0 can be set to zero
- R_theta and theta0 is modeled

Assumption
door movement => theta(1) = 0, phi(i) = 0 for all i
mirror movement => theta(i) = theta0 for all i, no assumption on phi(i)

i.e. first door movement pose will be used as camera pose reference