Class cleanup and updated SimpleOpenNI - Need to fix broken functions