【激光雷达点云障碍物检测】environment.cpp 主函数

主函数

#include "lidar.h"
#include "render.h"
#include "processPointClouds.h"
// using templates for processPointClouds so also include .cpp to help linker
#include "processPointClouds.cpp"
using namespace lidar_obstacle_detection;

std::vector initHighway(bool renderScene, pcl::visualization::PCLVisualizer::Ptr &viewer) {

	Car egoCar(Vect3(0, 0, 0), Vect3(4, 2, 2), Color(0, 1, 0), "egoCar");
	Car car1(Vect3(15, 0, 0), Vect3(4, 2, 2), Color(0, 0, 1), "car1");
	Car car2(Vect3(8, -4, 0), Vect3(4, 2, 2), Color(0, 0, 1), "car2");
	Car car3(Vect3(-12, 4, 0), Vect3(4, 2, 2), Color(0, 0, 1), "car3");

	std::vector cars;
	cars.push_back(egoCar);
	cars.push_back(car1);
	cars.push_back(car2);
	cars.push_back(car3);

	if (renderScene) {
		renderHighway(viewer);
		egoCar.render(viewer);
		car1.render(viewer);
		car2.render(viewer);
		car3.render(viewer);
	}

	return cars;
}

// Test load pcd
//void cityBlock(pcl::visualization::PCLVisualizer::Ptr& viewer){
//    ProcessPointCloudspointProcessor;
//    pcl::PointCloud::Ptr inputCloud = pointProcessor.loadPcd("../src/sensors/data/pcd/data_1/0000000000.pcd");
//    renderPointCloud(viewer,inputCloud,"cloud");
//}

// Initialize the simple Highway

// Test read Lidar data
void cityBlock(pcl::visualization::PCLVisualizer::Ptr &viewer, ProcessPointClouds *pointProcessorI,const pcl::PointCloud::Ptr &inputCloud) 
{
	// 1、滤波  滤波后点云存入filteredCloud		ok
	float filterRes = 0.4;
	Eigen::Vector4f minpoint(-10, -6.5, -2, 1);
	Eigen::Vector4f maxpoint(30, 6.5, 1, 1);
	pcl::PointCloud::Ptr filteredCloud = pointProcessorI->FilterCloud(inputCloud, filterRes, minpoint,maxpoint);
	
	// 2、将滤波后的点云分割成地面和障碍物 结果存入segmentCloud中		ok
	int maxIterations = 40;
	float distanceThreshold = 0.3;

	//2.1 返回地面点云 和 障碍物点云 
	//segmentCloud.first, "obstCloud" 
	//segmentCloud.second, "planeCloud"
	std::pair::Ptr, pcl::PointCloud::Ptr> segmentCloud = pointProcessorI->RansacSegmentPlane(filteredCloud, maxIterations, distanceThreshold);
	
	//2.2 选取待渲染的点云的种类分别为 障碍物、地面、全部点云
	//renderPointCloud(viewer, segmentCloud.first, "obstCloud", Color(1, 0, 0));
	//renderPointCloud(viewer, segmentCloud.second, "planeCloud", Color(0, 1, 0));
	renderPointCloud(viewer,inputCloud,"inputCloud");
	
	// 3、对去除地面后的障碍物点云进行聚类  segmentCloud.first, "obstCloud" 
	float clusterTolerance = 0.5;
	int minsize = 10;
	int maxsize = 140;
	//std::vector> cloudClusters 返回了11类 每类中又包含了属于该类的点云
	std::vector::Ptr> cloudClusters = pointProcessorI->EuclideanClustering(segmentCloud.first, clusterTolerance, minsize, maxsize);
	int clusterId = 0;
	std::vector colors = { Color(1, 0, 0), Color(0, 1, 0), Color(0, 0, 1) };
	for (pcl::PointCloud::Ptr cluster : cloudClusters)  //遍历每一类中的点
	{
		std::cout << "cluster size";
		pointProcessorI->numPoints(cluster);  // cloud->points.size()
		renderPointCloud(viewer, cluster, "obstCLoud" + std::to_string(clusterId),colors[clusterId % colors.size()]);
		// Fourth: Find bounding boxes for each obstacle cluster
		Box box = pointProcessorI->BoundingBox(cluster);  //找到每块点云的xyz轴上的最值
		renderBox(viewer, box, clusterId); //根据最值画框
		++clusterId;
	}
}

