point_cloud_transformers.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2010, Willow Garage, Inc.
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 #include "point_cloud_transformers.h"
00031 #include "rviz/properties/property.h"
00032 #include "rviz/properties/property_manager.h"
00033 #include "rviz/validate_floats.h"
00034 #include <OGRE/OgreColourValue.h>
00035 #include <OGRE/OgreVector3.h>
00036 #include <OGRE/OgreMatrix4.h>
00037 
00038 namespace rviz
00039 {
00040 
00041 static void getRainbowColor(float value, Ogre::ColourValue& color)
00042 {
00043   value = std::min(value, 1.0f);
00044   value = std::max(value, 0.0f);
00045 
00046   float h = value * 5.0f + 1.0f;
00047   int i = floor(h);
00048   float f = h - i;
00049   if ( !(i&1) ) f = 1 - f; // if i is even
00050   float n = 1 - f;
00051 
00052   if      (i <= 1) color[0] = n, color[1] = 0, color[2] = 1;
00053   else if (i == 2) color[0] = 0, color[1] = n, color[2] = 1;
00054   else if (i == 3) color[0] = 0, color[1] = 1, color[2] = n;
00055   else if (i == 4) color[0] = n, color[1] = 1, color[2] = 0;
00056   else if (i >= 5) color[0] = 1, color[1] = n, color[2] = 0;
00057 }
00058 
00059 uint8_t IntensityPCTransformer::supports(const sensor_msgs::PointCloud2ConstPtr& cloud)
00060 {
00061   updateChannels(cloud);
00062   return Support_Color;
00063 }
00064 
00065 uint8_t IntensityPCTransformer::score(const sensor_msgs::PointCloud2ConstPtr& cloud)
00066 {
00067   return 255;
00068 }
00069 
00070 bool IntensityPCTransformer::transform(const sensor_msgs::PointCloud2ConstPtr& cloud, uint32_t mask, const Ogre::Matrix4& transform, V_PointCloudPoint& points_out)
00071 {
00072   if (!(mask & Support_Color))
00073   {
00074     return false;
00075   }
00076 
00077   int32_t index = findChannelIndex(cloud, selected_channel_);
00078 
00079   if (index == -1)
00080   {
00081     if (selected_channel_ == "intensity")
00082     {
00083       index = findChannelIndex(cloud, "intensities");
00084       if (index == -1)
00085       {
00086         return false;
00087       }
00088     }
00089     else
00090     {
00091       return false;
00092     }
00093   }
00094 
00095   const uint32_t offset = cloud->fields[index].offset;
00096   const uint8_t type = cloud->fields[index].datatype;
00097   const uint32_t point_step = cloud->point_step;
00098   const uint32_t num_points = cloud->width * cloud->height;
00099 
00100   float min_intensity = 999999.0f;
00101   float max_intensity = -999999.0f;
00102   if (auto_compute_intensity_bounds_)
00103   {
00104     for (uint32_t i = 0; i < num_points; ++i)
00105     {
00106       float val = valueFromCloud<float>(cloud, offset, type, point_step, i);
00107       min_intensity = std::min(val, min_intensity);
00108       max_intensity = std::max(val, max_intensity);
00109     }
00110 
00111     min_intensity = std::max(-999999.0f, min_intensity);
00112     max_intensity = std::min(999999.0f, max_intensity);
00113     min_intensity_ = min_intensity;
00114     max_intensity_ = max_intensity;
00115   }
00116   else
00117   {
00118     min_intensity = min_intensity_;
00119     max_intensity = max_intensity_;
00120   }
00121   float diff_intensity = max_intensity - min_intensity;
00122   Color max_color = max_color_;
00123   Color min_color = min_color_;
00124 
00125   for (uint32_t i = 0; i < num_points; ++i)
00126   {
00127     float val = valueFromCloud<float>(cloud, offset, type, point_step, i);
00128 
00129     if (use_full_rgb_colors_) {
00130       float range = std::max(diff_intensity, 0.001f);
00131       float value = 1.0 - (val - min_intensity)/range;
00132       getRainbowColor(value, points_out[i].color);
00133     }
00134     else {
00135       float normalized_intensity = diff_intensity > 0.0f ? ( val - min_intensity ) / diff_intensity : 1.0f;
00136       normalized_intensity = std::min(1.0f, std::max(0.0f, normalized_intensity));
00137       points_out[i].color.r = max_color.r_*normalized_intensity + min_color.r_*(1.0f - normalized_intensity);
00138       points_out[i].color.g = max_color.g_*normalized_intensity + min_color.g_*(1.0f - normalized_intensity);
00139       points_out[i].color.b = max_color.b_*normalized_intensity + min_color.b_*(1.0f - normalized_intensity);
00140     }
00141   }
00142 
00143   return true;
00144 }
00145 
00146 void IntensityPCTransformer::reset()
00147 {
00148   min_intensity_ = 0.0f;
00149   max_intensity_ = 4096.0f;
00150   selected_channel_ = std::string("intensity");
00151   available_channels_.clear();
00152 }
00153 
00154 void IntensityPCTransformer::createProperties(PropertyManager* property_man, const CategoryPropertyWPtr& parent, const std::string& prefix, uint32_t mask, V_PropertyBaseWPtr& out_props)
00155 {
00156   if (mask & Support_Color)
00157   {
00158     channel_name_property_ = property_man->createProperty<EditEnumProperty>("Channel Name", prefix, boost::bind( &IntensityPCTransformer::getChannelName, this),
00159                                                                             boost::bind( &IntensityPCTransformer::setChannelName, this, _1), parent, this);
00160     setPropertyHelpText(channel_name_property_, "Select the channel to use to compute the intensity");
00161 
00162     use_full_rgb_colors_property_ = property_man->createProperty<BoolProperty>( "Use full RGB spectrum",
00163                                                                                 prefix, boost::bind( &IntensityPCTransformer::getUseFullRGBColors, this),
00164                                                                                 boost::bind( &IntensityPCTransformer::setUseFullRGBColors, this, _1),
00165                                                                                 parent, this);
00166     setPropertyHelpText(use_full_rgb_colors_property_, "Whether to interpolate strictly between min and max color or use the full RGB color spectrum for intensities");
00167 
00168     min_color_property_ = property_man->createProperty<ColorProperty>( "Min Color", prefix, boost::bind( &IntensityPCTransformer::getMinColor, this ),
00169                                                                        boost::bind( &IntensityPCTransformer::setMinColor, this, _1 ), parent, this );
00170     setPropertyHelpText(min_color_property_, "Color to assign the points with the minimum intensity.  Actual color is interpolated between this and Max Color.");
00171     max_color_property_ = property_man->createProperty<ColorProperty>( "Max Color", prefix, boost::bind( &IntensityPCTransformer::getMaxColor, this ),
00172                                                                        boost::bind( &IntensityPCTransformer::setMaxColor, this, _1 ), parent, this );
00173     setPropertyHelpText(max_color_property_, "Color to assign the points with the maximum intensity.  Actual color is interpolated between this and Min Color.");
00174     ColorPropertyPtr color_prop = max_color_property_.lock();
00175     // legacy "Color" support... convert it to max color
00176     color_prop->addLegacyName("Color");
00177 
00178     auto_compute_intensity_bounds_property_ = property_man->createProperty<BoolProperty>( "Autocompute Intensity Bounds", prefix, boost::bind( &IntensityPCTransformer::getAutoComputeIntensityBounds, this ),
00179                                                                                           boost::bind( &IntensityPCTransformer::setAutoComputeIntensityBounds, this, _1 ), parent, this );
00180     setPropertyHelpText(auto_compute_intensity_bounds_property_, "Whether to automatically compute the intensity min/max values.");
00181     min_intensity_property_ = property_man->createProperty<FloatProperty>( "Min Intensity", prefix, boost::bind( &IntensityPCTransformer::getMinIntensity, this ),
00182                                                                            boost::bind( &IntensityPCTransformer::setMinIntensity, this, _1 ), parent, this );
00183     setPropertyHelpText(min_intensity_property_, "Minimum possible intensity value, used to interpolate from Min Color to Max Color for a point.");
00184     max_intensity_property_ = property_man->createProperty<FloatProperty>( "Max Intensity", prefix, boost::bind( &IntensityPCTransformer::getMaxIntensity, this ),
00185                                                                            boost::bind( &IntensityPCTransformer::setMaxIntensity, this, _1 ), parent, this );
00186     setPropertyHelpText(max_intensity_property_, "Maximum possible intensity value, used to interpolate from Min Color to Max Color for a point.");
00187 
00188     out_props.push_back(channel_name_property_);
00189     out_props.push_back(use_full_rgb_colors_property_);
00190     out_props.push_back(min_color_property_);
00191     out_props.push_back(max_color_property_);
00192     out_props.push_back(auto_compute_intensity_bounds_property_);
00193     out_props.push_back(min_intensity_property_);
00194     out_props.push_back(max_intensity_property_);
00195 
00196     if (auto_compute_intensity_bounds_)
00197     {
00198       hideProperty(min_intensity_property_);
00199       hideProperty(max_intensity_property_);
00200     }
00201     else
00202     {
00203       showProperty(min_intensity_property_);
00204       showProperty(max_intensity_property_);
00205     }
00206 
00207     if (use_full_rgb_colors_)
00208     {
00209       hideProperty(min_color_property_);
00210       hideProperty(max_color_property_);
00211     }
00212     else
00213     {
00214       showProperty(min_color_property_);
00215       showProperty(max_color_property_);
00216     }
00217   }
00218 }
00219 
00220 void IntensityPCTransformer::setChannelName(const std::string& channel)
00221 {
00222   // If we validate channel here to be in the list of
00223   // available_channels_ it will always fail at load time, since
00224   // available_channels_ is populated dynamically as point cloud
00225   // messages arrive.  Therefore we don't validate it and we live with
00226   // the consequences at runtime.
00227   selected_channel_ = channel;
00228 
00229   propertyChanged(channel_name_property_);
00230 
00231   causeRetransform();
00232 }
00233 
00234 void IntensityPCTransformer::updateChannels(const sensor_msgs::PointCloud2ConstPtr& cloud)
00235 {
00236   V_string channels;
00237   for(size_t i = 0; i < cloud->fields.size(); ++i)
00238   {
00239     channels.push_back(cloud->fields[i].name);
00240   }
00241   std::sort(channels.begin(), channels.end());
00242 
00243   EditEnumPropertyPtr channel_prop = channel_name_property_.lock();
00244   ROS_ASSERT(channel_prop);
00245 
00246   if (channels != available_channels_)
00247   {
00248     channel_prop->clear();
00249     for(V_string::const_iterator it = channels.begin(); it != channels.end(); ++it)
00250     {
00251       const std::string& channel = *it;
00252       if (channel.empty())
00253       {
00254         continue;
00255       }
00256       channel_prop->addOption(channel);
00257     }
00258     available_channels_ = channels;
00259   }
00260 }
00261 
00262 void IntensityPCTransformer::setMaxColor( const Color& color )
00263 {
00264   max_color_ = color;
00265 
00266   propertyChanged(max_color_property_);
00267 
00268   causeRetransform();
00269 }
00270 
00271 void IntensityPCTransformer::setMinColor( const Color& color )
00272 {
00273   min_color_ = color;
00274 
00275   propertyChanged(min_color_property_);
00276 
00277   causeRetransform();
00278 }
00279 
00280 void IntensityPCTransformer::setMinIntensity( float val )
00281 {
00282   min_intensity_ = val;
00283   if (min_intensity_ > max_intensity_)
00284   {
00285     min_intensity_ = max_intensity_;
00286   }
00287 
00288   propertyChanged(min_intensity_property_);
00289 
00290   causeRetransform();
00291 }
00292 
00293 void IntensityPCTransformer::setMaxIntensity( float val )
00294 {
00295   max_intensity_ = val;
00296   if (max_intensity_ < min_intensity_)
00297   {
00298     max_intensity_ = min_intensity_;
00299   }
00300 
00301   propertyChanged(max_intensity_property_);
00302 
00303   causeRetransform();
00304 }
00305 
00306 void IntensityPCTransformer::setAutoComputeIntensityBounds(bool compute)
00307 {
00308   auto_compute_intensity_bounds_ = compute;
00309 
00310   if (auto_compute_intensity_bounds_)
00311   {
00312     hideProperty(min_intensity_property_);
00313     hideProperty(max_intensity_property_);
00314   }
00315   else
00316   {
00317     showProperty(min_intensity_property_);
00318     showProperty(max_intensity_property_);
00319   }
00320 
00321   propertyChanged(auto_compute_intensity_bounds_property_);
00322 
00323   causeRetransform();
00324 }
00325 
00326 void IntensityPCTransformer::setUseFullRGBColors(bool full_rgb)
00327 {
00328   use_full_rgb_colors_ = full_rgb;
00329 
00330   if (use_full_rgb_colors_)
00331   {
00332     hideProperty(min_color_property_);
00333     hideProperty(max_color_property_);
00334   }
00335   else
00336   {
00337     showProperty(min_color_property_);
00338     showProperty(max_color_property_);
00339   }
00340 
00341   propertyChanged(use_full_rgb_colors_property_);
00342 
00343   causeRetransform();
00344 }
00345 
00346 uint8_t XYZPCTransformer::supports(const sensor_msgs::PointCloud2ConstPtr& cloud)
00347 {
00348   int32_t xi = findChannelIndex(cloud, "x");
00349   int32_t yi = findChannelIndex(cloud, "y");
00350   int32_t zi = findChannelIndex(cloud, "z");
00351 
00352   if (xi == -1 || yi == -1 || zi == -1)
00353   {
00354     return Support_None;
00355   }
00356 
00357   if (cloud->fields[xi].datatype == sensor_msgs::PointField::FLOAT32)
00358   {
00359     return Support_XYZ;
00360   }
00361 
00362   return Support_None;
00363 }
00364 
00365 bool XYZPCTransformer::transform(const sensor_msgs::PointCloud2ConstPtr& cloud, uint32_t mask, const Ogre::Matrix4& transform, V_PointCloudPoint& points_out)
00366 {
00367   if (!(mask & Support_XYZ))
00368   {
00369     return false;
00370   }
00371 
00372   int32_t xi = findChannelIndex(cloud, "x");
00373   int32_t yi = findChannelIndex(cloud, "y");
00374   int32_t zi = findChannelIndex(cloud, "z");
00375 
00376   const uint32_t xoff = cloud->fields[xi].offset;
00377   const uint32_t yoff = cloud->fields[yi].offset;
00378   const uint32_t zoff = cloud->fields[zi].offset;
00379   const uint32_t point_step = cloud->point_step;
00380   const uint32_t num_points = cloud->width * cloud->height;
00381   uint8_t const* point = &cloud->data.front();
00382   for (uint32_t i = 0; i < num_points; ++i, point += point_step)
00383   {
00384     float x = *reinterpret_cast<const float*>(point + xoff);
00385     float y = *reinterpret_cast<const float*>(point + yoff);
00386     float z = *reinterpret_cast<const float*>(point + zoff);
00387 
00388     Ogre::Vector3 pos(x, y, z);
00389     pos = transform * pos;
00390     points_out[i].position = pos;
00391   }
00392 
00393   return true;
00394 }
00395 
00396 uint8_t RGB8PCTransformer::supports(const sensor_msgs::PointCloud2ConstPtr& cloud)
00397 {
00398   int32_t index = findChannelIndex(cloud, "rgb");
00399   if (index == -1)
00400   {
00401     return Support_None;
00402   }
00403 
00404   if (cloud->fields[index].datatype == sensor_msgs::PointField::INT32 ||
00405       cloud->fields[index].datatype == sensor_msgs::PointField::UINT32 ||
00406       cloud->fields[index].datatype == sensor_msgs::PointField::FLOAT32)
00407   {
00408     return Support_Color;
00409   }
00410 
00411   return Support_None;
00412 }
00413 
00414 bool RGB8PCTransformer::transform(const sensor_msgs::PointCloud2ConstPtr& cloud, uint32_t mask, const Ogre::Matrix4& transform, V_PointCloudPoint& points_out)
00415 {
00416   if (!(mask & Support_Color))
00417   {
00418     return false;
00419   }
00420 
00421   int32_t index = findChannelIndex(cloud, "rgb");
00422 
00423   const uint32_t off = cloud->fields[index].offset;
00424   const uint32_t point_step = cloud->point_step;
00425   const uint32_t num_points = cloud->width * cloud->height;
00426   uint8_t const* point = &cloud->data.front();
00427   for (uint32_t i = 0; i < num_points; ++i, point += point_step)
00428   {
00429     uint32_t rgb = *reinterpret_cast<const uint32_t*>(point + off);
00430     float r = ((rgb >> 16) & 0xff) / 255.0f;
00431     float g = ((rgb >> 8) & 0xff) / 255.0f;
00432     float b = (rgb & 0xff) / 255.0f;
00433     points_out[i].color = Ogre::ColourValue(r, g, b);
00434   }
00435 
00436   return true;
00437 }
00438 
00439 uint8_t RGBF32PCTransformer::supports(const sensor_msgs::PointCloud2ConstPtr& cloud)
00440 {
00441   int32_t ri = findChannelIndex(cloud, "r");
00442   int32_t gi = findChannelIndex(cloud, "g");
00443   int32_t bi = findChannelIndex(cloud, "b");
00444   if (ri == -1 || gi == -1 || bi == -1)
00445   {
00446     return Support_None;
00447   }
00448 
00449   if (cloud->fields[ri].datatype == sensor_msgs::PointField::FLOAT32)
00450   {
00451     return Support_Color;
00452   }
00453 
00454   return Support_None;
00455 }
00456 
00457 bool RGBF32PCTransformer::transform(const sensor_msgs::PointCloud2ConstPtr& cloud, uint32_t mask, const Ogre::Matrix4& transform, V_PointCloudPoint& points_out)
00458 {
00459   if (!(mask & Support_Color))
00460   {
00461     return false;
00462   }
00463 
00464   int32_t ri = findChannelIndex(cloud, "r");
00465   int32_t gi = findChannelIndex(cloud, "g");
00466   int32_t bi = findChannelIndex(cloud, "b");
00467 
00468   const uint32_t roff = cloud->fields[ri].offset;
00469   const uint32_t goff = cloud->fields[gi].offset;
00470   const uint32_t boff = cloud->fields[bi].offset;
00471   const uint32_t point_step = cloud->point_step;
00472   const uint32_t num_points = cloud->width * cloud->height;
00473   uint8_t const* point = &cloud->data.front();
00474   for (uint32_t i = 0; i < num_points; ++i, point += point_step)
00475   {
00476     float r = *reinterpret_cast<const float*>(point + roff);
00477     float g = *reinterpret_cast<const float*>(point + goff);
00478     float b = *reinterpret_cast<const float*>(point + boff);
00479     points_out[i].color = Ogre::ColourValue(r, g, b);
00480   }
00481 
00482   return true;
00483 }
00484 
00485 uint8_t FlatColorPCTransformer::supports(const sensor_msgs::PointCloud2ConstPtr& cloud)
00486 {
00487   return Support_Color;
00488 }
00489 
00490 uint8_t FlatColorPCTransformer::score(const sensor_msgs::PointCloud2ConstPtr& cloud)
00491 {
00492   return 0;
00493 }
00494 
00495 bool FlatColorPCTransformer::transform(const sensor_msgs::PointCloud2ConstPtr& cloud, uint32_t mask, const Ogre::Matrix4& transform, V_PointCloudPoint& points_out)
00496 {
00497   if (!(mask & Support_Color))
00498   {
00499     return false;
00500   }
00501 
00502   const uint32_t num_points = cloud->width * cloud->height;
00503   for (uint32_t i = 0; i < num_points; ++i)
00504   {
00505     points_out[i].color = Ogre::ColourValue(color_.r_, color_.g_, color_.b_);
00506   }
00507 
00508   return true;
00509 }
00510 
00511 void FlatColorPCTransformer::setColor(const Color& c)
00512 {
00513   color_ = c;
00514   propertyChanged(color_property_);
00515   causeRetransform();
00516 }
00517 
00518 void FlatColorPCTransformer::createProperties(PropertyManager* property_man, const CategoryPropertyWPtr& parent, const std::string& prefix, uint32_t mask, V_PropertyBaseWPtr& out_props)
00519 {
00520   if (mask & Support_Color)
00521   {
00522     color_property_ = property_man->createProperty<ColorProperty>("Color", prefix, boost::bind( &FlatColorPCTransformer::getColor, this ),
00523                                                                   boost::bind( &FlatColorPCTransformer::setColor, this, _1 ), parent, this);
00524     setPropertyHelpText(color_property_, "Color to assign to every point.");
00525 
00526     out_props.push_back(color_property_);
00527   }
00528 }
00529 
00530 uint8_t AxisColorPCTransformer::supports(const sensor_msgs::PointCloud2ConstPtr& cloud)
00531 {
00532   return Support_Color;
00533 }
00534 
00535 uint8_t AxisColorPCTransformer::score(const sensor_msgs::PointCloud2ConstPtr& cloud)
00536 {
00537   return 255;
00538 }
00539 
00540 bool AxisColorPCTransformer::transform(const sensor_msgs::PointCloud2ConstPtr& cloud, uint32_t mask, const Ogre::Matrix4& transform, V_PointCloudPoint& points_out)
00541 {
00542   if (!(mask & Support_Color))
00543   {
00544     return false;
00545   }
00546 
00547   int32_t xi = findChannelIndex(cloud, "x");
00548   int32_t yi = findChannelIndex(cloud, "y");
00549   int32_t zi = findChannelIndex(cloud, "z");
00550 
00551   const uint32_t xoff = cloud->fields[xi].offset;
00552   const uint32_t yoff = cloud->fields[yi].offset;
00553   const uint32_t zoff = cloud->fields[zi].offset;
00554   const uint32_t point_step = cloud->point_step;
00555   const uint32_t num_points = cloud->width * cloud->height;
00556   uint8_t const* point = &cloud->data.front();
00557 
00558   // compute bounds
00559 
00560   float min_value_current = 9999.0f;
00561   float max_value_current = -9999.0f;
00562   std::vector<float> values;
00563   values.reserve(num_points);
00564 
00565   for (uint32_t i = 0; i < num_points; ++i, point += point_step)
00566   {
00567     float x = *reinterpret_cast<const float*>(point + xoff);
00568     float y = *reinterpret_cast<const float*>(point + yoff);
00569     float z = *reinterpret_cast<const float*>(point + zoff);
00570 
00571     Ogre::Vector3 pos(x, y, z);
00572 
00573     if (use_fixed_frame_)
00574     {
00575       pos = transform * pos;
00576     }
00577 
00578     float val = pos[axis_];
00579     min_value_current = std::min(min_value_current, val);
00580     max_value_current = std::max(max_value_current, val);
00581 
00582     values.push_back(val);
00583   }
00584 
00585   if (auto_compute_bounds_)
00586   {
00587     min_value_ = min_value_current;
00588     max_value_ = max_value_current;
00589   }
00590 
00591   for (uint32_t i = 0; i < num_points; ++i)
00592   {
00593     float range = std::max(max_value_ - min_value_, 0.001f);
00594     float value = 1.0 - (values[i] - min_value_)/range;
00595     getRainbowColor(value, points_out[i].color);
00596   }
00597 
00598   return true;
00599 }
00600 
00601 void AxisColorPCTransformer::createProperties(PropertyManager* property_man, const CategoryPropertyWPtr& parent, const std::string& prefix, uint32_t mask, V_PropertyBaseWPtr& out_props)
00602 {
00603   if (mask & Support_Color)
00604   {
00605     axis_property_ = property_man->createProperty<EnumProperty>("Axis", prefix, boost::bind(&AxisColorPCTransformer::getAxis, this), boost::bind(&AxisColorPCTransformer::setAxis, this, _1),
00606                                                                 parent, this);
00607     EnumPropertyPtr prop = axis_property_.lock();
00608     prop->addOption("X", AXIS_X);
00609     prop->addOption("Y", AXIS_Y);
00610     prop->addOption("Z", AXIS_Z);
00611     setPropertyHelpText(axis_property_, "The axis to interpolate the color along.");
00612     auto_compute_bounds_property_ = property_man->createProperty<BoolProperty>( "Autocompute Value Bounds", prefix, boost::bind( &AxisColorPCTransformer::getAutoComputeBounds, this ),
00613                                                                               boost::bind( &AxisColorPCTransformer::setAutoComputeBounds, this, _1 ), parent, this );
00614 
00615     setPropertyHelpText(auto_compute_bounds_property_, "Whether to automatically compute the value min/max values.");
00616     min_value_property_ = property_man->createProperty<FloatProperty>( "Min Value", prefix, boost::bind( &AxisColorPCTransformer::getMinValue, this ),
00617                                                                               boost::bind( &AxisColorPCTransformer::setMinValue, this, _1 ), parent, this );
00618     setPropertyHelpText(min_value_property_, "Minimum value value, used to interpolate the color of a point.");
00619     max_value_property_ = property_man->createProperty<FloatProperty>( "Max Value", prefix, boost::bind( &AxisColorPCTransformer::getMaxValue, this ),
00620                                                                             boost::bind( &AxisColorPCTransformer::setMaxValue, this, _1 ), parent, this );
00621     setPropertyHelpText(max_value_property_, "Maximum value value, used to interpolate the color of a point.");
00622 
00623     use_fixed_frame_property_ = property_man->createProperty<BoolProperty>( "Use Fixed Frame", prefix, boost::bind( &AxisColorPCTransformer::getUseFixedFrame, this ),
00624                                                                             boost::bind( &AxisColorPCTransformer::setUseFixedFrame, this, _1 ), parent, this );
00625     setPropertyHelpText(use_fixed_frame_property_, "Whether to color the cloud based on its fixed frame position or its local frame position.");
00626 
00627     out_props.push_back(axis_property_);
00628     out_props.push_back(auto_compute_bounds_property_);
00629     out_props.push_back(min_value_property_);
00630     out_props.push_back(max_value_property_);
00631     out_props.push_back(use_fixed_frame_property_);
00632 
00633     if (auto_compute_bounds_)
00634     {
00635       hideProperty(min_value_property_);
00636       hideProperty(max_value_property_);
00637     }
00638     else
00639     {
00640       showProperty(min_value_property_);
00641       showProperty(max_value_property_);
00642     }
00643   }
00644 }
00645 
00646 void AxisColorPCTransformer::setUseFixedFrame(bool use)
00647 {
00648   use_fixed_frame_ = use;
00649   propertyChanged(use_fixed_frame_property_);
00650   causeRetransform();
00651 }
00652 
00653 void AxisColorPCTransformer::setAxis(int axis)
00654 {
00655   axis_ = axis;
00656   propertyChanged(axis_property_);
00657   causeRetransform();
00658 }
00659 
00660 void AxisColorPCTransformer::setMinValue( float val )
00661 {
00662   min_value_ = val;
00663   if (min_value_ > max_value_)
00664   {
00665     min_value_ = max_value_;
00666   }
00667 
00668   propertyChanged(min_value_property_);
00669 
00670   causeRetransform();
00671 }
00672 
00673 void AxisColorPCTransformer::setMaxValue( float val )
00674 {
00675   max_value_ = val;
00676   if (max_value_ < min_value_)
00677   {
00678     max_value_ = min_value_;
00679   }
00680 
00681   propertyChanged(max_value_property_);
00682 
00683   causeRetransform();
00684 }
00685 
00686 void AxisColorPCTransformer::setAutoComputeBounds(bool compute)
00687 {
00688   auto_compute_bounds_ = compute;
00689 
00690   if (auto_compute_bounds_)
00691   {
00692     hideProperty(min_value_property_);
00693     hideProperty(max_value_property_);
00694   }
00695   else
00696   {
00697     showProperty(min_value_property_);
00698     showProperty(max_value_property_);
00699   }
00700 
00701   propertyChanged(auto_compute_bounds_property_);
00702 
00703   causeRetransform();
00704 }
00705 
00706 }


rviz
Author(s): Dave Hershberger, Josh Faust
autogenerated on Mon Jan 6 2014 11:54:32