|
@@ -101,13 +101,12 @@ catch (const std::exception &e)
|
|
void configure_filters(rs2::decimation_filter &dec_filter, rs2::threshold_filter &thr_filter, rs2::spatial_filter &spat_filter, int dec_filter_magnitude, float min_dist, float max_dist, int magnitude, int smooth_alpha, int smooth_delta, int holes_fill)
|
|
void configure_filters(rs2::decimation_filter &dec_filter, rs2::threshold_filter &thr_filter, rs2::spatial_filter &spat_filter, int dec_filter_magnitude, float min_dist, float max_dist, int magnitude, int smooth_alpha, int smooth_delta, int holes_fill)
|
|
{
|
|
{
|
|
dec_filter.set_option(RS2_OPTION_FILTER_MAGNITUDE, dec_filter_magnitude);
|
|
dec_filter.set_option(RS2_OPTION_FILTER_MAGNITUDE, dec_filter_magnitude);
|
|
- thr_filter.set_option(RS2_OPTION_MIN_DISTANCE, min_dist); // 0.0
|
|
|
|
- thr_filter.set_option(RS2_OPTION_MAX_DISTANCE, max_dist); // 0.5
|
|
|
|
- spat_filter.set_option(RS2_OPTION_FILTER_MAGNITUDE, magnitude); // 5
|
|
|
|
- spat_filter.set_option(RS2_OPTION_FILTER_SMOOTH_ALPHA, smooth_alpha); // 1
|
|
|
|
- spat_filter.set_option(RS2_OPTION_FILTER_SMOOTH_DELTA, smooth_delta); // 1
|
|
|
|
- spat_filter.set_option(RS2_OPTION_HOLES_FILL, holes_fill); // 5
|
|
|
|
- // hole_filter.set_option(RS2_OPTION_HOLES_FILL, 1); //1
|
|
|
|
|
|
+ thr_filter.set_option(RS2_OPTION_MIN_DISTANCE, min_dist);
|
|
|
|
+ thr_filter.set_option(RS2_OPTION_MAX_DISTANCE, max_dist);
|
|
|
|
+ spat_filter.set_option(RS2_OPTION_FILTER_MAGNITUDE, magnitude);
|
|
|
|
+ spat_filter.set_option(RS2_OPTION_FILTER_SMOOTH_ALPHA, smooth_alpha);
|
|
|
|
+ spat_filter.set_option(RS2_OPTION_FILTER_SMOOTH_DELTA, smooth_delta);
|
|
|
|
+ spat_filter.set_option(RS2_OPTION_HOLES_FILL, holes_fill);
|
|
}
|
|
}
|
|
|
|
|
|
/* Apply filters.
|
|
/* Apply filters.
|
|
@@ -127,7 +126,6 @@ void apply_filters(rs2::decimation_filter &dec_filter, rs2::threshold_filter &th
|
|
depth_frame = depth_to_disparity.process(depth_frame);
|
|
depth_frame = depth_to_disparity.process(depth_frame);
|
|
depth_frame = spat_filter.process(depth_frame);
|
|
depth_frame = spat_filter.process(depth_frame);
|
|
depth_frame = disparity_to_depth.process(depth_frame);
|
|
depth_frame = disparity_to_depth.process(depth_frame);
|
|
- // filtered = hole_filter.process(depth_frame);
|
|
|
|
}
|
|
}
|
|
|
|
|
|
ColoredPoints scan(rs2::decimation_filter &dec_filter, rs2::threshold_filter &thr_filter, rs2::spatial_filter &spat_filter, rs2::disparity_transform &depth_to_disparity, rs2::disparity_transform &disparity_to_depth, rs2::pipeline &pipe)
|
|
ColoredPoints scan(rs2::decimation_filter &dec_filter, rs2::threshold_filter &thr_filter, rs2::spatial_filter &spat_filter, rs2::disparity_transform &depth_to_disparity, rs2::disparity_transform &disparity_to_depth, rs2::pipeline &pipe)
|
|
@@ -160,8 +158,6 @@ void scan_and_convert(rs2::decimation_filter &dec_filter, rs2::threshold_filter
|
|
ColoredPoints scanned_points = scan(dec_filter, thr_filter, spat_filter, depth_to_disparity, disparity_to_depth, pipe);
|
|
ColoredPoints scanned_points = scan(dec_filter, thr_filter, spat_filter, depth_to_disparity, disparity_to_depth, pipe);
|
|
std::cout << "\nConverting... ";
|
|
std::cout << "\nConverting... ";
|
|
PCLHelper::PtrCloud converted_points = PCLHelper::points_to_pcl(scanned_points.points, scanned_points.color);
|
|
PCLHelper::PtrCloud converted_points = PCLHelper::points_to_pcl(scanned_points.points, scanned_points.color);
|
|
- // PCLHelper::PtrCloud cropped_points = PCLHelper::crop_cloud(converted_points, -0.15, -0.2, 0.0, 0.15, 0.05, 0.3);
|
|
|
|
- // pointclouds.push_back(cropped_points);
|
|
|
|
pointclouds.push_back(converted_points);
|
|
pointclouds.push_back(converted_points);
|
|
}
|
|
}
|
|
|
|
|