Skip to content

Commit 92d84b2

Browse files
JakubWarkockiJakubWarkocki
committed
relative filters
1 parent 5c57eed commit 92d84b2

File tree

3 files changed

+87
-23
lines changed

3 files changed

+87
-23
lines changed

‎spatio_temporal_voxel_layer/include/spatio_temporal_voxel_layer/measurement_buffer.hpp‎

Lines changed: 5 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -73,7 +73,9 @@ enum class Filters
7373
{
7474
NONE = 0,
7575
VOXEL = 1,
76-
PASSTHROUGH = 2
76+
PASSTHROUGH = 2,
77+
VOXEL_RELATIVE = 3,
78+
PASSTHROUGH_RELATIVE = 4
7779
};
7880

7981
// conveniences for line lengths
@@ -95,6 +97,7 @@ class MeasurementBuffer
9597
tf2_ros::Buffer & tf,
9698
const std::string & global_frame,
9799
const std::string & sensor_frame,
100+
const std::string & z_reference_frame,
98101
const double & tf_tolerance,
99102
const double & min_d,
100103
const double & max_d,
@@ -155,7 +158,7 @@ class MeasurementBuffer
155158
const rclcpp::Duration _observation_keep_time, _expected_update_rate;
156159
rclcpp::Time _last_updated;
157160
boost::recursive_mutex _lock;
158-
std::string _global_frame, _sensor_frame, _source_name, _topic_name;
161+
std::string _global_frame, _sensor_frame, _source_name, _topic_name, _z_reference_frame;
159162
std::list<observation::MeasurementReading> _observation_list;
160163
double _min_obstacle_height, _max_obstacle_height, _obstacle_range, _tf_tolerance;
161164
double _min_z, _max_z, _vertical_fov, _vertical_fov_padding, _horizontal_fov;

‎spatio_temporal_voxel_layer/src/measurement_buffer.cpp‎

