void ColorizeDistanceFromPlane::colorize(
    const sensor_msgs::PointCloud2::ConstPtr& cloud_msg,
    const jsk_recognition_msgs::ModelCoefficientsArray::ConstPtr& coefficients_msg,
    const jsk_recognition_msgs::PolygonArray::ConstPtr& polygons)
  {
    boost::mutex::scoped_lock lock(mutex_);
    if (coefficients_msg->coefficients.size() == 0) {
      return;
    }
    // convert all the data into pcl format
    pcl::PointCloud<PointT>::Ptr cloud
      (new pcl::PointCloud<PointT>);
    pcl::fromROSMsg(*cloud_msg, *cloud);
    std::vector<pcl::ModelCoefficients::Ptr> coefficients
      = pcl_conversions::convertToPCLModelCoefficients(
        coefficients_msg->coefficients);
    
    // first, build ConvexPolygon::Ptr
    std::vector<ConvexPolygon::Ptr> convexes;
    for (size_t i = 0; i < polygons->polygons.size(); i++) {
      if (polygons->polygons[i].polygon.points.size() > 0) {
        ConvexPolygon convex =
          ConvexPolygon::fromROSMsg(polygons->polygons[i].polygon);
        ConvexPolygon::Ptr convex_ptr
          = boost::make_shared<ConvexPolygon>(convex);
        convexes.push_back(convex_ptr);
      }
      else {
        JSK_NODELET_ERROR_STREAM(__PRETTY_FUNCTION__ << ":: there is no points in the polygon");
      }
    }

    pcl::PointCloud<pcl::PointXYZRGB>::Ptr output_cloud
      (new pcl::PointCloud<pcl::PointXYZRGB>);
    
    for (size_t i = 0; i < cloud->points.size(); i++) {
      PointT p = cloud->points[i];
      pcl::PointXYZRGB p_output;
      pointFromXYZToXYZ<PointT, pcl::PointXYZRGB>(p, p_output);
      double d = distanceToConvexes(p, convexes);
      if (d != DBL_MAX) {
        uint32_t color = colorForDistance(d);
        p_output.rgb = *reinterpret_cast<float*>(&color);
        output_cloud->points.push_back(p_output);
      }
    }
    
    sensor_msgs::PointCloud2 ros_output;
    pcl::toROSMsg(*output_cloud, ros_output);
    ros_output.header = cloud_msg->header;
    pub_.publish(ros_output);
  }
  bool CollisionDetector::checkCollision(const sensor_msgs::JointState& joint,
                                         const geometry_msgs::PoseStamped& pose)
  {
    boost::mutex::scoped_lock lock(mutex_);
    JSK_NODELET_DEBUG("checkCollision is called.");

    // calculate the sensor transformation
    tf::StampedTransform sensor_to_world_tf;
    try {
      tf_listener_.lookupTransform(world_frame_id_, cloud_frame_id_, cloud_stamp_, sensor_to_world_tf);
    } catch (tf::TransformException& ex) {
      JSK_NODELET_ERROR_STREAM( "Transform error of sensor data: " << ex.what() << ", quitting callback");
      return false;
    }

    // transform point cloud
    Eigen::Matrix4f sensor_to_world;
    pcl_ros::transformAsMatrix(sensor_to_world_tf, sensor_to_world);
    pcl::transformPointCloud(cloud_, cloud_, sensor_to_world);

    self_mask_->assumeFrameFromJointAngle(joint, pose);

    // check containment for all point cloud
    bool contain_flag = false;
    pcl::PointXYZ p;
    for (size_t i = 0; i < cloud_.size(); i++) {
      p = cloud_.at(i);
      if (finite(p.x) && finite(p.y) && finite(p.z) &&
         self_mask_->getMaskContainment(p.x, p.y, p.z) == robot_self_filter::INSIDE) {
        contain_flag = true;
        break;
      }
    }

    if (contain_flag) {
      JSK_NODELET_INFO("collision!");
    } else {
      JSK_NODELET_INFO("no collision!");
    }
    return contain_flag;
  }