roslib improvements

* Moved all constructors to 'options' style
* Several small bug fixes
* Restructured math modules