There is still an issue with the published organized point cloud. According to http://pointclouds.org/documentation/tutorials/basic_structures.php the width in an organized cloud specifies the total number of points in a row (not the total number of points in the cloud as it happens now in the code). Also for the organized points the height should specify the number of points in a column, and should not be 1. Also the variable is_dense should be set to 0 (false) since there are NaN's in the cloud. I am not sure if this happens in the code.