Lines changed: 62 additions & 17 deletions
Original file line numberDiff line numberDiff line change
@@ -54,7 +54,7 @@ MeasurementBuffer::MeasurementBuffer(
5454
const double & observation_keep_time, const double & expected_update_rate,
5555
const double & min_obstacle_height, const double & max_obstacle_height,
5656
const double & obstacle_range, tf2_ros::Buffer & tf, const std::string & global_frame,
57-
const std::string & sensor_frame, const double & tf_tolerance,
57+
const std::string & sensor_frame, const std::string & z_reference_frame, const double & tf_tolerance,
5858
const double & min_d, const double & max_d, const double & vFOV,
5959
const double & vFOVPadding, const double & hFOV,
6060
const double & decay_acceleration, const bool & marking,
@@ -67,7 +67,7 @@ MeasurementBuffer::MeasurementBuffer(
6767
_expected_update_rate(rclcpp::Duration::from_seconds(expected_update_rate)),
6868
_last_updated(clock->now()),
6969
_global_frame(global_frame), _sensor_frame(sensor_frame), _source_name(source_name),
70-
_topic_name(topic_name), _min_obstacle_height(min_obstacle_height),
70+
_topic_name(topic_name), _z_reference_frame(z_reference_frame), _min_obstacle_height(min_obstacle_height),
7171
_max_obstacle_height(max_obstacle_height), _obstacle_range(obstacle_range),
7272
_tf_tolerance(tf_tolerance), _min_z(min_d), _max_z(max_d),
7373
_vertical_fov(vFOV), _vertical_fov_padding(vFOVPadding),
@@ -137,44 +137,89 @@ void MeasurementBuffer::BufferROSCloud(
137137
return;
138138
}
139139

140-
// transform the cloud in the global frame
141-
point_cloud_ptr cld_global(new sensor_msgs::msg::PointCloud2());
142-
geometry_msgs::msg::TransformStamped tf_stamped =
140+
// transform the cloud in the global frame, directly or indirectly to apply relative z filter
141+
point_cloud_ptr cld_transformed(new sensor_msgs::msg::PointCloud2());
142+
143+
geometry_msgs::msg::TransformStamped tf_direct_global =
143144
_buffer.lookupTransform(
144145
_global_frame, cloud.header.frame_id,
145146
tf2_ros::fromMsg(cloud.header.stamp));
146-
tf2::doTransform(cloud, *cld_global, tf_stamped);
147+
148+
geometry_msgs::msg::TransformStamped tf_z_reference =
149+
_buffer.lookupTransform(
150+
_z_reference_frame, cloud.header.frame_id,
151+
tf2_ros::fromMsg(cloud.header.stamp));
152+
153+
geometry_msgs::msg::TransformStamped tf_global_from_z_reference =
154+
_buffer.lookupTransform(
155+
_global_frame, _z_reference_frame,
156+
tf2_ros::fromMsg(cloud.header.stamp));
147157

148158
pcl::PCLPointCloud2::Ptr cloud_pcl(new pcl::PCLPointCloud2());
149159
pcl::PCLPointCloud2::Ptr cloud_filtered(new pcl::PCLPointCloud2());
150-
160+
pcl::PassThrough<pcl::PCLPointCloud2> pass_through_filter;
161+
pcl::VoxelGrid<pcl::PCLPointCloud2> sor;
162+
float v_s = static_cast<float>(_voxel_size);
151163
// remove points that are below or above our height restrictions, and
152164
// in the same time, remove NaNs and if user wants to use it, combine with a
153-
if (_filter == Filters::VOXEL) {
154-
pcl_conversions::toPCL(*cld_global, *cloud_pcl);
155-
pcl::VoxelGrid<pcl::PCLPointCloud2> sor;
165+
166+
switch (_filter)
167+
{
168+
case Filters::PASSTHROUGH :
169+
tf2::doTransform(cloud, *cld_transformed, tf_direct_global);
170+
pcl_conversions::toPCL(*cld_transformed, *cloud_pcl);
171+
pass_through_filter.setInputCloud(cloud_pcl);
172+
pass_through_filter.setKeepOrganized(false);
173+
pass_through_filter.setFilterFieldName("z");
174+
pass_through_filter.setFilterLimits(
175+
_min_obstacle_height, _max_obstacle_height);
176+
pass_through_filter.filter(*cloud_filtered);
177+
pcl_conversions::fromPCL(*cloud_filtered, *cld_transformed);
178+
break;
179+
case Filters::VOXEL :
180+
tf2::doTransform(cloud, *cld_transformed, tf_direct_global);
181+
pcl_conversions::toPCL(*cld_transformed, *cloud_pcl);
156182
sor.setInputCloud(cloud_pcl);
157183
sor.setFilterFieldName("z");
158184
sor.setFilterLimits(_min_obstacle_height, _max_obstacle_height);
159185
sor.setDownsampleAllData(false);
160-
float v_s = static_cast<float>(_voxel_size);
161186
sor.setLeafSize(v_s, v_s, v_s);
162187
sor.setMinimumPointsNumberPerVoxel(static_cast<unsigned int>(_voxel_min_points));
163188
sor.filter(*cloud_filtered);
164-
pcl_conversions::fromPCL(*cloud_filtered, *cld_global);
165-
} else if (_filter == Filters::PASSTHROUGH) {
166-
pcl_conversions::toPCL(*cld_global, *cloud_pcl);
167-
pcl::PassThrough<pcl::PCLPointCloud2> pass_through_filter;
189+
pcl_conversions::fromPCL(*cloud_filtered, *cld_transformed);
190+
break;
191+
case Filters::PASSTHROUGH_RELATIVE :
192+
tf2::doTransform(cloud, *cld_transformed, tf_z_reference);
193+
pcl_conversions::toPCL(*cld_transformed, *cloud_pcl);
168194
pass_through_filter.setInputCloud(cloud_pcl);
169195
pass_through_filter.setKeepOrganized(false);
170196
pass_through_filter.setFilterFieldName("z");
171197
pass_through_filter.setFilterLimits(
172198
_min_obstacle_height, _max_obstacle_height);
173199
pass_through_filter.filter(*cloud_filtered);
174-
pcl_conversions::fromPCL(*cloud_filtered, *cld_global);
200+
pcl_conversions::fromPCL(*cloud_filtered, *cld_transformed);
201+
tf2::doTransform(*cld_transformed, *cld_transformed, tf_global_from_z_reference);
202+
break;
203+
case Filters::VOXEL_RELATIVE :
204+
tf2::doTransform(cloud, *cld_transformed, tf_z_reference);
205+
pcl_conversions::toPCL(*cld_transformed, *cloud_pcl);
206+
sor.setInputCloud(cloud_pcl);
207+
sor.setFilterFieldName("z");
208+
sor.setFilterLimits(_min_obstacle_height, _max_obstacle_height);
209+
sor.setDownsampleAllData(false);
210+
sor.setLeafSize(v_s, v_s, v_s);
211+
sor.setMinimumPointsNumberPerVoxel(static_cast<unsigned int>(_voxel_min_points));
212+
sor.filter(*cloud_filtered);
213+
pcl_conversions::fromPCL(*cloud_filtered, *cld_transformed);
214+
tf2::doTransform(*cld_transformed, *cld_transformed, tf_global_from_z_reference);
215+
break;
216+
default:
217+
tf2::doTransform(cloud, *cld_transformed, tf_direct_global);
218+
break;
219+
175220
}
176221

177-
_observation_list.front()._cloud.reset(cld_global.release());
222+
_observation_list.front()._cloud.reset(cld_transformed.release());
178223
} catch (tf2::TransformException & ex) {
179224
// if fails, remove the empty observation
180225
_observation_list.pop_front();

‎spatio_temporal_voxel_layer/src/spatio_temporal_voxel_layer.cpp‎

Lines changed: 20 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -169,7 +169,7 @@ void SpatioTemporalVoxelLayer::onInitialize(void)
169169
double observation_keep_time, expected_update_rate, min_obstacle_height, max_obstacle_height;
170170
double min_z, max_z, vFOV, vFOVPadding;
171171
double hFOV, decay_acceleration, obstacle_range;
172-
std::string topic, sensor_frame, data_type, filter_str;
172+
std::string topic, sensor_frame, z_reference_frame, data_type, filter_str;
173173
bool inf_is_valid = false, clearing, marking;
174174
bool clear_after_reading, enabled;
175175
int voxel_min_points;
@@ -196,6 +196,7 @@ void SpatioTemporalVoxelLayer::onInitialize(void)
196196
declareParameter(source + "." + "horizontal_fov_angle", rclcpp::ParameterValue(1.04));
197197
declareParameter(source + "." + "decay_acceleration", rclcpp::ParameterValue(0.0));
198198
declareParameter(source + "." + "filter", rclcpp::ParameterValue(std::string("passthrough")));
199+
declareParameter(source + "." + "z_reference_frame", rclcpp::ParameterValue(_global_frame));
199200
declareParameter(source + "." + "voxel_min_points", rclcpp::ParameterValue(0));
200201
declareParameter(source + "." + "clear_after_reading", rclcpp::ParameterValue(false));
201202
declareParameter(source + "." + "enabled", rclcpp::ParameterValue(true));
@@ -231,6 +232,8 @@ void SpatioTemporalVoxelLayer::onInitialize(void)
231232
node->get_parameter(name_ + "." + source + "." + "decay_acceleration", decay_acceleration);
232233
// performs an approximate voxel filter over the data to reduce
233234
node->get_parameter(name_ + "." + source + "." + "filter", filter_str);
235+
// frame in which to calculate z bounds if relative filter is applied
236+
node->get_parameter(name_ + "." + source + "." + "z_reference_frame", z_reference_frame);
234237
// minimum points per voxel for voxel filter
235238
node->get_parameter(name_ + "." + source + "." + "voxel_min_points", voxel_min_points);
236239
// clears measurement buffer after reading values from it
@@ -242,15 +245,28 @@ void SpatioTemporalVoxelLayer::onInitialize(void)
242245
node->get_parameter(name_ + "." + source + "." + "model_type", model_type_int);
243246
ModelType model_type = static_cast<ModelType>(model_type_int);
244247

245-
if (filter_str == "passthrough") {
248+
switch (filter)
249+
{
250+
case "passthrough":
246251
RCLCPP_INFO(logger_, "Passthough filter activated.");
247252
filter = buffer::Filters::PASSTHROUGH;
248-
} else if (filter_str == "voxel") {
253+
break;
254+
case "voxel":
249255
RCLCPP_INFO(logger_, "Voxel filter activated.");
250256
filter = buffer::Filters::VOXEL;
251-
} else {
257+
break;
258+
case "passthrough_relative":
259+
RCLCPP_INFO(logger_, "Relative Passthough filter activated.");
260+
filter = buffer::Filters::PASSTHROUGH_RELATIVE;
261+
break;
262+
case "voxel_relative":
263+
RCLCPP_INFO(logger_, "Relative Voxel filter activated.");
264+
filter = buffer::Filters::VOXEL_RELATIVE;
265+
break;
266+
default:
252267
RCLCPP_INFO(logger_, "No filters activated.");
253268
filter = buffer::Filters::NONE;
269+
break;
254270
}
255271

256272
if (!(data_type == "PointCloud2" || data_type == "LaserScan")) {

0 commit comments

Comments
 (0)