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.

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

Quantile filtering new #82

Open
wants to merge 3 commits into
base: ros2
Choose a base branch
from
Open
Show file tree
Hide file tree
Changes from all commits
Commits
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
1 change: 1 addition & 0 deletions README.md
Original file line number Diff line number Diff line change
Expand Up @@ -18,4 +18,5 @@ Parameters
* `range_min` (float) - The minimum distance in meters a projected point should be. Points closer than this are discarded. Defaults to 0.45 meters.
* `range_max` (float) - The maximum distance in meters a projected point should be. Points further than this are discarded. Defaults to 10.0 meters.
* `scan_height` (int) - The row from the depth image to use for the laser projection. Defaults to 1.
* `quantile_value` (float) - The quantile value to use when computing the distance for each column. Defaults to 0.0 (use minimum value in each column).
* `output_frame` (string) - The frame id to publish in the LaserScan message. Defaults to "camera_depth_frame".
1 change: 1 addition & 0 deletions cfg/param.yaml
Original file line number Diff line number Diff line change
Expand Up @@ -4,4 +4,5 @@ depthimage_to_laserscan:
range_min: 0.45
range_max: 10.0
scan_height: 1
quantile_value: 0.0
output_frame: "camera_depth_frame"
60 changes: 45 additions & 15 deletions include/depthimage_to_laserscan/DepthImageToLaserScan.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -35,6 +35,8 @@

#include <cmath>
#include <string>
#include <algorithm>
#include <vector>

#include "depthimage_to_laserscan/DepthImageToLaserScan_export.h"
#include "depthimage_to_laserscan/depth_traits.hpp"
Expand Down Expand Up @@ -69,13 +71,19 @@ class DEPTHIMAGETOLASERSCAN_EXPORT DepthImageToLaserScan final
* radii for each angular increment. The output scan will output the closest radius that is
* still not smaller than range_min. This can be used to vertically compress obstacles into
* a single LaserScan.
* @param quantile_value The quantile value to use for calculating the distance for each column.
* This value determines which distance measurement to use from the multiple
* rows of depth data. For example, a quantile value of 0.1 will use the 10th
Copy link
Member

Choose a reason for hiding this comment

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

I feel the second sentence says the same information as the introduction but in a less direct way. A column is a more conceptually clear concept and is the standard language.

* percentile distance, providing a balance between ignoring outliers and
* maintaining accurate distance measurements. This helps in reducing noise
* and improving the stability of the laser scan data.
* @param frame_id The output frame_id for the LaserScan. This will probably NOT be the same frame_id as the
* depth image. Example: For OpenNI cameras, this should be set to 'camera_depth_frame' while
* the camera uses 'camera_depth_optical_frame'.
*
*/
explicit DepthImageToLaserScan(
float scan_time, float range_min, float range_max, int scan_height,
float scan_time, float range_min, float range_max, int scan_height, float quantile_value,
const std::string & frame_id);

