30 #include <OgreColourValue.h> 31 #include <OgreMatrix4.h> 32 #include <OgreVector3.h> 49 value = std::min(value, 1.0
f);
50 value = std::max(value, 0.0
f);
52 float h = value * 5.0f + 1.0f;
60 color[0] = n, color[1] = 0, color[2] = 1;
62 color[0] = 0, color[1] = n, color[2] = 1;
64 color[0] = 0, color[1] = 1, color[2] = n;
66 color[0] = n, color[1] = 1, color[2] = 0;
68 color[0] = 1, color[1] = n, color[2] = 0;
84 const Ogre::Matrix4& ,
110 const uint32_t offset = cloud->fields[index].offset;
111 const uint8_t type = cloud->fields[index].datatype;
112 const uint32_t point_step = cloud->point_step;
113 const uint32_t num_points = cloud->width * cloud->height;
115 float min_intensity = 999999.0f;
116 float max_intensity = -999999.0f;
119 for (uint32_t i = 0; i < num_points; ++i)
121 float val = valueFromCloud<float>(cloud, offset, type, point_step, i);
122 min_intensity = std::min(val, min_intensity);
123 max_intensity = std::max(val, max_intensity);
126 min_intensity = std::max(-999999.0
f, min_intensity);
127 max_intensity = std::min(999999.0
f, max_intensity);
136 float diff_intensity = max_intensity - min_intensity;
137 if (diff_intensity == 0)
143 diff_intensity = 1e20;
150 for (uint32_t i = 0; i < num_points; ++i)
152 float val = valueFromCloud<float>(cloud, offset, type, point_step, i);
153 float value = 1.0 - (val - min_intensity) / diff_intensity;
163 for (uint32_t i = 0; i < num_points; ++i)
165 float val = valueFromCloud<float>(cloud, offset, type, point_step, i);
166 float normalized_intensity = (val - min_intensity) / diff_intensity;
167 normalized_intensity = std::min(1.0
f, std::max(0.0
f, normalized_intensity));
168 points_out[i].color.r =
169 max_color.r * normalized_intensity + min_color.r * (1.0f - normalized_intensity);
170 points_out[i].color.g =
171 max_color.g * normalized_intensity + min_color.g * (1.0f - normalized_intensity);
172 points_out[i].color.b =
173 max_color.b * normalized_intensity + min_color.b * (1.0f - normalized_intensity);
182 QList<Property*>& out_props)
188 "Select the channel to use to compute the intensity", parent_property,
193 "Whether to use a rainbow of colors or interpolate between two",
196 new BoolProperty(
"Invert Rainbow",
false,
"Whether to invert rainbow colors", parent_property,
200 "Color to assign the points with the minimum intensity. " 201 "Actual color is interpolated between this and Max Color.",
205 "Color to assign the points with the maximum intensity. " 206 "Actual color is interpolated between this and Min Color.",
211 "Whether to automatically compute the intensity min/max values.",
216 "Minimum possible intensity value, used to interpolate from Min Color to Max Color for a point.",
220 "Max Intensity", 4096,
221 "Maximum possible intensity value, used to interpolate from Min Color to Max Color for a point.",
241 for (
size_t i = 0; i < cloud->fields.size(); ++i)
243 channels.push_back(cloud->fields[i].name);
245 std::sort(channels.begin(), channels.end());
250 for (V_string::const_iterator it = channels.begin(); it != channels.end(); ++it)
252 const std::string& channel = *it;
296 if (xi == -1 || yi == -1 || zi == -1)
301 if (cloud->fields[xi].datatype == sensor_msgs::PointField::FLOAT32)
311 const Ogre::Matrix4& ,
323 const uint32_t xoff = cloud->fields[xi].offset;
324 const uint32_t yoff = cloud->fields[yi].offset;
325 const uint32_t zoff = cloud->fields[zi].offset;
326 const uint32_t point_step = cloud->point_step;
327 uint8_t
const* point_x = &cloud->data.front() + xoff;
328 uint8_t
const* point_y = &cloud->data.front() + yoff;
329 uint8_t
const* point_z = &cloud->data.front() + zoff;
330 for (V_PointCloudPoint::iterator iter = points_out.begin(); iter != points_out.end();
331 ++iter, point_x += point_step, point_y += point_step, point_z += point_step)
333 iter->position.x = *
reinterpret_cast<const float*
>(point_x);
334 iter->position.y = *
reinterpret_cast<const float*
>(point_y);
335 iter->position.z = *
reinterpret_cast<const float*
>(point_z);
349 if (cloud->fields[index].datatype == sensor_msgs::PointField::INT32 ||
350 cloud->fields[index].datatype == sensor_msgs::PointField::UINT32 ||
351 cloud->fields[index].datatype == sensor_msgs::PointField::FLOAT32)
361 const Ogre::Matrix4& ,
371 const int32_t index = std::max(rgb, rgba);
373 const uint32_t off = cloud->fields[index].offset;
374 uint8_t
const* rgb_ptr = &cloud->data.front() + off;
375 const uint32_t point_step = cloud->point_step;
379 for (
int i = 0; i < 256; ++i)
381 rgb_lut[i] = float(i) / 255.0f;
385 for (V_PointCloudPoint::iterator iter = points_out.begin(); iter != points_out.end();
386 ++iter, rgb_ptr += point_step)
388 uint32_t rgb = *
reinterpret_cast<const uint32_t*
>(rgb_ptr);
389 iter->color.r = rgb_lut[(rgb >> 16) & 0xff];
390 iter->color.g = rgb_lut[(rgb >> 8) & 0xff];
391 iter->color.b = rgb_lut[rgb & 0xff];
392 iter->color.a = 1.0f;
397 for (V_PointCloudPoint::iterator iter = points_out.begin(); iter != points_out.end();
398 ++iter, rgb_ptr += point_step)
400 uint32_t rgb = *
reinterpret_cast<const uint32_t*
>(rgb_ptr);
401 iter->color.r = rgb_lut[(rgb >> 16) & 0xff];
402 iter->color.g = rgb_lut[(rgb >> 8) & 0xff];
403 iter->color.b = rgb_lut[rgb & 0xff];
404 iter->color.a = rgb_lut[rgb >> 24];
413 const Ogre::Matrix4& ,
423 const int32_t index = std::max(rgb, rgba);
425 const uint32_t off = cloud->fields[index].offset;
426 uint8_t
const* rgb_ptr = &cloud->data.front() + off;
427 const uint32_t point_step = cloud->point_step;
431 for (
int i = 0; i < 256; ++i)
433 rgb_lut[i] = float(i) / 255.0f;
437 for (V_PointCloudPoint::iterator iter = points_out.begin(); iter != points_out.end();
438 ++iter, rgb_ptr += point_step)
440 uint32_t rgb = *
reinterpret_cast<const uint32_t*
>(rgb_ptr);
441 float r = rgb_lut[(rgb >> 16) & 0xff];
442 float g = rgb_lut[(rgb >> 8) & 0xff];
443 float b = rgb_lut[rgb & 0xff];
444 float mono = 0.2989 * r + 0.5870 * g + 0.1140 * b;
445 iter->color.r = iter->color.g = iter->color.b = mono;
446 iter->color.a = 1.0f;
451 for (V_PointCloudPoint::iterator iter = points_out.begin(); iter != points_out.end();
452 ++iter, rgb_ptr += point_step)
454 uint32_t rgb = *
reinterpret_cast<const uint32_t*
>(rgb_ptr);
455 float r = rgb_lut[(rgb >> 16) & 0xff];
456 float g = rgb_lut[(rgb >> 8) & 0xff];
457 float b = rgb_lut[rgb & 0xff];
458 float mono = 0.2989 * r + 0.5870 * g + 0.1140 * b;
459 iter->color.r = iter->color.g = iter->color.b = mono;
460 iter->color.a = rgb_lut[rgb >> 24];
472 if (ri == -1 || gi == -1 || bi == -1)
477 if (cloud->fields[ri].datatype == sensor_msgs::PointField::FLOAT32)
487 const Ogre::Matrix4& ,
499 const uint32_t roff = cloud->fields[ri].offset;
500 const uint32_t goff = cloud->fields[gi].offset;
501 const uint32_t boff = cloud->fields[bi].offset;
502 const uint32_t point_step = cloud->point_step;
503 const uint32_t num_points = cloud->width * cloud->height;
504 uint8_t
const* point = &cloud->data.front();
505 for (uint32_t i = 0; i < num_points; ++i, point += point_step)
507 float r = *
reinterpret_cast<const float*
>(point + roff);
508 float g = *
reinterpret_cast<const float*
>(point + goff);
509 float b = *
reinterpret_cast<const float*
>(point + boff);
510 points_out[i].color = Ogre::ColourValue(r, g, b);
528 const Ogre::Matrix4& ,
536 Ogre::ColourValue color = color_property_->getOgreColor();
538 const uint32_t num_points = cloud->width * cloud->height;
539 for (uint32_t i = 0; i < num_points; ++i)
541 points_out[i].color = color;
549 QList<Property*>& out_props)
553 color_property_ =
new ColorProperty(
"Color", Qt::white,
"Color to assign to every point.",
555 out_props.push_back(color_property_);
583 const uint32_t xoff = cloud->fields[xi].offset;
584 const uint32_t yoff = cloud->fields[yi].offset;
585 const uint32_t zoff = cloud->fields[zi].offset;
586 const uint32_t point_step = cloud->point_step;
587 const uint32_t num_points = cloud->width * cloud->height;
588 uint8_t
const* point = &cloud->data.front();
591 int axis = axis_property_->getOptionInt();
592 std::vector<float>
values;
593 values.reserve(num_points);
595 if (use_fixed_frame_property_->getBool())
597 for (uint32_t i = 0; i < num_points; ++i, point += point_step)
602 pos.x = *
reinterpret_cast<const float*
>(point + xoff);
603 pos.y = *
reinterpret_cast<const float*
>(point + yoff);
604 pos.z = *
reinterpret_cast<const float*
>(point + zoff);
606 pos = transform * pos;
607 values.push_back(pos[axis]);
612 const uint32_t offsets[3] = {xoff, yoff, zoff};
613 const uint32_t off = offsets[axis];
614 for (uint32_t i = 0; i < num_points; ++i, point += point_step)
616 values.push_back(*reinterpret_cast<const float*>(point + off));
619 float min_value_current = 9999.0f;
620 float max_value_current = -9999.0f;
621 if (auto_compute_bounds_property_->getBool())
623 for (uint32_t i = 0; i < num_points; i++)
625 float val = values[i];
626 min_value_current = std::min(min_value_current, val);
627 max_value_current = std::max(max_value_current, val);
629 min_value_property_->setFloat(min_value_current);
630 max_value_property_->setFloat(max_value_current);
634 min_value_current = min_value_property_->getFloat();
635 max_value_current = max_value_property_->getFloat();
638 float range = max_value_current - min_value_current;
643 for (uint32_t i = 0; i < num_points; ++i)
645 float value = 1.0 - (values[i] - min_value_current) / range;
654 QList<Property*>& out_props)
658 axis_property_ =
new EnumProperty(
"Axis",
"Z",
"The axis to interpolate the color along.",
660 axis_property_->addOption(
"X", AXIS_X);
661 axis_property_->addOption(
"Y", AXIS_Y);
662 axis_property_->addOption(
"Z", AXIS_Z);
664 auto_compute_bounds_property_ =
666 "Whether to automatically compute the value min/max values.", parent_property,
667 SLOT(updateAutoComputeBounds()),
this);
669 min_value_property_ =
671 "Minimum value value, used to interpolate the color of a point.",
672 auto_compute_bounds_property_);
674 max_value_property_ =
676 "Maximum value value, used to interpolate the color of a point.",
677 auto_compute_bounds_property_);
680 "Use Fixed Frame",
true,
681 "Whether to color the cloud based on its fixed frame position or its local frame position.",
684 out_props.push_back(axis_property_);
685 out_props.push_back(auto_compute_bounds_property_);
686 out_props.push_back(use_fixed_frame_property_);
688 updateAutoComputeBounds();
694 bool auto_compute = auto_compute_bounds_property_->getBool();
695 min_value_property_->setHidden(auto_compute);
696 max_value_property_->setHidden(auto_compute);
699 disconnect(min_value_property_, SIGNAL(changed()),
this, SIGNAL(
needRetransform()));
700 disconnect(max_value_property_, SIGNAL(changed()),
this, SIGNAL(
needRetransform()));
704 connect(min_value_property_, SIGNAL(changed()),
this, SIGNAL(
needRetransform()));
705 connect(max_value_property_, SIGNAL(changed()),
this, SIGNAL(
needRetransform()));
706 auto_compute_bounds_property_->expand();
std::vector< double > values
virtual void clearOptions()
A single element of a property tree, with a name, value, description, and possibly children...
Ogre::ColourValue getOgreColor() const
void addOptionStd(const std::string &option)
Property specialized to enforce floating point max/min.
std::string getStdString()
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.
virtual float getFloat() const
virtual bool getBool() const
virtual void setReadOnly(bool read_only)
Prevent or allow users to edit this property from a PropertyTreeWidget.
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
static void getRainbowColor(float value, Ogre::ColourValue &color)