cloud_tools.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright 2013-2014, Unbounded Robotics Inc. 
00003  * Copyright 2011, Willow Garage, Inc. (hsv2rgb)
00004  * All rights reserved.
00005  *
00006  * Redistribution and use in source and binary forms, with or without
00007  * modification, are permitted provided that the following conditions are met:
00008  *
00009  *     * Redistributions of source code must retain the above copyright
00010  *       notice, this list of conditions and the following disclaimer.
00011  *     * Redistributions in binary form must reproduce the above copyright
00012  *       notice, this list of conditions and the following disclaimer in the
00013  *       documentation and/or other materials provided with the distribution.
00014  *     * Neither the name of the Unbounded Robotics, Inc. nor the names of its
00015  *       contributors may be used to endorse or promote products derived from
00016  *       this software without specific prior written permission.
00017  *
00018  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00019  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00020  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00021  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00022  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00023  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00024  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00025  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00026  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00027  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00028  * POSSIBILITY OF SUCH DAMAGE.
00029  */
00030 
00031 // Author: Michael Ferguson
00032 
00033 #include <simple_grasping/cloud_tools.h>
00034 
00035 namespace simple_grasping
00036 {
00037 
00038 // http://en.wikipedia.org/wiki/HSL_and_HSV#Converting_to_RGB
00039 // for points on a dark background you want somewhat lightened
00040 // colors generally... back off the saturation (s)      
00041 void hsv2rgb(const float h, const float s, const float v, float& r, float& g, float& b)
00042 {
00043   float c = v * s;
00044   float hprime = h/60.0;
00045   float x = c * (1.0 - fabs(fmodf(hprime, 2.0f) - 1));
00046 
00047   r = g = b = 0;
00048 
00049   if (hprime < 1) {
00050     r = c; g = x;
00051   } else if (hprime < 2) {
00052     r = x; g = c;
00053   } else if (hprime < 3) {
00054     g = c; b = x;
00055   } else if (hprime < 4) {
00056     g = x; b = c;
00057   } else if (hprime < 5) {
00058     r = x; b = c;
00059   } else if (hprime < 6) {
00060     r = c; b = x;
00061   }
00062 
00063   float m = v - c;
00064   r += m; g+=m; b+=m;
00065 }
00066 
00067 void colorizeCloud(pcl::PointCloud<pcl::PointXYZRGB>& cloud, float hue)
00068 {
00069   std::vector<pcl::PCLPointField> fields;
00070   pcl::getFields(cloud, fields);
00071   size_t rgb_field_index;
00072   for (rgb_field_index = 0; rgb_field_index < fields.size(); ++rgb_field_index)
00073   {
00074     if (fields[rgb_field_index].name == "rgb" ||
00075         fields[rgb_field_index].name == "rgba")
00076       break;
00077   }
00078 
00079   float r, g, b;
00080   hsv2rgb(hue, 0.8 /*saturation*/, 1.0 /*value*/, r, g, b);
00081 
00082   for (size_t j = 0; j < cloud.points.size(); ++j)
00083   {
00084     pcl::PointXYZRGB &p = cloud.points[j];
00085     unsigned char* pt_rgb = (unsigned char*) &p;
00086     pt_rgb += fields[rgb_field_index].offset;
00087     (*pt_rgb) = (unsigned char) (r * 255);
00088     (*(pt_rgb+1)) = (unsigned char) (g * 255);
00089     (*(pt_rgb+2)) = (unsigned char) (b * 255);
00090   }
00091 }
00092 
00093 double distancePointToPlane(const Eigen::Vector4f& point, const pcl::ModelCoefficients::Ptr plane)
00094 {
00095   Eigen::Vector4f pp(point);
00096   pp[3] = 1.0;
00097   Eigen::Vector4f m(plane->values[0], plane->values[1], plane->values[2], plane->values[3]);
00098   return pp.dot(m);
00099 }
00100 
00101 }  // namespace simple_grasping


simple_grasping
Author(s): Michael Ferguson
autogenerated on Fri Aug 26 2016 13:17:24