diff --git a/jsk_pcl_ros/include/jsk_pcl_ros/organized_statistical_outlier_removal.h b/jsk_pcl_ros/include/jsk_pcl_ros/organized_statistical_outlier_removal.h
index 368409793a..a9aa0f6667 100644
--- a/jsk_pcl_ros/include/jsk_pcl_ros/organized_statistical_outlier_removal.h
+++ b/jsk_pcl_ros/include/jsk_pcl_ros/organized_statistical_outlier_removal.h
@@ -63,7 +63,6 @@ namespace jsk_pcl_ros
     typedef message_filters::sync_policies::ApproximateTime<
     sensor_msgs::PointCloud2,
     jsk_recognition_msgs::ClusterPointIndices > ApproximateSyncPolicy;
-    typedef pcl::PointXYZRGB PointT;
     OrganizedStatisticalOutlierRemoval();
   protected:
     ////////////////////////////////////////////////////////
@@ -77,18 +76,16 @@ namespace jsk_pcl_ros
 
     virtual void configCallback (Config &config, uint32_t level);
 
-    virtual void filter(
-            const pcl::PointCloud<PointT>::Ptr cloud,
-            pcl::PointCloud<PointT>::Ptr cloud_filtered,
-            bool keep_organized);
-    virtual void filterCloud(const sensor_msgs::PointCloud2::ConstPtr& msg);
-    virtual void filterCloudWithClusterPointIndices(
+    void filter(const pcl::PCLPointCloud2::Ptr pcl_cloud,
+                pcl::PointIndices::Ptr pcl_indices_filtered,
+                bool keep_organized);
+    void filterCloud(const sensor_msgs::PointCloud2::ConstPtr& msg);
+    void filterCloudWithClusterPointIndices(
             const sensor_msgs::PointCloud2::ConstPtr& msg,
             const jsk_recognition_msgs::ClusterPointIndices::ConstPtr& cpi_msg);
 
 
-    virtual void updateDiagnostic(
-      diagnostic_updater::DiagnosticStatusWrapper &stat);
+    void updateDiagnostic(diagnostic_updater::DiagnosticStatusWrapper &stat);
 
     ////////////////////////////////////////////////////////
     // ROS variables
diff --git a/jsk_pcl_ros/src/organized_statistical_outlier_removal_nodelet.cpp b/jsk_pcl_ros/src/organized_statistical_outlier_removal_nodelet.cpp
index 67dbd35d12..db049875ab 100644
--- a/jsk_pcl_ros/src/organized_statistical_outlier_removal_nodelet.cpp
+++ b/jsk_pcl_ros/src/organized_statistical_outlier_removal_nodelet.cpp
@@ -37,6 +37,7 @@
 #include <jsk_topic_tools/diagnostic_utils.h>
 #include <pcl/pcl_base.h>
 #include <pcl/filters/statistical_outlier_removal.h>
+#include <pcl_conversions/pcl_conversions.h>
 #include <jsk_recognition_utils/pcl_util.h>
 
 namespace jsk_pcl_ros
@@ -115,11 +116,13 @@ namespace jsk_pcl_ros
   }
 
   void OrganizedStatisticalOutlierRemoval::filter(
-          pcl::PointCloud<PointT>::Ptr cloud,
-          pcl::PointCloud<PointT>::Ptr cloud_filtered,
+          const pcl::PCLPointCloud2::Ptr pcl_cloud,
+          pcl::PointIndices::Ptr pcl_indices_filtered,
           bool keep_organized)
   {
 #if PCL_VERSION_COMPARE (<, 1, 9, 0)
+    pcl::PointCloud<pcl::PointT>::Ptr cloud (new pcl::PointCloud<PointXYZRGB>);
+    pcl::fromPCLPointCloud2(pcl_cloud, *cloud);
     if (keep_organized) {
       // Initialize the spatial locator
       pcl::search::Search<PointT>::Ptr tree;
@@ -172,9 +175,6 @@ namespace jsk_pcl_ros
 
       double distance_threshold = mean + std_mul_ * stddev; // a distance that is bigger than this signals an outlier
 
-      pcl::PCLPointCloud2::Ptr pcl_cloud (new pcl::PCLPointCloud2);
-      pcl::PCLPointCloud2::Ptr pcl_cloud_filtered (new pcl::PCLPointCloud2);
-      pcl::toPCLPointCloud2(*cloud, *pcl_cloud);
       pcl_cloud_filtered->is_bigendian = pcl_cloud->is_bigendian;
       pcl_cloud_filtered->fields = pcl_cloud->fields;
       pcl_cloud_filtered->point_step = pcl_cloud->point_step;
@@ -207,25 +207,24 @@ namespace jsk_pcl_ros
         }
         nr_p++;
       }
