Handle NANs

Not sure how this used to work, or why it isn't in this catkinized version, but if there are NANs in a laserscan, I get an error in the scan matcher. Adding a test for isnan() on each ray fixes it.