sanity checks on pointcloud

ethz-asl · Nov 16, 2018 · 67443b1 · 67443b1
1 parent df2d05c
commit 67443b1
Show file tree

Hide file tree

Showing 3 changed files with 26 additions and 0 deletions.
diff --git a/include/lidar_align/sensors.h b/include/lidar_align/sensors.h
@@ -100,6 +100,8 @@ class Lidar {
 
   const size_t getNumberOfScans() const;
 
+  const size_t getTotalPoints() const;
+
   // note points are appended so any points in *pointcloud are preserved
   void getCombinedPointcloud(Pointcloud* pointcloud) const;
 

diff --git a/src/loader.cpp b/src/loader.cpp
@@ -41,6 +41,11 @@ void Loader::parsePointcloudMsg(const sensor_msgs::PointCloud2 msg,
       point.z = raw_point.z;
       point.intensity = raw_point.intensity;
 
+      if (!std::isfinite(point.x) || !std::isfinite(point.y) ||
+          !std::isfinite(point.z) || !std::isfinite(point.intensity)) {
+        continue;
+      }
+
       pointcloud->push_back(point);
     }
     pointcloud->header = raw_pointcloud.header;
@@ -54,6 +59,11 @@ void Loader::parsePointcloudMsg(const sensor_msgs::PointCloud2 msg,
       point.y = raw_point.y;
       point.z = raw_point.z;
 
+      if (!std::isfinite(point.x) || !std::isfinite(point.y) ||
+          !std::isfinite(point.z)) {
+        continue;
+      }
+
       pointcloud->push_back(point);
     }
     pointcloud->header = raw_pointcloud.header;
@@ -90,6 +100,12 @@ bool Loader::loadPointcloudFromROSBag(const std::string& bag_path,
       break;
     }
   }
+  if (lidar->getTotalPoints() == 0) {
+    ROS_ERROR_STREAM(
+        "No points were loaded, verify that the bag contains populated "
+        "messages of type sensor_msgs/PointCloud2");
+    return false;
+  }
 
   return true;
 }

diff --git a/src/sensors.cpp b/src/sensors.cpp
@@ -152,6 +152,14 @@ Lidar::Lidar(const LidarId& lidar_id) : lidar_id_(lidar_id){};
 
 const size_t Lidar::getNumberOfScans() const { return scans_.size(); }
 
+const size_t Lidar::getTotalPoints() const {
+  size_t num_points = 0;
+  for (const Scan& scan : scans_) {
+    num_points += scan.getRawPointcloud().size();
+  }
+  return num_points;
+}
+
 const LidarId& Lidar::getId() const { return lidar_id_; }
 
 void Lidar::addPointcloud(const LoaderPointcloud& pointcloud,