Skip to content

Commit

Permalink
Publish 16-bit images and avoid rviz bug on noetic (MR !2092)
Browse files Browse the repository at this point in the history 
A mesa update has triggered a bug in rviz on intel systems[0]. Doesn't
seem like it'll get fixed soon, so this changes the default point
display type to "flat squares." It doesn't look great close-up, but
works reasonably well for visualizing sample data and room-sized scenes.

* Also rearrange the rviz window config a bit to be more useful
* Update published images to have 16-bit dpeth
* Use the usual method ot fit range values into 16 bits
* Rename intensity/ambient topics and variables in the img_node
* Add reflectivity image output to the img_node

[0] ros-visualization/rviz#1508

Approved-by: Chris Bayruns
Approved-by: Pavlo Bashmakov
  • Loading branch information
Dima Garbuzov committed Apr 2, 2022
1 parent 95cf861 commit b82a5d9
Show file tree
Hide file tree
Showing 2 changed files with 129 additions and 114 deletions.
152 changes: 73 additions & 79 deletions ouster_ros/src/img_node.cpp
View file
Original file line number Diff line number Diff line change
@@ -1,8 +1,8 @@
/**
* @file
* @brief Example node to visualize range, ambient and intensity images
* @brief Example node to visualize range, near ir and signal images
*
* Publishes ~/range_image, ~/ambient_image, and ~/intensity_image. Please bear
* Publishes ~/range_image, ~/nearir_image, and ~/signal_image. Please bear
* in mind that there is rounding/clamping to display 8 bit images. For computer
* vision applications, use higher bit depth values in /os_cloud_node/points
*/
Expand All @@ -14,6 +14,7 @@
#include <ros/ros.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/image_encodings.h>

#include <iomanip>
#include <iostream>
Expand All @@ -31,11 +32,21 @@
namespace sensor = ouster::sensor;
namespace viz = ouster::viz;

using pixel_type = uint8_t;
constexpr size_t bit_depth = 8 * sizeof(pixel_type);
using pixel_type = uint16_t;
const size_t pixel_value_max = std::numeric_limits<pixel_type>::max();
constexpr double range_multiplier =
1.0 / 200.0; // assuming 200 m range typical

sensor_msgs::ImagePtr make_image_msg(size_t H, size_t W,
const ros::Time& stamp) {
sensor_msgs::ImagePtr msg{new sensor_msgs::Image{}};
msg->width = W;
msg->height = H;
msg->step = W * sizeof(pixel_type);
msg->encoding = sensor_msgs::image_encodings::MONO16;
msg->data.resize(W * H * sizeof(pixel_type));
msg->header.stamp = stamp;

return msg;
}

