Added cube extraction to SVO (needs debugged)