Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

MONO8 transformer for point cloud plugin #1145

Merged
merged 1 commit into from
Dec 11, 2017
Merged

MONO8 transformer for point cloud plugin #1145

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
5 changes: 5 additions & 0 deletions plugin_description.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -173,6 +173,11 @@
Sets the color of each point based on RGB8 data.
</description>
</class>
<class name="rviz/MONO8" type="rviz::MONO8PCTransformer" base_class_type="rviz::PointCloudTransformer">
<description>
Sets the mono color of each point based on RGB8 data.
</description>
</class>
<class name="rviz/RGBF32" type="rviz::RGBF32PCTransformer" base_class_type="rviz::PointCloudTransformer">
<description>
Sets the color of each point based on RGBF32 data.
Expand Down
52 changes: 52 additions & 0 deletions src/rviz/default_plugin/point_cloud_transformers.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -385,6 +385,57 @@ bool RGB8PCTransformer::transform(const sensor_msgs::PointCloud2ConstPtr& cloud,
return true;
}

bool MONO8PCTransformer::transform(const sensor_msgs::PointCloud2ConstPtr& cloud, uint32_t mask, const Ogre::Matrix4& transform, V_PointCloudPoint& points_out)
{
if (!(mask & Support_Color))
{
return false;
}

const int32_t rgb = findChannelIndex(cloud, "rgb");
const int32_t rgba = findChannelIndex(cloud, "rgba");
const int32_t index = std::max(rgb, rgba);

const uint32_t off = cloud->fields[index].offset;
uint8_t const* rgb_ptr = &cloud->data.front() + off;
const uint32_t point_step = cloud->point_step;

// Create a look-up table for colors
float rgb_lut[256];
for(int i = 0; i < 256; ++i)
{
rgb_lut[i] = float(i)/255.0f;
}
if (rgb != -1) // rgb
{
for (V_PointCloudPoint::iterator iter = points_out.begin(); iter != points_out.end(); ++iter, rgb_ptr += point_step)
{
uint32_t rgb = *reinterpret_cast<const uint32_t*>(rgb_ptr);
float r = rgb_lut[(rgb >> 16) & 0xff];
float g = rgb_lut[(rgb >> 8) & 0xff];
float b = rgb_lut[rgb & 0xff];
float mono = 0.2989 * r + 0.5870 * g + 0.1140 * b;
iter->color.r = iter->color.g = iter->color.b = mono;
iter->color.a = 1.0f;
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

AFAICT the defaults for green and blue channels will be 1.0, so I would think we need to set them to mono here as well to get the greyscale result that you show in the screenshot from the initial post. However, your image seems to show the pointcloud displaying as expected only setting the red. I want to confirm that only setting the red is sufficient before approving this PR (@wkentaro I know it's been a while since this PR was opened but if you had a specific reason for only setting the red channel I am interested).

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Ah, I misread the line and it is setting all three.

}
}
else // rgba
{
for (V_PointCloudPoint::iterator iter = points_out.begin(); iter != points_out.end(); ++iter, rgb_ptr += point_step)
{
uint32_t rgb = *reinterpret_cast<const uint32_t*>(rgb_ptr);
float r = rgb_lut[(rgb >> 16) & 0xff];
float g = rgb_lut[(rgb >> 8) & 0xff];
float b = rgb_lut[rgb & 0xff];
float mono = 0.2989 * r + 0.5870 * g + 0.1140 * b;
iter->color.r = iter->color.g = iter->color.b = mono;
iter->color.a = rgb_lut[rgb >> 24];
}
}

return true;
}

uint8_t RGBF32PCTransformer::supports(const sensor_msgs::PointCloud2ConstPtr& cloud)
{
int32_t ri = findChannelIndex(cloud, "r");
Expand Down Expand Up @@ -627,5 +678,6 @@ PLUGINLIB_EXPORT_CLASS( rviz::AxisColorPCTransformer, rviz::PointCloudTransforme
PLUGINLIB_EXPORT_CLASS( rviz::FlatColorPCTransformer, rviz::PointCloudTransformer )
PLUGINLIB_EXPORT_CLASS( rviz::IntensityPCTransformer, rviz::PointCloudTransformer )
PLUGINLIB_EXPORT_CLASS( rviz::RGB8PCTransformer, rviz::PointCloudTransformer )
PLUGINLIB_EXPORT_CLASS( rviz::MONO8PCTransformer, rviz::PointCloudTransformer )
PLUGINLIB_EXPORT_CLASS( rviz::RGBF32PCTransformer, rviz::PointCloudTransformer )
PLUGINLIB_EXPORT_CLASS( rviz::XYZPCTransformer, rviz::PointCloudTransformer )
9 changes: 9 additions & 0 deletions src/rviz/default_plugin/point_cloud_transformers.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -160,6 +160,15 @@ Q_OBJECT



class MONO8PCTransformer : public RGB8PCTransformer
{
Q_OBJECT
public:
virtual bool transform(const sensor_msgs::PointCloud2ConstPtr& cloud, uint32_t mask, const Ogre::Matrix4& transform, V_PointCloudPoint& points_out);
};



class RGBF32PCTransformer : public PointCloudTransformer
{
Q_OBJECT
Expand Down