int main(int argc, char** argv) {
ros::init(argc, argv, "img_node");
Expand All @@ -57,98 +68,81 @@ int main(int argc, char** argv) {

ros::Publisher range_image_pub =
nh.advertise<sensor_msgs::Image>("range_image", 100);
ros::Publisher ambient_image_pub =
nh.advertise<sensor_msgs::Image>("ambient_image", 100);
ros::Publisher intensity_image_pub =
nh.advertise<sensor_msgs::Image>("intensity_image", 100);
ros::Publisher nearir_image_pub =
nh.advertise<sensor_msgs::Image>("nearir_image", 100);
ros::Publisher signal_image_pub =
nh.advertise<sensor_msgs::Image>("signal_image", 100);
ros::Publisher reflec_image_pub =
nh.advertise<sensor_msgs::Image>("reflec_image", 100);

ouster_ros::Cloud cloud{};

viz::AutoExposure ambient_ae, intensity_ae;
viz::BeamUniformityCorrector ambient_buc;
viz::AutoExposure nearir_ae, signal_ae, reflec_ae;
viz::BeamUniformityCorrector nearir_buc;

std::stringstream encoding_ss;
encoding_ss << "mono" << bit_depth;
std::string encoding = encoding_ss.str();
ouster::img_t<double> nearir_image_eigen(H, W);
ouster::img_t<double> signal_image_eigen(H, W);
ouster::img_t<double> reflec_image_eigen(H, W);

auto cloud_handler = [&](const sensor_msgs::PointCloud2::ConstPtr& m) {
pcl::fromROSMsg(*m, cloud);

sensor_msgs::Image range_image;
sensor_msgs::Image ambient_image;
sensor_msgs::Image intensity_image;

range_image.width = W;
range_image.height = H;
range_image.step = W;
range_image.encoding = encoding;
range_image.data.resize(W * H * bit_depth /
(8 * sizeof(*range_image.data.data())));
range_image.header.stamp = m->header.stamp;

ambient_image.width = W;
ambient_image.height = H;
ambient_image.step = W;
ambient_image.encoding = encoding;
ambient_image.data.resize(W * H * bit_depth /
(8 * sizeof(*ambient_image.data.data())));
ambient_image.header.stamp = m->header.stamp;

intensity_image.width = W;
intensity_image.height = H;
intensity_image.step = W;
intensity_image.encoding = encoding;
intensity_image.data.resize(W * H * bit_depth /
(8 * sizeof(*intensity_image.data.data())));
intensity_image.header.stamp = m->header.stamp;

ouster::img_t<double> ambient_image_eigen(H, W);
ouster::img_t<double> intensity_image_eigen(H, W);

auto range_image = make_image_msg(H, W, m->header.stamp);
auto nearir_image = make_image_msg(H, W, m->header.stamp);
auto signal_image = make_image_msg(H, W, m->header.stamp);
auto reflec_image = make_image_msg(H, W, m->header.stamp);

// views into message data
auto range_image_map = Eigen::Map<ouster::img_t<pixel_type>>(
(pixel_type*)range_image->data.data(), H, W);
auto nearir_image_map = Eigen::Map<ouster::img_t<pixel_type>>(
(pixel_type*)nearir_image->data.data(), H, W);
auto signal_image_map = Eigen::Map<ouster::img_t<pixel_type>>(
(pixel_type*)signal_image->data.data(), H, W);
auto reflec_image_map = Eigen::Map<ouster::img_t<pixel_type>>(
(pixel_type*)reflec_image->data.data(), H, W);

// copy data out of Cloud message, with destaggering
for (size_t u = 0; u < H; u++) {
for (size_t v = 0; v < W; v++) {
const size_t vv = (v + W - px_offset[u]) % W;
const size_t index = u * W + vv;
const auto& pt = cloud[index];

if (pt.range == 0) {
reinterpret_cast<pixel_type*>(
range_image.data.data())[u * W + v] = 0;
} else {
reinterpret_cast<pixel_type*>(
range_image.data.data())[u * W + v] =
pixel_value_max -
std::min(std::round(pt.range * range_multiplier),
static_cast<double>(pixel_value_max));
}
ambient_image_eigen(u, v) = pt.ambient;
intensity_image_eigen(u, v) = pt.intensity;
}
}
const auto& pt = cloud[u * W + vv];

ambient_buc(ambient_image_eigen);
ambient_ae(ambient_image_eigen);
intensity_ae(intensity_image_eigen);
ambient_image_eigen = ambient_image_eigen.sqrt();
intensity_image_eigen = intensity_image_eigen.sqrt();
for (size_t u = 0; u < H; u++) {
for (size_t v = 0; v < W; v++) {
reinterpret_cast<pixel_type*>(
ambient_image.data.data())[u * W + v] =
ambient_image_eigen(u, v) * pixel_value_max;
reinterpret_cast<pixel_type*>(
intensity_image.data.data())[u * W + v] =
intensity_image_eigen(u, v) * pixel_value_max;
// 16 bit img: use 4mm resolution and throw out returns > 260m
auto r = (pt.range + 0b10) >> 2;
range_image_map(u, v) = r > pixel_value_max ? 0 : r;

nearir_image_eigen(u, v) = pt.ambient;
signal_image_eigen(u, v) = pt.intensity;
reflec_image_eigen(u, v) = pt.reflectivity;
}
}

// image processing
nearir_buc(nearir_image_eigen);
nearir_ae(nearir_image_eigen);
signal_ae(signal_image_eigen);
reflec_ae(reflec_image_eigen);
nearir_image_eigen = nearir_image_eigen.sqrt();
signal_image_eigen = signal_image_eigen.sqrt();

// copy data into image messages
nearir_image_map =
(nearir_image_eigen * pixel_value_max).cast<pixel_type>();
signal_image_map =
(signal_image_eigen * pixel_value_max).cast<pixel_type>();
reflec_image_map =
(reflec_image_eigen * pixel_value_max).cast<pixel_type>();

// publish
range_image_pub.publish(range_image);
ambient_image_pub.publish(ambient_image);
intensity_image_pub.publish(intensity_image);
nearir_image_pub.publish(nearir_image);
signal_image_pub.publish(signal_image);
reflec_image_pub.publish(reflec_image);
};

auto pc_sub =
nh.subscribe<sensor_msgs::PointCloud2>("points", 500, cloud_handler);
nh.subscribe<sensor_msgs::PointCloud2>("points", 100, cloud_handler);

ros::spin();
return EXIT_SUCCESS;
Expand Down
Loading

0 comments on commit b82a5d9

Please sign in to comment.