~DepthImageToLaserScan();
Expand Down Expand Up @@ -149,13 +157,16 @@ class DEPTHIMAGETOLASERSCAN_EXPORT DepthImageToLaserScan final
* @param cam_model The image_geometry camera model for this image.
* @param scan_msg The output LaserScan.
* @param scan_height The number of vertical pixels to feed into each angular_measurement.
* @param quantile_value The quantile value to use for calculating the distance for each column.
*
*/
template<typename T>
void convert(
const sensor_msgs::msg::Image::ConstSharedPtr & depth_msg,
const image_geometry::PinholeCameraModel & cam_model,
const sensor_msgs::msg::LaserScan::UniquePtr & scan_msg, const int & scan_height) const
const sensor_msgs::msg::LaserScan::UniquePtr & scan_msg,
const int & scan_height,
const float & quantile_value) const
{
// Use correct principal point from calibration
float center_x = cam_model.cx();
Expand All @@ -164,44 +175,63 @@ class DEPTHIMAGETOLASERSCAN_EXPORT DepthImageToLaserScan final
double unit_scaling = depthimage_to_laserscan::DepthTraits<T>::toMeters(T(1));
float constant_x = unit_scaling / cam_model.fx();

const T * depth_row = reinterpret_cast<const T *>(&depth_msg->data[0]);
int row_step = depth_msg->step / sizeof(T);

int offset = static_cast<int>(cam_model.cy() - static_cast<double>(scan_height) / 2.0);
depth_row += offset * row_step; // Offset to center of image
for (int v = offset; v < offset + scan_height_; v++, depth_row += row_step) {

std::vector<std::vector<double>> column_distances(depth_msg->width);

// Collect distances for each column
for (int v = offset; v < offset + scan_height; v++) {
const T * depth_row = reinterpret_cast<const T *>(&depth_msg->data[0]) + v * row_step;
for (uint32_t u = 0; u < depth_msg->width; u++) { // Loop over each pixel in row
T depth = depth_row[u];

double r = depth; // Assign to pass through NaNs and Infs
// Atan2(x, z), but depth divides out
double th = -std::atan2(static_cast<double>(u - center_x) * constant_x, unit_scaling);
int index = (th - scan_msg->angle_min) / scan_msg->angle_increment;

if (depthimage_to_laserscan::DepthTraits<T>::valid(depth)) { // Not NaN or Inf
// Calculate in XYZ
double x = (u - center_x) * depth * constant_x;
double z = depthimage_to_laserscan::DepthTraits<T>::toMeters(depth);

// Calculate actual distance
r = std::sqrt(std::pow(x, 2.0) + std::pow(z, 2.0));
}

// Determine if this point should be used.
double th = -std::atan2(static_cast<double>(u - center_x) * constant_x, unit_scaling);
int index = (th - scan_msg->angle_min) / scan_msg->angle_increment;

if (use_point(r, scan_msg->ranges[index], scan_msg->range_min, scan_msg->range_max)) {
scan_msg->ranges[index] = r;
column_distances[u].push_back(r);
}
}
}

// Compute quantile for each column and update scan ranges
for (uint32_t u = 0; u < depth_msg->width; u++) {
if (!column_distances[u].empty()) {
std::sort(column_distances[u].begin(), column_distances[u].end());
size_t index = static_cast<size_t>(quantile_value * column_distances[u].size());
double quantile_distance = column_distances[u][index];

double th = -std::atan2(static_cast<double>(u - center_x) * constant_x, unit_scaling);
int scan_index = (th - scan_msg->angle_min) / scan_msg->angle_increment;

// Update scan range
if (use_point(
quantile_distance, scan_msg->ranges[scan_index], scan_msg->range_min,
scan_msg->range_max))
{
scan_msg->ranges[scan_index] = quantile_distance;
}
}
}
}


///< image_geometry helper class for managing sensor_msgs/CameraInfo messages.
image_geometry::PinholeCameraModel cam_model_;

float scan_time_; ///< Stores the time between scans.
float range_min_; ///< Stores the current minimum range to use.
float range_max_; ///< Stores the current maximum range to use.
int scan_height_; ///< Number of pixel rows to use when producing a laserscan from an area.
float quantile_value_; ///< Quantile value to use for calculating the distance for each column.
///< Output frame_id for each laserscan. This is likely NOT the camera's frame_id.
std::string output_frame_id_;
};
Expand Down
8 changes: 4 additions & 4 deletions src/DepthImageToLaserScan.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -49,9 +49,9 @@ namespace depthimage_to_laserscan

DepthImageToLaserScan::DepthImageToLaserScan(
float scan_time, float range_min, float range_max,
int scan_height, const std::string & frame_id)
int scan_height, float quantile_value, const std::string & frame_id)
: scan_time_(scan_time), range_min_(range_min), range_max_(range_max), scan_height_(scan_height),
output_frame_id_(frame_id)
quantile_value_(quantile_value), output_frame_id_(frame_id)
{
}

