improved support for octrees

- RigidBodyDepthSensor outputs points in the sensor frame now instead of the world frame (which is the only thing a real sensor could do)
- Added a OcTreeSystem class which takes a state estimate + pointcloud as input, and populates a octree.
- Added a test which flies a quadrotor through the forest and builds the tree using these components.
- Lots of small fixes to support the above.