00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030 #include <OgreColourValue.h>
00031 #include <OgreMatrix4.h>
00032 #include <OgreVector3.h>
00033
00034 #include "rviz/properties/bool_property.h"
00035 #include "rviz/properties/color_property.h"
00036 #include "rviz/properties/editable_enum_property.h"
00037 #include "rviz/properties/enum_property.h"
00038 #include "rviz/properties/float_property.h"
00039 #include "rviz/validate_floats.h"
00040
00041 #include "point_cloud_transformers.h"
00042
00043 namespace rviz
00044 {
00045
00046 static void getRainbowColor(float value, Ogre::ColourValue& color)
00047 {
00048 value = std::min(value, 1.0f);
00049 value = std::max(value, 0.0f);
00050
00051 float h = value * 5.0f + 1.0f;
00052 int i = floor(h);
00053 float f = h - i;
00054 if ( !(i&1) ) f = 1 - f;
00055 float n = 1 - f;
00056
00057 if (i <= 1) color[0] = n, color[1] = 0, color[2] = 1;
00058 else if (i == 2) color[0] = 0, color[1] = n, color[2] = 1;
00059 else if (i == 3) color[0] = 0, color[1] = 1, color[2] = n;
00060 else if (i == 4) color[0] = n, color[1] = 1, color[2] = 0;
00061 else if (i >= 5) color[0] = 1, color[1] = n, color[2] = 0;
00062 }
00063
00064 uint8_t IntensityPCTransformer::supports(const sensor_msgs::PointCloud2ConstPtr& cloud)
00065 {
00066 updateChannels(cloud);
00067 return Support_Color;
00068 }
00069
00070 uint8_t IntensityPCTransformer::score(const sensor_msgs::PointCloud2ConstPtr& cloud)
00071 {
00072 return 255;
00073 }
00074
00075 bool IntensityPCTransformer::transform( const sensor_msgs::PointCloud2ConstPtr& cloud,
00076 uint32_t mask,
00077 const Ogre::Matrix4& transform,
00078 V_PointCloudPoint& points_out )
00079 {
00080 if( !( mask & Support_Color ))
00081 {
00082 return false;
00083 }
00084
00085 int32_t index = findChannelIndex( cloud, channel_name_property_->getStdString() );
00086
00087 if( index == -1 )
00088 {
00089 if( channel_name_property_->getStdString() == "intensity" )
00090 {
00091 index = findChannelIndex( cloud, "intensities" );
00092 if( index == -1 )
00093 {
00094 return false;
00095 }
00096 }
00097 else
00098 {
00099 return false;
00100 }
00101 }
00102
00103 const uint32_t offset = cloud->fields[index].offset;
00104 const uint8_t type = cloud->fields[index].datatype;
00105 const uint32_t point_step = cloud->point_step;
00106 const uint32_t num_points = cloud->width * cloud->height;
00107
00108 float min_intensity = 999999.0f;
00109 float max_intensity = -999999.0f;
00110 if( auto_compute_intensity_bounds_property_->getBool() )
00111 {
00112 for( uint32_t i = 0; i < num_points; ++i )
00113 {
00114 float val = valueFromCloud<float>(cloud, offset, type, point_step, i);
00115 min_intensity = std::min(val, min_intensity);
00116 max_intensity = std::max(val, max_intensity);
00117 }
00118
00119 min_intensity = std::max(-999999.0f, min_intensity);
00120 max_intensity = std::min(999999.0f, max_intensity);
00121 min_intensity_property_->setFloat( min_intensity );
00122 max_intensity_property_->setFloat( max_intensity );
00123 }
00124 else
00125 {
00126 min_intensity = min_intensity_property_->getFloat();
00127 max_intensity = max_intensity_property_->getFloat();
00128 }
00129 float diff_intensity = max_intensity - min_intensity;
00130 if( diff_intensity == 0 )
00131 {
00132
00133
00134
00135
00136 diff_intensity = 1e20;
00137 }
00138 Ogre::ColourValue max_color = max_color_property_->getOgreColor();
00139 Ogre::ColourValue min_color = min_color_property_->getOgreColor();
00140
00141 if( use_rainbow_property_->getBool() )
00142 {
00143 for (uint32_t i = 0; i < num_points; ++i)
00144 {
00145 float val = valueFromCloud<float>(cloud, offset, type, point_step, i);
00146 float value = 1.0 - (val - min_intensity)/diff_intensity;
00147 if(invert_rainbow_property_->getBool() ){
00148 value = 1.0 - value;
00149 }
00150 getRainbowColor(value, points_out[i].color);
00151 }
00152 }
00153 else
00154 {
00155 for (uint32_t i = 0; i < num_points; ++i)
00156 {
00157 float val = valueFromCloud<float>(cloud, offset, type, point_step, i);
00158 float normalized_intensity = ( val - min_intensity ) / diff_intensity;
00159 normalized_intensity = std::min(1.0f, std::max(0.0f, normalized_intensity));
00160 points_out[i].color.r = max_color.r * normalized_intensity + min_color.r * (1.0f - normalized_intensity);
00161 points_out[i].color.g = max_color.g * normalized_intensity + min_color.g * (1.0f - normalized_intensity);
00162 points_out[i].color.b = max_color.b * normalized_intensity + min_color.b * (1.0f - normalized_intensity);
00163 }
00164 }
00165
00166 return true;
00167 }
00168
00169 void IntensityPCTransformer::createProperties( Property* parent_property, uint32_t mask, QList<Property*>& out_props )
00170 {
00171 if( mask & Support_Color )
00172 {
00173 channel_name_property_ = new EditableEnumProperty( "Channel Name", "intensity",
00174 "Select the channel to use to compute the intensity",
00175 parent_property, SIGNAL( needRetransform() ), this );
00176
00177 use_rainbow_property_ = new BoolProperty( "Use rainbow", true,
00178 "Whether to use a rainbow of colors or interpolate between two",
00179 parent_property, SLOT( updateUseRainbow() ), this );
00180 invert_rainbow_property_ = new BoolProperty( "Invert Rainbow", false,
00181 "Whether to invert rainbow colors",
00182 parent_property, SLOT( updateUseRainbow() ), this );
00183
00184 min_color_property_ = new ColorProperty( "Min Color", Qt::black,
00185 "Color to assign the points with the minimum intensity. "
00186 "Actual color is interpolated between this and Max Color.",
00187 parent_property, SIGNAL( needRetransform() ), this );
00188
00189 max_color_property_ = new ColorProperty( "Max Color", Qt::white,
00190 "Color to assign the points with the maximum intensity. "
00191 "Actual color is interpolated between this and Min Color.",
00192 parent_property, SIGNAL( needRetransform() ), this );
00193
00194 auto_compute_intensity_bounds_property_ = new BoolProperty( "Autocompute Intensity Bounds", true,
00195 "Whether to automatically compute the intensity min/max values.",
00196 parent_property, SLOT( updateAutoComputeIntensityBounds() ), this );
00197
00198 min_intensity_property_ = new FloatProperty( "Min Intensity", 0,
00199 "Minimum possible intensity value, used to interpolate from Min Color to Max Color for a point.",
00200 parent_property );
00201
00202 max_intensity_property_ = new FloatProperty( "Max Intensity", 4096,
00203 "Maximum possible intensity value, used to interpolate from Min Color to Max Color for a point.",
00204 parent_property );
00205
00206 out_props.push_back( channel_name_property_ );
00207 out_props.push_back( use_rainbow_property_ );
00208 out_props.push_back( invert_rainbow_property_ );
00209 out_props.push_back( min_color_property_ );
00210 out_props.push_back( max_color_property_ );
00211 out_props.push_back( auto_compute_intensity_bounds_property_ );
00212 out_props.push_back( min_intensity_property_ );
00213 out_props.push_back( max_intensity_property_ );
00214
00215 updateUseRainbow();
00216 updateAutoComputeIntensityBounds();
00217 }
00218 }
00219
00220 void IntensityPCTransformer::updateChannels( const sensor_msgs::PointCloud2ConstPtr& cloud )
00221 {
00222 V_string channels;
00223 for(size_t i = 0; i < cloud->fields.size(); ++i )
00224 {
00225 channels.push_back(cloud->fields[i].name );
00226 }
00227 std::sort(channels.begin(), channels.end());
00228
00229 if( channels != available_channels_ )
00230 {
00231 channel_name_property_->clearOptions();
00232 for( V_string::const_iterator it = channels.begin(); it != channels.end(); ++it )
00233 {
00234 const std::string& channel = *it;
00235 if( channel.empty() )
00236 {
00237 continue;
00238 }
00239 channel_name_property_->addOptionStd( channel );
00240 }
00241 available_channels_ = channels;
00242 }
00243 }
00244
00245 void IntensityPCTransformer::updateAutoComputeIntensityBounds()
00246 {
00247 bool auto_compute = auto_compute_intensity_bounds_property_->getBool();
00248 min_intensity_property_->setHidden( auto_compute );
00249 max_intensity_property_->setHidden( auto_compute );
00250 if( auto_compute )
00251 {
00252 disconnect( min_intensity_property_, SIGNAL( changed() ), this, SIGNAL( needRetransform() ));
00253 disconnect( max_intensity_property_, SIGNAL( changed() ), this, SIGNAL( needRetransform() ));
00254 }
00255 else
00256 {
00257 connect( min_intensity_property_, SIGNAL( changed() ), this, SIGNAL( needRetransform() ));
00258 connect( max_intensity_property_, SIGNAL( changed() ), this, SIGNAL( needRetransform() ));
00259 }
00260 Q_EMIT needRetransform();
00261 }
00262
00263 void IntensityPCTransformer::updateUseRainbow()
00264 {
00265 bool use_rainbow = use_rainbow_property_->getBool();
00266 invert_rainbow_property_->setHidden( !use_rainbow );
00267 min_color_property_->setHidden( use_rainbow );
00268 max_color_property_->setHidden( use_rainbow );
00269 Q_EMIT needRetransform();
00270 }
00271
00272 uint8_t XYZPCTransformer::supports(const sensor_msgs::PointCloud2ConstPtr& cloud)
00273 {
00274 int32_t xi = findChannelIndex(cloud, "x");
00275 int32_t yi = findChannelIndex(cloud, "y");
00276 int32_t zi = findChannelIndex(cloud, "z");
00277
00278 if (xi == -1 || yi == -1 || zi == -1)
00279 {
00280 return Support_None;
00281 }
00282
00283 if (cloud->fields[xi].datatype == sensor_msgs::PointField::FLOAT32)
00284 {
00285 return Support_XYZ;
00286 }
00287
00288 return Support_None;
00289 }
00290
00291 bool XYZPCTransformer::transform(const sensor_msgs::PointCloud2ConstPtr& cloud, uint32_t mask, const Ogre::Matrix4& transform, V_PointCloudPoint& points_out)
00292 {
00293 if (!(mask & Support_XYZ))
00294 {
00295 return false;
00296 }
00297
00298 int32_t xi = findChannelIndex(cloud, "x");
00299 int32_t yi = findChannelIndex(cloud, "y");
00300 int32_t zi = findChannelIndex(cloud, "z");
00301
00302 const uint32_t xoff = cloud->fields[xi].offset;
00303 const uint32_t yoff = cloud->fields[yi].offset;
00304 const uint32_t zoff = cloud->fields[zi].offset;
00305 const uint32_t point_step = cloud->point_step;
00306 const uint32_t num_points = cloud->width * cloud->height;
00307 uint8_t const* point_x = &cloud->data.front() + xoff;
00308 uint8_t const* point_y = &cloud->data.front() + yoff;
00309 uint8_t const* point_z = &cloud->data.front() + zoff;
00310 for (V_PointCloudPoint::iterator iter = points_out.begin(); iter != points_out.end(); ++iter, point_x += point_step,
00311 point_y += point_step, point_z += point_step)
00312 {
00313 iter->position.x = *reinterpret_cast<const float*>(point_x);
00314 iter->position.y = *reinterpret_cast<const float*>(point_y);
00315 iter->position.z = *reinterpret_cast<const float*>(point_z);
00316 }
00317
00318 return true;
00319 }
00320
00321 uint8_t RGB8PCTransformer::supports(const sensor_msgs::PointCloud2ConstPtr& cloud)
00322 {
00323 int32_t index = findChannelIndex(cloud, "rgb");
00324 if (index == -1)
00325 {
00326 return Support_None;
00327 }
00328
00329 if (cloud->fields[index].datatype == sensor_msgs::PointField::INT32 ||
00330 cloud->fields[index].datatype == sensor_msgs::PointField::UINT32 ||
00331 cloud->fields[index].datatype == sensor_msgs::PointField::FLOAT32)
00332 {
00333 return Support_Color;
00334 }
00335
00336 return Support_None;
00337 }
00338
00339 bool RGB8PCTransformer::transform(const sensor_msgs::PointCloud2ConstPtr& cloud, uint32_t mask, const Ogre::Matrix4& transform, V_PointCloudPoint& points_out)
00340 {
00341 if (!(mask & Support_Color))
00342 {
00343 return false;
00344 }
00345
00346 int32_t index = findChannelIndex(cloud, "rgb");
00347
00348 const uint32_t off = cloud->fields[index].offset;
00349 uint8_t const* rgb_ptr = &cloud->data.front() + off;
00350 const uint32_t point_step = cloud->point_step;
00351
00352
00353 float rgb_lut[256];
00354 for(int i = 0; i < 256; ++i)
00355 {
00356 rgb_lut[i] = float(i)/255.0f;
00357 }
00358 for (V_PointCloudPoint::iterator iter = points_out.begin(); iter != points_out.end(); ++iter, rgb_ptr += point_step)
00359 {
00360 uint32_t rgb = *reinterpret_cast<const uint32_t*>(rgb_ptr);
00361 iter->color.r = rgb_lut[(rgb >> 16) & 0xff];
00362 iter->color.g = rgb_lut[(rgb >> 8) & 0xff];
00363 iter->color.b = rgb_lut[rgb & 0xff];
00364 iter->color.a = 1.0f;
00365 }
00366
00367 return true;
00368 }
00369
00370 uint8_t RGBF32PCTransformer::supports(const sensor_msgs::PointCloud2ConstPtr& cloud)
00371 {
00372 int32_t ri = findChannelIndex(cloud, "r");
00373 int32_t gi = findChannelIndex(cloud, "g");
00374 int32_t bi = findChannelIndex(cloud, "b");
00375 if (ri == -1 || gi == -1 || bi == -1)
00376 {
00377 return Support_None;
00378 }
00379
00380 if (cloud->fields[ri].datatype == sensor_msgs::PointField::FLOAT32)
00381 {
00382 return Support_Color;
00383 }
00384
00385 return Support_None;
00386 }
00387
00388 bool RGBF32PCTransformer::transform(const sensor_msgs::PointCloud2ConstPtr& cloud, uint32_t mask, const Ogre::Matrix4& transform, V_PointCloudPoint& points_out)
00389 {
00390 if (!(mask & Support_Color))
00391 {
00392 return false;
00393 }
00394
00395 int32_t ri = findChannelIndex(cloud, "r");
00396 int32_t gi = findChannelIndex(cloud, "g");
00397 int32_t bi = findChannelIndex(cloud, "b");
00398
00399 const uint32_t roff = cloud->fields[ri].offset;
00400 const uint32_t goff = cloud->fields[gi].offset;
00401 const uint32_t boff = cloud->fields[bi].offset;
00402 const uint32_t point_step = cloud->point_step;
00403 const uint32_t num_points = cloud->width * cloud->height;
00404 uint8_t const* point = &cloud->data.front();
00405 for (uint32_t i = 0; i < num_points; ++i, point += point_step)
00406 {
00407 float r = *reinterpret_cast<const float*>(point + roff);
00408 float g = *reinterpret_cast<const float*>(point + goff);
00409 float b = *reinterpret_cast<const float*>(point + boff);
00410 points_out[i].color = Ogre::ColourValue(r, g, b);
00411 }
00412
00413 return true;
00414 }
00415
00416 uint8_t FlatColorPCTransformer::supports(const sensor_msgs::PointCloud2ConstPtr& cloud)
00417 {
00418 return Support_Color;
00419 }
00420
00421 uint8_t FlatColorPCTransformer::score(const sensor_msgs::PointCloud2ConstPtr& cloud)
00422 {
00423 return 0;
00424 }
00425
00426 bool FlatColorPCTransformer::transform( const sensor_msgs::PointCloud2ConstPtr& cloud,
00427 uint32_t mask,
00428 const Ogre::Matrix4& transform,
00429 V_PointCloudPoint& points_out )
00430 {
00431 if( !( mask & Support_Color ))
00432 {
00433 return false;
00434 }
00435
00436 Ogre::ColourValue color = color_property_->getOgreColor();
00437
00438 const uint32_t num_points = cloud->width * cloud->height;
00439 for( uint32_t i = 0; i < num_points; ++i )
00440 {
00441 points_out[i].color = color;
00442 }
00443
00444 return true;
00445 }
00446
00447 void FlatColorPCTransformer::createProperties( Property* parent_property, uint32_t mask, QList<Property*>& out_props )
00448 {
00449 if( mask & Support_Color )
00450 {
00451 color_property_ = new ColorProperty( "Color", Qt::white,
00452 "Color to assign to every point.",
00453 parent_property, SIGNAL( needRetransform() ), this );
00454 out_props.push_back( color_property_ );
00455 }
00456 }
00457
00458 uint8_t AxisColorPCTransformer::supports(const sensor_msgs::PointCloud2ConstPtr& cloud)
00459 {
00460 return Support_Color;
00461 }
00462
00463 uint8_t AxisColorPCTransformer::score(const sensor_msgs::PointCloud2ConstPtr& cloud)
00464 {
00465 return 255;
00466 }
00467
00468 bool AxisColorPCTransformer::transform( const sensor_msgs::PointCloud2ConstPtr& cloud,
00469 uint32_t mask,
00470 const Ogre::Matrix4& transform,
00471 V_PointCloudPoint& points_out )
00472 {
00473 if( !( mask & Support_Color ))
00474 {
00475 return false;
00476 }
00477
00478 int32_t xi = findChannelIndex( cloud, "x" );
00479 int32_t yi = findChannelIndex( cloud, "y" );
00480 int32_t zi = findChannelIndex( cloud, "z" );
00481
00482 const uint32_t xoff = cloud->fields[xi].offset;
00483 const uint32_t yoff = cloud->fields[yi].offset;
00484 const uint32_t zoff = cloud->fields[zi].offset;
00485 const uint32_t point_step = cloud->point_step;
00486 const uint32_t num_points = cloud->width * cloud->height;
00487 uint8_t const* point = &cloud->data.front();
00488
00489
00490 int axis = axis_property_->getOptionInt();
00491 std::vector<float> values;
00492 values.reserve( num_points );
00493 Ogre::Vector3 pos;
00494 if( use_fixed_frame_property_->getBool() )
00495 {
00496 for (uint32_t i = 0; i < num_points; ++i, point += point_step)
00497 {
00498
00499
00500
00501 pos.x = *reinterpret_cast<const float*>(point + xoff);
00502 pos.y = *reinterpret_cast<const float*>(point + yoff);
00503 pos.z = *reinterpret_cast<const float*>(point + zoff);
00504
00505 pos = transform * pos;
00506 values.push_back( pos[ axis ]);
00507 }
00508 }
00509 else
00510 {
00511 const uint32_t offsets[ 3 ] = { xoff, yoff, zoff };
00512 const uint32_t off = offsets[ axis ];
00513 for (uint32_t i = 0; i < num_points; ++i, point += point_step)
00514 {
00515 values.push_back( *reinterpret_cast<const float*>( point + off ));
00516 }
00517 }
00518 float min_value_current = 9999.0f;
00519 float max_value_current = -9999.0f;
00520 if( auto_compute_bounds_property_->getBool() )
00521 {
00522 for( uint32_t i = 0; i < num_points; i++ )
00523 {
00524 float val = values[ i ];
00525 min_value_current = std::min( min_value_current, val );
00526 max_value_current = std::max( max_value_current, val );
00527 }
00528 min_value_property_->setFloat( min_value_current );
00529 max_value_property_->setFloat( max_value_current );
00530 }
00531 else
00532 {
00533 min_value_current = min_value_property_->getFloat();
00534 max_value_current = max_value_property_->getFloat();
00535 }
00536
00537 float range = max_value_current - min_value_current;
00538 if( range == 0 )
00539 {
00540 range = 0.001f;
00541 }
00542 for( uint32_t i = 0; i < num_points; ++i )
00543 {
00544 float value = 1.0 - ( values[ i ] - min_value_current ) / range;
00545 getRainbowColor( value, points_out[i].color );
00546 }
00547
00548 return true;
00549 }
00550
00551 void AxisColorPCTransformer::createProperties( Property* parent_property, uint32_t mask, QList<Property*>& out_props )
00552 {
00553 if( mask & Support_Color )
00554 {
00555 axis_property_ = new EnumProperty( "Axis", "Z",
00556 "The axis to interpolate the color along.",
00557 parent_property, SIGNAL( needRetransform() ), this );
00558 axis_property_->addOption( "X", AXIS_X );
00559 axis_property_->addOption( "Y", AXIS_Y );
00560 axis_property_->addOption( "Z", AXIS_Z );
00561
00562 auto_compute_bounds_property_ = new BoolProperty( "Autocompute Value Bounds", true,
00563 "Whether to automatically compute the value min/max values.",
00564 parent_property, SLOT( updateAutoComputeBounds() ), this );
00565
00566 min_value_property_ = new FloatProperty( "Min Value", -10,
00567 "Minimum value value, used to interpolate the color of a point.",
00568 auto_compute_bounds_property_ );
00569
00570 max_value_property_ = new FloatProperty( "Max Value", 10,
00571 "Maximum value value, used to interpolate the color of a point.",
00572 auto_compute_bounds_property_ );
00573
00574 use_fixed_frame_property_ = new BoolProperty( "Use Fixed Frame", true,
00575 "Whether to color the cloud based on its fixed frame position or its local frame position.",
00576 parent_property, SIGNAL( needRetransform() ), this );
00577
00578 out_props.push_back( axis_property_ );
00579 out_props.push_back( auto_compute_bounds_property_ );
00580 out_props.push_back( use_fixed_frame_property_ );
00581
00582 updateAutoComputeBounds();
00583 }
00584 }
00585
00586 void AxisColorPCTransformer::updateAutoComputeBounds()
00587 {
00588 bool auto_compute = auto_compute_bounds_property_->getBool();
00589 min_value_property_->setHidden( auto_compute );
00590 max_value_property_->setHidden( auto_compute );
00591 if( auto_compute )
00592 {
00593 disconnect( min_value_property_, SIGNAL( changed() ), this, SIGNAL( needRetransform() ));
00594 disconnect( max_value_property_, SIGNAL( changed() ), this, SIGNAL( needRetransform() ));
00595 }
00596 else
00597 {
00598 connect( min_value_property_, SIGNAL( changed() ), this, SIGNAL( needRetransform() ));
00599 connect( max_value_property_, SIGNAL( changed() ), this, SIGNAL( needRetransform() ));
00600 auto_compute_bounds_property_->expand();
00601 }
00602 Q_EMIT needRetransform();
00603 }
00604
00605 }
00606
00607 #include <pluginlib/class_list_macros.h>
00608 PLUGINLIB_EXPORT_CLASS( rviz::AxisColorPCTransformer, rviz::PointCloudTransformer )
00609 PLUGINLIB_EXPORT_CLASS( rviz::FlatColorPCTransformer, rviz::PointCloudTransformer )
00610 PLUGINLIB_EXPORT_CLASS( rviz::IntensityPCTransformer, rviz::PointCloudTransformer )
00611 PLUGINLIB_EXPORT_CLASS( rviz::RGB8PCTransformer, rviz::PointCloudTransformer )
00612 PLUGINLIB_EXPORT_CLASS( rviz::RGBF32PCTransformer, rviz::PointCloudTransformer )
00613 PLUGINLIB_EXPORT_CLASS( rviz::XYZPCTransformer, rviz::PointCloudTransformer )