Expand Down Expand Up @@ -156,9 +156,9 @@ sensor_msgs::msg::LaserScan::UniquePtr DepthImageToLaserScan::convert_msg(
scan_msg->ranges.assign(ranges_size, std::numeric_limits<float>::quiet_NaN());

if (depth_msg->encoding == sensor_msgs::image_encodings::TYPE_16UC1) {
convert<uint16_t>(depth_msg, cam_model_, scan_msg, scan_height_);
convert<uint16_t>(depth_msg, cam_model_, scan_msg, scan_height_, quantile_value_);
} else if (depth_msg->encoding == sensor_msgs::image_encodings::TYPE_32FC1) {
convert<float>(depth_msg, cam_model_, scan_msg, scan_height_);
convert<float>(depth_msg, cam_model_, scan_msg, scan_height_, quantile_value_);
} else {
std::stringstream ss;
ss << "Depth image has unsupported encoding: " << depth_msg->encoding;
Expand Down
4 changes: 3 additions & 1 deletion src/DepthImageToLaserScanROS.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -69,10 +69,12 @@ DepthImageToLaserScanROS::DepthImageToLaserScanROS(const rclcpp::NodeOptions & o

int scan_height = this->declare_parameter("scan_height", 1);

float quantile_value = this->declare_parameter("quantile_value", 0.0);

std::string output_frame = this->declare_parameter("output_frame", "camera_depth_frame");

dtl_ = std::make_unique<depthimage_to_laserscan::DepthImageToLaserScan>(
scan_time, range_min, range_max, scan_height, output_frame);
scan_time, range_min, range_max, scan_height, quantile_value, output_frame);
}

DepthImageToLaserScanROS::~DepthImageToLaserScanROS()
Expand Down
70 changes: 53 additions & 17 deletions test/DepthImageToLaserScanTest.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -36,6 +36,16 @@
#include <limits>
#include <random>

#include "depthimage_to_laserscan/depth_traits.hpp"
#if __has_include("image_geometry/pinhole_camera_model.hpp")
#include "image_geometry/pinhole_camera_model.hpp"
#else
// This header was deprecated as of https://github.com/ros-perception/vision_opencv/pull/448
// (for Iron), and will be completely removed for J-Turtle. However, we still need it in
// Humble, since the .hpp doesn't exist there.
#include "image_geometry/pinhole_camera_model.h"
#endif

// Bring in my package's API, which is what I'm testing
#include <depthimage_to_laserscan/DepthImageToLaserScan.hpp>

Expand All @@ -45,6 +55,7 @@ const float g_scan_time = 1.0 / 30.0;
const float g_range_min = 0.45;
const float g_range_max = 10.0;
const int g_scan_height = 1;
const float g_quantile_value = 0.5;
const char g_output_frame[] = "camera_depth_frame";

// Inputs
Expand All @@ -55,7 +66,7 @@ sensor_msgs::msg::CameraInfo::SharedPtr info_msg_;
TEST(ConvertTest, setupLibrary)
{
depthimage_to_laserscan::DepthImageToLaserScan dtl(g_scan_time, g_range_min,
g_range_max, g_scan_height, g_output_frame);
g_range_max, g_scan_height, g_quantile_value, g_output_frame);

depth_msg_.reset(new sensor_msgs::msg::Image);
depth_msg_->header.stamp.sec = 0;
Expand Down Expand Up @@ -128,7 +139,7 @@ TEST(ConvertTest, setupLibrary)
TEST(ConvertTest, testExceptions)
{
depthimage_to_laserscan::DepthImageToLaserScan dtl(g_scan_time, g_range_min,
g_range_max, g_scan_height, g_output_frame);
g_range_max, g_scan_height, g_quantile_value, g_output_frame);

// Test supported image encodings for exceptions
// Does not segfault as long as scan_height = 1
Expand All @@ -145,7 +156,7 @@ TEST(ConvertTest, testScanHeight)
{
for (int scan_height = 1; scan_height <= 100; scan_height++) {
depthimage_to_laserscan::DepthImageToLaserScan dtl(g_scan_time, g_range_min,
g_range_max, scan_height, g_output_frame);
g_range_max, scan_height, g_quantile_value, g_output_frame);
uint16_t low_value = 500;
uint16_t high_value = 3000;

Expand All @@ -169,16 +180,41 @@ TEST(ConvertTest, testScanHeight)
// Convert
sensor_msgs::msg::LaserScan::SharedPtr scan_msg = dtl.convert_msg(depth_msg_, info_msg_);

// Test for minimum
// 0.9f represents 10 percent margin on range
float high_float_thresh = static_cast<float>(high_value) * 1.0f / 1000.0f * 0.9f;
for (size_t i = 0; i < scan_msg->ranges.size(); i++) {
// If this is a valid point
if (scan_msg->range_min <= scan_msg->ranges[i] &&
scan_msg->ranges[i] <= scan_msg->range_max)
{
// Make sure it's not set to the high_value
ASSERT_LT(scan_msg->ranges[i], high_float_thresh);
image_geometry::PinholeCameraModel cam_model;
cam_model.fromCameraInfo(info_msg_);
// Use correct principal point from calibration
float center_x = cam_model.cx();
float unit_scaling = 0.001f;
float constant_x = unit_scaling / cam_model.fx();

// Calculate quantile values for each column in the depth image
size_t quantile_index = static_cast<size_t>(g_quantile_value * scan_height);

// Collect distances for each column

for (int u = 0; u < data_len; u++) { // Loop over each pixel in row
std::vector<uint16_t> column_values;
for (int v = 0; v < scan_height; v++) {
uint16_t * data_row = reinterpret_cast<uint16_t *>(&depth_msg_->data[0]) + (offset + v) *
row_step;
column_values.push_back(data_row[u]);
}
std::sort(column_values.begin(), column_values.end());
double depth_data_column_quantile = column_values[quantile_index];

double x = (u - center_x) * depth_data_column_quantile * constant_x;
double z = depth_data_column_quantile * 0.001f;
double r = std::sqrt(std::pow(x, 2.0) + std::pow(z, 2.0));

double th = std::atan2(static_cast<double>(u - center_x) * constant_x, unit_scaling);
uint16_t index = (th - scan_msg->angle_min) / scan_msg->angle_increment;

// Now we can compare the quantile of the column in the depthimage with the
// equivalent depth calculated back from the result of convert_msg
if (index < depth_msg_->width && std::isfinite(scan_msg->ranges[index])) {
ASSERT_NEAR(
r, scan_msg->ranges[index],
0.01f * (scan_msg->ranges[index]));
}
}
}
Expand All @@ -197,7 +233,7 @@ TEST(ConvertTest, testRandom)
}

depthimage_to_laserscan::DepthImageToLaserScan dtl(g_scan_time, g_range_min,
g_range_max, g_scan_height, g_output_frame);
g_range_max, g_scan_height, g_quantile_value, g_output_frame);

// Convert
sensor_msgs::msg::LaserScan::SharedPtr scan_msg = dtl.convert_msg(depth_msg_, info_msg_);
Expand Down Expand Up @@ -226,7 +262,7 @@ TEST(ConvertTest, testNaN)
}

