diff --git a/rtc/PointCloudViewer/PointCloudViewer.cpp b/rtc/PointCloudViewer/PointCloudViewer.cpp
index a7ed114b2f0..adcedd5a436 100644
--- a/rtc/PointCloudViewer/PointCloudViewer.cpp
+++ b/rtc/PointCloudViewer/PointCloudViewer.cpp
@@ -12,6 +12,7 @@
 #include <pcl/filters/voxel_grid.h>
 #include "PointCloudViewer.h"
 #include "hrpsys/idl/pointcloud.hh"
+#include <string>
 
 // Module specification
 // <rtc-template block="module_spec">
@@ -119,18 +120,49 @@ RTC::ReturnCode_t PointCloudViewer::onExecute(RTC::UniqueId ec_id)
   if (m_cloudIn.isNew()){
     m_cloudIn.read();
 
-    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
-    cloud->points.resize(m_cloud.width*m_cloud.height);
-    float *src = (float *)m_cloud.data.get_buffer();
-    for (unsigned int i=0; i<cloud->points.size(); i++){
-      cloud->points[i].x = src[0];
-      cloud->points[i].y = src[1];
-      cloud->points[i].z = src[2];
-      src += 4;
+    bool is_color_points = false;
+    for (int i = 0; i < m_cloud.fields.length(); i++) {
+        std::string tmp_name(m_cloud.fields[i].name);
+        if (tmp_name.find("r") != std::string::npos || tmp_name.find("g") != std::string::npos || tmp_name.find("b") != std::string::npos) {
+            is_color_points = true; // color pointcloud should have rgb field 
+        }
     }
 
-    if (!m_viewer.wasStopped()){
-        m_viewer.showCloud(cloud);
+    // currently only support PointXYZ and PointXYZRGB
+    if (is_color_points) { 
+        pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
+        cloud->is_dense = m_cloud.is_dense;
+        float *src = reinterpret_cast<float*>(m_cloud.data.get_buffer());
+        for (unsigned int i = 0; i< (m_cloud.width * m_cloud.height); i++){
+            pcl::PointXYZRGB tmp_point;
+            tmp_point.x = src[0];
+            tmp_point.y = src[1];
+            tmp_point.z = src[2];
+            uint32_t rgb = *reinterpret_cast<uint32_t*>(&(src[3])); // http://docs.pointclouds.org/1.7.2/a01059.html#a1678cfbe6e832aa61ec0de773cab15ae
+            tmp_point.r = (uint8_t)((rgb >> 16) & 0x0000ff);
+            tmp_point.g = (uint8_t)((rgb >> 8) & 0x0000ff);
+            tmp_point.b = (uint8_t)((rgb) & 0x0000ff);
+            if (m_cloud.is_dense || rgb != 0) { // rgb == 0 means invalid point
+                cloud->push_back(tmp_point);
+            }
+            src += 4;
+        }
+        if (!m_viewer.wasStopped()){
+            m_viewer.showCloud(cloud);
+        }
+    } else {
+        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
+        cloud->points.resize(m_cloud.width*m_cloud.height);
+        float *src = (float *)m_cloud.data.get_buffer();
+        for (unsigned int i=0; i<cloud->points.size(); i++){
+            cloud->points[i].x = src[0];
+            cloud->points[i].y = src[1];
+            cloud->points[i].z = src[2];
+            src += 4;
+        }
+        if (!m_viewer.wasStopped()){
+            m_viewer.showCloud(cloud);
+        }        
     }
   }