Added debug and stream utils and fixed some bugs on octree construction.