Skip to content

Commit

Permalink
nothing works
Browse files Browse the repository at this point in the history
  • Loading branch information
@S1ink
S1ink committed Feb 24, 2024
1 parent b79f8c7 commit ee123ad
Show file tree
Hide file tree
Showing 3 changed files with 78 additions and 68 deletions.
4 changes: 2 additions & 2 deletions src/shared/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -226,9 +226,9 @@ set(PCL_SELECTED_LIBRARIES # did not work :(
PCL_SAMPLE_CONSENSUS_LIBRARIES
)
target_link_libraries(${PROJECT_NAME}_shared
# PRIVATE ${PCL_LIBRARIES}
# PRIVATE debug ${PCL_LIBRARIES}
PRIVATE PCL_COMMON PCL_IO PCL_OCTREE #PCL_FILTERS
PRIVATE SickScanInternal_lib
PRIVATE general SickScanInternal_lib
# PRIVATE sick_scan_xd_shared_lib
# PRIVATE "${PROJECT_SOURCE_DIR}/lib/${SICK_SCAN_LIB_REL}"
)
Expand Down
97 changes: 54 additions & 43 deletions src/shared/filtering.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -403,7 +403,7 @@ void progressive_morph_filter(
const std::vector<IntT>& selection,
pcl::Indices& ground,
const float base_,
const int max_window_size_,
const float max_window_size_,
const float cell_size_,
const float initial_distance_,
const float max_distance_,
Expand All @@ -417,22 +417,22 @@ void progressive_morph_filter(
float window_size = 0.0f;
float height_threshold = 0.0f;

while(window_size < max_window_size_) {
while (window_size < max_window_size_) {

// Determine the initial window size.
if(exponential_)
if (exponential_)
window_size = cell_size_ * (2.0f * std::pow(base_, iteration) + 1.0f);
else
window_size = cell_size_ * (2.0f * (iteration + 1) * base_ + 1.0f);

// Calculate the height threshold to be used in the next iteration.
if(iteration == 0)
if (iteration == 0)
height_threshold = initial_distance_;
else
height_threshold = slope_ * (window_size - window_sizes[iteration - 1]) * cell_size_ + initial_distance_;

// Enforce max distance on height threshold
if(height_threshold > max_distance_)
if (height_threshold > max_distance_)
height_threshold = max_distance_;

window_sizes.push_back(window_size);
Expand All @@ -444,23 +444,35 @@ void progressive_morph_filter(

// Ground indices are initially limited to those points in the input cloud we
// wish to process
if(selection.size() > 0 && selection.size() <= cloud_.size()) {
if (selection.size() > 0 && selection.size() <= cloud_.size()) {
ground = selection;
} else {
}
else {
ground.resize(cloud_.size());
for(size_t i = 0; i < cloud_.size(); i++) {
for (size_t i = 0; i < cloud_.size(); i++) {
ground[i] = i;
}
}

pcl::octree::OctreePointCloudSearch<PointT> tree{ 1.f };
const std::shared_ptr< const pcl::PointCloud<PointT> >
cloud_shared_ref{ &cloud_, [](const pcl::PointCloud<PointT>*){} };
cloud_shared_ref{ &cloud_, [](const pcl::PointCloud<PointT>*) {} };
const std::shared_ptr< const pcl::Indices >
ground_shared_ref{ &ground, [](const pcl::Indices*){} };
ground_shared_ref{ &ground, [](const pcl::Indices*) {} };

// only need to store z-coord for each morph operation (not full points) -- init to size of current selection
std::vector<float>
morph_max_z_temp{},
morph_max_z_final{}, // used for +z obstructions like rocks
morph_min_z_temp{},
morph_min_z_final{}; // used for -z obstructions like craters
morph_max_z_temp.resize(cloud_.size());
morph_max_z_final.resize(cloud_.size());
morph_min_z_temp.resize(cloud_.size()); // we have to make these the same size as the source cloud since the octree exports indices in the full range anc mapping backwards would be a pain
morph_min_z_final.resize(cloud_.size());

// Progressively filter ground returns using morphological open
for(size_t i = 0; i < window_sizes.size(); i++) {
for (size_t i = 0; i < window_sizes.size(); i++) {

// // Limit filtering to those points currently considered ground returns
// typename pcl::PointCloud<PointT>::Ptr
Expand All @@ -483,14 +495,14 @@ void progressive_morph_filter(

// cache for the indices of the points in each window -- init to size of current selection
std::vector<pcl::Indices>
pt_window_indices{ ground.size() };
// pt_window_indices.resize(ground.size());
pt_window_indices{};
pt_window_indices.resize(ground.size());

const float half_res = window_sizes[i] / 2.f;

// populate each window with contained points (indices)
for(size_t _idx = 0; _idx < ground.size(); _idx++) {
const PointT& _pt = cloud_[ground[_idx]]; // retreive source (x, y) for each pt in selection
for (size_t _idx = 0; _idx < ground.size(); _idx++) {
const PointT& _pt = cloud_[ground[_idx]]; // retrieve source (x, y) for each pt in selection
tree.boxSearch(
Eigen::Vector3f{
_pt.x - half_res,
Expand All @@ -506,45 +518,44 @@ void progressive_morph_filter(
);
}

// only need to store z-coord for each morph operation (not full points) -- init to size of current selection
std::vector<float>
morph_max_z_temp{ (float)ground.size() },
morph_max_z_final{ (float)ground.size() }, // used for +z obstructions like rocks
morph_min_z_temp{ (float)ground.size() },
morph_min_z_final{ (float)ground.size() }; // used for -z obstructions like craters
// morph_max_z_temp.resize(ground.size());
// morph_max_z_final.resize(ground.size());

// morph open stage 1 (morph erode)
for(size_t src_pt_idx = 0; src_pt_idx < ground.size(); src_pt_idx++) { // loop through windows for each point
for (size_t src_pt_idx = 0; src_pt_idx < ground.size(); src_pt_idx++) { // loop through windows for each point

const pcl::Indices& window_indices = pt_window_indices[src_pt_idx];
morph_max_z_temp[src_pt_idx] = morph_min_z_temp[src_pt_idx] = cloud_[src_pt_idx].z; // initialize to be the same as original cloud

for(const auto window_pt_idx : window_indices) { // min and max z for each window
float& _max_z_temp = morph_max_z_temp[ground[src_pt_idx]];
float& _min_z_temp = morph_min_z_temp[ground[src_pt_idx]];

_max_z_temp = _min_z_temp = cloud_[ground[src_pt_idx]].z; // initialize to be the same as original cloud

for (const auto window_pt_idx : window_indices) { // min and max z for each window
const float
_z = cloud_[window_pt_idx].z;
if(_z < morph_max_z_temp[src_pt_idx])
morph_max_z_temp[src_pt_idx] = _z;
if(_z > morph_min_z_temp[src_pt_idx])
morph_min_z_temp[src_pt_idx] = _z;
if (_z < _max_z_temp)
_max_z_temp = _z;
if (_z > _min_z_temp)
_min_z_temp = _z;
}

const float _t = _max_z_temp;

}
// morph open stage 2 (morph dilate)
for(size_t src_pt_idx = 0; src_pt_idx < ground.size(); src_pt_idx++) {
for (size_t src_pt_idx = 0; src_pt_idx < ground.size(); src_pt_idx++) {

const pcl::Indices& window_indices = pt_window_indices[src_pt_idx];
morph_max_z_final[src_pt_idx] = morph_min_z_final[src_pt_idx] = cloud_[src_pt_idx].z; // initialize to be the same as original cloud
float& _max_z_final = morph_max_z_final[ground[src_pt_idx]];
float& _min_z_final = morph_min_z_final[ground[src_pt_idx]];

_max_z_final = _min_z_final = cloud_[ground[src_pt_idx]].z; // initialize to be the same as original cloud

for(const auto window_pt_idx : window_indices) { // min and max z for each window
for (const auto window_pt_idx : window_indices) { // min and max z for each window
const float
_z_max = morph_max_z_temp[window_pt_idx], // use results from stage 1
_z_min = morph_min_z_temp[window_pt_idx];
if(_z_max > morph_max_z_final[src_pt_idx])
morph_max_z_final[src_pt_idx] = _z_max;
if(_z_min < morph_min_z_final[src_pt_idx])
morph_min_z_final[src_pt_idx] = _z_min;
_z_min = morph_min_z_temp[window_pt_idx]; // ISSUE: window_pt_idx is in the domain of the entire cloud's indices
if (_z_max > _max_z_final)
_max_z_final = _z_max;
if (_z_min < _min_z_final)
_min_z_final = _z_min;
}

}
Expand All @@ -555,9 +566,9 @@ void progressive_morph_filter(
for (size_t sel_idx = 0; sel_idx < ground.size(); sel_idx++) {

const float
diff_max = cloud_[ground[sel_idx]].z - morph_max_z_final[sel_idx],
diff_min = morph_min_z_final[sel_idx] - cloud_[ground[sel_idx]].z;
if (diff_max < height_thresholds[i] && diff_min < height_thresholds[i])
diff_max = cloud_[ground[sel_idx]].z - morph_max_z_final[ground[sel_idx]]; // DEBUGGING: diff_max was always 0.0...
// diff_min = morph_min_z_final[ground[sel_idx]] - cloud_[ground[sel_idx]].z;
if (diff_max < height_thresholds[i] /* && diff_min < height_thresholds[i] */)
pt_indices.push_back(ground[sel_idx]); // change this to directly edit the ground selection

}
Expand Down
45 changes: 22 additions & 23 deletions src/shared/lidar_api.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -240,28 +240,27 @@ namespace ldrp {

struct {
float
min_scan_theta_deg = -90.f, // max scan theta angle used for cutoff -- see ms100 operating manual for coordinate system
max_scan_theta_deg = +90.f, // min scan theta angle used for cutoff
min_scan_range_cm = 10.f, // the minimum scan range
max_pmf_range_cm = 200.f, // max range for points used in PMF
max_z_thresh_cm = 75.f, // for the "mid cut" z-coord filter
min_z_thresh_cm = 25.f,
min_scan_theta_deg = -90.f, // max scan theta angle used for cutoff -- see ms100 operating manual for coordinate system
max_scan_theta_deg = +90.f, // min scan theta angle used for cutoff
min_scan_range_cm = 10.f, // the minimum scan range
max_pmf_range_cm = 200.f, // max range for points used in PMF
max_z_thresh_cm = 75.f, // for the "mid cut" z-coord filter
min_z_thresh_cm = 25.f,
// filter by intensity? (test this)

voxel_size_cm = 3.f, // voxel cell size used during voxelization filter
map_resolution_cm = 5.f, // the resolution of each grid cell
pmf_window_base_cm = 0.4f,
pmf_cell_size_cm = 5.f,
pmf_init_distance_cm = 5.f,
pmf_max_distance_cm = 12.f,
pmf_slope = 0.2f;
int32_t
pmf_max_window_size = 40;
voxel_size_cm = 3.f, // voxel cell size used during voxelization filter
map_resolution_cm = 5.f, // the resolution of each grid cell
pmf_window_base = 1.f,
pmf_max_window_size_cm = 40.f,
pmf_cell_size_cm = 5.f,
pmf_init_distance_cm = 5.f,
pmf_max_distance_cm = 12.f,
pmf_slope = 0.2f;

} fpipeline;
} _config;

struct { // states to be connunicated across threads
struct { // states to be communicated across threads
int32_t log_level{ 1 };

std::atomic<bool> enable_threads{false};
Expand Down Expand Up @@ -475,13 +474,13 @@ namespace ldrp {
// apply pmf to selected points
progressive_morph_filter( // !!!: Crashing somewhere in here :(
voxelized_points, pre_pmf_range_filtered, pmf_filtered_ground,
this->_config.fpipeline.pmf_window_base_cm,
this->_config.fpipeline.pmf_max_window_size,
this->_config.fpipeline.pmf_cell_size_cm,
this->_config.fpipeline.pmf_init_distance_cm,
this->_config.fpipeline.pmf_max_distance_cm,
this->_config.fpipeline.pmf_window_base,
this->_config.fpipeline.pmf_max_window_size_cm * 1e-2f,
this->_config.fpipeline.pmf_cell_size_cm * 1e-2f,
this->_config.fpipeline.pmf_init_distance_cm * 1e-2f,
this->_config.fpipeline.pmf_max_distance_cm * 1e-2f,
this->_config.fpipeline.pmf_slope,
false
true
);
// obstacles = (base - ground)
pc_negate_selection(
Expand All @@ -491,7 +490,7 @@ namespace ldrp {
);

// TEST
pc_normalize_selection(voxelized_points.points, pmf_filtered_ground);
pc_normalize_selection(voxelized_points.points, pmf_filtered_obstacles);
if(this->_config.pcd_logging_mode & PCD_LOGGING_NT) {
this->_nt.test_filtered_points.Set(
std::span<const uint8_t>{
Expand Down

0 comments on commit ee123ad

Please sign in to comment.