void simpleHighway(pcl::visualization::PCLVisualizer::Ptr &viewer) {
	// ----------------------------------------------------
	// -----Open 3D viewer and display simple highway -----
	// ----------------------------------------------------
	// RENDER OPTIONS
	bool renderScene = false;
	bool render_obst = false;
	bool render_plane = false;
	bool render_cluster = true;
	bool render_box = true;
	std::vector cars = initHighway(renderScene, viewer);

	// TODO:: Create lidar sensor
	Lidar *lidar = new Lidar(cars, 0);
	pcl::PointCloud::Ptr inputCloud = lidar->scan();
	//    renderRays(viewer,lidar->position,inputCloud);
	renderPointCloud(viewer, inputCloud, "inputCloud");

	// TODO:: Create point processor
	ProcessPointClouds pointProcessor;
	std::pair::Ptr, pcl::PointCloud::Ptr> segmentCloud = pointProcessor.SegmentPlane(
		inputCloud, 100, 0.2);
	if (render_obst) {
		renderPointCloud(viewer, segmentCloud.first, "obstCloud", Color(1, 0, 0));
	}
	if (render_plane) {
		renderPointCloud(viewer, segmentCloud.second, "planeCloud", Color(0, 1, 0));
	}

	std::vector::Ptr> cloudClusters = pointProcessor.Clustering(segmentCloud.first, 1.0,3, 30);
	int clusterId = 0;
	std::vector colors = { Color(1, 0, 0), Color(0, 1, 0), Color(0, 0, 1) };
	for (pcl::PointCloud::Ptr cluster : cloudClusters)
	{
		if (render_cluster) 
		{
			std::cout << "cluster size:  ";
			pointProcessor.numPoints(cluster);
			renderPointCloud(viewer, cluster, "obstCLoud" + std::to_string(clusterId),
				colors[clusterId % colors.size()]);
			++clusterId;
		}
		if (render_box) 
		{
			Box box = pointProcessor.BoundingBox(cluster);
			renderBox(viewer, box, clusterId);
		}
		++clusterId;
	}
	renderPointCloud(viewer, segmentCloud.second, "planeCloud");
}



//setAngle: SWITCH CAMERA ANGLE {XY, TopDown, Side, FPS}
void initCamera(CameraAngle setAngle, pcl::visualization::PCLVisualizer::Ptr &viewer) {

	viewer->setBackgroundColor(0, 0, 0);
	// set camera position and angle
	viewer->initCameraParameters();
	// distance away in meters
	int distance = 16;
	switch (setAngle) {
	case XY:
		viewer->setCameraPosition(-distance, -distance, distance, 1, 1, 0);
		break;
	case TopDown:
		viewer->setCameraPosition(0, 0, distance, 1, 0, 1);
		break;
	case Side:
		viewer->setCameraPosition(0, -distance, 0, 0, 0, 1);
		break;
	case FPS:
		viewer->setCameraPosition(-10, 0, 0, 0, 0, 1);
	}
	if (setAngle != FPS)
		viewer->addCoordinateSystem(1.0);
}

// char* argv[] means array of char pointers, whereas char** argv means pointer to a char pointer.
int main(int argc, char **argv) {
	std::cout << "starting enviroment" << std::endl;

	pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer("3D Viewer"));
	CameraAngle setAngle = XY;
	initCamera(setAngle, viewer); //设置不同的观察视角

	// For simpleHighway function
	//    simpleHighway(viewer);
	//    cityBlock(viewer);
	//    while (!viewer->wasStopped ())
	//    {
	//     viewer->spinOnce ();
	//    }
	//
	//  Stream cityBlock function
	//  ProcessPointClouds 类
	ProcessPointClouds *pointProcessorI = new ProcessPointClouds();
	std::vector stream = pointProcessorI->streamPcd("D:\\SFND_Lidar_Obstacle_Detection\\SFND_Lidar_Obstacle_Detection\\data\\pcd\\data_2");
	auto streamIterator = stream.begin();//从文件中的第一个点云文件开始 
	pcl::PointCloud::Ptr inputCloudI; //创建点云对象

	while (!viewer->wasStopped()) {
		// Clear viewer
		viewer->removeAllPointClouds();
		viewer->removeAllShapes();
		//inputCloudI 为每一帧点云数据
		inputCloudI = pointProcessorI->loadPcd((*streamIterator).string()); //对每一个点云进行处理
		cityBlock(viewer, pointProcessorI, inputCloudI);//主要处理程序 参数为视窗、点云处理类、点云
		streamIterator++;
		if (streamIterator == stream.end()) {
			streamIterator = stream.begin();
		}
		viewer->spinOnce();
	}
}