-      pcl::fromPCLPointCloud2(*pcl_cloud_filtered, *cloud_filtered);
     }
     else
     {
-      pcl::StatisticalOutlierRemoval<PointT> sor;
-      sor.setInputCloud(cloud);
-      sor.setMeanK (mean_k_);
-      sor.setStddevMulThresh (std_mul_);
-      sor.setNegative (negative_);
-      sor.filter (*cloud_filtered);
+      pcl::StatisticalOutlierRemoval<pcl::PCLPointCloud2> sor;
+      sor.setInputCloud(pcl_cloud);
+      sor.setMeanK(mean_k_);
+      sor.setStddevMulThresh(std_mul_);
+      sor.setNegative(negative_);
+      sor.filter(pcl_indices_filtered->indices);
     }
 #else
-    pcl::StatisticalOutlierRemoval<PointT> sor;
-    sor.setInputCloud(cloud);
-    sor.setMeanK (mean_k_);
-    sor.setStddevMulThresh (std_mul_);
-    sor.setNegative (negative_);
+    pcl::StatisticalOutlierRemoval<pcl::PCLPointCloud2> sor;
+    sor.setInputCloud(pcl_cloud);
+    sor.setMeanK(mean_k_);
+    sor.setStddevMulThresh(std_mul_);
+    sor.setNegative(negative_);
     sor.setKeepOrganized (keep_organized);
-    sor.filter (*cloud_filtered);
+    sor.filter(pcl_indices_filtered->indices);
 #endif
   }
 
@@ -239,14 +238,21 @@ namespace jsk_pcl_ros
       NODELET_ERROR("keep_organized parameter is true, but input pointcloud is not organized.");
     }
     bool keep_organized = keep_organized_ && !msg->is_dense;
-    pcl::PointCloud<PointT>::Ptr cloud (new pcl::PointCloud<PointT>);
-    pcl::PointCloud<PointT>::Ptr cloud_filtered (new pcl::PointCloud<PointT>);
-    pcl::fromROSMsg(*msg, *cloud);
+    pcl::PCLPointCloud2::Ptr pcl_cloud (new pcl::PCLPointCloud2);
+    pcl_conversions::toPCL(*msg, *pcl_cloud);
 
     // filter
-    OrganizedStatisticalOutlierRemoval::filter(cloud, cloud_filtered, keep_organized);
-
-    pcl::toROSMsg(*cloud_filtered, output);
+    pcl::PointIndices::Ptr pcl_indices_filtered (new pcl::PointIndices());
+    pcl_indices_filtered->indices.resize(pcl_cloud->data.size());
+    OrganizedStatisticalOutlierRemoval::filter(pcl_cloud, pcl_indices_filtered, keep_organized);
+    pcl::PCLPointCloud2::Ptr pcl_cloud_filtered (new pcl::PCLPointCloud2);
+    pcl::ExtractIndices<pcl::PCLPointCloud2> ex;
+    ex.setInputCloud(pcl_cloud);
+    ex.setKeepOrganized(keep_organized);
+    ex.setNegative(false);
+    ex.setIndices(pcl_indices_filtered);
+    ex.filter(*pcl_cloud_filtered);
+    pcl_conversions::fromPCL(*pcl_cloud_filtered, output);
 #if PCL_VERSION_COMPARE (<, 1, 9, 0)
     if (keep_organized) {
       // Copy the common fields
@@ -277,45 +283,34 @@ namespace jsk_pcl_ros
       NODELET_ERROR("keep_organized parameter is true, but input pointcloud is not organized.");
     }
     bool keep_organized = keep_organized_ && !msg->is_dense;
