Switch beam model by map label field (#334)

at-wat · Sep 3, 2020 · 2eb36f2 · 2eb36f2
1 parent 2e85c15
commit 2eb36f2
Show file tree

Hide file tree

Showing 9 changed files with 328 additions and 39 deletions.
diff --git a/include/mcl_3dl/chunked_kdtree.h b/include/mcl_3dl/chunked_kdtree.h
@@ -117,9 +117,14 @@ class ChunkedKdtree
       chunk.second.kdtree_->setPointRepresentation(point_rep_);
     }
   }
+  const typename pcl::PointCloud<POINT_TYPE>::ConstPtr& getInputCloud() const
+  {
+    return input_cloud_;
+  }
   void setInputCloud(
-      const typename pcl::PointCloud<POINT_TYPE>::ConstPtr cloud)
+      const typename pcl::PointCloud<POINT_TYPE>::ConstPtr& cloud)
   {
+    input_cloud_ = cloud;
     if (chunks_.size())
       chunks_.clear();
     ChunkOriginalIds ids;
@@ -284,6 +289,7 @@ class ChunkedKdtree
   using ChunkCloud = std::unordered_map<ChunkId, typename pcl::PointCloud<POINT_TYPE>, ChunkId>;
   using ChunkOriginalIds = std::unordered_map<ChunkId, std::vector<size_t>, ChunkId>;
   ChunkMap chunks_;
+  typename pcl::PointCloud<POINT_TYPE>::ConstPtr input_cloud_;
 };
 }  // namespace mcl_3dl
 

diff --git a/include/mcl_3dl/lidar_measurement_models/lidar_measurement_model_beam.h b/include/mcl_3dl/lidar_measurement_models/lidar_measurement_model_beam.h
@@ -62,6 +62,7 @@ class LidarMeasurementModelBeam : public LidarMeasurementModelBase
   float sin_total_ref_;
   float map_grid_min_;
   float map_grid_max_;
+  uint32_t filter_label_max_;
 
   PointCloudRandomSampler sampler_;
 
@@ -76,6 +77,10 @@ class LidarMeasurementModelBeam : public LidarMeasurementModelBase
   {
     return sin_total_ref_;
   }
+  inline uint32_t getFilterLabelMax() const
+  {
+    return filter_label_max_;
+  }
 
   void loadConfig(
       const ros::NodeHandle& nh,

diff --git a/include/mcl_3dl/point_conversion.h b/include/mcl_3dl/point_conversion.h
@@ -69,15 +69,25 @@ bool fromROSMsg(
   const int y_idx = getPointCloud2FieldIndex(msg, "y");
   const int z_idx = getPointCloud2FieldIndex(msg, "z");
   const int intensity_idx = getPointCloud2FieldIndex(msg, "intensity");
+  const int label_idx = getPointCloud2FieldIndex(msg, "label");
 
   if (x_idx == -1 || y_idx == -1 || z_idx == -1)
   {
     ROS_ERROR("Given PointCloud2 doesn't have x, y, z fields");
     return false;
   }
   if (intensity_idx != -1)
+  {
+    if (label_idx != -1)
+    {
+      return fromROSMsgImpl<mcl_3dl::PointXYZIL, PointT>(msg, pc);
+    }
     return fromROSMsgImpl<pcl::PointXYZI, PointT>(msg, pc);
-
+  }
+  if (label_idx != -1)
+  {
+    return fromROSMsgImpl<pcl::PointXYZL, PointT>(msg, pc);
+  }
   return fromROSMsgImpl<pcl::PointXYZ, PointT>(msg, pc);
 }
 }  // namespace mcl_3dl

diff --git a/include/mcl_3dl/raycast.h b/include/mcl_3dl/raycast.h
@@ -48,11 +48,13 @@ class Raycast
     Vec3 pos_;
     bool collision_;
     float sin_angle_;
+    const POINT_TYPE* point_;
 
-    CastResult(const Vec3 pos, bool collision, float sin_angle)
+    CastResult(const Vec3& pos, bool collision, float sin_angle, const POINT_TYPE* point)
       : pos_(pos)
       , collision_(collision)
       , sin_angle_(sin_angle)
+      , point_(point)
     {
     }
   };
@@ -94,6 +96,7 @@ class Raycast
       bool collision(false);
       float sin_ang(0.0);
 
+      const POINT_TYPE* point = nullptr;
       POINT_TYPE center;
       center.x = pos_.x_;
       center.y = pos_.y_;
@@ -105,6 +108,7 @@ class Raycast
               std::sqrt(2.0) * grid_max_ / 2.0, id, sqdist, 1))
       {
         collision = true;
+        point = &(kdtree_->getInputCloud()->points[id[0]]);
 
         const float d0 = std::sqrt(sqdist[0]);
         const Vec3 pos_prev = pos_ - (inc_ * 2.0);
@@ -124,7 +128,7 @@ class Raycast
           sin_ang = 1.0;
         }
       }
