Copied the better demosaicing algorithm from avin2/SensorKinect. This was originally from ROS and does a much better job of demosaicing than the original code.