30 #include <OgreColourValue.h> 31 #include <OgreMatrix4.h> 32 #include <OgreVector3.h> 50 value = std::min(value, 1.0
f);
51 value = std::max(value, 0.0
f);
53 float h = value * 5.0f + 1.0f;
56 if ( !(i&1) ) f = 1 - f;
59 if (i <= 1) color[0] = n, color[1] = 0, color[2] = 1;
60 else if (i == 2) color[0] = 0, color[1] = n, color[2] = 1;
61 else if (i == 3) color[0] = 0, color[1] = 1, color[2] = n;
62 else if (i == 4) color[0] = n, color[1] = 1, color[2] = 0;
63 else if (i >= 5) color[0] = 1, color[1] = n, color[2] = 0;
105 const uint32_t offset = cloud->fields[index].offset;
106 const uint8_t type = cloud->fields[index].datatype;
107 const uint32_t point_step = cloud->point_step;
108 const uint32_t num_points = cloud->width * cloud->height;
110 float min_intensity = 999999.0f;
111 float max_intensity = -999999.0f;
114 for( uint32_t i = 0; i < num_points; ++i )
116 float val = valueFromCloud<float>(cloud, offset, type, point_step, i);
117 min_intensity = std::min(val, min_intensity);
118 max_intensity = std::max(val, max_intensity);
121 min_intensity = std::max(-999999.0
f, min_intensity);
122 max_intensity = std::min(999999.0
f, max_intensity);
131 float diff_intensity = max_intensity - min_intensity;
132 if( diff_intensity == 0 )
138 diff_intensity = 1e20;
145 for (uint32_t i = 0; i < num_points; ++i)
147 float val = valueFromCloud<float>(cloud, offset, type, point_step, i);
148 float value = 1.0 - (val - min_intensity)/diff_intensity;
157 for (uint32_t i = 0; i < num_points; ++i)
159 float val = valueFromCloud<float>(cloud, offset, type, point_step, i);
160 float normalized_intensity = ( val - min_intensity ) / diff_intensity;
161 normalized_intensity = std::min(1.0
f, std::max(0.0
f, normalized_intensity));
162 points_out[i].color.r = max_color.r * normalized_intensity + min_color.r * (1.0f - normalized_intensity);
163 points_out[i].color.g = max_color.g * normalized_intensity + min_color.g * (1.0f - normalized_intensity);
164 points_out[i].color.b = max_color.b * normalized_intensity + min_color.b * (1.0f - normalized_intensity);
176 "Select the channel to use to compute the intensity",
180 "Whether to use a rainbow of colors or interpolate between two",
183 "Whether to invert rainbow colors",
187 "Color to assign the points with the minimum intensity. " 188 "Actual color is interpolated between this and Max Color.",
192 "Color to assign the points with the maximum intensity. " 193 "Actual color is interpolated between this and Min Color.",
197 "Whether to automatically compute the intensity min/max values.",
201 "Minimum possible intensity value, used to interpolate from Min Color to Max Color for a point.",
205 "Maximum possible intensity value, used to interpolate from Min Color to Max Color for a point.",
225 for(
size_t i = 0; i < cloud->fields.size(); ++i )
227 channels.push_back(cloud->fields[i].name );
229 std::sort(channels.begin(), channels.end());
234 for( V_string::const_iterator it = channels.begin(); it != channels.end(); ++it )
236 const std::string& channel = *it;
237 if( channel.empty() )
280 if (xi == -1 || yi == -1 || zi == -1)
285 if (cloud->fields[xi].datatype == sensor_msgs::PointField::FLOAT32)
304 const uint32_t xoff = cloud->fields[xi].offset;
305 const uint32_t yoff = cloud->fields[yi].offset;
306 const uint32_t zoff = cloud->fields[zi].offset;
307 const uint32_t point_step = cloud->point_step;
308 const uint32_t num_points = cloud->width * cloud->height;
309 uint8_t
const* point_x = &cloud->data.front() + xoff;
310 uint8_t
const* point_y = &cloud->data.front() + yoff;
311 uint8_t
const* point_z = &cloud->data.front() + zoff;
312 for (V_PointCloudPoint::iterator iter = points_out.begin(); iter != points_out.end(); ++iter, point_x += point_step,
313 point_y += point_step, point_z += point_step)
315 iter->position.x = *
reinterpret_cast<const float*
>(point_x);
316 iter->position.y = *
reinterpret_cast<const float*
>(point_y);
317 iter->position.z = *
reinterpret_cast<const float*
>(point_z);
331 if (cloud->fields[index].datatype == sensor_msgs::PointField::INT32 ||
332 cloud->fields[index].datatype == sensor_msgs::PointField::UINT32 ||
333 cloud->fields[index].datatype == sensor_msgs::PointField::FLOAT32)
350 const int32_t index = std::max(rgb, rgba);
352 const uint32_t off = cloud->fields[index].offset;
353 uint8_t
const* rgb_ptr = &cloud->data.front() + off;
354 const uint32_t point_step = cloud->point_step;
358 for(
int i = 0; i < 256; ++i)
360 rgb_lut[i] = float(i)/255.0f;
364 for (V_PointCloudPoint::iterator iter = points_out.begin(); iter != points_out.end(); ++iter, rgb_ptr += point_step)
366 uint32_t rgb = *
reinterpret_cast<const uint32_t*
>(rgb_ptr);
367 iter->color.r = rgb_lut[(rgb >> 16) & 0xff];
368 iter->color.g = rgb_lut[(rgb >> 8) & 0xff];
369 iter->color.b = rgb_lut[rgb & 0xff];
370 iter->color.a = 1.0f;
375 for (V_PointCloudPoint::iterator iter = points_out.begin(); iter != points_out.end(); ++iter, rgb_ptr += point_step)
377 uint32_t rgb = *
reinterpret_cast<const uint32_t*
>(rgb_ptr);
378 iter->color.r = rgb_lut[(rgb >> 16) & 0xff];
379 iter->color.g = rgb_lut[(rgb >> 8) & 0xff];
380 iter->color.b = rgb_lut[rgb & 0xff];
381 iter->color.a = rgb_lut[rgb >> 24];
397 const int32_t index = std::max(rgb, rgba);
399 const uint32_t off = cloud->fields[index].offset;
400 uint8_t
const* rgb_ptr = &cloud->data.front() + off;
401 const uint32_t point_step = cloud->point_step;
405 for(
int i = 0; i < 256; ++i)
407 rgb_lut[i] = float(i)/255.0f;
411 for (V_PointCloudPoint::iterator iter = points_out.begin(); iter != points_out.end(); ++iter, rgb_ptr += point_step)
413 uint32_t rgb = *
reinterpret_cast<const uint32_t*
>(rgb_ptr);
414 float r = rgb_lut[(rgb >> 16) & 0xff];
415 float g = rgb_lut[(rgb >> 8) & 0xff];
416 float b = rgb_lut[rgb & 0xff];
417 float mono = 0.2989 * r + 0.5870 * g + 0.1140 * b;
418 iter->color.r = iter->color.g = iter->color.b = mono;
419 iter->color.a = 1.0f;
424 for (V_PointCloudPoint::iterator iter = points_out.begin(); iter != points_out.end(); ++iter, rgb_ptr += point_step)
426 uint32_t rgb = *
reinterpret_cast<const uint32_t*
>(rgb_ptr);
427 float r = rgb_lut[(rgb >> 16) & 0xff];
428 float g = rgb_lut[(rgb >> 8) & 0xff];
429 float b = rgb_lut[rgb & 0xff];
430 float mono = 0.2989 * r + 0.5870 * g + 0.1140 * b;
431 iter->color.r = iter->color.g = iter->color.b = mono;
432 iter->color.a = rgb_lut[rgb >> 24];
444 if (ri == -1 || gi == -1 || bi == -1)
449 if (cloud->fields[ri].datatype == sensor_msgs::PointField::FLOAT32)
468 const uint32_t roff = cloud->fields[ri].offset;
469 const uint32_t goff = cloud->fields[gi].offset;
470 const uint32_t boff = cloud->fields[bi].offset;
471 const uint32_t point_step = cloud->point_step;
472 const uint32_t num_points = cloud->width * cloud->height;
473 uint8_t
const* point = &cloud->data.front();
474 for (uint32_t i = 0; i < num_points; ++i, point += point_step)
476 float r = *
reinterpret_cast<const float*
>(point + roff);
477 float g = *
reinterpret_cast<const float*
>(point + goff);
478 float b = *
reinterpret_cast<const float*
>(point + boff);
479 points_out[i].color = Ogre::ColourValue(r, g, b);
505 Ogre::ColourValue color = color_property_->getOgreColor();
507 const uint32_t num_points = cloud->width * cloud->height;
508 for( uint32_t i = 0; i < num_points; ++i )
510 points_out[i].color = color;
521 "Color to assign to every point.",
523 out_props.push_back( color_property_ );
551 const uint32_t xoff = cloud->fields[xi].offset;
552 const uint32_t yoff = cloud->fields[yi].offset;
553 const uint32_t zoff = cloud->fields[zi].offset;
554 const uint32_t point_step = cloud->point_step;
555 const uint32_t num_points = cloud->width * cloud->height;
556 uint8_t
const* point = &cloud->data.front();
559 int axis = axis_property_->getOptionInt();
560 std::vector<float>
values;
561 values.reserve( num_points );
563 if( use_fixed_frame_property_->getBool() )
565 for (uint32_t i = 0; i < num_points; ++i, point += point_step)
570 pos.x = *
reinterpret_cast<const float*
>(point + xoff);
571 pos.y = *
reinterpret_cast<const float*
>(point + yoff);
572 pos.z = *
reinterpret_cast<const float*
>(point + zoff);
574 pos = transform * pos;
575 values.push_back( pos[ axis ]);
580 const uint32_t offsets[ 3 ] = { xoff, yoff, zoff };
581 const uint32_t off = offsets[ axis ];
582 for (uint32_t i = 0; i < num_points; ++i, point += point_step)
584 values.push_back( *reinterpret_cast<const float*>( point + off ));
587 float min_value_current = 9999.0f;
588 float max_value_current = -9999.0f;
589 if( auto_compute_bounds_property_->getBool() )
591 for( uint32_t i = 0; i < num_points; i++ )
593 float val = values[ i ];
594 min_value_current = std::min( min_value_current, val );
595 max_value_current = std::max( max_value_current, val );
597 min_value_property_->setFloat( min_value_current );
598 max_value_property_->setFloat( max_value_current );
602 min_value_current = min_value_property_->getFloat();
603 max_value_current = max_value_property_->getFloat();
606 float range = max_value_current - min_value_current;
611 for( uint32_t i = 0; i < num_points; ++i )
613 float value = 1.0 - ( values[ i ] - min_value_current ) / range;
625 "The axis to interpolate the color along.",
627 axis_property_->addOption(
"X", AXIS_X );
628 axis_property_->addOption(
"Y", AXIS_Y );
629 axis_property_->addOption(
"Z", AXIS_Z );
631 auto_compute_bounds_property_ =
new BoolProperty(
"Autocompute Value Bounds",
true,
632 "Whether to automatically compute the value min/max values.",
633 parent_property, SLOT( updateAutoComputeBounds() ),
this );
636 "Minimum value value, used to interpolate the color of a point.",
637 auto_compute_bounds_property_ );
640 "Maximum value value, used to interpolate the color of a point.",
641 auto_compute_bounds_property_ );
643 use_fixed_frame_property_ =
new BoolProperty(
"Use Fixed Frame",
true,
644 "Whether to color the cloud based on its fixed frame position or its local frame position.",
647 out_props.push_back( axis_property_ );
648 out_props.push_back( auto_compute_bounds_property_ );
649 out_props.push_back( use_fixed_frame_property_ );
651 updateAutoComputeBounds();
657 bool auto_compute = auto_compute_bounds_property_->getBool();
658 min_value_property_->setHidden( auto_compute );
659 max_value_property_->setHidden( auto_compute );
662 disconnect( min_value_property_, SIGNAL( changed() ),
this, SIGNAL(
needRetransform() ));
663 disconnect( max_value_property_, SIGNAL( changed() ),
this, SIGNAL(
needRetransform() ));
667 connect( min_value_property_, SIGNAL( changed() ),
this, SIGNAL(
needRetransform() ));
668 connect( max_value_property_, SIGNAL( changed() ),
this, SIGNAL(
needRetransform() ));
669 auto_compute_bounds_property_->expand();
Ogre::ColourValue getOgreColor() const
std::vector< double > values
virtual void clearOptions()
A single element of a property tree, with a name, value, description, and possibly children...
virtual float getFloat() const
void addOptionStd(const std::string &option)
Property specialized to enforce floating point max/min.
std::string getStdString()
virtual bool getBool() const
bool setFloat(float new_value)
Float-typed "SLOT" version of setValue().
std::vector< PointCloud::Point > V_PointCloudPoint
std::vector< std::string > V_string
virtual void setHidden(bool hidden)
Hide or show this property in any PropertyTreeWidget viewing its parent.
int32_t findChannelIndex(const sensor_msgs::PointCloud2ConstPtr &cloud, const std::string &channel)
Property specialized to provide getter for booleans.
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
static void getRainbowColor(float value, Ogre::ColourValue &color)