depthimage_to_laserscan::DepthImageToLaserScan dtl(g_scan_time, g_range_min,
g_range_max, g_scan_height, g_output_frame);
g_range_max, g_scan_height, g_quantile_value, g_output_frame);

// Convert
sensor_msgs::msg::LaserScan::SharedPtr scan_msg = dtl.convert_msg(float_msg, info_msg_);
Expand Down Expand Up @@ -254,7 +290,7 @@ TEST(ConvertTest, testPositiveInf)
}

depthimage_to_laserscan::DepthImageToLaserScan dtl(g_scan_time, g_range_min,
g_range_max, g_scan_height, g_output_frame);
g_range_max, g_scan_height, g_quantile_value, g_output_frame);

// Convert
sensor_msgs::msg::LaserScan::SharedPtr scan_msg = dtl.convert_msg(float_msg, info_msg_);
Expand Down Expand Up @@ -289,7 +325,7 @@ TEST(ConvertTest, testNegativeInf)
}

depthimage_to_laserscan::DepthImageToLaserScan dtl(g_scan_time, g_range_min,
g_range_max, g_scan_height, g_output_frame);
g_range_max, g_scan_height, g_quantile_value, g_output_frame);

// Convert
sensor_msgs::msg::LaserScan::SharedPtr scan_msg = dtl.convert_msg(float_msg, info_msg_);
Expand Down