PointCloud Minimum Spanning Tree Visualization with Node Appearance Count and Highlighted Point

#include ... using namespace pcl; ... class Graph { public: ... void KruskalMST(PointCloudT::Ptr cloud, std::vector& result, std::unordered_map<int, int>& pointCount) { ... // Count the number of points in the minimum spanning tree int numPoints = 0; for (const auto& edge : result) { const auto& src_point = cloud->points[edge.src]; const auto& tgt_point = cloud->points[edge.tgt]; numPoints += 2; pointCount[edge.src]++; pointCount[edge.tgt]++; } std::cout << "Number of points in the minimum spanning tree: " << numPoints << std::endl; }

// Find the point with maximum y value among the points that appear at least 3 times
int findMaxPoint(std::unordered_map<int, int>& pointCount, PointCloudT::Ptr cloud)
{
	int maxPoint = -1;
	int maxCount = 0;
	for (const auto& pair : pointCount)
	{
		if (pair.second >= 3)
		{
			const auto& point = cloud->points[pair.first];
			if (point.y > maxPoint)
			{
				maxPoint = point.y;
				maxCount = pair.second;
			}
		}
	}
	return maxPoint;
}

}; ... int main() { ... // Perform Kruskal's algorithm to find the minimum spanning tree std::vector result; std::unordered_map<int, int> pointCount; graph.KruskalMST(cloud, result, pointCount);

// Find the point with maximum y value among the points that appear at least 3 times
int maxPoint = graph.findMaxPoint(pointCount, cloud);

// Visualize the point cloud and color the point with maximum y value in green
pcl::visualization::PCLVisualizer viewer("Point Cloud Viewer");
viewer.setBackgroundColor(0, 0, 0);
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> single_color(cloud, 255, 255, 255);
viewer.addPointCloud<pcl::PointXYZ>(cloud, single_color, "cloud");

// Color the point with maximum y value in green
for (int i = 0; i < cloud->size(); ++i)
{
	if (cloud->points[i].y == maxPoint)
	{
		cloud->points[i].r = 0;
		cloud->points[i].g = 255;
		cloud->points[i].b = 0;
	}
}

// Update the point cloud in the viewer
viewer.updatePointCloud<pcl::PointXYZ>(cloud, single_color, "cloud");
viewer.spin();

...

}