blob_properties.cc

Go to the documentation of this file.

/*
 * Copyright (c) 2008, Willow Garage, Inc.
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     * Neither the name of the Willow Garage, Inc. nor the names of its
 *       contributors may be used to endorse or promote products derived from
 *       this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 */

#include "blob_properties.h"
#include "gcc_version.h"

namespace semanticmodel
{

using std::vector;
using std::map;
using std::string;

typedef vector<float> Color;
typedef vector<string> Colors;


GeometricInfo centroidAndDiameter (const PointCloud& cloud)
{
  boost::optional<double> min_x, min_y, min_z, max_x, max_y, max_z;
  double sx=0, sy=0, sz=0;
  const size_t n=cloud.points.size();
  ROS_ASSERT (n>0);
  BOOST_FOREACH (const Point& p, cloud)
  {
    sx += p.x;
    sy += p.y;
    sz += p.z;
    if (!min_x)
    {
      min_x = p.x;
      min_y = p.y;
      min_z = p.z;
      max_x = p.x;
      max_y = p.y;
      max_z = p.z;
    }
    else
    {
      // Guaranteed that all min/max are initialized
      if (*min_x > p.x)
        min_x = p.x;
      if (*min_y > p.y)
        min_y = p.y;
      if (*min_z > p.z)
        min_z = p.z;
      if (*max_x < p.x)
        max_x = p.x;
      if (*max_y < p.y)
        max_y = p.y;
      if (*max_z < p.z)
        max_z = p.z;
    }
  }

#ifdef SEMANTIC_MODEL_GCC_46
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wuninitialized"
#endif

  const double dx = *max_x - *min_x;
  const double dy = *max_y - *min_y;
  const double dz = *max_z - *min_z;
  
#ifdef SEMANTIC_MODEL_GCC_46
#pragma GCC diagnostic pop
#endif

  return GeometricInfo(sx/n, sy/n, sz/n, std::max(dx, std::max(dy, dz)));
}



Color color (float r, float g, float b)
{
  Color c(3);
  c[0] = r;
  c[1] = g;
  c[2] = b;
  return c;
}

void rgbToHsv(const float r, const float g, const float b,
              float& h, float& s, float& v)
{
  const float max = std::max(r, std::max(g, b));
  const float min = std::min(r, std::min(g, b));
  const float d = max-min;
  s = max==0.0 ? 0.0 : d/max;
  if (max==min) {
    h = 0;
  }
  else {
    if (max==r)
      h = (g - b) / d + (g < b ? 6 : 0);
    else if (max==g)
      h = (b - r) / d + 2;
    else
      h = (r - g) / d + 4;
    h /= 6;
  }
  v = max;
}


enum { BLACK, GREY, WHITE, RED, YELLOW, GREEN, BLUE };

Colors createColors ()
{
  Colors c(7);
  c[BLACK] = string("black");
  c[GREY] = "grey";
  c[WHITE] = "white";
  c[RED] = "red";
  c[YELLOW] = "yellow";
  c[GREEN] = "green";
  c[BLUE] = "blue";
  return c;
}

int matchingColor(float r, float g, float b)
{
  ROS_ASSERT(r>=0 && g>=0 && b>=0 && r<=1 && g<=1 && b<=1);
  float h=0, s=0, v=0;
  rgbToHsv (r, g, b, h, s, v);
  ROS_ASSERT (h>=0 && h<=1 && s>=0 && s<=1 && v>=0 && v<=1);

  if (v<0.3)
    return BLACK;
  else if (v>0.85 && s<0.15)
    return WHITE;
  else if (s<0.2)
    return GREY;
  if (h < 0.1 || h >= 0.75)
    return RED;
  else if (h >= 0.1 && h < 0.15)
    return YELLOW;
  else if (h >= 0.15 && h < 0.45)
    return GREEN;
  else // if (h >= 0.45 && h < 0.75)
    return BLUE;
}


string blobColorName(const Blob& blob)
{
  static vector<string> colors = createColors();

  typedef map<int, int> Histogram;
  Histogram counts;
  BOOST_FOREACH (const Point& p, *blob.cloud)
  {
    float r, g, b;
    uint32_t rgb = *reinterpret_cast<const int*>(&p.rgb);
    uint8_t rb = (rgb >> 16) & 0x0000ff;
    uint8_t gb = (rgb >> 8)  & 0x0000ff;
    uint8_t bb = (rgb)       & 0x0000ff;
    r = rb/255.0;
    g = gb/255.0;
    b = bb/255.0;
    counts[matchingColor(r, g, b)] += 1;
  }
  
  int best_count=-1;
  int best=-42;
  BOOST_FOREACH (const Histogram::value_type& e, counts)
  {
    if (e.second>best_count)
    {
      best_count = e.second;
      best = e.first;
    }
  }
  ROS_ASSERT (best_count>0);
  return colors[best];
}

} // namespace