Publish organized pointcloud as self-filtered pointcloud.

Add filtered_cloud_keep_organized parameter to OctomapUpdater to publish organized and self filtered pointcloud.

1. Add readXmlParam for boolean value
2. Fill the filtered point with nan to keep organized