-
Notifications
You must be signed in to change notification settings - Fork 42
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Improve surface cell selector and reorganize menu
- Loading branch information
1 parent
76281ac
commit 0915b7a
Showing
6 changed files
with
254 additions
and
86 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
62 changes: 62 additions & 0 deletions
62
ros/wavemap_rviz_plugin/include/wavemap_rviz_plugin/visuals/cell_selector.h
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,62 @@ | ||
| #ifndef WAVEMAP_RVIZ_PLUGIN_VISUALS_CELL_SELECTOR_H_ | ||
| #define WAVEMAP_RVIZ_PLUGIN_VISUALS_CELL_SELECTOR_H_ | ||
|
|
||
| #include <ros/console.h> | ||
| #include <rviz/properties/enum_property.h> | ||
| #include <rviz/properties/float_property.h> | ||
| #include <wavemap/config/type_selector.h> | ||
| #include <wavemap/data_structure/volumetric/hashed_wavelet_octree.h> | ||
|
|
||
| namespace wavemap::rviz_plugin { | ||
| struct CellSelectionMode : public TypeSelector<CellSelectionMode> { | ||
| using TypeSelector<CellSelectionMode>::TypeSelector; | ||
|
|
||
| enum Id : TypeId { kSurface, kBand }; | ||
|
|
||
| static constexpr std::array names = {"Surface", "Band"}; | ||
| }; | ||
|
|
||
| class CellSelector : public QObject { | ||
| Q_OBJECT | ||
| public: // NOLINT | ||
| CellSelector(rviz::Property* submenu_root_property, | ||
| std::function<void()> redraw_map); | ||
|
|
||
| void initializePropertyMenu(); | ||
|
|
||
| void setMap(const VolumetricDataStructureBase::ConstPtr& hashed_map); | ||
|
|
||
| bool shouldBeDrawn(const OctreeIndex& cell_index, | ||
| FloatingPoint cell_log_odds) const; | ||
| bool isUnknown(FloatingPoint log_odds) const { | ||
| return std::abs(log_odds) < unknown_occupancy_threshold_; | ||
| } | ||
| bool hasFreeNeighbor(const OctreeIndex& cell_index) const; | ||
|
|
||
| private Q_SLOTS: // NOLINT | ||
| // These Qt slots get connected to signals indicating changes in the | ||
| // user-editable properties | ||
| void cellSelectionModeUpdateCallback(); | ||
| void thresholdUpdateCallback(); | ||
|
|
||
| private: | ||
| std::function<void()> redraw_map_; | ||
| HashedWaveletOctree::ConstPtr hashed_map_; | ||
|
|
||
| // Selection mode and thresholds | ||
| CellSelectionMode cell_selection_mode_ = CellSelectionMode::kSurface; | ||
| FloatingPoint surface_occupancy_threshold_ = 1e-3f; | ||
| FloatingPoint band_min_occupancy_threshold_ = 1e-3f; | ||
| FloatingPoint band_max_occupancy_threshold_ = 1e6f; | ||
| FloatingPoint unknown_occupancy_threshold_ = 1e-4f; | ||
|
|
||
| // User-editable property variables, contained in the visual's submenu | ||
| rviz::EnumProperty cell_selection_mode_property_; | ||
| rviz::FloatProperty surface_occupancy_threshold_property_; | ||
| rviz::FloatProperty band_min_occupancy_threshold_property_; | ||
| rviz::FloatProperty band_max_occupancy_threshold_property_; | ||
| rviz::FloatProperty unknown_occupancy_threshold_property_; | ||
| }; | ||
| } // namespace wavemap::rviz_plugin | ||
|
|
||
| #endif // WAVEMAP_RVIZ_PLUGIN_VISUALS_CELL_SELECTOR_H_ |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,154 @@ | ||
| #include "wavemap_rviz_plugin/visuals/cell_selector.h" | ||
|
|
||
| #include <wavemap/iterator/grid_iterator.h> | ||
|
|
||
| namespace wavemap::rviz_plugin { | ||
| std::array<Index3D, 26> generateNeighborIndexOffsets() { | ||
| std::array<Index3D, 26> neighbor_offsets{}; | ||
| size_t array_idx = 0u; | ||
| for (const Index3D& index : Grid<3>(-Index3D::Ones(), Index3D::Ones())) { | ||
| if (index != Index3D::Zero()) { | ||
| neighbor_offsets[array_idx] = index; | ||
| ++array_idx; | ||
| } | ||
| } | ||
| std::sort( | ||
| neighbor_offsets.begin(), neighbor_offsets.end(), | ||
| [](const auto& lhs, const auto& rhs) { return lhs.norm() < rhs.norm(); }); | ||
| return neighbor_offsets; | ||
| } | ||
| static const auto kNeighborOffsets = generateNeighborIndexOffsets(); | ||
|
|
||
| CellSelector::CellSelector(rviz::Property* submenu_root_property, | ||
| std::function<void()> redraw_map) | ||
| : redraw_map_(std::move(redraw_map)), | ||
| cell_selection_mode_property_( | ||
| "Cell selector", "", "Mode determining what cells to draw.", | ||
| submenu_root_property, SLOT(cellSelectionModeUpdateCallback()), this), | ||
| surface_occupancy_threshold_property_( | ||
| "Log odds threshold", surface_occupancy_threshold_, | ||
| "Classification threshold above which a cell is considered occupied. " | ||
| "Ranges from -Inf to Inf.", | ||
| submenu_root_property, SLOT(thresholdUpdateCallback()), this), | ||
| band_min_occupancy_threshold_property_( | ||
| "Min log odds", band_min_occupancy_threshold_, | ||
| "Only show cells whose occupancy exceeds this threshold. " | ||
| "Ranges from -Inf to Inf.", | ||
| submenu_root_property, SLOT(thresholdUpdateCallback()), this), | ||
| band_max_occupancy_threshold_property_( | ||
| "Max log odds", band_max_occupancy_threshold_, | ||
| "Only show cells whose occupancy falls below this threshold. " | ||
| "Ranges from -Inf to Inf.", | ||
| submenu_root_property, SLOT(thresholdUpdateCallback()), this), | ||
| unknown_occupancy_threshold_property_( | ||
| "Unknown log odds", unknown_occupancy_threshold_, | ||
| "Hide cells whose absolute occupancy falls below this threshold, " | ||
| "considering them as unknown. Ranges from -Inf to Inf.", | ||
| submenu_root_property, SLOT(thresholdUpdateCallback()), this) { | ||
| // Initialize the property menu | ||
| initializePropertyMenu(); | ||
| } | ||
|
|
||
| void CellSelector::initializePropertyMenu() { | ||
| cell_selection_mode_property_.clearOptions(); | ||
| for (const auto& name : CellSelectionMode::names) { | ||
| cell_selection_mode_property_.addOption(name); | ||
| } | ||
| cell_selection_mode_property_.setStringStd(cell_selection_mode_.toStr()); | ||
| surface_occupancy_threshold_property_.setHidden(cell_selection_mode_ != | ||
| CellSelectionMode::kSurface); | ||
| band_min_occupancy_threshold_property_.setHidden(cell_selection_mode_ != | ||
| CellSelectionMode::kBand); | ||
| band_max_occupancy_threshold_property_.setHidden(cell_selection_mode_ != | ||
| CellSelectionMode::kBand); | ||
| } | ||
|
|
||
| void CellSelector::setMap( | ||
| const VolumetricDataStructureBase::ConstPtr& hashed_map) { | ||
| hashed_map_ = | ||
| std::dynamic_pointer_cast<const HashedWaveletOctree>(hashed_map); | ||
| if (!hashed_map_ && cell_selection_mode_ == CellSelectionMode::kSurface) { | ||
| ROS_WARN( | ||
| "Cell selection mode 'Surface' only supports HashedWaveletOctree maps. " | ||
| "Falling back to mode 'Band'."); | ||
| cell_selection_mode_ = CellSelectionMode::kBand; | ||
| initializePropertyMenu(); | ||
| } | ||
| } | ||
|
|
||
| bool CellSelector::shouldBeDrawn(const OctreeIndex& cell_index, | ||
| FloatingPoint cell_log_odds) const { | ||
| switch (cell_selection_mode_.toTypeId()) { | ||
| case CellSelectionMode::kSurface: | ||
| // Skip free cells | ||
| if (cell_log_odds < surface_occupancy_threshold_ || | ||
| isUnknown(cell_log_odds)) { | ||
| return false; | ||
| } | ||
| // Skip cells that are occluded by neighbors on all sides | ||
| if (!hasFreeNeighbor(cell_index)) { | ||
| return false; | ||
| } | ||
| break; | ||
| case CellSelectionMode::kBand: | ||
| // Skip cells that don't meet the occupancy threshold | ||
| if (cell_log_odds < band_min_occupancy_threshold_ || | ||
| band_max_occupancy_threshold_ < cell_log_odds || | ||
| isUnknown(cell_log_odds)) { | ||
| return false; | ||
| } | ||
| break; | ||
| } | ||
| return true; | ||
| } | ||
|
|
||
| bool CellSelector::hasFreeNeighbor(const OctreeIndex& cell_index) const { | ||
| for (const auto& offset : kNeighborOffsets) { // NOLINT | ||
| const OctreeIndex neighbor_index = {cell_index.height, | ||
| cell_index.position + offset}; | ||
| const FloatingPoint neighbor_log_odds = | ||
| hashed_map_->getCellValue(neighbor_index); | ||
| // Check if the neighbor is free and observed | ||
| if (neighbor_log_odds < surface_occupancy_threshold_ && | ||
| !isUnknown(neighbor_log_odds)) { | ||
| return true; | ||
| } | ||
| } | ||
| return false; | ||
| } | ||
|
|
||
| void CellSelector::cellSelectionModeUpdateCallback() { | ||
| // Update the cached cell selection mode value | ||
| const CellSelectionMode old_cell_selection_mode = cell_selection_mode_; | ||
| cell_selection_mode_ = | ||
| CellSelectionMode(cell_selection_mode_property_.getStdString()); | ||
|
|
||
| // Show/hide properties that only affect certain modes | ||
| surface_occupancy_threshold_property_.setHidden(cell_selection_mode_ != | ||
| CellSelectionMode::kSurface); | ||
| band_min_occupancy_threshold_property_.setHidden(cell_selection_mode_ != | ||
| CellSelectionMode::kBand); | ||
| band_max_occupancy_threshold_property_.setHidden(cell_selection_mode_ != | ||
| CellSelectionMode::kBand); | ||
|
|
||
| // Redraw the map if the color mode changed | ||
| if (cell_selection_mode_ != old_cell_selection_mode) { | ||
| std::invoke(redraw_map_); | ||
| } | ||
| } | ||
|
|
||
| void CellSelector::thresholdUpdateCallback() { | ||
| // Update the thresholds | ||
| surface_occupancy_threshold_ = | ||
| surface_occupancy_threshold_property_.getFloat(); | ||
| band_min_occupancy_threshold_ = | ||
| band_min_occupancy_threshold_property_.getFloat(); | ||
| band_max_occupancy_threshold_ = | ||
| band_max_occupancy_threshold_property_.getFloat(); | ||
| unknown_occupancy_threshold_ = | ||
| unknown_occupancy_threshold_property_.getFloat(); | ||
|
|
||
| // Redraw the map | ||
| std::invoke(redraw_map_); | ||
| } | ||
| } // namespace wavemap::rviz_plugin |
File renamed without changes.
Oops, something went wrong.