Publish organized pointcloud as self-filtered pointcloud.

1. Add readXmlParam for boolean value
2. fill the filtered point with nan to keep organized