From 116eaf50617e1e8d9de430826a71998c4098a816 Mon Sep 17 00:00:00 2001
From: Charles Cochran <charles_cochran@jabil.com>
Date: Thu, 23 May 2024 13:38:28 -0400
Subject: [PATCH] Fix PCL KDTree empty cloud exception

See https://github.com/Livox-SDK/LIO-Livox/issues/49 and
https://github.com/Livox-SDK/LIO-Livox/issues/61.
---
 src/lio/Estimator.cpp | 4 +++-
 1 file changed, 3 insertions(+), 1 deletion(-)

diff --git a/src/lio/Estimator.cpp b/src/lio/Estimator.cpp
index 08ea90a..3cf03eb 100644
--- a/src/lio/Estimator.cpp
+++ b/src/lio/Estimator.cpp
@@ -927,7 +927,9 @@ void Estimator::Estimate(std::list<LidarFrame>& lidarFrameList,
   Eigen::Vector3d exPbl = -1.0 * exRbl * exTlb.topRightCorner(3,1);
   kdtreeCornerFromLocal->setInputCloud(laserCloudCornerFromLocal);
   kdtreeSurfFromLocal->setInputCloud(laserCloudSurfFromLocal);
-  kdtreeNonFeatureFromLocal->setInputCloud(laserCloudNonFeatureFromLocal);
+  if(!laserCloudNonFeatureFromLocal->empty()) {
+    kdtreeNonFeatureFromLocal->setInputCloud(laserCloudNonFeatureFromLocal);
+  }
 
   std::unique_lock<std::mutex> locker3(map_manager->mtx_MapManager);
   for(int i = 0; i < 4851; i++){