-      return CastResult(pos_, collision, sin_ang);
+      return CastResult(pos_, collision, sin_ang, point);
     }
     bool operator!=(const Iterator& a) const
     {

diff --git a/src/lidar_measurement_model_beam.cpp b/src/lidar_measurement_model_beam.cpp
@@ -87,6 +87,10 @@ void LidarMeasurementModelBeam::loadConfig(
   double ang_total_ref;
   pnh.param("ang_total_ref", ang_total_ref, M_PI / 6.0);
   sin_total_ref_ = sinf(ang_total_ref);
+
+  int filter_label_max;
+  pnh.param("filter_label_max", filter_label_max, static_cast<int>(0xFFFFFFFF));
+  filter_label_max_ = filter_label_max;
 }
 
 void LidarMeasurementModelBeam::setGlobalLocalizationStatus(
@@ -158,15 +162,17 @@ LidarMeasurementResult LidarMeasurementModelBeam::measure(
         map_grid_min_, map_grid_max_);
     for (auto point : ray)
     {
-      if (point.collision_)
+      if (!point.collision_)
+        continue;
+      if (point.point_->label > filter_label_max_)
+        continue;
+
+      // reject total reflection
+      if (point.sin_angle_ > sin_total_ref_)
       {
-        // reject total reflection
-        if (point.sin_angle_ > sin_total_ref_)
-        {
-          score_beam *= beam_likelihood_;
-        }
-        break;
+        score_beam *= beam_likelihood_;
       }
+      break;
     }
   }
   if (score_beam < beam_likelihood_min_)

diff --git a/src/mcl_3dl.cpp b/src/mcl_3dl.cpp
@@ -512,6 +512,7 @@ class MCL3dlNode
           std::dynamic_pointer_cast<LidarMeasurementModelBeam>(
               lidar_measurements_["beam"]);
       const float sin_total_ref = beam_model->getSinTotalRef();
+      const uint32_t filter_label_max = beam_model->getFilterLabelMax();
       for (auto& p : pc_particle_beam->points)
       {
         const int beam_header_id = p.label;
@@ -522,36 +523,38 @@ class MCL3dlNode
             params_.map_grid_min_, params_.map_grid_max_);
         for (auto point : ray)
         {
-          if (point.collision_)
+          if (!point.collision_)
+            continue;
+          if (point.point_->label > filter_label_max)
+            continue;
+
+          visualization_msgs::Marker marker;
+          marker.header.frame_id = params_.frame_ids_["map"];
+          marker.header.stamp = header.stamp;
+          marker.ns = "Ray collisions";
+          marker.id = markers.markers.size();
+          marker.type = visualization_msgs::Marker::CUBE;
+          marker.action = 0;
+          marker.pose.position.x = point.pos_.x_;
+          marker.pose.position.y = point.pos_.y_;
+          marker.pose.position.z = point.pos_.z_;
+          marker.pose.orientation.x = 0.0;
+          marker.pose.orientation.y = 0.0;
+          marker.pose.orientation.z = 0.0;
+          marker.pose.orientation.w = 1.0;
+          marker.scale.x = marker.scale.y = marker.scale.z = 0.4;
+          marker.lifetime = ros::Duration(0.2);
+          marker.frame_locked = true;
+          marker.color.a = 0.8;
+          marker.color.r = 1.0;
+          marker.color.g = 0.0;
+          marker.color.b = 0.0;
+          if (point.sin_angle_ < sin_total_ref)
           {
-            visualization_msgs::Marker marker;
-            marker.header.frame_id = params_.frame_ids_["map"];
-            marker.header.stamp = header.stamp;
-            marker.ns = "Ray collisions";
-            marker.id = markers.markers.size();
-            marker.type = visualization_msgs::Marker::CUBE;
-            marker.action = 0;
-            marker.pose.position.x = point.pos_.x_;
-            marker.pose.position.y = point.pos_.y_;
-            marker.pose.position.z = point.pos_.z_;
-            marker.pose.orientation.x = 0.0;
-            marker.pose.orientation.y = 0.0;
-            marker.pose.orientation.z = 0.0;
-            marker.pose.orientation.w = 1.0;
-            marker.scale.x = marker.scale.y = marker.scale.z = 0.4;
-            marker.lifetime = ros::Duration(0.2);
-            marker.frame_locked = true;
-            marker.color.a = 0.8;
-            marker.color.r = 1.0;
-            marker.color.g = 0.0;
-            marker.color.b = 0.0;
-            if (point.sin_angle_ < sin_total_ref)
-            {
-              marker.color.a = 0.2;
-            }
-            markers.markers.push_back(marker);
-            break;
+            marker.color.a = 0.2;
           }
+          markers.markers.push_back(marker);
+          break;
         }
       }
 

diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
@@ -40,6 +40,11 @@ add_rostest_gtest(test_expansion_resetting tests/expansion_resetting_rostest.tes
 target_link_libraries(test_expansion_resetting ${catkin_LIBRARIES})
 add_dependencies(test_expansion_resetting mcl_3dl)
 
+add_rostest_gtest(test_beam_label tests/beam_label_rostest.test
+    src/test_beam_label.cpp)
+target_link_libraries(test_beam_label ${catkin_LIBRARIES})
+add_dependencies(test_beam_label mcl_3dl)
+
 add_rostest_gtest(test_global_localization tests/global_localization_rostest.test
     src/test_global_localization.cpp)
 target_link_libraries(test_global_localization ${catkin_LIBRARIES})