Merge pull request #68 from ethz-asl/fix/map_lookups

fix classification bug and update docs for map interface

panoptic_mapping/include/panoptic_mapping/tools/planning_interface.h

@@ -29,9 +29,38 @@ class PlanningInterface {
   const SubmapCollection& getSubmapCollection() const { return *submaps_; }
 
   // Lookups.
-  bool isObserved(const Point& position,
+  /**
+   * @brief Check if a point is observed in the map.
+   *
+   * @param position Position of point in world frame.
+   * @param consider_change_state If false considers only submaps considered
+   * present, i.e. whose ChangeState is active or persistent.
+   * @param include_inactive_maps If false considers only active submaps.
+   * @return True if the point was observed.
+   */
+  bool isObserved(const Point& position, bool consider_change_state = true,
                   bool include_inactive_maps = true) const;
+
+  /**
+   * @brief Compute the voxel state of a point in the map for planning.
+   *
+   * @param position Position of point in world frame.
+   * @return VoxelState of the given point.
+   */
   VoxelState getVoxelState(const Point& position) const;
+
+  /**
+   * @brief Computes the truncated signed distance function (TSDF) at a point in
+   * the multi-resolution map.
+   *
+   * @param position Position of point in world frame.
+   * @param distance Pointer to value to store the distance in.
+   * @param consider_change_state If true considers only submaps considered
+   * present, i.e. whose ChangeState is active or persistent.
+   * @param include_free_space If true points lying only in free space are
+   * considered observed.
+   * @return True if the point was observed and thus a distance returned.
+   */
   bool getDistance(const Point& position, float* distance,
                    bool consider_change_state = true,
                    bool include_free_space = true) const;

panoptic_mapping/src/tools/planning_interface.cpp

@@ -15,9 +15,9 @@ PlanningInterface::PlanningInterface(
     : submaps_(std::move(submaps)) {}
 
 bool PlanningInterface::isObserved(const Point& position,
+                                   bool consider_change_state,
                                    bool include_inactive_maps) const {
   Timer timer("planning_interface/is_observed");
-  // TODO(schmluk): Update this to latest convetions.
   for (const Submap& submap : *submaps_) {
     if (include_inactive_maps || submap.isActive()) {
       const Point position_S = submap.getT_S_M() * position;
@@ -114,7 +114,7 @@ PlanningInterface::VoxelState PlanningInterface::getVoxelState(
     return VoxelState::kExpectedFree;
   }
   return VoxelState::kUnknown;
-}  // namespace panoptic_mapping
+}
 
 bool PlanningInterface::getDistance(const Point& position, float* distance,
                                     bool consider_change_state,
@@ -166,11 +166,13 @@ bool PlanningInterface::getDistance(const Point& position, float* distance,
     const Point position_S = submap.getT_S_M() * position;
     if (submap.getBoundingVolume().contains_S(position_S)) {
       // Check classification for inactive submaps.
+      // NOTE(schmluk): Might not always be necessary, as geometry should be
+      // mostly unchanged when not being part of the submap.
       if (submap.hasClassLayer() && !submap.isActive()) {
         const ClassVoxel* class_voxel =
             submap.getClassLayer().getVoxelPtrByCoordinates(position_S);
         if (class_voxel) {
-          if (class_voxel->belongsToSubmap()) {
+          if (!class_voxel->belongsToSubmap()) {
             continue;
           }
         }