-    pcl::PointCloud<PointT>::Ptr cloud (new pcl::PointCloud<PointT>);
-    pcl::PointCloud<PointT>::Ptr cloud_filtered (new pcl::PointCloud<PointT>);
-    pcl::fromROSMsg(*msg, *cloud);
-    cloud_filtered->points.resize(cloud->points.size());
-    PointT nan_point;
-    nan_point.x = nan_point.y = nan_point.z = std::numeric_limits<float>::quiet_NaN();
-    std::fill(cloud_filtered->points.begin(), cloud_filtered->points.end(), nan_point);
+    pcl::PCLPointCloud2::Ptr pcl_cloud (new pcl::PCLPointCloud2);
+    pcl::PCLPointCloud2::Ptr pcl_cloud_filtered (new pcl::PCLPointCloud2);
+    pcl_conversions::toPCL(*msg, *pcl_cloud);
+    // fill nan in pcl_cloud_filtered
+    pcl_cloud_filtered->data.resize(pcl_cloud->data.size());
+    std::fill(pcl_cloud_filtered->data.begin(), pcl_cloud_filtered->data.end(), 0);
 
-    pcl::ExtractIndices<PointT> ex;
-    ex.setInputCloud(cloud);
+    pcl::ExtractIndices<pcl::PCLPointCloud2> ex;
+    ex.setInputCloud(pcl_cloud);
     ex.setKeepOrganized(keep_organized);
     ex.setNegative(false);
 
     for (size_t i = 0; i < cpi_msg->cluster_indices.size(); i++)
     {
-      pcl::IndicesPtr indices;
-      pcl::PointCloud<PointT>::Ptr cluster_cloud(new pcl::PointCloud<PointT>);
-      pcl::PointCloud<PointT>::Ptr cluster_cloud_filtered(new pcl::PointCloud<PointT>);
-      indices.reset (new std::vector<int> (cpi_msg->cluster_indices[i].indices));
-      ex.setIndices(indices);
-      ex.filter(*cluster_cloud);
-      OrganizedStatisticalOutlierRemoval::filter(cluster_cloud, cluster_cloud_filtered, keep_organized);
-      for (size_t j = 0; j < indices->size(); j++)
-      {
-        int ind = (*indices)[j];
-        if (!std::isnan(cluster_cloud_filtered->points[ind].x) &&
-            !std::isnan(cluster_cloud_filtered->points[ind].y) &&
-            !std::isnan(cluster_cloud_filtered->points[ind].z))
-        {
-          cloud_filtered->points[ind].x = cluster_cloud_filtered->points[ind].x;
-          cloud_filtered->points[ind].y = cluster_cloud_filtered->points[ind].y;
-          cloud_filtered->points[ind].z = cluster_cloud_filtered->points[ind].z;
-          cloud_filtered->points[ind].rgb = cluster_cloud_filtered->points[ind].rgb;
-        }
-      }
-
+      pcl::PCLPointCloud2::Ptr pcl_cluster_cloud (new pcl::PCLPointCloud2);
+      pcl::PCLPointCloud2::Ptr pcl_cluster_cloud_filtered (new pcl::PCLPointCloud2);
+      pcl::PointIndices::Ptr pcl_cluster_indices (new pcl::PointIndices());
+      pcl::PointIndices::Ptr pcl_cluster_indices_filtered (new pcl::PointIndices());
+      pcl_conversions::toPCL(cpi_msg->cluster_indices[i], *pcl_cluster_indices);
+      ex.setIndices(pcl_cluster_indices);
+      ex.filter(*pcl_cluster_cloud);
+      pcl_cluster_indices_filtered->indices.resize(pcl_cluster_cloud->data.size());
+      OrganizedStatisticalOutlierRemoval::filter(pcl_cluster_cloud, pcl_cluster_indices_filtered, keep_organized);
+      ex.setIndices(pcl_cluster_indices_filtered);
+      ex.filter(*pcl_cluster_cloud_filtered);
+      pcl::PCLPointCloud2::concatenate(*pcl_cloud_filtered, *pcl_cluster_cloud_filtered);
     }
-
-    pcl::toROSMsg(*cloud_filtered, output);
+    pcl_conversions::fromPCL(*pcl_cloud_filtered, output);
 #if PCL_VERSION_COMPARE (<, 1, 9, 0)
     if (keep_organized) {
       // Copy the common fields