update increment cases interprettation and properly handle null point…

… selections

extra/nt-bridge/CMakeLists.txt

@@ -29,6 +29,7 @@ execute_process(
 		--build ${PROJECT_SOURCE_DIR}/protobuf/cmake-build
 		--target install
 		--config Release
+		--parallel
 )
 
 if(WIN32)

extra/nt-bridge/src/main.cpp

@@ -160,7 +160,7 @@ class NTBridgeNode : public rclcpp::Node {
 
 				imu.header.stamp.sec = static_cast<int32_t>( val.time() / 1000000L );
 				imu.header.stamp.nanosec = static_cast<int32_t>( val.time() % 1000000L ) * 1000U;
-				imu.header.frame_id = "lidar";
+				imu.header.frame_id = "world";
 
 				this->imu_pub->publish(imu);
 

src/ldrp/filtering.hpp

@@ -536,8 +536,13 @@ void pc_negate_selection(
 	std::vector<IntT>& negated
 ) {
 	if(base.size() <= selection.size()) {
+		// negated.clear();
 		return;
 	}
+	// if(selection.size() == 0) {
+	// 	negated = base;
+	// 	return;
+	// }
 	negated.resize(base.size() - selection.size());
 	size_t
 		_base = 0,

src/ldrp/grid.hpp

@@ -529,8 +529,8 @@ class QuantizedRatioGrid : public RatioGrid< Ratio_t, Int_t, Float_t, X_Major, M
 
 	/** Increments the base and accumulator counters for each point in the provided selections, which are determined by:
 	 * 1. if the BASE and ACCUM selections are both null (empty), all points are used to increment both counters
-	 * 2. if the BASE is null (empty) and the ACCUM selection is not, increment all base cells and increment selected accum cells
-	 * 3. if the BASE is non-null and the ACCUM IS null (empty), increment both counters for the selected base indices
+	 * 2. if the BASE is null (empty) and the ACCUM selection is not, increment BOTH for accum selection
+	 * 3. if the BASE is non-null and the ACCUM is empty, ONLY increment the base selection (no accum increments -- EMPTY TREATED NORMALLY)
 	 * 4. if the BASE and ACCUM selections are both non-null, increment the base selection and the valid accum selection which represents a subset of the base selection (incrementing an ACCUM w/o a BASE is invalid since it leads to a div by 0)
 	 * >> TEMPLATE PARAMS >>
 	 * @param PointT - the point type for the input pointcloud
@@ -574,24 +574,16 @@ class QuantizedRatioGrid : public RatioGrid< Ratio_t, Int_t, Float_t, X_Major, M
 						this->buffer[i] = this->normalizeAndQuantize<Quantization_Exponent, Ratio_Normalize_Rebase, Ratio_Normalize_Thresh>(*_cell);
 					}
 				}
-			} else {						// CASE 2: increment all base + selected accumulator indices
-				size_t
-					_pt = 0,
-					_sel = 0;
-				for(; _pt < cloud.points.size(); _pt++) {
+			} else {						// CASE 2: increment both for accumulator selection
+				for(const pcl::index_t _pt : accum_selection) {
 					const PointT& pt = cloud.points[_pt];
 					const int64_t i = this->cellIdxOf(static_cast<FloatT>(pt.x), static_cast<FloatT>(pt.y));
 #ifndef GRID_IMPL_SKIP_BOUND_CHECKING
-					if (i < 0 || i >= this->area()) {	// opposite the usual so that we only need one macro test!
-						if(_sel < accum_selection.size() && accum_selection[_sel] == _pt) _sel++;	// need to test for increment even if point is invalid
-					} else	// else since we tested the opposite!
+					if (i >= 0 && i < this->area())
 #endif
 					{
 						Cell_T* _cell = this->grid + i;
-						if(_sel < accum_selection.size() && accum_selection[_sel] == _pt) {
-							_cell->accum += Super_T::template literalR(1);
-							_sel++;
-						}
+						_cell->accum += Super_T::template literalR(1);
 						_cell->base += Super_T::template literalR(1);
 
 						this->buffer[i] = this->normalizeAndQuantize<Quantization_Exponent, Ratio_Normalize_Rebase, Ratio_Normalize_Thresh>(*_cell);
@@ -601,7 +593,7 @@ class QuantizedRatioGrid : public RatioGrid< Ratio_t, Int_t, Float_t, X_Major, M
 
 		} else {
 
-			if(accum_selection.empty()) {	// CASE 3: increment both for entire base selection
+			if(accum_selection.empty()) {	// CASE 3: increment only base selection
 				for(const pcl::index_t _pt : base_selection) {
 					const PointT& pt = cloud.points[_pt];
 					const int64_t i = this->cellIdxOf(static_cast<FloatT>(pt.x), static_cast<FloatT>(pt.y));
@@ -610,7 +602,6 @@ class QuantizedRatioGrid : public RatioGrid< Ratio_t, Int_t, Float_t, X_Major, M
 #endif
 					{
 						Cell_T* _cell = this->grid + i;
-						_cell->accum += Super_T::template literalR(1);
 						_cell->base += Super_T::template literalR(1);
 
 						this->buffer[i] = this->normalizeAndQuantize<Quantization_Exponent, Ratio_Normalize_Rebase, Ratio_Normalize_Thresh>(*_cell);

src/ldrp/perception.cpp

@@ -328,72 +328,96 @@ void PerceptionNode::process(const pcl::PointCloud<pcl::PointXYZ>& cloud, const
 	;
 #endif
 
+
+	/** WARNING: The below algorithm is tricky since an empty, "null" selection
+	 * accomplishes different things depending on the specific method call! 
+	 * The current control-flow is specifically engineered to avoid misinterpretations. */
+
 	// voxelize points
 	voxel_filter(
-		cloud, DEFAULT_NO_SELECTION, voxel_cloud,
+		cloud, DEFAULT_NO_SELECTION, voxel_cloud,	// <-- default selection = use all points
 		voxel_size_m, voxel_size_m, voxel_size_m
 	);
 
 	// filter points under "high cut" thresh
 	carteZ_filter(
-		voxel_cloud, DEFAULT_NO_SELECTION, z_high_filtered,
+		voxel_cloud, DEFAULT_NO_SELECTION, z_high_filtered,		// <-- default selection = use all points
 		-std::numeric_limits<float>::infinity(),
 		max_z_thresh_m
 	);
+	if(z_high_filtered.size() <= 0) return;		// no points to process
 
 	// further filter points below "low cut" thresh
 	carteZ_filter(
 		voxel_cloud, z_high_filtered, z_low_subset_filtered,
 		-std::numeric_limits<float>::infinity(),
 		min_z_thresh_m
 	);
-
-	// get the points inbetween high and low thresholds --> treated as wall obstacles
-	pc_negate_selection(
-		z_high_filtered,
-		z_low_subset_filtered,
-		z_mid_filtered_obstacles
-	);
 
-	// filter close enough points for PMF
-	pc_filter_distance(
-		voxel_cloud.points,
-		z_low_subset_filtered,
-		pre_pmf_range_filtered,
-		0.f, max_pmf_range_m,
-		origin
-	);
-
-	// apply pmf to selected points
-	progressive_morph_filter(
-		voxel_cloud, pre_pmf_range_filtered, pmf_filtered_ground,
-		pmf_window_base,
-		pmf_max_window_size_m,
-		pmf_cell_size_m,
-		pmf_init_distance_m,
-		pmf_max_distance_m,
-		pmf_slope,
-		false
-	);
-
-	// obstacles = (base - ground)
-	pc_negate_selection(
-		pre_pmf_range_filtered,
-		pmf_filtered_ground,
-		pmf_filtered_obstacles
-	);
-
-	this->accumulator.incrementRatio<pcl::PointXYZ, std::ratio<3, 2>, 50, 100>(	// insert PMF obstacles
-		voxel_cloud,
-		pre_pmf_range_filtered,		// base
-		pmf_filtered_obstacles		// subset
-	);
-	this->accumulator.incrementRatio<pcl::PointXYZ, std::ratio<3, 2>, 50, 100>(	// insert z-thresh obstacles
-		voxel_cloud,
-		z_mid_filtered_obstacles,	// base
-		DEFAULT_NO_SELECTION		// use all of base
-	);
-	// trim ratios
+	const bool
+		only_z_mid = z_low_subset_filtered.size() <= 0,		// no points at "groud level" --> skip advanced processing and just add these to the grid
+		has_z_mid = z_low_subset_filtered.size() != z_high_filtered.size();		// are there any points at all in the "wall range"? --> if not, don't attempt to negate the selection, or add points to the grid
+	if(!only_z_mid) {	// need points in order to continue (otherwise this gets misinterpretted as "use all points")
+
+		if(has_z_mid) {
+			// get the points inbetween high and low thresholds --> treated as wall obstacles
+			pc_negate_selection(
+				z_high_filtered,
+				z_low_subset_filtered,
+				z_mid_filtered_obstacles
+			);
+		}
+
+		// filter close enough points for PMF
+		pc_filter_distance(
+			voxel_cloud.points,
+			z_low_subset_filtered,
+			pre_pmf_range_filtered,		// NOTE: this will contain all indices even if the entire cloud is selected (no default empty select)
+			0.f, max_pmf_range_m,
+			origin
+		);
+		if(pre_pmf_range_filtered.size() > 0) {		// need points in order to continue (otherwise this gets reinterpretted as "use all points")
+
+			// apply pmf to selected points
+			progressive_morph_filter(
+				voxel_cloud, pre_pmf_range_filtered, pmf_filtered_ground,	// see the note above -- an empty output selection means no ground points
+				pmf_window_base,
+				pmf_max_window_size_m,
+				pmf_cell_size_m,
+				pmf_init_distance_m,
+				pmf_max_distance_m,
+				pmf_slope,
+				false
+			);
+
+			// obstacles = (base - ground)
+			if(pmf_filtered_ground.size() == 0) {	// the case where all subset points are obstacles...
+				pmf_filtered_obstacles.clear();
+				std::swap(pre_pmf_range_filtered, pmf_filtered_obstacles);	// setup the "accumulator" selection as containing the selection (null base) to trigger "case 2" of incrementRatio best efficiency
+			} else {
+				pc_negate_selection(	// normal set difference computation
+					pre_pmf_range_filtered,
+					pmf_filtered_ground,
+					pmf_filtered_obstacles
+				);
+			}	// in the case that all prefiltered points are ground, incrementRatio opperates correctly by default
+
+			this->accumulator.incrementRatio<pcl::PointXYZ, std::ratio<3, 2>, 50, 100>(	// insert PMF obstacles
+				voxel_cloud,
+				pre_pmf_range_filtered,		// base
+				pmf_filtered_obstacles		// subset (accumulator)
+			);
+
+		} else {}	// no points to process for PMF...
+
+	} else {}	// no low (groud) points to process
+	if(has_z_mid) {		// only attempt if z_mid_filtered_obstacles is not empty to prevent a misinterprettation by incrementRatio (this would increment all the cells in the grid)
+		this->accumulator.incrementRatio<pcl::PointXYZ, std::ratio<3, 2>, 50, 100>(	// insert z-thresh obstacles (increment both halves of ratio)
+			voxel_cloud,
+			DEFAULT_NO_SELECTION,		// trigger "case 2" of incrementRatio (recently changed)
+			(only_z_mid ? z_high_filtered : z_mid_filtered_obstacles)	// accumulator --> increments both for this selection since base is null
+		);
+	}
 
 }